3

2 つの 3D 点群間の剛体変換行列を推測しようとしています。2 つの点群は次のとおりです。

  • kinect からのキーポイント (kinect_keypoints)。
  • 3D オブジェクト (ボックス) からのキーポイント (object_keypoints)。

私は2つのオプションを試しました：

[1]。剛体変換を見つけるアルゴリズムの実装。

**1.Calculate the centroid of each point cloud.**

**2.Center the points according to the centroid.**

**3. Calculate the covariance matrix**
cvSVD( &_H, _W, _U, _V,  CV_SVD_U_T );
cvMatMul( _V,_U, &_R );
**4. Calculate the rotartion matrix using the SVD descomposition of the covariance matrix**

float _Tsrc[16] = { 1.f,0.f,0.f,0.f,
    0.f,1.f,0.f,0.f,
    0.f,0.f,1.f,0.f,
    -_gc_src.x,-_gc_src.y,-_gc_src.z,1.f };  // 1: src points to the origin
float _S[16] = { _scale,0.f,0.f,0.f,
    0.f,_scale,0.f,0.f,
    0.f,0.f,_scale,0.f,
    0.f,0.f,0.f,1.f };  // 2: scale the src points
float _R_src_to_dst[16] = { _Rdata[0],_Rdata[3],_Rdata[6],0.f, 
    _Rdata[1],_Rdata[4],_Rdata[7],0.f,
    _Rdata[2],_Rdata[5],_Rdata[8],0.f,
    0.f,0.f,0.f,1.f }; // 3: rotate the scr points
float _Tdst[16] = { 1.f,0.f,0.f,0.f, 
    0.f,1.f,0.f,0.f, 
    0.f,0.f,1.f,0.f, 
    _gc_dst.x,_gc_dst.y,_gc_dst.z,1.f }; // 4: from scr to dst

// _Tdst * _R_src_to_dst * _S * _Tsrc
mul_transform_mat( _S, _Tsrc, Rt );
mul_transform_mat( _R_src_to_dst, Rt, Rt );
mul_transform_mat( _Tdst, Rt, Rt );

[2]。opencv の推定アフィン 3D を使用します。

        float _poseTrans[12];
        std::vector<cv::Point3f> first, second;             
        cv::Mat aff(3,4,CV_64F, _poseTrans);
        std::vector<cv::Point3f> first, second; (first-->kineckt_keypoints and second-->object_keypoints)
        cv::estimateAffine3D( first, second, aff, inliers );

        float _poseTrans2[16];

        for (int i=0; i<12; ++i)
        {
            _poseTrans2[i] = _poseTrans[i];
        }

        _poseTrans2[12] = 0.f;
        _poseTrans2[13] = 0.f;
        _poseTrans2[14] = 0.f;
        _poseTrans2[15] = 1.f;

最初の問題は変換が正しくないことであり、2 番目の問題では、kinect ポイント クラウドに結果のマトリックスを乗算すると、一部の値が無限になります。

これらのオプションのいずれかからの解決策はありますか? または、PCL 以外の代替手段はありますか?

前もって感謝します。

4

1 に答える 1