【SLAM】视觉SLAM十四讲（四：相机模型与非线性优化）

图像去畸变

对一张桶形失真的图片进行去畸变
畸变模型有两种：

• 径向畸变：由镜头形状导致
  
• 切向畸变：透镜与成像平面未严格平行

去畸变代码

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;

    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);   // 去畸变以后的图
    double x,y,r,xd,yd;
    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {

            double u_distorted = 0, v_distorted = 0;
            //利用内参将像素点转化为实际点
            x = (u-cx)/fx;
            y = (v-cy)/fy;
            //计算出该实际点距离原点半径
            r = sqrt(pow(x,2)+pow(x,2));
            //根据畸变变换公式求原始图像的坐标
            xd = x*(1 + k1*pow(r,2) + k2*pow(r,4)) + 2*p1*x*y + p2*(pow(r,2) + 2*pow(x,2));
            yd = y*(1 + k1*pow(r,2) + k2*pow(r,4)) + p1*(pow(r,2)+2*pow(y,2)) + 2*p2*x*y;
            //利用内参公式将纠正后的原始坐标转换为像素坐标
            u_distorted = fx*xd + cx;
            v_distorted = fy*yd + 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;
            }
        }

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

双目视差的使用

假设双目计算的视差已给定，根据双目模型，画出点云。

使用双目模型画出点云的代码（视觉SLAM十四讲P101）

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> pointcloud;
    
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {

            Vector4d point(0, 0, 0, left.at(v, u) / 255.0); // 前三维为xyz,第四维为颜色
           
            // 根据双目模型计算 point 的位置
            unsigned int depth = disparity.ptr(v)[u]; //深度值
            if(depth == 0)continue; //深度为0表示未测量到
            point[2]=(fx*d*1000)/depth;
            point[1]=(v-cy)*point[2]/fy;
            point[0]=(u-cx)*point[2]/fx;
            cout<<"point = [ "<

矩阵微分

参考https://blog.csdn.net/pipisorry/article/details/68961388
x为N阶列向量，A为N*N阶矩阵
1.d(Ax)/dx是什么？A的转置
2.d(x’Ax)/dx是什么？(A‘+A)x
3.证明xA’x=tr(Axx’)
暂时不会

高斯牛顿法的曲线拟合实验

思想：
对于不方便直接求解的最小二乘问题，用迭代的方法更新增量，使目标函数下降。确定增量的常用方法有两种，泰勒展开不同的阶数

• 一阶梯度（最速下降法）：增量= - J’
• 二阶梯度（牛顿法）：增量= -J’/H
  J是雅克比矩阵，H是海塞矩阵

高斯牛顿法在泰勒展开一阶后，将展开式的最小二乘对增量求导，令增量的导数为0后，得到J’J近似H。

• 优点：免去计算复杂的H
• 缺点：H是可逆且正定的，J’J是半正定的，求出的增量可能不稳定，导致算法无法收敛

gaossnewton.cpp参考https://github.com/YCJin9/sparse_BA/blob/master/GN.cpp

#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main ( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double ae=2.0, be=-1.0, ce=5.0;     // abc参数的估计值

    vector x_data, y_data;      // 数据

    cout<<"generating data…… "< 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }
        // 更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;
        cout << "total cost: " << cost << endl;
    }
    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}