## 实现高斯牛顿法进行点云配准

### 高斯牛顿法相关代码

`gaussian_new_method.cpp` 主要提供了关于高斯牛顿法的几个函数，有几个比较简单就不说了，对要补充的三个函数下面进行介绍。

• `Eigen::Vector3d InterpMapValueWithDerivatives(map_t* map,Eigen::Vector2d& coords)`：通过周围四个已知点插值计算出指定坐标点的势场值和梯度，这里可以用课程中提到的拉格朗日双方向插值来进行计算。注意计算出来的梯度需要考虑到尺度的影响。补充代码如下：
``````Eigen::Vector3d InterpMapValueWithDerivatives(map_t* map,Eigen::Vector2d& coords)
{
Eigen::Vector3d ans;

/// calculate coordinates on the given map (noticed: might not be exact interger)
/// remember the origin of map locates in center of the map thus need to plus half of size to get the real index
double index_x = (coords(0) - map->origin_x) / map->resolution + map->size_x / 2;
double index_y = (coords(1) - map->origin_y) / map->resolution + map->size_y / 2;
int16_t index_x0 = floor(index_x);
int16_t index_y0 = floor(index_y);

/// calculate u, v using four nearest points
double u, v;
u = index_x - index_x0;
v = index_y - index_y0;

/// calcualte scores for four nearest points
double z1 = map->cells[MAP_INDEX(map, index_x0, index_y0)].score;
double z2 = map->cells[MAP_INDEX(map, index_x0 + 1, index_y0)].score;
double z3 = map->cells[MAP_INDEX(map, index_x0 + 1, index_y0 + 1)].score;
double z4 = map->cells[MAP_INDEX(map, index_x0, index_y0 + 1)].score;

/// score of given coordinate in the map
ans(0) = (1 - u) * (1 - v) * z1 + u * (1 - v) * z2 + u * v * z3 + (1 - u) * v *z4;

/// noticed: need to remove scale influence by using resolution
ans(1) = (v * (z3 - z4) + (1 - v) * (z2 - z1)) / map->resolution;
ans(2) = (u * (z3 - z2) + (1 - u) * (z4 - z1)) / map->resolution;

return ans;
}
``````
• `void ComputeHessianAndb(map_t* map, Eigen::Vector3d now_pose,std::vector<Eigen::Vector2d>& laser_pts,Eigen::Matrix3d& H, Eigen::Vector3d& b)` ： 计算 H 和 b 用来进行高斯优化，这里通过笔记中的方法计算即可，需要注意的是激光点和机器人的相对以及绝对 pose 的关系，不要用混，以及 J 矩阵的维度：
``````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();

for (Eigen::Vector2d pt: laser_pts) {

/// compute laser pt pose globally
Eigen::Vector2d pt_pose = GN_TransPoint(pt, now_pose);

Eigen::Matrix<double, 2, 3> ds;
ds << 1, 0, -sin(now_pose(2) * pt(0)) - cos(now_pose(2)) * pt(1),
0, 1, cos(now_pose(2) * pt(0)) - sin(now_pose(2)) * pt(1);

/// compute score & grident of that point in map
Eigen::Vector3d score_gradient = InterpMapValueWithDerivatives(map, pt_pose);
double score = score_gradient(0);

/// noticed the dimension of J should 1 x 3
Eigen::RowVector3d J = gradient.transpose() * ds;
H += J.transpose() * J;
b += J.transpose() * (1 - score);

}

}
``````
• `void GaussianNewtonOptimization(map_t*map,Eigen::Vector3d& init_pose,std::vector<Eigen::Vector2d>& laser_pts)`: 进行高斯优化，这一部分按照常规操作即可
``````void GaussianNewtonOptimization(map_t*map,Eigen::Vector3d& init_pose,std::vector<Eigen::Vector2d>& laser_pts)
{
int maxIteration = 20;
Eigen::Vector3d now_pose = init_pose;
Eigen::Matrix3d H;
Eigen::Vector3d b;

for(int i = 0; i < maxIteration;i++)
{

ComputeHessianAndb(map, now_pose, laser_pts, H, b);

Eigen::Vector3d delta_x = H.colPivHouseholderQr().solve(b);

now_pose += delta_x;
}
init_pose = now_pose;

}
``````

## 对上一题算法进行改进的方案

• 使用另一种算法进行迭代，比如 LM 算法
• 对点云进行预处理，进行运动畸变的去除，过滤噪声等
• 使用更多点进行插值

## 分支定界相关

Built with Hugo
Theme Stack designed by Jimmy