相机与模型

相机与图像

一、相机模型

1. 针孔相机模型

1）内参

设 $O-x-y-z$ 为相机坐标系,习惯上我们让 $z$轴指向相机前方,$x$ 向右,$y$ 向下。$O$ 为摄像机的光心,也是针孔模型中的针孔。现实世界的空间点$P$,经过小孔 $O$ 投影之后,落在物理成像平面 $O^{\prime }-x^{\prime }-y^{\prime }$ 上,成像点为 $P^{\prime }$。设$P$ 的坐标为 $[X,Y,Z{]}^T$ ,$P^{\prime }$为 $[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$ ,并且设物理成像平面到小孔的距离为$f$(焦距)。那么,根据三角形相似关系,有:
$$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}$$
那么 ，整理后
$$\begin{gathered} X^{\prime }=f\frac{X}{Z} \\ {Y}^{\prime }=f\frac{Y}{Z} \end{gathered}$$
我们得到了相机坐标系和物理成像坐标系之间的关系，但为了描述像素与相机之间的关系，还需要在在物理成像平面上固定一个像素平面 $o-u-v$。我们在像素平面得到了 $P^{\prime }$ 的像素坐标:$[u,v{]}^T$。

像素坐标系通常的定义方式是:原点 $o^{\prime }$ 位于图像的左上角,$u$ 轴向右与 $x$ 轴平行,$v$轴向下与$y$轴平行。像素坐标系与成像平面之间,相差了一个缩放和一个原点的平移。我们设像素坐标在$u$轴上缩放了 α 倍,在$v$上缩放了 β 倍。同时,原点平移了 $[c_x,c_y{]}^T$ 。那么,$P^{\prime }$ 的坐标与像素坐标$ [u, v]^ T$的关系为:
$$\begin{gathered} u=\alpha X^{\prime }+c_x \\ v=\beta Y^{\prime }+c_y \end{gathered}$$
把$\alpha f$ 合并成$f_x$，$\beta f$合并成$f_y$ ，并将$X^{\prime },Y^{\prime }$带入上式，可得：
$$\begin{gathered} u=f_x\frac{X}{Z}+c_x \\ v=f_y\frac{Y}{Z}+c_y \end{gathered}$$
其中,$f$ 的单位为米,$\alpha ,\beta$ 的单位为像素每米,所以$f_x,f_y$ 的单位为像素。把该式写成矩阵形式,会更加简洁,不过左侧需要用到齐次坐标:
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_x\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\frac{1}{Z}K{P}_c$$
该式中,我们把中间的量组成的矩阵称为相机的内参数矩阵(Camera Intrinsics)K。

通常,相机的内参在出厂之后是固定的,不会在使用过程中发生变化，而有时需要自己去标定内参，比如说张正友标定法标定相机内参。

2）外参

我们使用的是 $P$ 在相机坐标系下的坐标。由于相机在运动,所以 $P$ 的相机坐标应该是它的世界坐标(记为$P_w$ ),
根据相机的当前位姿,变换到相机坐标系下的结果。相机的位姿有它的旋转矩阵和平移向量来描述
$$Z{P}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K(R{P}_w+t)=KT{P}_w$$
相机的位姿 R, t 又称为相机的外参数(Camera Extrinsics)。相比于不变的内参,外参会随着相机运动发生改变,同时也是 SLAM中待估计的目标,代表着机器人的轨迹。

最后,我们小结一下单目相机的成像过程:

1. 首先,世界坐标系下有一个固定的点$P$ ,世界坐标为$Pw$ ;
2. 由于相机在运动,它的运动由 $R,t$或变换矩阵 $T\in SE(3)$ 描述。P 的相机坐标为:${\tilde{P}}_c=R{P}_w+t$。
3. 这时的${\tilde{P}}_c$仍有 $X,Y,Z$ 三个量,把它们投影到归一化平面$Z=1$ 上,得到 $P$的归
   一化相机坐标$:P_c=[X/Z,Y/Z,1{]}^T$ 。
4. 最后,P 的归一化坐标经过内参后,对应到它的像素坐标:$P_{uv}=K{P}_c$ 。

综上所述,我们一共谈到了四种坐标:世界、相机、归一化相机和像素坐标。四种坐标系的变换反映了整个成像的过程。

2. 双目相机模型

双目相机的原理是通过同步采集左右相机的图像,计算图像间视差,来估计每一个像素的深度。双目相机一般由左眼和右眼两个水平放置的相机组成。我们可以把两个相机都看作针孔相机。它们是水平放置的,意味两个相机的光圈中心都位于 x 轴上。它们的距离称为双目相机的基线(Baseline, 记作 b),是双目相机的重要参数。

$O_L,O_R$ 为左右光圈中心,蓝色框为成像平面,$f$ 为焦距。$u_L$和$u_R$ 为成像平面的坐标。请注意按照图中坐标定义,$u_R$ 应该是负数,所以图中标出的距离为 $-u_R$

