(学习笔记)手动图像去畸变

实现了视觉SLAM十四讲的图像去畸变
opencv有提供去畸变函数，自己手写实现一遍理解更深刻

#include 
#include 

using namespace std;
using namespace cv;

int main(int argc, char *argv[])
{
    // 畸变参数
    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;
    // 图像路径
    //换成自己的图像路径
    string path = "/home/yjh/opencv_study/image/distorted.png";
    // 图像是灰度图
    Mat image = imread(path, 0);
    // 图像的行和列
    int rows = image.rows, cols = image.cols;
    // 去畸变以后的图
    Mat image_undistort = Mat(rows, cols, CV_8UC1);

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++){
        for (int u = 0; u < cols; u++){
            // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
            // 归一化处理后Z = 1，单目相机的深度信息丢失了
            double x = (u - cx) / fx, y = (v - cy) / fy;
            // r 表示归一化平面内一点P(x, y)与坐标系原点之间的距离
            double r = sqrt(x * x + y * y);
            // 参见公式(5-10)、(5-11)、(5-12)
            // 对归一化平面上的点计算径向畸变和切向畸变
            double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
            double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
            // 将畸变后的点通过内参矩阵投影到像素平面，得到该点在图像上的正确位置
            double u_distorted = fx * x_distorted + cx;
            double v_distorted = fy * y_distorted + cy;

            //赋值（最邻近插值）
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at(v, u) = image.at((int) v_distorted, (int) u_distorted);
            } 
            else {
                image_undistort.at(v, u) = 0;
            }    
        }
    }
    //画出畸变后的图像
    imshow("image", image);
    imshow("image undistorted", image_undistort);
    waitKey(0);
    return 0;
}