高博SLAM基础课第四讲—

高博SLAM基础课第四讲——相机模型

（1）本节内容
1、针孔相机模型 2、误差来源——畸变 3、双目相机模型

（2）需要的基础知识
单独成章节，不需要太多基础

（3）开发环境
编译平台：ubuntu16.04，
编译软件：IDE：Clion 编译器：Cmake 语言标准：C++11

（4）学习内容

1、针孔相机模型

小孔模型能够把三维世界中的物体投影到一个二维成像平面。同理，可以用这个简单的模型来解释相机的成像模型。

对这个简单的针孔模型进行几何建模。设O−x−y−zO−x−y−z为相机坐标系，习惯上让z轴指向相机前方，x向右，y向下。O和为摄像机的光心，也是针孔模型中的针孔。现实世界的空间点P，经过小孔O投影之后，落在物理成像平面O′−x′−y′−z′上，成像点为P′。设P的坐标为[X,Y,Z]T，P′为[X′,Y′,Z′]T，并且设物理成像平面到小孔的距离为ff焦距）。那么，根据三角形相似关系，有：

其中负号表示成的像是倒立的。为了简化模型，可以将成像平面对称到相机前方，和三维空间点一起放在摄像机坐标系的同一侧，如图4-2中间的样子所示。这样可以把公式中的负号去掉，使式子更加简洁：

整理得：

上式描述了点P和它的像之间的空间关系。不过，在相机中，最终获得的是一个个的像素，这需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，在物理成像平面上固定一个像素平面o−u−v。在像素平面上得到了P′的像素坐标：[u,v]T
像素坐标系通常的定义方式是：原点o′位于图像的左上角，u轴向右与x轴平行，v轴向下与y轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。设像素坐标在u轴上缩放了α倍，在v上缩放了β倍。同时，原点平移了[cx,cy]T。那么，P′点的坐标与像素坐标[u,v]T的关系为：

该式中，把中间的量组成的矩阵称为相机的内参数矩阵K。

K有4个未知数和相机的构造相关，fx,fy和相机的焦距，像素的大小有关；cx,cy是平移的距离，和相机成像平面的大小有关。

求解K的过程叫做相机的标定，通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变化。有的相机生产商会告诉相机的内参，而有时需要自己确定相机的内参，也就是所谓的标定。标定算法已成熟，在网络上可以找到大量的标定教学。

外参数

其中，p是图像中像点的像素坐标，K是相机的内参数矩阵，P是相机坐标系下的三维点坐标。
上面推导使用的三维点坐标是在相机坐标系下的，相机坐标系并不是一个“稳定”的坐标系，其会随着相机的移动而改变坐标的原点和各个坐标轴的方向，用该坐标系下坐标进行计算，显然不是一个明智的选择。需要引进一个稳定不变坐标系：世界坐标系，该坐标系是绝对不变，SLAM中的视觉里程计就是求解相机在世界坐标系下的运动轨迹。

设Pc是P在相机坐标系坐标，Pw是其在世界坐标系下的坐标，可以使用一个旋转矩阵R和一个平移向量t，将Pc变换为Pw

2、畸变

为了获得好的成像效果，在相机前方加了透镜。透镜的加入对成像过程中光线的传播会产生新的影响：一是透镜自身的形状对光线传播的影响，二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使得光线穿过透镜投影到成像平面时的位置发生变化。
由透镜形状引起的畸变称之为径向畸变。它们主要分为两大类，桶形畸变和枕形畸变

切向畸变。

平面上的任意一点p可以用笛卡尔坐标表示为[x,y]T，也可以写成极坐标的形式[r,θ]T，
其中r表示点p离坐标系原点的距离，θ表示和水平轴的夹角。
对于径向畸变，可以用一个多项式函数来描述畸变前后的坐标变化：

其中[x,y]T[x,y]T是为纠正的点的坐标，[xcorrccted,ycorrccted]T[xcorrccted,ycorrccted]T是纠正后的点的坐标，它们都是归一化平面上的点。
对于切向畸变，有：

通过五个畸变系数找到相机坐标系中的一点在像素平面上的正确位置：
将三维空间点投影到归一化平面。设它的归一化坐标为[x,y]T。
对归一化平面上的点进行径向畸变和切向畸变纠正。

3.将纠正后的点通过内参数矩阵投影到像素平面，得到改点在图像上的正确位置。

3、双目相机

转载自https://blog.csdn.net/weixin_38593194/article/details/86348975

作业1：

去畸变代码：

#include <opencv2/opencv.hpp>
#include <string>
#include <math.h>

using namespace std;

