7

私は登録プロジェクトに取り組んでいます。キネクトの前でいくつかのオブジェクトが回転する椅子があります。

ペアワイズ登録を成功させることができますが、予想どおり、多少のドリフトがあります (画像の結果)。

蓄積されたエラーをグローバルに最小化するために LUM を使用したい (そして、それをフレーム全体に「拡散」する) が、最終的にフレームが宙に浮いてしまう。(画像下のコード)

これは、LUM の使用法に明らかな誤りがありますか? --- LUM に完全なポイントクラウドをやみくもにフィードするのではなく、キーポイントと機能を使用します

すべての例で双方向ではなく一方向のエッジが追加されるのはなぜですか?

ここに画像の説明を入力

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()
4

1 に答える 1