现在,考虑一个空间点 $P$ ,它在左眼和右眼各成一像,记作$P_L,P_R$ 。由于相机基线的存在,这两个成像位置是不同的。理想情况下,由于左右相机只有在 x 轴上有位移,因此$P$ 的像也只在$x$ 轴(对应图像的 $u$ 轴)上有差异。我们记它在左侧的坐标为 $u_L$ ,右侧坐标为$u_R$ 。那么,它们的几何关系如上图所示。根据三角形$P-P_L-P_R和P-O_L-O_R$的相似关系,有
$$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$$
$$z=\frac{fb}{d},d=u_L-u_R$$

3. 图像操作

我们知道，一张图像是有像素组成的，在一张灰度图中,每个像素位置$(x,y)$ 对
应到一个灰度值$I,$所以一张宽度为 w,高度为 h 的图像,数学形式可以记成一个矩阵:
$I(x,y)\in R^{w\times h}.$我们通过OpenCV库来熟悉图像的读取，存储以及遍历操作

1）图像基本操作

#include <iostream>
#include "opencv2/core.hpp"
#include "opencv2/highgui.hpp"

int main(int argc, char** argv) 
{
    cv::Mat grayim(600, 800, CV_8UC1);
    cv::Mat colorim(600, 800, CV_8UC3);

    // 遍历所有像素，并设置像素值
    for (int i = 0; i < grayim.rows; i++) {   
        // 获取第i行首像素指针
        uchar *p = grayim.ptr<uchar>(i);
        for (int j = 0; j < grayim.cols; j++) {   
            // 对第i行的每个像素操作
            p[j] = (i+j)%255;
        }  
    }
    
    //遍历所有像素,并设置像素值
    //for( int i = 0; i < grayim.rows; ++i)
    //	for( int j = 0; j < grayim.cols; ++j )
    //		grayim.at<uchar>(i,j) = (i+j)%255;
    cv::imshow("gray",grayim);
    cv::waitKey(1000);

    // 遍历所有像素，并设置像素值
    for (int i = 0; i < colorim.rows; i++) {
        for (int j = 0; j < colorim.cols; j++) {
            cv::Vec3b pixel;
            pixel[0] = i%255;
            pixel[1] = j%255;
            pixel[3] = 0;
            colorim.at<cv::Vec3b>(i,j) = pixel;
        }
    }
    cv::imshow("colorim",colorim);
    cv::waitKey(1000);

    // 读取图片
    cv::Mat image;
    image = cv::imread(argv[1]);
    cv::imshow("image",image);
    cv::waitKey(0);
    return 0;
}

2） 双目图像生成点云图

通过双目视觉的左右图像，计算图像对应的视差图，然后计算各像素在相机坐标系下的坐标，他们将构成点云。

Pangolin显示点云

#include <iostream>
#include <string>
#include "opencv2/opencv.hpp"
#include "opencv2/highgui.hpp"
#include <Eigen/Core>

#include <pangolin/pangolin.h>
#include <unistd.h>

std::string left_file = "/home/tgc/练习/opencv/save/left.png";
std::string right_file = "/home/tgc/练习/opencv/save/right.png";

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &pointcloud);

int main(int argc, char** argv)
{   


    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;
     //焦距
    double f = 0.04;    


      // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    // 神奇的参数
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    // 生成点云
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> pointcloud;

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Eigen::Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // 根据双目模型计算 point 的位置
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            // double depth = fx * b / (disparity.at<uchar>(v, u));
            double depth = f * b * 1000 /(disparity.at<uchar>(v, u));

            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    // cv::imshow("disparity", disparity / 96.0);
    // cv::waitKey(0);
    // 画出点云
    showPointCloud(pointcloud);

    return 0;
}


void showPointCloud(const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        std::cerr << "Point cloud is empty!" << std::endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

PCL显示点云

#include <iostream>
#include <string>
#include "opencv2/opencv.hpp"
#include "opencv2/highgui.hpp"
#include <Eigen/Core>
#include <pcl-1.9/pcl/io/pcd_io.h>
#include <pcl-1.9/pcl/point_types.h>
#include <pcl-1.9/pcl/point_cloud.h>   
  
std::string left_file = "/home/tgc/练习/opencv/save/left.png";
std::string right_file = "/home/tgc/练习/opencv/save/right.png";  


int main(int argc, char** argv)
{   


    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;
     //焦距
    double f = 0.04;    

  	typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
    // 新建一个点云
    PointCloud::Ptr pointCloud( new PointCloud );

    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    for (int v = 0; v < left.rows; v++) {
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Eigen::Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色
            // Eigen::Vector3d point; // 前三维为xyz,第四维为颜色

            // 根据双目模型计算 point 的位置
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            // double depth = fx * b / (disparity.at<uchar>(v, u));
            double depth = f * b * 1000 /(disparity.at<uchar>(v, u));

            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            PointT p;
            p.x = point[0];
            p.y = point[1];
            p.z = point[2];
            p.b = left.data[ v*left.step+u*left.channels() ];
            p.g = left.data[ v*left.step+u*left.channels() +1 ];
            p.r = left.data[ v*left.step+u*left.channels() +2 ];

            pointCloud->points.push_back(p);
            
        }
    }

    pointCloud->is_dense = false;
    std::cout << "point number is :" << pointCloud->size() << std::endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
    return 0;
}

参考连接 ： 视觉slam十四讲