IVC.Ceres scan matching

详细记录一下公式CS

公式CS中，hk表示的是每一帧的激光点，总共有K个，其坐标是相对于激光雷达坐标系中（也就是scan）的坐标。
思路：假设我们将scan的坐标系经过T的坐标变换，计算经过坐标变换之后的scan与submap的一致程度，计算出使得一致程度最大的T，得到最优的位姿估计。
假设T取某一个值，我们hk乘以坐标变换之T之后，转换为submap坐标系中的坐标。submap中该点理应处于占据位置，也就是说submap中该点的占据概率理应为1才是一致，式子中$1-M_{smooth}（T_{\xi }{h}_k）$表示的是经过坐标变换之后的点K的不一致性，所以平方求和求最小，使得这种不一致最小，也就是一致性最大。