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.
这样位姿就求出来了。