手撕SLAM算法

最新推荐文章于 2025-06-23 22:53:33 发布

人间小客

最新推荐文章于 2025-06-23 22:53:33 发布

阅读量231

点赞数

CC 4.0 BY-SA版权

分类专栏：面试刷题文章标签：算法

版权归作者所有，如有转载，请注明转载作者

本文链接：https://blog.youkuaiyun.com/Android_WPF/article/details/125924313

面试刷题专栏收录该内容

11 篇文章

订阅专栏

第一节，ICP算法

$P_2 = P_1+t$
$W = q2*q1^{T}$

/**
 * 手撕ICP算法
 * points2 = R* points1+ t
 * W = q2*q1,transpose()
 * 估计值为R1，t1
 * wpf
 * */
#include <iostream>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
using namespace Eigen;
using namespace std;

int main() {
    //创建一组三维对应点，创建一个旋转矩阵R，一个平移t，求出对应的一组三维点
    const vector<Vector3d> Points1{{2,3,5},{6,3,2},{9,4,2}};
    vector<Vector3d> Points2;
    AngleAxisd rotation_vector(M_PI_4,Vector3d(0,0,1));
    cout.precision(3);
    Matrix3d R = rotation_vector.toRotationMatrix();
    cout<<R<<endl;
    Vector3d t(10,15,84);
    cout<<t<<endl;
    for(int i = 0;i<3;i++){
        Points2.emplace_back(R*Points1[i]+t);
    }
    //去中心化，求和，每个数据减去对应的和，创建矩阵W
    Vector3d p1(0,0,0);
    Vector3d p2(0,0,0);
    for (int i = 0; i < 3; ++i) {
        p1 += Points1[i];
        p2 += Points2[i];
    }
    p1 = p1/3;
    p2 = p2/3;
    vector<Vector3d> q1(3);
    vector<Vector3d> q2(3);
    for (int i = 0; i < 3; ++i) {
        q1[i] = Points1[i]-p1;
        q2[i] = Points2[i]-p2;
    }
    Matrix3d W = Matrix3d::Zero();
    for (int i = 0; i < 3; ++i) {

    W+= Vector3d(q2[i].x(),q2[i].y(),q2[i].z())*Vector3d(q1[i].x(),q1[i].y(),q1[i].z()).transpose();
    }
    //SVD分解
    JacobiSVD<Matrix3d> svd(W,ComputeFullU|Eigen::ComputeFullV);
    Matrix3d U = svd.matrixU();
    Matrix3d V = svd.matrixV();

    Matrix3d R1 = U*(V.transpose());
    if(R1.determinant()<0) R1 = -R1;
    cout<<R1<<endl;
    Vector3d t1 =Vector3d(p2.x(),p2.y(),p2.z())-R1*Vector3d(p1.x(),p1.y(),p1.z());
    cout<<t1.x()<<" "<<t1.y()<<" "<<t1.z()<<endl;
    //利用这个方法求解R
    //求解t
    //输出最后的结果
    std::cout << "Hello, World!" << std::endl;
    return 0;
}

代码运行结果

/home/wpf/Test/study_cpp/ICP/cmake-build-debug/ICP
0.707 -0.707 0
0.707 0.707 0
0 0 1 10 15 84
0.707 -0.707 1.11e-16
0.707 0.707 -3.33e-16
-1.67e-16 2.78e-16 1
10 15 84
Hello, World!

Process finished with exit code 0

代码分析：
由于这里的点仅仅设置3个，所以计算量不大，在实际工程中匹配点的数目也是100左右，运算不会太慢
注意SVD分解的使用