0. 变换算法
0.0 混合变换
在上图的中，假设每个坐标系相对于前一个坐标系都是已知的，现在已知cP要求aP
既然每个坐标系相对前一个坐标系都是已知的，那么就意味着我们可以根据cP倒着一步步变换成aP
首先是变换成bP：
然后再由bP变换成aP：
当然，分开写比较不简洁，我们依然是比较习惯写成一个单独的表达式，那么只要综合上述两个过程，联立两条式子，就可以得到下面这条简单的表达式：
其中：
我们可以求得：
其实我个人更加喜欢从几何的角度来观察这个问题，感觉会更加简单一点。已知cP要求aP的话，我们就需要找到{C}关于{A}的变换算子，从直观上看图就可以知道，这是一个一般的变换算子，其中包含了旋转和平移。
首先单纯看平移部分，我们可以发现其实是沿着aPcorg平移的，我们又可以得到：
接着我们单纯看旋转部分，我们无法直接得到从{A}到{C}的旋转矩阵，但是我们知道从{A}到{B}的旋转矩阵和从{B}到{C}的旋转矩阵，因此我们可以得到：
至此，我们已经可以根据求得的信息写出一个一般变换的算子了：
其实两个结果是相同的。
0.1 逆变换
已知坐标系{B}相对于{A}，为了得到{A}相对于{B}的描述，我们需要求出
的逆，我们可以直接将4x4的齐次变换矩阵求逆得到，但是这样的操作并没有使用到变换的性质，我们还可以使用另外一种方式，利用变换的性质求出逆矩阵。
首先我们明确一下要求的目标：
在这里先忘记前面说的话，我们就单纯地把任务看成要求一个变换算子，那么我们就得知道旋转矩阵和位置矢量。
那么任务就变成了，由已知量：
求出未知量：
我们从位置矢量开始考虑起，我们已知了aPborg，要求bPaorg，那么就是一个将aPborg从{A}映射到{B}的过程。还记得一般坐标系映射是怎么做的吗？
bingo~
看着上面这个式子，有没有什么想法？
将{B}坐标系原点相对于{A}的描述重新映射回{B}，那不就是0吗？
由此可化简得到：
这就求出bPaorg了。
那么接下来，我们看一下旋转矩阵。
我们知道，旋转矩阵是一个正交矩阵，正交矩阵又有着特殊的性质，其转置等于逆
所以，轻而易举，旋转矩阵也求出来了。
至此，我们就可以得到完整的变换算子：
这是求齐次变换逆矩阵一般而且有效的方法。