ICP 求解相機思路

1.以前仍然是須要建立find_feature_matches，和pixel2cam，一個是用來匹配描述子的，一個是把像素座標轉成歸一化平面座標的。裏面的變量都要帶上&。
2.由於是3d-3d.因此這裏讀取了兩個深度圖。depth1,depth2.跟以前同樣。不一樣的是第二張圖片的像素座標也要轉成空間點座標。轉成空間點座標不須要旋轉和平移。
ushort d1=depth.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y)[int(keypoints_1[m.queryIdx].pt.x]
這裏就把.pt當作是座標塊。
float dd1=float(d1)/1000.0;
Point2d p1,p2;
p1=pixel2cam(keypoints_1[m.queryIdx].pt,K]也不是座標塊。
pts1.pushback(Point3f(p1.x*dd1,p1.y*dd1,dd1))
3.定義了一個函數pose_estimation_3d3d,裏面變量就是pts1,pts2,R,t.R,t是求的結過。因此知道pts1,pts2就能夠了。
先計算質心Point3f p1,p2.計算完質心，計算pts1,pts2的去質心座標，並賦值給q1,q2.而後算W
W爲qi*qi.transpose.要保證形式是Eigen::Vector3d.
接下來只須要把w放進Eigen::JacobiSVD函數裏面就能夠了
Eigen::JacobiSVD svd(w,Eigen::MatrixComputeFullU|Eigen::MatrixComputerFullV)
而後定義U,V
Eigen::Matrix3d U,V.
U=svd.matrixU();
V=svd.matrixV();
R_=U*(V.transpose())
t_=Eigen::Vector3d(p1)-R*Eigen::Vector3d(p2);
而後把R_和t_轉成R,t就能夠了。就是R=Mat_double(3,3)<<R_(0,0)這種。
這裏求出的R,t是第二幀到第一幀的變換。而實際上要求的是第一幀到第二幀的變換。因此對R，t求逆。
R 的逆就是它的轉置。R.t().t的逆是-R.t()*t.
這樣位姿就求出來了。