欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识！
您现在的位置是： 首页

OpenCV中的鱼眼相机模型详解

程序员文章站 2022-03-07 12:45:12
...

针孔相机、鱼眼相机模型推导：
一、针孔相机模型
空间的三维物体要成像到相机的 CMOS/CCD 上面，形成了图像。图像上的每个点对应空间上的一个点。
将世界坐标系上的一点 (x,y,z)映射到CMOS/CCD图像坐标系上，只要两步：
1、世界坐标系的坐标转化到相机坐标系。
2、相机坐标系坐标转化到图像坐标系。
1.1 世界坐标系到相机坐标系
OpenCV中的鱼眼相机模型详解

即要将师姐坐标系上的点(Xw,Yw,Zw)T转化成相机坐标系上的点(Xc,Yc,Zc)T。
根据线性代数(计算机图形学)的知识可以知道，坐标系的转化只需要将原坐标乘以一个旋转矩阵再加上一个平移矩阵即可。公式如下：
OpenCV中的鱼眼相机模型详解

写成齐次矩阵的形式如下：
OpenCV中的鱼眼相机模型详解

平移矩阵有三个参数，分别是相机坐标系x,y,z轴相对于世界坐标系的位移
旋转矩阵也有三个参数，分别是相机坐标系x,y,z轴相对于世界坐标系的旋转角度
其中，旋转矩阵的具体形式如下：
OpenCV中的鱼眼相机模型详解

相机的外参就是旋转矩阵的三个参数加上平移矩阵的三个参数。代表了相机相对于世界坐标系的位姿
1.2 相机坐标系坐标转化到图像坐标系
OpenCV中的鱼眼相机模型详解

相机坐标系上的点(X,Y,Z)T,转化成图像坐标系上将会变成
OpenCV中的鱼眼相机模型详解

用齐次坐标表示相当于( X , Y , Z , 1 )T → ( f X , f Y , Z ) T,用矩阵乘法来表示：
OpenCV中的鱼眼相机模型详解

如果相机的CMOS/CCD的位置不在像面的中心位置，则要添加两个参数px和py:
OpenCV中的鱼眼相机模型详解

现在我们能得到空间上的点在图像坐标系上的坐标位置，但是该坐标的单位是米，我们需要将其转化成像素单位，所以要有比例因子mx、my:
OpenCV中的鱼眼相机模型详解

如果相机CMOS的x轴和y轴不垂直，还要有一个参数s：
OpenCV中的鱼眼相机模型详解

这就是相机的内参数矩阵。
空间的点Pw左乘一个旋转矩阵R，加上旋转矩阵t，得到了该点在相机坐标系上的坐标。再左乘相机的内参数矩阵K就得到了该点在图像坐标系上的像素坐标。
OpenCV中的鱼眼相机模型详解

至此，针孔相机模型推导完成。

二、鱼眼相机模型
相比针孔模型可以将3d点直接投影到归一化平面，鱼眼相机则多了一个中间过程：先将3d点投影到单位球面，再将单位球面上的点投影到归一化平面上。废话不多说，请看鱼眼相机投影模型示意图：
OpenCV中的鱼眼相机模型详解

针孔相机的模型为:rd=ftan(θ)
鱼眼相机模型的公式表示为：rd=fθ
鱼眼相机模型有多种，此处只选择了其中一种

类似针孔相机模型的推导，鱼眼相机成像也很简单。
首先，我们用上面的方法可以得到空间上的点在相机坐标系上的表示。
OpenCV中的鱼眼相机模型详解

简单推导可得：
OpenCV中的鱼眼相机模型详解

这样，就可以得到该点在鱼眼相机图像坐标系上的位置了。
OpenCV中的鱼眼相机模型详解

鱼眼相机的成像模型
成像模型是这样的
OpenCV中的鱼眼相机模型详解
OpenCV中的鱼眼相机模型详解

正向成像过程基本分为三步：
1.针孔成像，得到理想坐标
2.鱼眼畸变
3.调整图像坐标
3D世界中一点P，坐标为X，首先针孔投影到相平面坐标是[a,b]，:
OpenCV中的鱼眼相机模型详解

且有
OpenCV中的鱼眼相机模型详解

之后施加鱼眼畸变：
OpenCV中的鱼眼相机模型详解

畸变坐标为[x’,y’]，满足:
OpenCV中的鱼眼相机模型详解

最后调整到像素坐标系[u; v],满足：
OpenCV中的鱼眼相机模型详解

其中k1,k2,k3,k4为鱼眼相机的四个畸变系数，fx,fy为相机的x轴焦距和y轴焦距，cx和cy为相机的主点坐标，
问题：
对应相机坐标系的一点P,我们如何通过鱼眼相机的4个畸变系数找到这个点在像素平面上的正确位置？
步骤如下：
1.将世界坐标系的三维坐标点投影摄像头坐标系
设P为世界坐标系中的三维点为X，摄像机坐标系中P的坐标为Xc(x,y,z):Xc=RX+t
通过外参数的旋转和平移转换。
2.摄像头坐标系点转换到归一化图像平面
将点转换为归一化平面为（a,b）a=x/z and b=y/z
3.对归一化平面的点进行畸变纠正
给定归一化坐标，可以求出畸变纠正后的坐标( x ′ , y ′ )
OpenCV中的鱼眼相机模型详解

4.转换到图像平面上
最后得到图像平面上的点（ u , v ）
OpenCV中的鱼眼相机模型详解

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;
}
相关标签： 传感器融合

上一篇： 本地项目上传到GitHub

下一篇： rikirobot的IMU数据融合优化——解决导航中静止状态下的姿态漂移

推荐阅读