问题：如图，已知相机的外参，并已知正方体的底部正方形ABCD在图像上的坐标，如何反投影求得正方形ABCD在世界坐标系上的坐标点？
（粗的代表摄像头坐标系，细的代表世界坐标系）
如图红色为x轴，绿色为y轴，蓝色为z。
主要解决思路是：反投影将图像坐标化为归一化平面。由于摄像头在世界坐标系上的高度已知，将归一化平面的坐标乘以尺度并转换就能反投影得到ABCD在世界坐标系上的点。
1.针孔相机模型的投影与反投影
代码如下
#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 <opencv2/opencv.hpp>
#include <tf/transform_broadcaster.h>
using namespace cv;
using namespace std;
using namespace Eigen;
void project_point(Point3f pt3d,Mat R,Mat t,Mat K,Point2f& pt2d)
{
cv::Mat p3Dw (pt3d);
// Transform into Camera Coords.
cv::Mat p3Dc = R*p3Dw+t;
cout<<"p3Dc"<<p3Dc<<endl;
// Project into Image
const float invz = 1/p3Dc.at<float>(2);
const float x = p3Dc.at<float>(0)*invz;
const float y = p3Dc.at<float>(1)*invz;
cout<<"project norm plan x="<<x<<"y="<<y<<endl;
const float fx=K.at<float>(0,0);
const float fy=K.at<float>(1,1);
const float cx=K.at<float>(0,2);
const float cy=K.at<float>(1,2);
const float u = fx*x+cx;
const float v = fy*y+cy;
pt2d.x=u;
pt2d.y=v;
}
void unproject_point(Point2f pt2d,Mat R,Mat t,Mat K,Mat& p3Dw)
{
const float fx=K.at<float>(0,0);
const float fy=K.at<float>(1,1);
const float cx=K.at<float>(0,2);
const float cy=K.at<float>(1,2);
float u=pt2d.x;
float v=pt2d.y;
float x=(u-cx)/fx;
float y=(v-cy)/fy;
Point3f p3DNormPlan;
p3DNormPlan.x=x;
p3DNormPlan.y=y;
p3DNormPlan.z=1;
Mat mp3Dc(p3DNormPlan);
cout<<"unproject normplan"<<p3DNormPlan<<endl;
float static_y=t.at<float>(0,1);
cout<<"static_y"<<static_y<<endl;
Mat mp3DNormPlanWorld=R.inv()*(mp3Dc-t);
cout<<"mp3DNormPlanWorld"<<mp3DNormPlanWorld<<endl;
double scale=(double)fabs(static_y/mp3DNormPlanWorld.at<float>(0,1));
cout<<"scale"<<scale<<endl;
p3Dw=scale*mp3DNormPlanWorld;
cout<<"p3Dw"<<p3Dw<<endl;
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "projecttest");
float len=4;
//1.1 get para intrinsic
float f = 200, w = 1000, h = 1000;
cv::Mat K = (Mat_<float>(3, 3) <<
f, 0, w/2,
0, f, h/2,
0, 0, 1);
//1.2 get para extrinsic
Mat cv_R12=(cv::Mat_<float>(3, 3) <<
0,0,1,
-1,0,0,
0,-1,0);
cv::Mat cv_t12 = (cv::Mat_<float>(3, 1) << 0,0,len/2);
Eigen::Matrix3d e_R12;
e_R12<<0,0,1,
-1,0,0,
0,-1,0;
//t
Eigen::Vector3d e_t12={0,0,len/2};
Mat cv_R21=cv_R12.inv();
Mat cv_t21=-cv_R12.inv()*cv_t12;
Eigen::Matrix3d e_R21=e_R12.inverse();
Eigen::Vector3d e_t21=-e_R12.inverse()*e_t12;
cout<<"e_t21"<<e_t21<<endl;
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);
// Set our initial shape type to be a cube
//uint32_t shape = visualization_msgs::Marker::CUBE;
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 = 1.5*len;
marker.pose.position.y = 0;
marker.pose.position.z = len/2;
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 = len;
// 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"));
}
// //x>0 ABCD EFGH
Point3f cube_center={len,0,len/2};
Point3f A={cube_center.x-len/2,cube_center.y+len/2,cube_center.z-len/2};
Point3f B={cube_center.x-len/2,cube_center.y-len/2,cube_center.z-len/2};
Point3f C={cube_center.x+len/2,cube_center.y-len/2,cube_center.z-len/2};
Point3f D={cube_center.x+len/2,cube_center.y+len/2,cube_center.z-len/2};
cout<<"ABCD"<<A<< B<<C<<D<<endl;
vector<Point3f> vp3d;
vp3d.push_back(A);
vp3d.push_back(B);
vp3d.push_back(C);
vp3d.push_back(D);
//3.get binImage
vector<Point2f> vpt2d;
vector<Point> contour;
for(size_t i=0;i<vp3d.size();i++)
{
Point2f prj_p;
project_point(vp3d[i],cv_R21,cv_t21,K,prj_p);
cout<<"prj_p i="<<i<<" "<<prj_p<<endl;
Point p2d={(int)prj_p.x,(int)prj_p.y};
vpt2d.push_back(prj_p);
contour.push_back(p2d);
}
Mat binImage=Mat::zeros(cvSize(w,h),CV_8UC1);
// Mat mask = Mat::zeros(binImage.size(), CV_8UC1);
vector<vector<Point> > contours;
contours.push_back(contour);
cv::drawContours(binImage, contours, -1, cv::Scalar::all(255),CV_FILLED);
for(size_t i=0;i<vpt2d.size();i++)
{
Mat mp3d;
unproject_point(vpt2d[i],cv_R21,cv_t21,K,mp3d);
// cout<<"x="<<mp3d.at<float>(0,0)<<
// "y="<<mp3d.at<float>(1,0)<<
// "z="<<mp3d.at<float>(2,0)<<endl;
}
imshow("binImage",binImage);
waitKey();
//4.unproject to plan
// cout<<"prj2dp"<<prj_A<<endl;
}
2.鱼眼相机的投影与反投影
鱼眼相机代码如下
主要的是调用opencv自带的投影和反投影函数。
其中反投影函数undistortPoints 如果不输入外参，输出的是归一化的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 <opencv2/opencv.hpp>
#include <tf/transform_broadcaster.h>
using namespace cv;
using namespace std;
using namespace Eigen;
int main( int argc, char** argv )
{
ros::init(argc, argv, "projecttest");
float len=4;
//1.1 get para intrinsic
float f = 200, w = 1000, h = 1000;
cv::Mat K = (Mat_<float>(3, 3) <<
f, 0, w/2,
0, f, h/2,
0, 0, 1);
cv::Mat coff=(Mat_<float>(1,4) <<0,0.001,0,0.0001);
//1.2 get para extrinsic
Mat cv_R12=(cv::Mat_<float>(3, 3) <<
0,0,1,
-1,0,0,
0,-1,0);
cv::Mat cv_t12 = (cv::Mat_<float>(3, 1) << 0,0,len/2);
Eigen::Matrix3d e_R12;
e_R12<<0,0,1,
-1,0,0,
0,-1,0;
//t
Eigen::Vector3d e_t12={0,0,len/2};
Mat cv_R21=cv_R12.inv();
Mat cv_t21=-cv_R12.inv()*cv_t12;
Eigen::Matrix3d e_R21=e_R12.inverse();
Eigen::Vector3d e_t21=-e_R12.inverse()*e_t12;
cout<<"e_t21"<<e_t21<<endl;
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);
// Set our initial shape type to be a cube
//uint32_t shape = visualization_msgs::Marker::CUBE;
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 = 1.5*len;
marker.pose.position.y = 0;
marker.pose.position.z = len/2;
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 = len;
// 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"));
}
// //x>0 ABCD EFGH
Point3f cube_center={len,0,len/2};
Point3f A={cube_center.x-len/2,cube_center.y+len/2,cube_center.z-len/2};
Point3f B={cube_center.x-len/2,cube_center.y-len/2,cube_center.z-len/2};
Point3f C={cube_center.x+len/2,cube_center.y-len/2,cube_center.z-len/2};
Point3f D={cube_center.x+len/2,cube_center.y+len/2,cube_center.z-len/2};
cout<<"ABCD"<<A<< B<<C<<D<<endl;
vector<Point3f> vp3d;
vp3d.push_back(A);
vp3d.push_back(B);
vp3d.push_back(C);
vp3d.push_back(D);
//3.get binImage
vector<Point2f> vpt2d;
vector<Point> contour;
cv::Mat rvec21;
Rodrigues(cv_R21,rvec21);
projectPoints(vp3d,rvec21,cv_t21,K,coff,vpt2d);
for(size_t i=0;i<vpt2d.size();i++)
{
cout<<"prj x="<<vpt2d[i].x<<" y="<<vpt2d[i].y<<endl;
Point pt;
pt.x=vpt2d[i].x;
pt.y=vpt2d[i].y;
contour.push_back(pt);
}
Mat binImage=Mat::zeros(cvSize(w,h),CV_8UC1);
// Mat mask = Mat::zeros(binImage.size(), CV_8UC1);
vector<vector<Point> > contours;
contours.push_back(contour);
cv::drawContours(binImage, contours, -1, cv::Scalar::all(255),CV_FILLED);
std::vector<cv::Point2f> upj2d_norm_plan_cam;
undistortPoints(vpt2d,upj2d_norm_plan_cam,K,coff);
float static_world_z=cv_t21.at<float>(0,1);
for(size_t i=0;i<upj2d_norm_plan_cam.size();i++ )
{
cout<<"norm x="<<upj2d_norm_plan_cam[i].x<<" y="<<upj2d_norm_plan_cam[i].y<<endl;
Point3f tp3d;
tp3d.x=upj2d_norm_plan_cam[i].x;
tp3d.y=upj2d_norm_plan_cam[i].y;
tp3d.z=1;
Mat mPt(tp3d);
Mat mp3DNormPlanWorld=cv_R21.inv()*(mPt-cv_t21);
cout<<"mp3DNormPlanWorld"<<mp3DNormPlanWorld<<endl;
double scale=(double)fabs(static_world_z/mp3DNormPlanWorld.at<float>(0,1));
Mat mp3DPlanWorld=scale*mp3DNormPlanWorld;
cout<<"mp3DPlanWorld"<<mp3DPlanWorld<<endl;
}
imshow("binImage",binImage);
waitKey();
return 0;
}