string image_file = "../test.png";   // 请确保路径正确

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

    // 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file,0);   // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {

            double u_distorted = 0, v_distorted = 0;
            // TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines)
            // start your code here
            //image_undistort中含有非畸变的图像坐标
            //将image_undistort的坐标通过内参转换到归一化坐标系下，此时得到的归一化坐标是对的
            //将得到的归一化坐标系进行畸变处理
            //将畸变处理后的坐标通过内参转换为图像坐标系下的坐标
            //这样就相当于是在非畸变图像的图像坐标和畸变图像的图像坐标之间建立了一个对应关系
            //相当于是非畸变图像坐标在畸变图像中找到了映射
            //对畸变图像进行遍历之后，然后赋值（一般需要线性插值，因为畸变后图像的坐标不一定是整数的），即可得到矫正之后的图像
            double x1,y1,x2,y2;
            x1 = (u-cx)/fx;
            y1 = (v-cy)/fy;
            double r2;
            r2 = pow(x1,2)+pow(y1,2);
            x2  = x1*(1+k1*r2+k2*pow(r2,2))+2*p1*x1*y1+p2*(r2+2*x1*x1);
            y2 = y1*(1+k1*r2+k2*pow(r2,2))+p1*(r2+2*y1*y1)+2*p2*x1*y1;

            u_distorted = fx*x2+cx;
            v_distorted = fy*y2+cy;

            // end your code here

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
            } else {
                image_undistort.at<uchar>(v, u) = 0;
            }
        }

    // 画图去畸变后图像
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}

1.Clion识别路径时的当前路径在工程文件夹下的cmaka-build-debug文件夹中

Run->Edit Configurations中修改Working directory可以修改当前路径

2.计算的x1，y1皆为相机Z=1平面

3.要注意x和u是横坐标，y和v是纵坐标的对应关系

结果如下

注意到去畸变后由于插值做法使得珠子上的铁环产生锯齿，这个特征或许会影响特征点的鲁棒性

待学习后续知识后推导

另外调用opencv库的做法代码如下

Mat imageAPI;
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    cameraMatrix.at<double>(0, 0) = fx;
    cameraMatrix.at<double>(0, 1) = 0;
    cameraMatrix.at<double>(0, 2) = cx;
    cameraMatrix.at<double>(1, 1) = fy;
    cameraMatrix.at<double>(1, 2) = cy;
    cameraMatrix.at<double>(2,2)=1;
//标定矩阵
    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
    distCoeffs.at<double>(0, 0) = k1;
    distCoeffs.at<double>(1, 0) = k2;
    distCoeffs.at<double>(2, 0) = p1;
    distCoeffs.at<double>(3, 0) = p2;
    distCoeffs.at<double>(4, 0) = 0;
//畸变向量
    Mat view, rview, map1, map2;
    Size imageSize;
    imageSize = image.size();
    initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
                            getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
                            imageSize, CV_16SC2, map1, map2);
    remap(image, imageAPI, map1, map2, INTER_LINEAR);

    // 画图去畸变后图像
    cv::imshow("imageAPI", imageAPI);
    cv::waitKey();

此输出为

以后需要用再深究，花时间有点久了。

作业2：

代码如下：

#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径，如果不对，请调整
string left_file = "/home/steve/left.png";
string right_file = "/home/steve/right.png";
string disparity_file = "/home/steve/disparity.png";

// 在panglin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<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;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Mat disparity = cv::imread(disparity_file, 0); // disparty 为CV_8U,单位为像素

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

    // TODO 根据双目模型计算点云
    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v+=1)
        for (int u = 0; u < left.cols; u+=1) {

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

            // start your code here (~6 lines)
            // 根据双目模型计算 point 的位置
            // start your code here (~6 lines) // 根据双目模型计算 point 的位置
            unsigned char d=disparity.ptr<unsigned char>(v)[u];

            point[2]=(fx*b)/(double)d;
            point[1]=(v-cy)*point[2]/fy;
            point[0]=(u-cx)*point[2]/fx;
            pointcloud.push_back(point);

            // end your code here
        }

    // 画出点云
    showPointCloud(pointcloud);
    return 0;
}

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

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << 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;
}

效果：

自写部分：

unsigned char d=disparity.ptr<unsigned char>(v)[u];
            point[2]=(fx*b)/(double)d;
            point[1]=(v-cy)*point[2]/fy;
            point[0]=(u-cx)*point[2]/fx;
            pointcloud.push_back(point);

https://blog.csdn.net/fb_941219/article/details/89716815#2Disparity_71
有详解

此处读取视差图要用uchar格式，用short读会出问题
由视差求深度的公式为depth=f×baseline/disparity 此处f和d的单位都是像素，除得用米表示的深度
不过由于此处x，y都是按比例缩放的，只要正确读出了disparity，在点云中显示都是一样的（范围不同）

希望知道怎么用这些库啊..

PS：

vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>;

其实上述的这段代码才是标准的定义容器方法，只是我们一般情况下定义容器的元素都是C++中的类型，所以可以省略，这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。