计算统一坐标系时遇到问题

[yxecho115]

你好，想请教一个问题：在使用IRLS with history reweighting计算统一坐标系时，输入的pairwise registration的变换矩阵T01、T02、T12分别如下所示：
T01 = [[1.0，-1.2149189387855586e-05， -0.00014383767847903073，-11.460397720336914],
[1.2157798664702568e-05, 1.0, 5.9850561228813604e-05, -1.2087631225585938],
[0.0001438369508832693, -5.9852311096619815e-05, 1.0, -0.43892526626586914],
[0.0, 0.0, 0.0, 1.0]];

T02 = [[1.0，-5.033568413637113e-06， 9.842186409514397e-05，-1.8885232210159302],
[5.042900284024654e-06, 1.0, -9.480899461777881e-05, -11.029406547546387],
[-9.842139115789905e-05, 9.480948938289657e-05, 1.0, -12.239741325378418],
[0.0, 0.0, 0.0, 1.0]];

T12 = [[1.0，-5.77977665727758e-11， 2.6372001804553236e-10，9.558798789978027],
[-5.779732248356595e-11, 1.0, 8.342294910423931e-11, -9.82029914855957],
[-2.637197404897762e-10, -8.342281032636123e-11, 1.0, -11.802338600158691],
[0.0, 0.0, 0.0, 1.0]];

这三个中旋转矩阵都近似于单位矩阵，按理说得到的global registration变换矩阵中旋转矩阵应该也是近似单位矩阵的。
但计算得到的global registration的变换矩阵是类似这个数值范围的：
T0 = [[-0.7116015140757603，-0.6879857888161842， 0.14247399605562958，-5.370030503337107],
[-0.10789030996916145, -0.09337589548789123, -0.9897679643010234, -5.043447974465397],
[0.6942499306300612, -0.7196919455772777, -0.007780571402392206, 0.1193426041857415],
[0.0, 0.0, 0.0, 1.0]];
从而导致最后配准后的点云和原始点云位置偏移很大，而且还有很大的旋转。

请问这是什么原因导致的？如何解决呢？十分感谢！

[yxecho115]

另外我统一使用R=eye3*3，t=[1, 1, 1]等值，测试了一下，得到的统一变换矩阵也都不是由单位旋转矩阵构成。

[HpWang-whu]

你好 @yxecho115,
这应该是因为SGHR中使用的旋转解算并没有指定参考点云，而是使用间接平差(而非附有条件的间接平差)的方式得到的(局部)最优解，也就是没有点云的位姿被设置为单位阵。
如果输入的R=eye3*3，得到的点云的旋转应该都是一致的（但不是单位矩阵），如果想要指定某个点云为参考点云，只需要所有点云位姿乘以参考点云位姿的逆即可，这样应该就都是单位矩阵了。