OpenCV局部坐标系和世界坐标系转换

本文实现局部坐标系和世界坐标系的转换。

１．局部坐标系－>世界坐标系

假设机器在p1处在世界坐标系下的位姿为（x，y，z，θ）=（1，1，0，0），p1 和p2之间的转换（局部坐标系）为t12表示为(1,1,0,45)，

求p2处世界坐标系下的位姿（x，y，z，θ）？

结果为（2，2，0，45）

程序如下：

#include<opencv2/opencv.hpp>
#include<iostream>
using namespace std;
using namespace cv;

void set_pose_by_R_t( Mat R, Mat t,Mat& T)
{
Mat tmpT = cv::Mat::eye(4,4,R.type());
R.copyTo(tmpT.rowRange(0,3).colRange(0,3));
t.copyTo(tmpT.rowRange(0,3).col(3));
T=tmpT.clone();
}
void get_pose_R_t(Mat T,Mat& R, Mat& t)
{
R=Mat::zeros(3,3,T.type());
t=Mat::zeros(3,1,T.type());
T.rowRange(0,3).colRange(0,3).copyTo(R);
T.rowRange(0,3).col(3).copyTo(t);
}
int main()
{
//1.p1 world position expression by opencv
cv::Mat p1rvec = (cv::Mat_<float>(3, 1) << 0,0,0);
Mat p1R;
Rodrigues(p1rvec*CV_PI / 180,p1R);
cv::Mat p1t = (cv::Mat_<float>(3, 1) << 1, 1, 0);
Mat p1T;
set_pose_by_R_t(p1R,p1t,p1T);
cout<<p1T<<endl;

//2. t12 value
cv::Mat t12rvec = (cv::Mat_<float>(3, 1) << 0,0,45);
Mat t12R;
Rodrigues(t12rvec*CV_PI / 180,t12R);
cv::Mat t12t = (cv::Mat_<float>(3, 1) << 1, 1, 0);
Mat t12T;
set_pose_by_R_t(t12R,t12t,t12T);
cout<<t12T<<endl;

//3.calculate p2 world positon by opencv
//3.1 calculate by 3*3 matrix  p2Rcv=p1R*t12R;;  p2tcv=p1R*t12t+t12t;
Mat p2Rcv=p1R*t12R;
Mat p2tcv=p1R*t12t+t12t;
Mat p2Tcv;
set_pose_by_R_t(p2Rcv,p2tcv,p2Tcv);
cout<<p2Tcv<<endl;
Mat p2rvec;
Rodrigues(p2Rcv, p2rvec);
p2rvec=p2rvec*180/ CV_PI;
cout<<"p2 R"<<p2rvec<<endl;
cout<<"p2 t"<<p2tcv<<endl;

//3.1 calculate by 4*4 matrix  p2T=p1T*T12;
Mat p2T=p1T*t12T;
Mat p2Rcv2;
Mat p2tcv2;
get_pose_R_t(p2T,p2Rcv2,p2tcv2);

cout<<"4*4T"<<p2T<<endl;
Mat p2rvec2;
Rodrigues(p2Rcv2, p2rvec2);
p2rvec2=p2rvec2*180/ CV_PI;
cout<<"4*4 p2 R"<<p2rvec2<<endl;
cout<<"4*4 p2 t"<<p2tcv2<<endl;
return 0;
}

２．世界坐标系－>局部坐标系

假设机器在p1处在世界坐标系下的位姿为（x，y，z，θ）=（1，1，0，0），p2处世界坐标系下的位姿（x，y，z，θ）=（2，2，0，45）

求p1 和p2之间的转换t12。

程序如下：

#include<opencv2/opencv.hpp>
#include<iostream>
using namespace std;
using namespace cv;

void set_pose_by_R_t( Mat R, Mat t,Mat& T)
{
Mat tmpT = cv::Mat::eye(4,4,R.type());
R.copyTo(tmpT.rowRange(0,3).colRange(0,3));
t.copyTo(tmpT.rowRange(0,3).col(3));
T=tmpT.clone();
}
void get_pose_R_t(Mat T,Mat& R, Mat& t)
{
R=Mat::zeros(3,3,T.type());
t=Mat::zeros(3,1,T.type());
T.rowRange(0,3).colRange(0,3).copyTo(R);
T.rowRange(0,3).col(3).copyTo(t);
}
int main()
{
//1.p1 world position expression by opencv
cv::Mat p1rvec = (cv::Mat_<float>(3, 1) << 0,0,0);
Mat p1R;
Rodrigues(p1rvec*CV_PI / 180,p1R);
cv::Mat p1t = (cv::Mat_<float>(3, 1) << 1, 1, 0);
Mat p1T;
set_pose_by_R_t(p1R,p1t,p1T);
cout<<p1T<<endl;

//2.p2 world position
cv::Mat p2rvec = (cv::Mat_<float>(3, 1) << 0,0,45);
Mat p2R;
Rodrigues(p2rvec*CV_PI / 180,p2R);
cv::Mat p2t = (cv::Mat_<float>(3, 1) << 2, 2, 0);
Mat p2T;
set_pose_by_R_t(p2R,p2t,p2T);
cout<<p2T<<endl;

//3.calculate t12
Mat t12T=p1T.inv()*p2T;
cout<<t12T<<endl;
Mat t12R;
Mat t12t;
get_pose_R_t(t12T,t12R,t12t);
Mat t12rvec;
Rodrigues(t12R, t12rvec);
t12rvec=t12rvec*180/ CV_PI;
cout<<"t12rvec"<<t12rvec<<endl;
cout<<"t12t"<<t12t<<endl;
return 0;
}