深蓝slam作业

最新推荐文章于 2024-09-13 16:27:15 发布

rain111i

最新推荐文章于 2024-09-13 16:27:15 发布

阅读量249

点赞数 1

文章标签：算法

本文链接：https://blog.csdn.net/2301_80193801/article/details/139408222

版权

第二节解决Odom_Calib.cpp中solve函数：中有一个这样的代码

Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;
 
    //TODO: 求解线性最小二乘
    // Eigen::Matrix<double, 9, 1> params;
    // // params = (A.transpose() * A).inverse() * A.transpose() * b;
    // params = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    // std::cout << params.transpose() << std::endl;
    // for (size_t i = 0; i < 3; i++) {
    //     correct_matrix.block<1, 3>(i, 0) = params.block<3, 1>(i * 3, 0).transpose();
    // }
    Eigen::Matrix<double,9,1> X_ = A.colPivHouseholderQr().solve(b);
    correct_matrix << X_(0),X_(1),X_(2),X_(3),X_(4),X_(5),X_(6),X_(7),X_(8);
    //end of TODO
 
    return correct_matrix;
}

此处的Eigen::Matrix<double,9,1> X_ = A.colPivHouseholderQr().solve(b);使用来解方程Ax=b的；函数 A.colPivHouseholderQr().solve(b)。就是一个解这个函数的的函数。

构造最小二乘的函数：

bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{

    if(now_len<INT_MAX)
    {
        //TODO: 
        A(now_len%data_len*3,0)=Odom(0);
        A(now_len%data_len*3,1)=Odom(1);
        A(now_len%data_len*3,2)=Odom(2);
        A(now_len%data_len*3+1,3)=Odom(0);
        A(now_len%data_len*3+1,4)=Odom(1);
        A(now_len%data_len*3+1,5)=Odom(2);
        A(now_len%data_len*3+2,6)=Odom(0);
        A(now_len%data_len*3+2,7)=Odom(1);
        A(now_len%data_len*3+2,8)=Odom(2);

        b(now_len%data_len*3)=scan(0);
        b(now_len%data_len*3+1)=scan(1);
        b(now_len%data_len*3+2)=scan(2);

        //end of TODO
        now_len++;
        return true;
    }
    else
    {
        return false;
    }
}

计算位姿差函数：

//求解得到两帧数据之间的位姿差
//即求解当前位姿　在　上一时刻　坐标系中的坐标
Eigen::Vector3d cal_delta_distence(Eigen::Vector3d odom_pose)
{
 
    Eigen::Vector3d d_pos; //return value
    now_pos = odom_pose;
    //TODO:
    //向量旋转的方方式，轴角
    d_pos = now_pos - last_pos;
    Eigen::AngleAxisd temp(last_pos(2),Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d trans=temp.matrix().inverse();
    d_pos = trans*d_pos;
    last_pos = now_pos; 
//方法二矩阵的方式
//    Eigen::Matrix3d trans_temp;
//    trans_temp<< cos(last_pos(2)), -sin(last_pos(2)), 0,
//            sin(last_pos(2)), cos(last_pos(2)),  0,
//            0,                           0,                       1;
//
//    d_pos = now_pos - last_pos;
//    d_pos = trans_temp.transpose()*d_pos;
//    last_pos = now_pos;
 
    //end of TODO:
    return d_pos;
}
----------------------------
Eigen::Vector3d  cal_delta_distence(Eigen::Vector3d odom_pose)
{

    Eigen::Vector3d d_pos;  //return value
    now_pos = odom_pose;

    //TODO:

    Eigen::Matrix3d Tnow,Tprev;

    double theta = last_pos(2);
    double x = last_pos(0);
    double y = last_pos(1);

    Tprev << cos(theta),-sin(theta),x,
             sin(theta), cos(theta),y,
                      0,          0,       1;

   
    x = now_pos(0);
    y = now_pos(1);
    theta = now_pos(2);
    Tnow << cos(theta),-sin(theta),x,
             sin(theta), cos(theta),y,
                      0,          0,       1;

 
    Eigen::Matrix3d T = Tprev.inverse() * Tnow;

    d_pos(0) = T(0,2);
    d_pos(1) = T(1,2);
    d_pos(2) = atan2(T(1,0),T(0,0));

    //end of TODO:

    return d_pos;
}

svd分解的意思是将矩阵携程由三个部分相乘的形式

第三节：运动畸变去除

 void Lidar_MotionCalibration(
        tf::Stamped<tf::Pose> frame_base_pose,
        tf::Stamped<tf::Pose> frame_start_pose,
        tf::Stamped<tf::Pose> frame_end_pose,
        std::vector<double>& ranges,
        std::vector<double>& angles,
        int startIndex,
        int& beam_number)
    {
        //TODO
        // frame_base_pose.inverse;
        tf::Quaternion start_q = frame_start_pose.getRotation();
        tf::Quaternion end_q = frame_end_pose.getRotation();
        tf::Vector3 start_xy(frame_start_pose.getOrigin().getX(), frame_start_pose.getOrigin().getY(), 1);
        tf::Vector3 end_xy(frame_end_pose.getOrigin().getX(), frame_end_pose.getOrigin().getY(), 1);
 
        for (size_t i = startIndex; i < startIndex + beam_number; i++) {
            tf::Vector3 mid_xy = start_xy.lerp(end_xy, (i - startIndex) / (beam_number - 1));
            tf::Quaternion mid_q = start_q.slerp(end_q, (i - startIndex) / (beam_number - 1));
            tf::Transform mid_frame(mid_q, mid_xy);
            double x = ranges[i] * cos(angles[i]);
            double y = ranges[i] * sin(angles[i]);
            tf::Vector3 calib_point = frame_base_pose.inverse() * mid_frame * tf::Vector3(x, y, 1);
            ranges[i] = sqrt(calib_point[0] * calib_point[0] + calib_point[1] * calib_point[1]);
            angles[i] = atan2(calib_point[1], calib_point[0]);
        }
        //end of TODO
    }

原文链接：https://blog.csdn.net/p942005405/article/details/119494918

第六节：图优化：激光SLAM理论与实践第六次作业（G2o 优化方法）_激光slam从理论到实践作业-CSDN博客