PCL - Global Registration with LUM - point-cloud-library

Question

I'm working on a registration project, I have a chair with some objects rotating in front of a kinect.
I can have successful pairwise registration, but as expected there is some drift (result in image).
I want to use LUM, in order to have a global minimization of the accumulated error (and then "spread" it across frames), but I end up having the frames floating in the air. (code below the image)
Is this there any obvious mistake in the usage of LUM?
---I use keypoints+features, not blindly feeding LUM with full pointclouds
Why all the examples add one-directional edges and not bi-directional?
PARAM_LUM_centroidDistTHRESH = 0.30;
PARAM_LUM_MaxIterations = 100;
PARAM_LUM_ConvergenceThreshold = 0.0f;
int NeighborhoodNUMB = 2;
int FrameDistLOOPCLOSURE = 5;
PARAM_CORR_REJ_InlierThreshold = 0.020;
pcl::registration::LUM<pcl::PointXYZRGBNormal> lum;
lum.setMaxIterations( PARAM_LUM_MaxIterations );
lum.setConvergenceThreshold( PARAM_LUM_ConvergenceThreshold );
QVector< pcl::PointCloud<pcl::PointXYZRGB>::Ptr > cloudVector_ORGan_P_;
for (int iii=0; iii<totalClouds; iii++)
{
// read - iii_cloud_ORGan_P_
// transform it with pairwise registration result
cloudVector_ORGan_P_.append( iii_cloud_ORGan_P_ );
}
for (size_t iii=0; iii<totalClouds; iii++)
{
pcl::compute3DCentroid( *cloudVector_ORGan_P_[iii], centrVector[iii] );
pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB,pcl::Normal> ne;
//blah blah parameters
//compute normals with *ne*
//pcl::removeNaNFromPointCloud
//pcl::removeNaNNormalsFromPointCloud
pcl::ISSKeypoint3D< pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> keyPointDetector;
//blah balh parameters;
//keyPointDetector.compute
//then remove NAN keypoints
pcl::SHOTColorEstimationOMP< pcl::PointXYZRGBNormal,pcl::PointXYZRGBNormal,pcl::SHOT1344 > featureDescriptor;
//featureDescriptor.setSearchSurface( **ful_unorganized_cloud_in_here** );
//featureDescriptor.setInputNormals( **normals_from_above____in_here** );
//featureDescriptor.setInputCloud( **keypoints_from_above__in_here** );
//blah blah parameters
//featureDescriptor.compute
//delete NAN *Feature* + corresp. *Keypoints* with *.erase*
}
for (size_t iii=0; iii<totalClouds; iii++)
{
lum.addPointCloud( KEYptVector_UNorg_P_[iii] );
}
for (size_t iii=1; iii<totalClouds; iii++)
{
for (size_t jjj=0; jjj<iii; jjj++)
{
double cloudCentrDISTANCE = ( centrVector[iii] - centrVector[jjj] ).norm();
if ( (cloudCentrDISTANCE<PARAM_LUM_centroidDistTHRESH && qAbs(iii-jjj)<=NeighborhoodNUMB) ||
(cloudCentrDISTANCE<PARAM_LUM_centroidDistTHRESH && qAbs(iii-jjj)> FrameDistLOOPCLOSURE) )
{
int sourceID;
int targetID;
if (qAbs(iii-jjj)<=NeighborhoodNUMB) // so that connection are e.g. 0->1, 1->2, 2->3, 3->4, 4->5, 5->0
{ // not sure if it helps
sourceID = jjj;
targetID = iii;
}
else
{
sourceID = iii;
targetID = jjj;
}
*source_cloud_KEYpt_P_ = *lum.getPointCloud(sourceID);
*target_cloud_KEYpt_P_ = *lum.getPointCloud(targetID);
*source_cloud_FEATures = *FEATtVector_UNorg_P_[sourceID];
*target_cloud_FEATures = *FEATtVector_UNorg_P_[targetID];
// KeyPoint Estimation
pcl::registration::CorrespondenceEstimation<keyPointTYPE,keyPointTYPE> corrEst;
corrEst.setInputSource( source_cloud_FEATures );
corrEst.setInputTarget( target_cloud_FEATures );
corrEst.determineCorrespondences( *corrAll );
// KeyPoint Rejection
pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZRGBNormal> corrRej;
corrRej.setInputSource( source_cloud_KEYpt_P_ );
corrRej.setInputTarget( target_cloud_KEYpt_P_ );
corrRej.setInlierThreshold( PARAM_CORR_REJ_InlierThreshold );
corrRej.setMaximumIterations( 10000 );
corrRej.setRefineModel( true );
corrRej.setInputCorrespondences( corrAll );
corrRej.getCorrespondences( *corrFilt );
lum.setCorrespondences( sourceID, targetID, corrFilt );
} // if
} // jjj
} // iii
lum.compute();
// PCLVisualizer - show this - lum.getConcatenatedCloud()

Answer 1

After many days of experimenting with LUM, I decided to move to another tool for Graph optimization, namely g2o. You can see the result in the image, it's not perfect (see small translational drift # frontal view), but it's reasonable and much better than simple pairwise incremental registration (no very-apparent rotational drift!),
If you are interested, I propose downloading the github version! It's the most up-to-date, while other versions - like this - are outdated, and personally I had some compilation issues, both when compiling the library itself or my source code)