C++ 相对于初始姿态的姿态估计

我有一组图像点和相应的3d对象点，我使用一台经过校准的相机来找到姿势。使用

SolvePnP（）

我可以计算R和T，也可以使用公式

position=-R.T（）*T

计算相机位置。现在我的问题是：
我想计算一个初始姿势，然后我计算的任何姿势都应该是相对于我计算的初始姿势。我怎么做？

---

如有任何意见或建议，将不胜感激

确实有帮助。您可能希望使用RPP：

这会对姿势施加一定的平滑度，主要用于跟踪目的

---

代码在这里：

你的意思是，你想确定相机移动了多少？@Niko这是我的任务。但我得到的数值并不那么准确。所以我认为当它有一个初始参考点时，它可能会给出更好的结果