问题:
对应相机坐标系的一点P,我们如何通过鱼眼相机的4个畸变系数找到这个点在像素平面上的正确位置?
步骤如下:
1.将世界坐标系的三维坐标点投影摄像头坐标系
设P为世界坐标系中的三维点为
X
X
X,摄像机坐标系中P的坐标为
X
c
(
x
,
y
,
z
)
Xc(x,y,z)
Xc(x,y,z):
X
c
=
R
X
+
t
Xc = R X + t
Xc=RX+t
通过外参数的旋转和平移转换。
2.摄像头坐标系点转换到归一化图像平面
将点转换为归一化平面为(a,b)
a
=
x
/
z
a
n
d
b
=
y
/
z
a = x / z \ and \ b = y / z
a=x/z and b=y/z
3.对归一化平面的点进行畸变纠正
给定归一化坐标,可以求出畸变纠正后的坐标
(
x
′
,
y
′
)
(x',y')
(x′,y′)
r
2
=
a
2
+
b
2
θ
=
a
t
a
n
(
r
)
θ
d
=
θ
(
1
+
k
1
θ
2
+
k
2
θ
4
+
k
3
θ
6
+
k
4
θ
8
)
x
′
=
(
θ
d
/
r
)
a
y
′
=
(
θ
d
/
r
)
b
r^2 = a^2 + b^2 \\ \theta = atan(r)\\\theta_d = \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8)\\x' = (\theta_d / r) a \\ y' = (\theta_d / r) b
r2=a2+b2θ=atan(r)θd=θ(1+k1θ2+k2θ4+k3θ6+k4θ8)x′=(θd/r)ay′=(θd/r)b
4.转换到图像平面上
最后得到图像平面上的点
(
u
,
v
)
(u,v)
(u,v)
u
=
f
x
(
x
′
+
α
y
′
)
+
c
x
v
=
f
y
y
′
+
c
y
u = f_x (x' + \alpha y') + c_x \\ v = f_y y' + c_y
u=fx(x′+αy′)+cxv=fyy′+cy
5实现代码
实现代码如下:
opencv_project_points 为opencv 实现函数 void cv::fisheye::projectPoints();
void fish_project_point(Point3d pt3d,Mat R,Mat t,Mat K,Mat coff,Point2d& pt2d)为我根据以上公式实现的
#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<opencv2/calib3d.hpp>
#include <tf/transform_broadcaster.h>
using namespace cv;
using namespace std;
using namespace Eigen;
void opencv_project_points(InputArray objectPoints, OutputArray imagePoints, InputArray _rvec,
InputArray _tvec, InputArray _K, InputArray _D, double alpha)
{
// will support only 3-channel data now for points
CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2));
size_t n = objectPoints.total();
CV_Assert(_rvec.total() * _rvec.channels() == 3 && (_rvec.depth() == CV_32F || _rvec.depth() == CV_64F));
CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F));
CV_Assert(_tvec.getMat().isContinuous() && _rvec.getMat().isContinuous());
Vec3d om = _rvec.depth() == CV_32F ? (Vec3d)*_rvec.getMat().ptr<Vec3f>() : *_rvec.getMat().ptr<Vec3d>();
Vec3d T = _tvec.depth() == CV_32F ? (Vec3d)*_tvec.getMat().ptr<Vec3f>() : *_tvec.getMat().ptr<Vec3d>();
CV_Assert(_K.size() == Size(3,3) && (_K.type() == CV_32F || _K.type() == CV_64F) && _D.type() == _K.type() && _D.total() == 4);
cv::Vec2d f, c;
if (_K.depth() == CV_32F)
{
Matx33f K = _K.getMat();
f = Vec2f(K(0, 0), K(1, 1));
c = Vec2f(K(0, 2), K(1, 2));
}
else
{
Matx33d K = _K.getMat();
f = Vec2d(K(0, 0), K(1, 1));
c = Vec2d(K(0, 2), K(1, 2));
}
Vec4d k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr<Vec4f>(): *_D.getMat().ptr<Vec4d>();
Matx33d R;
Matx<double, 3, 9> dRdom;
Rodrigues(om, R, dRdom);
cv::Affine3d aff(om, T);
const Vec3f* Xf = objectPoints.getMat().ptr<Vec3f>();
const Vec3d* Xd = objectPoints.getMat().ptr<Vec3d>();
Vec2f *xpf = imagePoints.getMat().ptr<Vec2f>();
Vec2d *xpd = imagePoints.getMat().ptr<Vec2d>();
for(size_t i = 0; i < n; ++i)
{
Vec3d Xi = objectPoints.depth() == CV_32F ? (Vec3d)Xf[i] : Xd[i];
Vec3d Y = aff*Xi;
Vec2d x(Y[0]/Y[2], Y[1]/Y[2]);
cout<<"a="<<x(0)<<"b="<<x(1)<<endl;
double r2 = x.dot(x);
double r = std::sqrt(r2);
cout<<"r"<<r<<endl;
// Angle of the incoming ray:
double theta = atan(r);
double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
double inv_r = r > 1e-8 ? 1.0/r : 1;
double cdist = r > 1e-8 ? theta_d * inv_r : 1;
Vec2d xd1 = x * cdist;
Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]);
Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]);
if (objectPoints.depth() == CV_32F)
xpf[i] = final_point;
else
xpd[i] = final_point;
}
}
void fish_project_point(Point3d pt3d,Mat R,Mat t,Mat K,Mat coff,Point2d& pt2d)
{
//1. Transform into Camera Coords.
Vec3d p3Dw (pt3d);
cv::Affine3d aff(R, t);
//cv::Mat p3Dc = R*p3Dw+t;
Vec3d p3Dc = aff*p3Dw;
cout<<"p3Dc"<<p3Dc<<endl;
// 2.Project into norm plan
double invz = 1/p3Dc(2);
double a = p3Dc(0)*invz;
double b = p3Dc(1)*invz;
//cv::Mat p3Dw (pt3d);
//cv::Mat p3Dc = R*p3Dw+t;
// const double invz = 1/p3Dc.at<double>(2);
// const double a = p3Dc.at<double>(0)*invz;
// const double b = p3Dc.at<double>(1)*invz;
cout<<"project norm plan x="<<a<<"y="<<b<<endl;
//3.undistort a b
const double k1=coff.at<double>(0,0);
const double k2=coff.at<double>(0,1);
const double k3=coff.at<double>(0,2);
const double k4=coff.at<double>(0,3);
//cout<<"k1"<<k1<<" "<<k2<<" "<<k3<<" "<<k4<<endl;
double r2=a*a+b*b;
double r=std::sqrt(r2);
cout<<"r"<<r<<endl;
double theta = atan(r);
double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
double theta_d = theta + k1*theta3 + k2*theta5 + k3*theta7 + k4*theta9;
double inv_r = r > 1e-8 ? 1.0/r : 1;
double cdist = r > 1e-8 ? theta_d * inv_r : 1;
double xd=a*cdist;
double yd=b*cdist;
// double xd=theta_d*a*inv_r;
// double yd=theta_d*b*inv_r;
double fx=K.at<double>(0,0);
double fy=K.at<double>(1,1);
double cx=K.at<double>(0,2);
double cy=K.at<double>(1,2);
//cout<<"fx"<<fx<<" "<<fy<<" "<<cx<<" "<<cy<<endl;
double u = fx*xd+cx;
double v = fy*yd+cy;
pt2d.x=u;
pt2d.y=v;
return ;
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "projecttest");
double len=4;
//1.1 get para intrinsic
double f = 200, w = 1000, h = 1000;
cv::Mat K = (Mat_<double>(3, 3) <<
f, 0, w/2,
0, f, h/2,
0, 0, 1);
cv::Mat coff=(Mat_<double>(1,4) <<0.02,0.1,0.0001,0.0001);
//1.2 get para extrinsic
Mat cv_R12=(cv::Mat_<double>(3, 3) <<
0,0,1,
-1,0,0,
0,-1,0);
cv::Mat cv_t12 = (cv::Mat_<double>(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;
cout<<"cv_t21"<<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;
// //x>0 ABCD EFGH
Point3d cube_center={len,0,len/2};
Point3d A={cube_center.x-len/2,cube_center.y+len/2,cube_center.z-len/2};
Point3d B={cube_center.x-len/2,cube_center.y-len/2,cube_center.z-len/2};
Point3d C={cube_center.x+len/2,cube_center.y-len/2,cube_center.z-len/2};
Point3d D={cube_center.x+len/2,cube_center.y+len/2,cube_center.z-len/2};
cout<<"ABCD"<<A<< B<<C<<D<<endl;
vector<Point3d> vp3d;
vp3d.push_back(A);
vp3d.push_back(B);
vp3d.push_back(C);
vp3d.push_back(D);
//3.get binImage
vector<Point2d> vpt2d;
vector<Point2d> fishPrj2d;
cv::Mat rvec21;
Rodrigues(cv_R21,rvec21);
cout<<"rvec21"<<rvec21<<endl;
//opencv implement
fisheye::projectPoints(vp3d,vpt2d,rvec21,cv_t21,K,coff);
//opencv_project_points(vp3d,vpt2d, rvec21,cv_t21,K,coff,0);
//my implement
for(size_t i=0;i<vp3d.size();i++)
{
Point3d p3d=vp3d[i];
Point2d p2d;
fish_project_point(p3d,cv_R21,cv_t21,K,coff,p2d);
fishPrj2d.push_back(p2d);
}
for(int i=0;i<vpt2d.size();i++)
{
cout<<"prj x="<<vpt2d[i].x<<" y="<<vpt2d[i].y<<endl;
cout<<"my prj x="<<fishPrj2d[i].x<<" y="<<fishPrj2d[i].y<<endl;
}
return 0;
}
参考文献
https://docs.opencv.org/3.4/db/d58/group__calib3d__fisheye.html