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

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

---

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

---

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

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

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

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

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

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

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>(