攝像頭與imu座標轉換
最近由於實習需要在玩orb slam，然後想在攝像頭上安裝一個imu來輔助測量。在把imu貼在攝像頭後面之後，一個問題出現了：攝像頭的座標系與imu的座標系不會對得很準，即它們的座標很難人工對準。要讓它們的座標軸都重合，需要一個轉換。
上圖中，
C1：攝像頭從初始狀態（即單位陣I，這個單位陣是指攝像頭開機的時候當做起始狀態）到T1時刻的轉換爲C1；
C2：攝像頭從初始狀態（即單位陣I）到T2時刻的轉換爲C2；
Rc：從T1時刻的狀態轉換到T2時刻的狀態所需要的轉換（旋轉）矩陣；
M1：imu初始狀態（即單位陣，這個單位陣是指imu與世界座標系重合時爲單位陣。）到T1時刻的轉換爲M1；
M2：imu初始狀態（即單位陣，這個單位陣是指imu與世界座標系重合時爲單位陣。）到T2時刻的轉換爲M2；
Rm：從T1時刻的狀態轉換到T2時刻的狀態所需要的轉換（旋轉）矩陣；
R：imu自身的座標系到與攝像頭的座標系之間存在的轉換。
爲什麼說攝像頭初始狀態是單位陣呢？
在orb slam開始運行時，它並不知道這個世界的方位是怎樣的。於是它會把它的初始狀態當做原點，以後新的姿態都是相對於初始狀態來說的。在旋轉矩陣中，單位陣表示沒有旋轉。
求解轉換矩陣R：
我們的目標是求出轉換矩陣R，這一從imu座標系上的數據就能轉換到攝像頭的座標系上進行計算了。顯然：
C2 = Rc×C1；
M2 = Rm×M1；
C1 = R×M1；
C2 = R×M2；
於是：R×M2 = Rc×R×M1
即：R×Rm×M1 = Rc×R×M1
所以：R×Rm = Rc×R
這裏要注意的是，旋轉矩陣都是在左邊，即左乘的。順序要保持一致，推導出來的公式纔是對的。
我們要求解的是R，所以我們要先計算出Rm和Rc。orb slam在運行的時候會一直輸出攝像頭的估計姿態矩陣，也就是類似於C1和C2，所以可以通過C1和C2來計算出Rc。Rc = C2×inv(C1);
在運行的過程中也可以測量到imu的姿態M1和M2，通過M1和M2來計算出Rm。
但是對於矩陣方程：R×Rm = Rc×R，即使知道Rc和Rm也難算出R。所以要進行一些小小的轉換。
可以將上面的方程展開如下：
將上面的方程經過轉換之後，即可得到：
其中A是一個9X9的矩陣。一般來說，A的行列式不等於0，所以沒有非零解，所以在matlab下可以通過svd函數求出近似解。
[u,s,v] = svd(A'*A);得到的v的最後一列即是近似解。