非线性最小化问题:
其中 ,是2×2的旋转矩阵
范数
对于PP-ICP来说,,为权重
对于PL-ICP来说,
简化为最小二乘问题:
上述问题的解为,用四维向量表示,并且约束。
对于给定点, 定义矩阵:
上述最小化问题可转化为:
展开有:
忽略常数项:
定义,约束可写成。
问题即可转化为:
subject to
使用拉格朗日乘子求解:
定义函数:
最优性条件 :
解为:
回代到约束中有:
上式为的四次多项式。
将分成四块:
由于(25)中的稀疏的,所以仅需计算的最后一列。
其中
代入(25)中,得到
将写成如下形式:
其中:
由于
最终可得到:
求解方程(31),参照gpc_solve()函数:(偷懒,矩阵很向量写法不规范)
令:
式(31)展开:
令:
各个系数整合 /* q = p - l^2 */ ,得到5个系数项。然后用Eigen::PolynomialSolver求解得到