PCL - Global Registration with LUM

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)