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+p1t;
    Mat p2Rcv=p1R*t12R;
    Mat p2tcv=p1R*t12t+p1t;
    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.2 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;
}