一定把vslam拿下！！！-Ch4 相机模型与非线性优化(小白版）

一定把vslam拿下！！！（小白版）// 部分内容转载自他人博客！侵删

---

深蓝学院视觉十四讲课后作业——ch3 李群李代数
本文记录了学习的部分笔记心得与课后习题 主要用途为学习巩固、方便自己查阅
部分内容参阅互联网或者书籍知识，部分图片来自网络，欢迎批评指正
如有问题，可及时与我沟通讨论！

---

图像去畸变 (2 分，约 1 小时)

现实生活中的图像总存在畸变。原则上来说，针孔透视相机应该将三维世界中的直线投影成直线，但
是当我们使用广角和鱼眼镜头时，由于畸变的原因，直线在图像里看起来是扭曲的。本次作业，你将尝试
如何对一张图像去畸变，得到畸变前的图像。

运行完成结果如下：
可看到右图是畸变被消除后

代码更改如下：（其实就是将for循环中的对openCV的调用改成自己计算的数据 并增添了调出原图像的指令，方便进行对比）

//
// Created by 高翔 on 2017/12/15.
// Changed by ruru on 2022/11/15

#include 
#include 

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 x;
            x = (u - cx) / fx;
            double y;
            y = (v - cy) / fy;
            double r;
            r = sqrt(x * x + y * y);
            
            double x_dis = x * (1 + k1 * r * r + k2 * r*r*r*r) + 2 * p1 * x *y + p2 * (r * r + 2 * x * x);
            double y_dis = y * (1 + k1 * r * r + k2 * r*r*r*r) + 2 * p2 * x *y + p1 * (r * r + 2 * y * y);

            double u_dis = fx * x_dis + cx;
            double v_dis = fy * y_dis + cy;

            //赋值
            if(u_dis >= 0 && v_dis >= 0 && u_dis < cols && v_dis < rows)
            {
                image_undistort.at<uchar>(v,u) = image.at<uchar>((int) v_dis, (int) u_dis);
            }
            else{
                image_undistort.at<uchar>(v,u) = 0;	//.at(int y， int x)可以用来存取图像中对应坐标为（x，y）的元素坐标
            }
        }
    }

    //画出去畸变后的图像
  
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();
 	cv::imshow("distorted", image);
 	
    return 0;
}

CMakeLists 如下：

cmake_minimum_required(VERSION 3.1)
project(undistort_image)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(undistort_image undistort_image.cpp)
target_link_libraries(undistort_image ${OpenCV_LIBS})

鱼眼模型与去畸变 (2 分，约 2 小时)

在很多视觉 SLAM 的应用里，我们都会选择广角或鱼眼相机作为主要的视觉传感器。与针孔相机不
同，鱼眼相机的视野往往可以在 150◦ 以上，甚至超过 180◦。普通的畸变模型在鱼眼相机下工作的并不好，
幸好鱼眼相机有自己定义的畸变模型。
请参阅 OpenCV 文档(https://docs.opencv.org/3.4/db/d58/group__calib3d__fisheye.html)，
完成以下问题：
1. 请说明鱼眼相机相比于普通针孔相机在 SLAM 方面的优势。
可视范围更广，相机拍摄角度大，可获得更多的信息

2. 请整理并描述 OpenCV 中使用的鱼眼畸变模型（等距投影）是如何定义的，它与上题的畸变模型
有何不同。

3. 完成 fisheye.cpp 文件中的内容。针对给定的图像，实现它的畸变校正。要求：通过手写方式实现，
不允许调用 OpenCV 的 API。
完成代码运行后结果如下：

代码更改如下：

//
// Created by xiang on 2021/9/9.
//

#include 

// 文件路径，如果不对，请调整
std::string input_file = "./fisheye.jpg";

int main(int argc, char **argv) {
  // 本程序实现鱼眼的等距投影去畸变模型
  // 畸变参数（本例设为零）
  double k1 = 0, k2 = 0, k3 = 0, k4 = 0;

  // 内参
  double fx = 689.21, fy = 690.48, cx = 1295.56, cy = 942.17;

  cv::Mat image = cv::imread(input_file);
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC3); // 去畸变以后的图

  // 计算去畸变后图像的内容
  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
      //反向投影
      double x = (u - cx)/fx;
      double y = (v - cy)/fy;

      //引入畸变
      double r = sqrt(x*x + y*y);
      double theta = atan(r);
      double theta_d = theta + k1*pow(theta, 3) + k2*pow(theta, 5) + k3*pow(theta, 7) + k4*pow(theta, 9);
      double x_distorted = theta_d*x/r;
      double y_distorted = theta_d*y/r;
      
      //重新投影
      u_distorted = x_distorted*fx + cx;
      v_distorted = y_distorted*fy + cy;

      // end your code here

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

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

  return 0;
}

