标签：map pose Eigen Vector3d 激光 param SLAM5 now NDT

1.1 基于优化的方法(Optimization-based Method)

理论部分：




参考代码：

/**
 * @brief ComputeCompleteHessianAndb
 * 计算H*dx = b中的H和b
 * @param map
 * @param now_pose
 * @param laser_pts
 * @param H
 * @param b
 */
void ComputeHessianAndb(map_t* map, Eigen::Vector3d now_pose,
                        std::vector<Eigen::Vector2d>& laser_pts,
                        Eigen::Matrix3d& H, Eigen::Vector3d& b)
{
    H = Eigen::Matrix3d::Zero();
    b = Eigen::Vector3d::Zero();

    Eigen::Matrix3d trans = GN_V2T(now_pose);
    Eigen::Matrix<double, 2, 3> d_st;
    double sin_theta = std::sin(now_pose(2));
    double cos_theta = std::cos(now_pose(2));
    for (const auto& pt : laser_pts) {
        Eigen::Vector2d st = GN_TransPoint(pt, trans);
        Eigen::Vector3d map_value = InterpMapValueWithDerivatives(map, st);
        d_st << 1.0, 0.0, -sin_theta * pt(0) - cos_theta * pt(1),
                0.0, 1.0, cos_theta * pt(0) - sin_theta * pt(1);
        Eigen::Vector3d d_ms = (map_value.tail(2).transpose() * d_st).transpose();
        H += d_ms * d_ms.transpose();
        b += d_ms * (1.0 - map_value(0));
    }
}

标签：map,pose,Eigen,Vector3d,激光,param,SLAM5,now,NDT
来源： https://blog.csdn.net/lsxpu/article/details/122158189

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。