I am working on a registration project, I have a chair with some objects rotating in front of a kinek.
I can have successful pair registration, but, as expected, there is some drift (result in the image).
I want to use LUM to have a global minimization of the accumulated error (and then “spread” it over the frames), but in the end I have frames floating in the air. (code below the image)
Is there an obvious mistake in using LUM? --- I use key points + functions, and not blind power LUM with full pointclouds
Why do all examples add unidirectional edges rather than bidirectional?
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 eg 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()
source share