三点确定刚体变换，但为什么配准还是会出错
三点为什么可以完成粗配准
假如说，我们有两组一一对应的点，且每组点的变换都是固定的，那我们其实可以完美的完成配准拿到旋转矩阵和平移。
这个地方不难证明，我们只需要加载变换前后（只经过旋转平移变换）的两组点进行配准即可。
// Distance BEFORE registration
PrintPairwise("BEFORE coarse registration: src vs tgt (per-landmark)", src, tgt);
// Rigid landmark alignment: finds R, t that maps src -> tgt
auto landT = vtkSmartPointer<vtkLandmarkTransform>::New();
landT->SetSourceLandmarks(src);
landT->SetTargetLandmarks(tgt);
landT->SetModeToRigidBody();
landT->Update();
vtkMatrix4x4* M = landT->GetMatrix();
PrintMatrix4x4(M, "Recovered 4x4 (src -> tgt) from 3 landmarks");
// Apply the recovered transform to the source landmarks
auto aligned = vtkSmartPointer<vtkPoints>::New();
aligned->DeepCopy(src);
vtkIdType n = aligned->GetNumberOfPoints();
for (vtkIdType i = 0; i < n; ++i) {
double p[3];
aligned->GetPoint(i, p);
double q[3];
// Apply M * [p; 1]
double x = M->GetElement(0,0)*p[0] + M->GetElement(0,1)*p[1] + M->GetElement(0,2)*p[2] + M->GetElement(0,3);
double y = M->GetElement(1,0)*p[0] + M->GetElement(1,1)*p[1] + M->GetElement(1,2)*p[2] + M->GetElement(1,3);
double z = M->GetElement(2,0)*p[0] + M->GetElement(2,1)*p[1] + M->GetElement(2,2)*p[2] + M->GetElement(2,3);
q[0] = x; q[1] = y; q[2] = z;
aligned->SetPoint(i, q);
}
// Distance AFTER registration
PrintPairwise("AFTER coarse registration: src_aligned vs tgt (per-landmark)", aligned, tgt);
输出结果：
==== BEFORE coarse registration: src vs tgt (per-landmark) ====
idx 0 dist = 196.771976 mm
idx 1 dist = 193.225195 mm
idx 2 dist = 190.270620 mm
mean = 193.422597 RMSE = 193.440857 max = 196.771976 (idx 0)
===========================
==== Recovered 4x4 (src -> tgt) from 3 landmarks ====
0.991941 0.034640 -0.121870 -8.770897
-0.024152 0.995958 0.086507 4.528239
0.124374 -0.082867 0.988769 -9.568360
0.000000 0.000000 0.000000 1.000000
====================
==== AFTER coarse registration: src_aligned vs tgt (per-landmark) ====
idx 0 dist = 0.000017 mm
idx 1 dist = 0.000000 mm
idx 2 dist = 0.000017 mm
mean = 0.000011 RMSE = 0.000014 max = 0.000017 (idx 0)
===========================
这个地方我们可以看到结果可以说明，两组点已经完美的对应上了。
实际中的“噪声”
我们知道，这里的配准其实就是在求一个刚体变换，但往往实际没有上面的实验那么简单，在手术导航中之所以有难度是因为医生的选点和规划点不是只靠一个变换就都可以得到的，可能是人工的误差；可能是探针的计算误差；可能是等等等等因素。那么这个地方我们就引入了噪声的概念。
现在我们简单一点，我们先只在其中一个点加入噪声，看看会发生什么：
double p0[3];
src->GetPoint(0, p0);
p0[0] += 1.0; // X轴 +1 mm
src->SetPoint(0, p0);
输出结果：
==== BEFORE coarse registration: src vs tgt (per-landmark) ====
idx 0 dist = 197.612331 mm
idx 1 dist = 193.225195 mm
idx 2 dist = 190.270620 mm
mean = 193.702715 RMSE = 193.726197 max = 197.612331 (idx 0)
===========================
==== Recovered 4x4 (src -> tgt) from 3 landmarks ====
0.989615 0.054146 -0.133153 9.054733
-0.042082 0.994888 0.091803 2.449190
0.137443 -0.085246 0.986835 -11.052786
0.000000 0.000000 0.000000 1.000000
====================
==== AFTER coarse registration: src_aligned vs tgt (per-landmark) ====
idx 0 dist = 0.178478 mm
idx 1 dist = 0.139641 mm
idx 2 dist = 0.214425 mm
mean = 0.177515 RMSE = 0.180122 max = 0.214425 (idx 2)
===========================
是的，两组点之间开始有误差了，但是为什么我只在第一个点的x方向上加了一毫米 三个点都会产生一定程度的偏移呢？ \(\min_{R,t} \sum_{i=1}^N \|RP_i + t - Q_i\|^2\) 程序拿到三个点之后，不知道谁是“好点”，不知道谁是“坏点”，权重都一样，所以我们这里要找到一个变换使得大家整体的误差最小，而不是保证其中几个点完全匹配从而让另外一个点差很远。（见上面公式）
现在我们看到，整体的误差其实还是比较小的，但是粗配准是为了给精配准服务的，他提供了一个基本的位姿。精配准对这个初始的位姿是非常依赖的，一个好的初始位姿可以大大拉高精配准的下限。
这个地方我还是只让第一个点在x方向上加1，看下在精配准的点在只是经过初始位姿变换后是什么样子的：
==== BEFORE coarse registration: src landmark vs tgt landmark ====
idx 0 dist = 197.612331 mm
idx 1 dist = 193.225195 mm
idx 2 dist = 190.270620 mm
mean = 193.702715 RMSE = 193.726197 max = 197.612331 (idx 0)
===========================
==== Recovered 4x4 (src -> tgt) from 3 landmarks ====
0.989615 0.054146 -0.133153 9.054733
-0.042082 0.994888 0.091803 2.449190
0.137443 -0.085246 0.986835 -11.052786
0.000000 0.000000 0.000000 1.000000
====================
==== AFTER coarse registration: aligned landmark vs tgt landmark ====
idx 0 dist = 0.178478 mm
idx 1 dist = 0.139641 mm
idx 2 dist = 0.214425 mm
mean = 0.177515 RMSE = 0.180122 max = 0.214425 (idx 2)
===========================
==== FULL POINTS: aligned source all points vs target all points ====
idx 0 dist = 0.929222 mm
idx 1 dist = 0.935586 mm
idx 2 dist = 0.724332 mm
idx 3 dist = 0.424718 mm
idx 4 dist = 0.108687 mm
idx 5 dist = 0.305733 mm
idx 6 dist = 0.821122 mm
idx 7 dist = 0.479635 mm
idx 8 dist = 0.434446 mm
idx 9 dist = 0.867656 mm
idx 10 dist = 0.751247 mm
idx 11 dist = 0.686804 mm
idx 12 dist = 0.120900 mm
idx 13 dist = 0.203745 mm
idx 14 dist = 0.178629 mm
idx 15 dist = 0.301882 mm
idx 16 dist = 0.430832 mm
idx 17 dist = 0.590086 mm
idx 18 dist = 0.784816 mm
idx 19 dist = 1.059872 mm
idx 20 dist = 0.942134 mm
idx 21 dist = 1.015027 mm
idx 22 dist = 1.019704 mm
idx 23 dist = 1.042373 mm
idx 24 dist = 0.885994 mm
idx 25 dist = 0.864768 mm
idx 26 dist = 0.691435 mm
idx 27 dist = 0.503406 mm
idx 28 dist = 0.609591 mm
idx 29 dist = 0.187736 mm
idx 30 dist = 0.265118 mm
idx 31 dist = 0.916349 mm
idx 32 dist = 0.697933 mm
mean = 0.629743 RMSE = 0.696219 max = 1.059872 (idx 19)
这个地方我打印出了每一个点与对应点之间的距离，32个点在经过初始位姿变换后的误差可以说是翻了三倍。这还只是我只动了其中一个点在x方向偏了1 mm的情况，真实情况可能比这个糟糕的多，再经过精配准不断迭代陷入局部最优后，最后得出来的结果可以说是灾难的。
误差对配准的影响
现在我们已经知道了，一个点便宜一毫米的误差对于整体的影响，且人工选点一定存在误差。那么，一个更加实际的问题来了。既然误差不可避免，那有没有办法让同样大小的误差，对配准矩阵影响更小？
为了回答这个问题，我保持以下条件全部一致：
- 三个Landmark数量始终为3；
- 每组Landmark都加入相同的1 mm扰动；
- 骨模型、验证点、配准算法完全一致。
我这里只改变了三个点在髋臼的分布，分成了三组A、B、C：
数据如下：
==== BEFORE registration: src32 vs tgt32 (no transform) ====
idx 0 dist = 193.850850 mm
idx 1 dist = 193.406997 mm
idx 2 dist = 194.368492 mm
idx 3 dist = 190.518493 mm
idx 4 dist = 190.400876 mm
idx 5 dist = 191.215666 mm
idx 6 dist = 195.164780 mm
idx 7 dist = 195.935436 mm
idx 8 dist = 196.050034 mm
idx 9 dist = 196.380151 mm
idx 10 dist = 196.902932 mm
idx 11 dist = 195.894701 mm
idx 12 dist = 194.218807 mm
idx 13 dist = 194.599825 mm
idx 14 dist = 194.349398 mm
idx 15 dist = 192.314404 mm
idx 16 dist = 190.056263 mm
idx 17 dist = 190.500445 mm
idx 18 dist = 191.025506 mm
idx 19 dist = 191.574501 mm
idx 20 dist = 192.228293 mm
idx 21 dist = 193.074556 mm
idx 22 dist = 193.784392 mm
idx 23 dist = 194.418649 mm
idx 24 dist = 195.036733 mm
idx 25 dist = 195.623982 mm
idx 26 dist = 196.222794 mm
idx 27 dist = 196.551229 mm
idx 28 dist = 196.801000 mm
idx 29 dist = 197.032094 mm
idx 30 dist = 197.061324 mm
idx 31 dist = 197.017811 mm
mean = 194.174419 RMSE = 194.187306 max = 197.061324 (idx 30)
===========================
############### GROUP A ###############
Read E:/test/CMakeProject1/landmark test/A_3point_src.txt count=3
Read E:/test/CMakeProject1/landmark test/A_3point_tgt.txt count=3
==== Recovered 4x4 from group A ====
0.993741 0.023318 -0.109251 -26.955465
-0.014800 0.996832 0.078145 12.291654
0.110727 -0.076039 0.990938 -7.284571
0.000000 0.000000 0.000000 1.000000
====================
==== GROUP A aligned src32 vs tgt32 (verification) ====
idx 0 dist = 0.168718 mm
idx 1 dist = 0.257376 mm
idx 2 dist = 0.260325 mm
idx 3 dist = 0.620717 mm
idx 4 dist = 0.686596 mm
idx 5 dist = 0.633127 mm
idx 6 dist = 0.388021 mm
idx 7 dist = 0.233166 mm
idx 8 dist = 0.225284 mm
idx 9 dist = 0.223883 mm
idx 10 dist = 0.207720 mm
idx 11 dist = 0.373703 mm
idx 12 dist = 0.388178 mm
idx 13 dist = 0.408625 mm
idx 14 dist = 0.514627 mm
idx 15 dist = 0.632199 mm
idx 16 dist = 0.820744 mm
idx 17 dist = 0.795010 mm
idx 18 dist = 0.749099 mm
idx 19 dist = 0.697128 mm
idx 20 dist = 0.629360 mm
idx 21 dist = 0.542227 mm
idx 22 dist = 0.469793 mm
idx 23 dist = 0.413429 mm
idx 24 dist = 0.364904 mm
idx 25 dist = 0.329610 mm
idx 26 dist = 0.324581 mm
idx 27 dist = 0.340556 mm
idx 28 dist = 0.374310 mm
idx 29 dist = 0.403346 mm
idx 30 dist = 0.424700 mm
idx 31 dist = 0.447732 mm
mean = 0.448400 RMSE = 0.483222 max = 0.820744 (idx 16)
===========================
############### GROUP B ###############
Read E:/test/CMakeProject1/landmark test/B_3point_src.txt count=3
Read E:/test/CMakeProject1/landmark test/B_3point_tgt.txt count=3
==== Recovered 4x4 from group B ====
0.993756 0.021270 -0.109531 -27.138996
-0.012337 0.996589 0.081603 7.579020
0.110893 -0.079743 0.990628 -7.800784
0.000000 0.000000 0.000000 1.000000
====================
==== GROUP B aligned src32 vs tgt32 (verification) ====
idx 0 dist = 0.176317 mm
idx 1 dist = 0.295185 mm
idx 2 dist = 0.283817 mm
idx 3 dist = 0.673278 mm
idx 4 dist = 0.745531 mm
idx 5 dist = 0.689568 mm
idx 6 dist = 0.391621 mm
idx 7 dist = 0.186128 mm
idx 8 dist = 0.154620 mm
idx 9 dist = 0.304693 mm
idx 10 dist = 0.121840 mm
idx 11 dist = 0.341578 mm
idx 12 dist = 0.525737 mm
idx 13 dist = 0.468971 mm
idx 14 dist = 0.535643 mm
idx 15 dist = 0.703012 mm
idx 16 dist = 0.905249 mm
idx 17 dist = 0.890606 mm
idx 18 dist = 0.856176 mm
idx 19 dist = 0.816632 mm
idx 20 dist = 0.761279 mm
idx 21 dist = 0.686757 mm
idx 22 dist = 0.623473 mm
idx 23 dist = 0.574602 mm
idx 24 dist = 0.526573 mm
idx 25 dist = 0.478589 mm
idx 26 dist = 0.445946 mm
idx 27 dist = 0.433947 mm
idx 28 dist = 0.438721 mm
idx 29 dist = 0.437281 mm
idx 30 dist = 0.437026 mm
idx 31 dist = 0.435173 mm
mean = 0.510799 RMSE = 0.554452 max = 0.905249 (idx 16)
===========================
############### GROUP C ###############
Read E:/test/CMakeProject1/landmark test/C_3point_src.txt count=3
Read E:/test/CMakeProject1/landmark test/C_3point_tgt.txt count=3
==== Recovered 4x4 from group C ====
0.995420 0.008283 -0.095235 -47.637428
-0.000719 0.996860 0.079185 7.964192
0.095591 -0.078753 0.992300 -6.126265
0.000000 0.000000 0.000000 1.000000
====================
==== GROUP C aligned src32 vs tgt32 (verification) ====
idx 0 dist = 0.278493 mm
idx 1 dist = 0.402777 mm
idx 2 dist = 0.258873 mm
idx 3 dist = 1.296968 mm
idx 4 dist = 1.440469 mm
idx 5 dist = 1.261397 mm
idx 6 dist = 0.388899 mm
idx 7 dist = 0.114230 mm
idx 8 dist = 0.674219 mm
idx 9 dist = 1.031678 mm
idx 10 dist = 0.356897 mm
idx 11 dist = 0.293526 mm
idx 12 dist = 1.291240 mm
idx 13 dist = 0.900111 mm
idx 14 dist = 0.842474 mm
idx 15 dist = 1.331790 mm
idx 16 dist = 1.824125 mm
idx 17 dist = 1.816238 mm
idx 18 dist = 1.777341 mm
idx 19 dist = 1.734827 mm
idx 20 dist = 1.668559 mm
idx 21 dist = 1.568658 mm
idx 22 dist = 1.494239 mm
idx 23 dist = 1.443461 mm
idx 24 dist = 1.399058 mm
idx 25 dist = 1.352162 mm
idx 26 dist = 1.331921 mm
idx 27 dist = 1.326480 mm
idx 28 dist = 1.344323 mm
idx 29 dist = 1.338818 mm
idx 30 dist = 1.327906 mm
idx 31 dist = 1.305342 mm
mean = 1.131797 RMSE = 1.240648 max = 1.824125 (idx 16)
===========================
================= SUMMARY (verification on 32-pt cloud) =================
group | srcSpread | tgtSpread | meanErr | RMSE | maxErr | maxIdx
-------+-----------+-----------+---------+---------+---------+-------
A | 51.1021 | 51.1021 | 0.4484 | 0.4832 | 0.8207 | 16
B | 43.0452 | 42.9959 | 0.5108 | 0.5545 | 0.9052 | 16
C | 20.6977 | 20.8796 | 1.1318 | 1.2406 | 1.8241 | 16
================================================================
Expected trend: smaller srcSpread / tgtSpread -> larger RMSE
================================================================
==== Popup viewer for Group A ====
White: target 32pts | Blue: coarse tgt 3pts | Red: coarse src 3pts | Colored: aligned 32pts
==== Popup viewer for Group B ====
White: target 32pts | Blue: coarse tgt 3pts | Red: coarse src 3pts | Colored: aligned 32pts
==== Popup viewer for Group C ====
White: target 32pts | Blue: coarse tgt 3pts | Red: coarse src 3pts | Colored: aligned 32pts
其实这份数据可以得出很多结论了，我这里总结一下很明显的。
- landmark的分布越分散，也就是所围成的面积越大误差越小。
- 在landmark所围成的三角形的区域内误差相对偏小，在外侧的点误差相对较大