获得世界坐标系中3d点在图像上的投影坐标并显示

1.世界坐标系转换为摄像头坐标系无旋转和平移（初始的时候摄像头坐标系和世界坐标系重合）
如果初始的时候摄像头坐标系和世界坐标系重合，要获得世界坐标中3d点在任意位姿拍到的图像上的投影坐标并显示，只有考虑位姿变换。
如下程序所示：

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include<time.h>
#include <unistd.h>
#include<iostream>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
#include <tf/transform_broadcaster.h>
#include<opencv2/opencv.hpp>
using namespace std;
using namespace Eigen;
using namespace cv;
double fx = 200,fy=200, w = 2000, h = 2000;
double cx=w/2;
double cy=h/2;
int main( int argc, char** argv )
{
    ros::init(argc, argv, "tfshow");
    cout<<"start program"<<endl;
    double len=10;
    //1.1 get para intrinsic
    
    cv::Mat K = (Mat_<double>(3, 3) <<
                 fx, 0, cx,
                 0, fy, cy,
                 0, 0, 1);
    double k1=0.0,k2=0,k3=0.0,k4=0.0,k5=0.0;
    cv::Mat coff=(Mat_<double>(1,4) <<k1,k2,k3,k4,k5);
    double cam_h=1;
    //1.2 get para extrinsic
    Mat cv_R12=(cv::Mat_<double>(3, 3) <<
                1,0,0,
                0,0,-1,
                0,1,0);
    cv::Mat cv_t12 = (cv::Mat_<double>(3, 1) << 0,0,cam_h);
    Eigen::Matrix3d e_R12;
    e_R12<<1,0,0,
            0,0,-1,
            0,1,0;
    
    //t
    Eigen::Vector3d e_t12={0,0,1};
    Mat cv_R21=cv_R12.inv();
    Mat cv_t21=-cv_R12.inv()*cv_t12;
    Mat cv_T21 = cv::Mat::eye(4,4,cv_R21.type());
    cv_R21.copyTo(cv_T21.rowRange(0,3).colRange(0,3));
    cv_t21.copyTo(cv_T21.rowRange(0,3).col(3));
    //cout<<"cvT21"<<cv_T21<<endl;
    
    Eigen::Matrix3d e_R21=e_R12.inverse();
    Eigen::Vector3d e_t21=-e_R12.inverse()*e_t12;
    //cout<<"e_t21"<<e_t21<<endl;
    Mat cv_extriT = cv::Mat::eye(4,4,cv_R12.type());
    
    //2.get adbc
    //  //x>0 ABCD EFGH
    Point3d cube_center={0,0,0};
    Point3d A={cube_center.x-len/2,cube_center.y-len/2,10};
    Point3d B={cube_center.x+len/2,cube_center.y-len/2,10};
    Point3d C={cube_center.x+len/2,cube_center.y+len/2,10};
    Point3d D={cube_center.x-len/2,cube_center.y+len/2,10};
    vector<Point3d> vp3d;
    vp3d.push_back(A);
    vp3d.push_back(B);
    vp3d.push_back(C);
    vp3d.push_back(D);
    
    cout<<"ABCDE"<<A<< B<<C<<D<<0.5*(A+B)<<endl;
    //3.get pos
    //R,t
    vector<Mat> vCams;
    int N=60;
    //    //2.1.get cam center world pos
    vector<Eigen::Vector3d> v_pos_c;
    vector<Eigen::Quaterniond> v_q_c;
    //2.1.get cam pos in world axis
    for(int i=0;i!=N;++i)
    {   //world pos         //cam in world axis
        Mat T_w = cv::Mat::eye(4,4,cv_R12.type());
        Mat R_w = cv::Mat::eye(3,3,cv_R12.type());
        //double px=(double)(100*i)/N;
        double px=(double)i-2/N;
        cv::Mat cv_t_w = (Mat_<double>(3, 1) <<px,0,0);
        R_w.copyTo(T_w.rowRange(0,3).colRange(0,3));
        cv_t_w.copyTo(T_w.rowRange(0,3).col(3));
        //project R,t
        Mat T_prj=T_w;
        vCams.push_back(T_prj);
        //vCams.push_back(T_w);
    }
    
    //4.get project point2f
    int num_observations=0;
    for (size_t i=0;i<vCams.size();i++) {
        Mat T=vCams[i];
        cv::Mat R = T.rowRange(0,3).colRange(0,3);
        cv::Mat t = T.rowRange(0,3).col(3);
        Mat rvec;
        cv::Rodrigues(R,rvec);
        //cout<<"rvec"<<rvec<<"t"<<t<<endl;
        vector<Point2d> prj2d;
        cv::projectPoints(vp3d,rvec,t,K,coff,prj2d);
        vector<Point> contour;
        for(size_t j=0;j<prj2d.size();j++)
        {
            num_observations++;
            contour.push_back(prj2d[j]);
        }
        Mat binImage=Mat::zeros(cvSize(w,h),CV_8UC1); 
        vector<vector<Point> > contours;
        contours.push_back(contour);
        cv::drawContours(binImage, contours, -1, cv::Scalar::all(255),CV_FILLED);
        namedWindow("binImage",WINDOW_NORMAL);
        imshow("binImage",binImage);
        waitKey();
    }
    return 0;
}