CMakelists如下：（在原有里加上可执行并链接库即可）

add_executable(fisheye fisheye.cpp)
target_link_libraries(fisheye ${OpenCV_LIBRARIES})

4. 为什么在这张图像中，我们令畸变参数 k1, . . . , k4 = 0，依然可以产生去畸变的效果？
由OpenCV中的畸变模型可以看出，当k 1 , k 2 , k 3 , k 4 = 0 时，畸变模型就转化成了理想的等距投影模型，此时仍然具有去畸变能力。
尝试更改k1~k4的值，得出不同结果：、
如k1=k3=1，k2=k4=0时 如下图所示, 而k1=k2=k3=k4=1时 ，如下图所示：

5. 在鱼眼模型中，去畸变是否带来了图像内容的损失？如何避免这种图像内容上的损失呢？
鱼眼图一般为圆形，边缘的信息被压缩的很密，经过去除畸变后原图中间的部分会被保留的很好，而边缘位置一般都会被拉伸的很严重、视觉效果差，所以通常会进行切除，因此肯定会带来图像内容的损失。增大去畸变时图像的尺寸，或者使用单目相机和鱼眼相机图像进行融合，补全丢失的信息。

双目视差的使用 (2 分，约 1 小时)

双目相机的一大好处是可以通过左右目的视差来恢复深度。课程中我们介绍了由视差计算深度的过程。
本题，你需要根据视差计算深度，进而生成点云数据。本题的数据来自 Kitti 数据集 [2]。
Kitti 中的相机部分使用了一个双目模型。双目采集到左图和右图，然后我们可以通过左右视图恢复出深度。经典双目恢复深度的算法有 BM(Block Matching), SGBM(Semi-Global Block Matching)[3, 4] 等，但本题不探讨立体视觉内容（那是一个大问题）。我们假设双目计算的视差已经给定，请你根据双目模型，画出图像对应的点云，并显示到 Pangolin 中。
理论部分：

1. 推导双目相机模型下，视差与 XY Z 坐标的关系式。请给出由像素坐标加视差 u, v, d 推导 XY Z与已知 XY Z 推导 u, v, d 两个关系。
2. 推导在右目相机下该模型将发生什么改变。
   编程部分：本题给定的左右图见 code/left.png 和 code/right.png，视差图亦给定，见 code/right.png。
   双目的参数如下：fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157.
   且双目左右间距（即基线）为：d = 0.573 m.
   请根据以上参数，计算相机数据对应的点云，并显示到 Pangolin 中。程序请参考 code/disparity.cpp 文件。
   运行结果如下：
   
   
   代码如下：

//
// Created by 高翔 on 2017/12/15.
// Changed by ruru on 2021/11/16

#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

// 文件路径，如果不对，请调整
string left_file = "./left.png";
string right_file = "./right.png";
string disparity_file = "./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 d = 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++)
        for (int u = 0; u < left.cols; u++) {

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

            // start your code here (~6 lines)
            double disparity_pix = disparity.at<uchar>(v, u);
            double X = (u-cx)*d/disparity_pix;
            double Y = (v-cy)*d/disparity_pix*fx/fy;
            double Z = d*fx/disparity_pix;
            point[0] = X;
            point[1] = Y;
            point[2] = Z;
            pointcloud.push_back(point);


            // 根据双目模型计算 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;
}

CMakelists 如下：（在原有的中 加上pangolin和eigen库的调用 以及新的可执行和链接库）

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})

add_executable(disparity disparity.cpp)
target_link_libraries(disparity ${OpenCV_LIBRARIES})
target_link_libraries(disparity ${Pangolin_LIBRARIES})