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;
}