題目回顧:
設有小蘿卜一號和小蘿卜二號位於世界坐標系中，小蘿卜一號的位姿為：q1=[0.35,0.2,0.3,0.1],
t2=[0.3,0.1,0.1]^T (q的第一項為實部。請你把q歸一化后在進行計算)。這里的q和t的表達的是Tcw，也就是世界到相機的變換關系。小蘿卜二號的位姿為q2=[-0.5,0.4,-0.1,0.2],t=[-0.1,0.5,0.3]^T.現在，小蘿卜一號看到某個點在自身的坐標系下，坐標為p=[0.5,0,0.2]^T ,求該向量在小蘿卜二號坐標系下的坐標，請編程實現此事。
解:
pw:某個點在世界坐標系下的坐標
T_1w :表示世界坐標系到小蘿卜一號坐標系的變換關系
T_2w:表示世界坐標系到小蘿卜二號坐標系的變換關系
P2 :表示該點在小蘿卜二號坐標系下的坐標（即為所求）
單位四元數到旋轉矩陣R的變化關系可參考書上55頁。之后變換矩陣T=[R t]
[0 1]
由變換關系可列出下面的式子:
p = T_1w * Pw 可解出來pw
p2=T_2W*pW 帶入上式解出來的Pw即可求出來p2
具體代碼實現如下:
1 #include<iostream> 2 #include<Eigen/Core> 3 4 //包含幾何模塊 5 #include<Eigen/Geometry> 6 using namespace std; 7 8 int main(int argc,char **argv) 9 { 10 /*變量定義*/ 11 Eigen::Quaterniond Q1(0.2,0.3,0.1,0.35); //四元數的表示(w ,x,y,z) 12 Eigen::Quaterniond Q2(0.4,-0.1,0.2,-0.5); 13 Eigen::Vector3d t1(0.3,0.1,0.1); 14 Eigen::Vector3d t2(-0.1,0.5,0.3); 15 Eigen::Vector3d p(0.5,0,0.2); //在一號小蘿卜下的坐標 16 Eigen::Vector3d pw ; //世界坐標 17 Eigen::Vector3d p2; //求在二號小蘿卜的坐標 p2 18 19 /*歐氏變換矩陣使用Eigen::Isometry */ 20 Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity(); 21 Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity(); 22 23 /*歸一化*/ 24 Q1.normalize(); 25 Q2.normalize(); 26 27 /*輸出歸一化參數*/ 28 // cout<<"Q1 is "<<Q1.x()<<endl<<Q1.y()<< endl <<Q1.z()<< endl<<Q1.w()<<endl; 29 // cout<<"Q2 is "<<Q2.x()<<endl<<Q2.y()<< endl <<Q2.z()<< endl<<Q2.w()<<endl; 30 31 cout<<"after normalize； "<< endl << Q2.coeffs()<<endl; 32 33 /*設置變換矩陣的參數*/ 34 T_1w.rotate(Q1); 35 T_1w.pretranslate(t1); 36 T_2w.rotate(Q2); 37 T_2w.pretranslate(t2); 38 39 /* p = T1w * pw 求解pw*/ 40 pw = T_1w.inverse() * p; 41 42 /* p2 = T_2w * pw 求解p2*/ 43 p2 = T_2w * pw; 44 45 /*輸出在小蘿卜二號下的該點坐標*/ 46 cout<<"該點在小蘿卜二號下的坐標為: "<<p2.transpose()<<endl; 47 48 return 0; 49 }
歡迎大家關注我的微信公眾號「佛系師兄」，里面有關於 Ceres 以及 OpenCV 庫的一些文章。
比如
「反復研究好幾遍，我才發現關於 CMake 變量還可以這樣理解！」
更多好的文章會優先在里面不定期分享！打開微信客戶端，掃描下方二維碼即可關注！
