cartographer Tsdf insert

1. range insert tsdf

参数：const sensor::RangeData& range_data, GridInterface* grid
即：激光点云数据 + 栅格

1. 首先获得截断距离 默认0.2m， 从.lua中通过proto中读取
2. 根据truncation_distance，range_data，tsdf 确定地图是否增长使得 tsdf地图包含所有的点云数据
3. 如果需要计算法向量 (周围多个hit点的拟合)
   -激光法线的距离or 射线激光扫描角度法线的权重 更新时，计算法线
   -将激光点排序 --一顿神操作
   -为“ range_data”中的每个“ return”估算法线
4. 遍历所有hit点，插入hit点，hit,origin,normal,tsdf
5. tsdf更新完成 更新状态update_indices_刷新，该状态防止一帧激光数据多次更新一个栅格点

权重：距离 * 角度 * 法线
更新：线上的 截断距离

1.1地图是否生长：

Grid2d中，与概率栅格一样

1. 遍历激光点云$p_i(x,y)$。
   对于每个点云，首先计算机器人到该点$p_i(x,y)$的向量，$p_i-p_r$
   然后确定该点云可能到达的最大距离$p_i+truncatio{n}_{d}istance\ast (p_i-p_r)$
   最后将该点压如边界容器中，
2. 根据边界容器中最大值和最小值两个点确定是否增长，具体做法如下，
3. 判断该点是否被包含在该栅格中（limits_.Contains），若包含则退出，否则执行4
4. 主要是扩张栅格地图（Grid2D类），长、宽分别扩大一倍，即面积扩大4倍。具体做法如下：
5. 根据当前长宽重新确定新栅格的，根据原栅格原点计算新原点， $ma{x}_{new}=max+resolution\ast \frac{长}{2}$，以及新的长、宽，为2倍关系。
6. 将原来栅格中的数据拷贝到新该栅格中，涉及到stride(新栅格的宽度)，offset(旧栅格在新栅格中第一个数据$(x_{offset}+stride\ast y_{offset}$)
7. 即可将就数据拷贝到新的数据中，行、列两层for循环，设定新栅格每行的个数，并将旧行中数据拷贝进行，其中涉及到：坐标使用：$offset+j+i\ast 新宽stride$和$j+i\ast 旧宽$
8. 最后将旧栅格中的known_cells_box_根据范围确定新栅格的known_cells_box_。

1.2 求激光点的法线

• 为“ range_data”中的每个“ return”估算法线

1. 确定法线计算中 样本的最大个数 和 半径
2. 遍历每一个激光点 ，计算每个点的法向量，做如下操作：
   for (size_t current_point = 0; current_point < range_data.returns.size(); ++current_point)
3. 以当前点为中心向两边搜索采样点，一边一半点 ， 得出左右的起始点，用于计算法线
4. 当end-begin 点个数小于2 ，则 直接返回 $atan((\frac{y}{x}{)}_{origin-point})$
5. 找出 begin -> end 中与当前点距离满足一定条件的点，对每个点与当前point计算垂线
   当前点与得出 $delta(-y,x)=point-sampl{e}_{p}oint$
6. 保证法向量与激光方向同向
7. 多个采样点计算法向量求取平均，并返回法向量角度

1.3 插入tsdf

1. 当hit - origin 长度range小于截断距离时，该点直接return
2. 计算截断比例 truncation_ratio = truncation_distance / range
3. 判断是否更新fee，若不更新，则起点为 start = origin +(1-truncation_ratio) * ray
4. 终点 end = origin + (1.0f + truncation_ratio) * ray
5. 分辨率提高1000倍，得出新起始点 new_start，new_end
6. 进而得出 该线上的 所有点 vector<new_start->new_end>
7. 计算角度权重因子：
   weight_factor_angle_ray_normal = 当前激光的角度与法线夹角的差带入一个高斯分布
   计算 前面法线角度与当前角度之差 angle_ray_normal = (normal - atan(origin-hit) )
   用射线与对应的法向量之间的夹角构建高斯分布权重 (angle_ray_normal,sigma读取)
   weight_factor_angle_ray_normal = 上述
8. 计算长度的权重因子
   weight_factor_range = 与激光的长度成 类似反比关系
   $weight=\frac{1}{ran{g}^{sigma}}$
9. 更新栅格 遍历 6中 计算的起始点：vector<new_start->new_end> 中的点
   当该点这次被更新过时，跳过，否则 得到格子中心的位姿 (map坐标系)
   计算原点到该更新点中心的距离 distance_cell_to_origin = (cell_center - origin).norm();
   计算 更新距离计算 update_tsd = 激光长度- 原点到该更新点中心的距离
   如果更新的是 点到激光法线的距离 update_tsd = (cell_center - hit) .dot(Eigen::Vector2f{std::cos(normal_orientation), std::sin(normal_orientation)})
   更新距离在一定范围之内 update_tsd = update_tsd在[-truncation_distance, truncation_distance]间
10. 更新权重计算 update_weight = 根据离hit点的距离计算高斯权重 × 角度权重和距离权重
11. 将权重和距离更新到tsdf中 跟新tsdf 更新 权重:累加权重 距离:加权距离值
    updated_weight = tsd_and_weight.second + update_weight;
    updated_sdf = (tsd_and_weight.first * tsd_and_weight.second + update_sdf * update_weight) / updated_weight;

2.tsdf new insert

参数：const sensor::RangeData& range_data, GridInterface* grid。即：激光点云数据 + 栅格

1. 获取截断距离(.lua) ，并将grid转化成子类tsdf
2. 遍历每个hit点，将hit点投影到对应容器中，格式：std::unordered_map<int,std::vector<Eigen::Vector2f>> 每个栅格的相同hit点放到同一容器
   idx求取：hit对应点(x,y),则更新栅格点
3. 遍历 2中的容器，cell_index(idx / limits.cell_limits().num_y_cells, idx % limits.cell_limits().num)
4. 当一个栅格中的点个数小于3 时，临近寻找点，使其达到3个点，小技巧：先找邻域(例如5*5邻域x,y：-2,-1,0,1,2),且记录向外扩张的num。扩张时：根据栅格数一次扩张，最大扩张3个栅格，且有3个点时退出扩张。
5. 当有3个点时，根据激光点 d0, d1, d2 拟合直线，具体算法见Deming regression。
6. 用线更新单元格 cell_index,line,origin,tsdf*,截断距离,扩张尺寸 参数，具体算法见下：

   1. 归一化点斜式直线 k,-1/ sqrt(k²⁺¹2)
   2. 计算 origin -> hit :origin_to_observation = origin - current_cell_center
      并且，与上述直线近似平行
   3. 计算origin_to_observation 与直线的夹角
   4. 计算 idx，cell_index[0]*limits.cell_limits().num_x_cells + cell_index[1];
   5. 更新free_cells = idx，长度，直线向量
      长度 =std::min(truncation_distance/std::abs(std::cos(angle_ray_line)),0.5)
   6. 计算原点 origin 到直线的距离
   7. 计算延伸距离 extention = 0.05 × （1+0.5× extention_size ）
   8. 计算要更新的栅格的索引 cell_offset = int( std::ceil(truncation_distance / limits.resolution()) );
   9. 遍历每个栅格索引，其中索引为 Eigen::Array2i(cell_index[0]+x_offset, cell_index[1]+y_offset);
   10. 若 该点不包含在tsdf图中，则continue，否则，进行11
   11. 求取该点中新坐标 cell_center = limits.GetCellCenter(cell_iter)
   12. 计算该点到直线的距离 distance_to_line = std::abs(ax_iter + by_iter + c) / distance_denominator;
   13. 当距离大于等于截断距离， continue ； 否则：
   14. 若该点的投影在该直线的栅格范围之外则 continue，否则：
   15. 计算priority = cell_center -> origin 的距离priority，
   16. 优先应用单元格ApplyCellWithPriority(cell_iter, distance_to_line, update_weight, priority, tsdf); 具体实现如下:
   17. 计算idx，并取出updated_tsdf_and_weight_[idx] ：value;
   18. 将要把更新的点都存起来
       当 value[2] > priority
       updated_tsdf_and_weight_[idx] = Eigen::Vector3f(sdf_value, weight, priority);
       else if (value[2] == priority)
       float weight_update = (weight + value[1]) / 2.0f;
       float tsdf_update = (sdf_value*weight + value[0]*value[1]) / weight_update;
       weight_update = std::min(static_cast(options_.maximum_weight()), weight_update);
       updated_tsdf_and_weight_[idx] = Eigen::Vector3f(tsdf_update, weight_update, priority);

7. 更新先前点 UpdateWithPriority(TSDF2D* tsdf)
   根据6中计算的点，迭代进行更新，并清除容器
8. 若更新free时，进行free更新，长度已知，更新很容易
9. tsdf更新完成，跟上面类似。