1.世界坐标系转换为摄像头坐标系有旋转和平移（初始的时候摄像头坐标系和世界坐标系有旋转和平移由外参数矩阵E表示）
世界坐标系转换为摄像头坐标系有旋转和平移（有固定的外参数矩阵），要获得世界坐标中3d点在任意位姿拍到的图像上的投影坐标并显示，需要考虑位姿变换和外参数变换两个变换。
如下程序所示：

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include<time.h>
#include <unistd.h>
#include<iostream>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
#include <tf/transform_broadcaster.h>
#include<opencv2/opencv.hpp>
using namespace std;
using namespace Eigen;
using namespace cv;
double fx = 400,fy=400, w = 2000, h = 2000;
double cx=w/2;
double cy=h/2;
int main( int argc, char** argv )
{
    ros::init(argc, argv, "tfshow");
    cout<<"start program"<<endl;
    double len=6;
    //1.1 get para intrinsic

    cv::Mat K = (Mat_<double>(3, 3) <<
                 fx, 0, cx,
                 0, fy, cy,
                 0, 0, 1);
    double k1=0.0,k2=0,k3=0.0,k4=0.0,k5=0.0;
    cv::Mat coff=(Mat_<double>(1,4) <<k1,k2,k3,k4,k5);
    double cam_h=1;
    //1.2 get para extrinsic
//    Mat cv_R12=(cv::Mat_<double>(3, 3) <<
//                1,0,0,
//                0,0,-1,
//                0,1,0);
    Mat cv_R12=(cv::Mat_<double>(3, 3) <<
                -1,0,0,
                0,0,-1,
                0,-1,0);
    cv::Mat cv_t12 = (cv::Mat_<double>(3, 1) << 0,0,cam_h);
//    Eigen::Matrix3d e_R12;
//    e_R12<<1,0,0,
//            0,0,-1,
//            0,1,0;
        Eigen::Matrix3d e_R12;
        e_R12<<-1,0,0,
                0,0,-1,
                0,-1,0;
    //t
    Eigen::Vector3d e_t12={0,0,cam_h};
    Mat cv_R21=cv_R12.inv();
    Mat cv_t21=-cv_R12.inv()*cv_t12;
    Mat cv_T21 = cv::Mat::eye(4,4,cv_R21.type());
    cv_R21.copyTo(cv_T21.rowRange(0,3).colRange(0,3));
    cv_t21.copyTo(cv_T21.rowRange(0,3).col(3));
    //cout<<"cvT21"<<cv_T21<<endl;

    Eigen::Matrix3d e_R21=e_R12.inverse();
    Eigen::Vector3d e_t21=-e_R12.inverse()*e_t12;
    //cout<<"e_t21"<<e_t21<<endl;
    Mat cv_extriT = cv::Mat::eye(4,4,cv_R12.type());
    cv_R21.copyTo(cv_extriT.rowRange(0,3).colRange(0,3));
    cv_t21.copyTo(cv_extriT.rowRange(0,3).col(3));
    tf::TransformBroadcaster br;
       tf::Transform transform;
       transform.setOrigin( tf::Vector3(e_t12(0), e_t12(1), e_t12(2)));
       Eigen::Quaterniond eq(e_R12);
       tf::Quaternion tq(eq.x(),eq.y(),eq.z(),eq.w());

       transform.setRotation(tq);
       ros::NodeHandle n;
       ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
       int k=0;
       cout<<"start program"<<endl;
        //2.get cube position and publish
       while(k<2)
       {
           visualization_msgs::Marker marker;
           marker.header.frame_id = "/world";
           marker.header.stamp = ros::Time::now();
           // Set the namespace and id for this marker.  This serves to create a unique ID
           // Any marker sent with the same namespace and id will overwrite the old one
           marker.ns = "basic_shapes";
           marker.id = 0;
           // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
           marker.type = visualization_msgs::Marker::CUBE;
           // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
           marker.action = visualization_msgs::Marker::ADD;
           // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
           marker.pose.position.x =0;
           marker.pose.position.y = -len;
           marker.pose.position.z = 0;
           marker.pose.orientation.x = 0.0;
           marker.pose.orientation.y = 0.0;
           marker.pose.orientation.z = 0.0;
           marker.pose.orientation.w = 1.0;

           // Set the scale of the marker -- 1x1x1 here means 1m on a side
           marker.scale.x = len;
           marker.scale.y = len;
           marker.scale.z = 0.1;

           // Set the color -- be sure to set alpha to something non-zero!
           marker.color.r = 0.0f;
           marker.color.g = 1.0f;
           marker.color.b = 0.0f;
           marker.color.a = 1.0;
           //marker.lifetime = ros::Duration();
           marker_pub.publish(marker);
           sleep(1);
           k++;
           br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "camera"));
       }

    //2.get adbc
    //  //x>0 ABCD EFGH
    Point3d cube_center={0,-len,0};
    Point3d A={cube_center.x-len/2,cube_center.y-len/2,0};
    Point3d B={cube_center.x+len/2,cube_center.y-len/2,0};
    Point3d C={cube_center.x+len/2,cube_center.y+len/2,0};
    Point3d D={cube_center.x-len/2,cube_center.y+len/2,0};
    vector<Point3d> vp3d;
    vp3d.push_back(A);
    vp3d.push_back(B);
    vp3d.push_back(C);
    vp3d.push_back(D);

    cout<<"ABCDE"<<A<< B<<C<<D<<0.5*(A+B)<<endl;
    //3.get pos
    //R,t
    vector<Mat> vCams;
    double N=10;
    //    //2.1.get cam center world pos
    vector<Eigen::Vector3d> v_pos_c;
    vector<Eigen::Quaterniond> v_q_c;
    //2.1.get cam pos in world axis
    for(int i=0;i!=N;++i)
    {   //world pos         //cam in world axis
        Mat T_w = cv::Mat::eye(4,4,cv_R12.type());
        Mat R_w = cv::Mat::eye(3,3,cv_R12.type());
        //double px=(double)(100*i)/N;
        double px=(double)i/N;
        cv::Mat cv_t_w = (Mat_<double>(3, 1) <<px,0,0);
        R_w.copyTo(T_w.rowRange(0,3).colRange(0,3));
        cv_t_w.copyTo(T_w.rowRange(0,3).col(3));
        //project R,t

        Mat T_prj=T_w*cv_extriT;
        vCams.push_back(T_prj);
        //vCams.push_back(T_w);
    }

    //4.get project point2f
    int num_observations=0;
    for (size_t i=0;i<vCams.size();i++) {
        Mat T=vCams[i];
        cv::Mat R = T.rowRange(0,3).colRange(0,3);
        cv::Mat t = T.rowRange(0,3).col(3);
        Mat rvec;
        cv::Rodrigues(R,rvec);
        //cout<<"rvec"<<rvec<<"t"<<t<<endl;
        vector<Point2d> prj2d;
        cv::projectPoints(vp3d,rvec,t,K,coff,prj2d);
        vector<Point> contour;
        Mat binImage=Mat::zeros(cvSize(w,h),CV_8UC1);
        for(size_t j=0;j<prj2d.size();j++)
        {
            num_observations++;
            contour.push_back(prj2d[j]);
            cv::circle(binImage,prj2d[j],10*j,cvScalar(255,255,255));
        }

        vector<vector<Point> > contours;
        contours.push_back(contour);
        cv::drawContours(binImage, contours, -1, cv::Scalar::all(255),CV_FILLED);
        namedWindow("binImage",WINDOW_NORMAL);
        imshow("binImage",binImage);
        waitKey();
    }
    return 0;
}