|
拿到一个算法公式，不知道怎么推的，不知道有没有大佬能帮忙看一下；
首先明确的是采用PL-ICP算法，即误差估计采用点到直线的距离。根据文献知道要求解的问题是个最小二乘问题，下面几个图是文献内容   ，忽略拉格朗日余项得到，图片说明,我根据这个公式写的matlab代码仿真结果不对，然后大佬给我的代码里面 M矩阵是一样的，但是右边g矩阵跟我推出来不一样，我就不知道这个g矩阵他是怎么推的
``` temp_term = point_cloud_new.x(index_new) * sin(point_cloud_ref.gamma(index_ref) - delta_pose.theta) - point_cloud_new.y(index_new) * cos(point_cloud_ref.gamma(index_ref) - delta_pose.theta);
% 采用初始位姿变换后的新建点云对应点
transformed_x = cos(delta_pose.theta)*point_cloud_new.x(index_new) - sin(delta_pose.theta)*point_cloud_new.y(index_new) + delta_pose.x;
transformed_y = sin(delta_pose.theta)*point_cloud_new.x(index_new) + cos(delta_pose.theta)*point_cloud_new.y(index_new) + delta_pose.y;
% 点到直线的距离
point_line_dis = cos_gamma * (transformed_x - point_cloud_ref.x(index_ref)) + sin_gamma * (transformed_y - point_cloud_ref.y(index_ref));
% 计算(H'H)
M(1, 1) = cos_gamma * cos_gamma;
M(1, 2) = cos_gamma * sin_gamma;
M(1, 3) = cos_gamma * temp_term;
M(2, 2) = sin_gamma * sin_gamma;
M(2, 3) = sin_gamma * temp_term;
M(3, 3) = temp_term * temp_term;
% 计算H'h(\Delta x_0,\Delta x_0,\Delta \theta_0)
m(1) = cos_gamma * point_line_dis;
m(2) = sin_gamma * point_line_dis;
m(3) = temp_term * point_line_dis;
% 计算逆矩阵
M_inv = inv(M);
% 计算位姿变换参数的增量
d_pose = -(M_inv * m);
就是这个m矩阵，不知道怎么退出来的
|