座標系的旋轉

1、簡單3維座標系旋轉平移

原始座標系下座標點p1(0,2,1)。原始座標系繞自身z軸旋轉-90度變換為新座標系，p1在新座標系下的座標點為p2（-2,0,1）

得出結論：

求原始點在目標座標系下的座標，需要將原始座標系繞自身3軸旋轉與目標座標系軸向重合，得到旋轉矩陣r，求得原始座標系原點在目標座標系下的3維座標t(△x,△y,△z)，那麼p1在目標座標系下的座標點p2 = p1 * r + t;

實戰演練（控制點投影影像）：

前提1、相機外標定引數定義tx,ty,tz相機在imu座標系下的偏移量(座標點)，rx，ry，rz分別是相機繞自己軸旋轉到與imu座標軸重合的角度（後期計算imu點在相機系下的座標時先平移，平移過程需要減去座標原點的值，後旋轉）

前提2、標定場有控制點p1在北東高下的座標（e,n,u）和當前的投影度帶號

前提3、相機的內外參已知，相機中心在imu座標系下的座標為tcam2imu,相機旋轉到imu座標系下的旋轉矩陣為rcam2imu

前提4、慣導當前的經緯度lng，lat，h，roll，pitch，heading已知

思路：1、求控制點在ecef下的座標p1

2、求出ecef座標系到當地水平座標系的偏移量，通過當前imu的經緯度計算得到imu中心點的ecef座標即tsp2ecef

3、求出ecef座標系到當地水平系的旋轉矩陣，通過當前imu的經緯度計算ecef旋轉到當地水平座標系的旋轉矩陣recef2sp

4、求p1在當地水平座標系下的點p2 = (p1 - tsp2ecef) * recef2sp

5、求當地水平座標系到載體座標系的旋轉矩陣rsp2body（這裡重點提到旋轉矩陣內heading取負值是因為，imu輸出的heading定義東偏為正，但是右手系旋轉法則是逆時針為正（西偏為正，東偏為負）所以在構建旋轉矩陣時要對heading取反（-heading），同時旋轉矩陣旋轉角度範圍（-180,180）和航向範圍（0-360）在旋轉上矩陣值是一樣的，不用再將範圍統一，直接使用即可），rsp2body=getmatrixbyangle（pitch，roll，-heading，zxy）

6、求p2在載體系的點p3 = p2 * rsp2body

7、構建相機在載體系下的平移tcam2body（tx,ty,tz）帶符號,構建相機旋轉到載體系下的旋轉矩陣rcam2body=getmatrixbyangle（rx，ry，rz，zxy）

8、求p3在相機系下的點p4 = (p3 - tcam2body) * rcam2body.inverse()（求逆矩陣）;(旋轉矩陣的轉置transpose和逆inverse是一樣的)

9、求p4在影象座標系下的點p5 cv::projectpoints

座標系的旋轉

點旋轉和座標系旋轉

點旋轉和座標系旋轉

python旋轉座標系四元數旋轉座標系

座標系的旋轉

點旋轉和座標系旋轉

點旋轉和座標系旋轉

python旋轉座標系 四元數旋轉座標系

相關推薦

python旋轉座標系四元數旋轉座標系