视觉slam十四讲 学习笔记-4

目录

相机模型

单目相机

双目相机

状态估计问题

 实践：点云拼接

批量状态估计问题

非线性最小二乘 

实践

-----------作业------------

---

相机模型

单目相机

  下图中：u 轴向右与 x 轴平行，v轴向下与 y 轴平行，像素坐标系与成像平面之间,相差了一个缩放和一个原点的平移。我们设像素坐标在 u 轴上缩放了 α 倍,在 v 上缩放了 β 倍。同时,原点平移了 [c x , c y ] T 。式3中： 

 这四个参数称为相机的内参（即表示的是缩放和平移）一般来说是相近或者数值一样。

 上图中的矩阵形式乘开即为展开形式，其中矩阵即为内参矩阵K 

 上图中：中间的量组成的矩阵称为相机的内参数矩阵K，世界坐标记为Pw，中的括号就是先将世界坐标系转化为相机的坐标系，然后乘以K内参矩阵再进行一次转换。这里分别是齐次和非齐次的表示方式。

畸变：其中k和p为参数，属于相机的内参 。

我们知道平面上的任意一点 p 可以用笛卡尔坐标表示为 [x, y] T , 也可以把它写成极坐标的形式[r, θ] T ,其中 r 表示点 p 离坐标系原点的距离,θ 表示和水平轴的夹角。径向畸变可看成坐标点沿着长度方向发生了变化 δr, 也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生了变化,也就是水平夹角发生了变化 δθ。

双目相机

非线性优化

牛顿法  ：

牛顿法，高斯-牛顿法_gauss newton method_Yemiekai的博客-优快云博客

LM-Levenberg–Marquardt algorithm ：（1:16:20）【一起读书】视觉SLAM十四讲 第6讲（中）矩阵向量求导思路总结 非线性最小二乘若干解法原理推导分析 这讲准备了很久 希望对大家有帮助_哔哩哔哩_bilibili

最小二乘的引出： （书P123）

对于某一次观测的观测方程：

 

噪声项：，其中Q为一个3*3协方差矩阵。

由于噪声的存在当我们知道xy的值时候去推算出的z，z就满足正态分布（因为如果没有噪声，推出来的z肯定是一个准确的值，由于有一个满足高斯分布的噪声，那么推出来的值也是满足下面的高斯分布的）：

 

 上式中：P为条件概率，表示的是在条件xy下z所发生的概率，这里的z是一个变量，我们需要去求的，我们要去求得z取某个值的时候使得p最大（这样的含义就是在xy观测下，最有可能的位姿为z）。这个公式表示，在给定机器人位姿和地图特征的条件下，观测数据的条件概率服从（这里的等式理解为服从更好）以预测函数h为均值、以观测噪声协方差矩阵Q为协方差矩阵的高斯分布。（因为噪声是服从高斯分布的，h函数具体是一个值）

考虑一个任意的高维高斯分布 x ∼ N (μ, Σ)---μ为均值，Σ为协方差，它的概率密度函数展开形式为:

取对数

 因为我们要求x取什么值时P最大，前半部分和x无关，所以这里只看这一部分

所以要求P(x)最大，就是求这部分的最小值

 对于高斯分布，将其带入上式化简部分：

即要求这部分的最小值

 

 

-------------------------------------------------------------

首先回顾一下两个方程，第一个是运动方程，第二个是观测方程，参考：

x表示机器人（相机）的实际坐标；y表示某一个特征点的实际坐标；u表示内部传感器得出来的运动量（比如我们可以使用位移传感器和IMU测得 ）；z也表示传感器的感知的观测量（第j个特征点的坐标）；w、v表示噪声。

我们从这篇极大似然估计的博客可以得知：我们可以通过最大化似然函数来求出最接近实际的参数值。我们这里就是要通过u和v的值来尽可能估计出x和y的值。

我们将所有待估计的变量放在一个“状态变量”中（即将x和y放在一起作为x考虑）:

我们对机器人状态的估计,就是求已知输入数据 u 和观测数据 z 的条件下,计算上面的状态x的条件概率分布，记为P(x|z,u)

特别地，当我们没有测量运动的传感器，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 P (x|z) 的条件概率分布。

利用贝叶斯法则可以转化一下研究对象，最后是正比于P(z|x)P(x)的（因为分母部分与待估计的状态 x 无关，所以不考虑分母部分）：

 这时我们要求的使得P(x|z)最大化就变成了：最大化似然和先验的乘积。

如果此时在不清楚机器人具体的位姿的情况下，不知道P(x)，就只能尽可能考虑最大化P(z|x)。

最大化高斯分布：

考虑一个高斯分布：，他的概率密度函数为：

 这里我们对其进行取负对数从而方便求导（因为我们要找最大值）：

 因为对数是一个单调函数，所以并不会改变最值的位置，而这里我们取得是负对数，所以我们之前求最大就变成了求最小。

 实践：点云拼接

opencv的cmake配置：

cmake_minimum_required( VERSION 2.8 )
project( imageBasics )

# 添加c++ 11标准支持
set( CMAKE_CXX_FLAGS "-std=c++11" )

# 寻找OpenCV库
find_package( OpenCV 3 REQUIRED )
# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )

add_executable( imageBasics imageBasics.cpp )
# 链接OpenCV库
target_link_libraries( imageBasics ${OpenCV_LIBS} )

程序中我们演示了： 图像读取、显示、像素遍历、拷贝、赋值等。

#include <iostream>
#include <chrono>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main ( int argc, char** argv )
{
    // 读取argv[1]指定的图像
    cv::Mat image;
    image = cv::imread ( argv[1] ); //cv::imread函数读取指定路径下的图像
    // 判断图像文件是否正确读取
    if ( image.data == nullptr ) //数据不存在,可能是文件不存在
    {
        cerr<<"文件"<<argv[1]<<"不存在."<<endl;
        return 0;
    }
    
    // 文件顺利读取, 首先输出一些基本信息
    cout<<"图像宽为"<<image.cols<<",高为"<<image.rows<<",通道数为"<<image.channels()<<endl;
    cv::imshow ( "image", image );      // 用cv::imshow显示图像
    cv::waitKey ( 0 );                  // 暂停程序,等待一个按键输入
    // 判断image的类型
    if ( image.type() != CV_8UC1 && image.type() != CV_8UC3 )
    {
        // 图像类型不符合要求
        cout<<"请输入一张彩色图或灰度图."<<endl;
        return 0;
    }

    // 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
    // 使用 std::chrono 来给算法计时
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for ( size_t y=0; y<image.rows; y++ )
    {
        // 用cv::Mat::ptr获得图像的行指针
        unsigned char* row_ptr = image.ptr<unsigned char> ( y );  // row_ptr是第y行的头指针
        for ( size_t x=0; x<image.cols; x++ )
        {
            // 访问位于 x,y 处的像素
            unsigned char* data_ptr = &row_ptr[ x*image.channels() ]; // data_ptr 指向待访问的像素数据
            // 输出该像素的每个通道,如果是灰度图就只有一个通道
            for ( int c = 0; c != image.channels(); c++ )
            {
                unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
            }
        }
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"遍历图像用时："<<time_used.count()<<" 秒。"<<endl;

    // 关于 cv::Mat 的拷贝
    // 直接赋值并不会拷贝数据
    cv::Mat image_another = image;
    // 修改 image_another 会导致 image 发生变化
    image_another ( cv::Rect ( 0,0,100,100 ) ).setTo ( 0 ); // 将左上角100*100的块置零
    cv::imshow ( "image", image );
    cv::waitKey ( 0 );
    
    // 使用clone函数来拷贝数据
    cv::Mat image_clone = image.clone();
    image_clone ( cv::Rect ( 0,0,100,100 ) ).setTo ( 255 );
    cv::imshow ( "image", image );
    cv::imshow ( "image_clone", image_clone );
    cv::waitKey ( 0 );

    // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
    cv::destroyAllWindows();
    return 0;
}

终端运行：

cd build
cmake ..
make
./imageBasics ../ubuntu.png 

安装点云：

sudo apt-get install libpcl-dev
sudo apt-get install pcl-tools 

运行：

cd joinMap
build/joinMap  #执行可执行程序，需要在pose.txt文件下执行这个命令
pcl_viewer map.pcd   #使用pcl显示点云

批量状态估计问题

这部分没太懂

非线性最小二乘 

实践：ceres和g2o

ceres安装：

Ubuntu18.04安装Ceres，图文详解_振华OPPO的博客-优快云博客_ceres安装

使用ceres拟合曲线（求解最小二乘问题）

以下代码最重要的部分就是：struct代价函数和构建最小二乘问题

ceres的使用说明：

1. 定义 Cost Function 模型，代码中对应的是CURVE_FITTING_COST。方法是去书写一个类（这里用的struct结构体）,并在类中定义带模板参数的 () 运算符。

2. 调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度,我们有若
干种选择:(1)使用 Ceres 的自动求导(Auto Diff);(2)使用数值求导(Numeric Diff);(3)自行推导解析的导数形式,提供给 Ceres。其中自动求导在编码上是最方便的,于是我们就使用自动求导

3. 自动求导需要指定误差项和优化变量的维度。这里的误差则是标量（ 误差为y-exp(ax^2+bx+c)  ），维度只有一维。优化的是 a, b, c 三个量,维度为 3。于是,在自动求导类的模板参数中设定变量维度为1，3对应代码<CURVE_FITTING_COST, 1, 3>

4. 设定好问题后,调用 solve 函数进行求解。你可以在 option 里配置(非常详细的)优
化选项。例如,我们可以选择使用 Line Search 还是 Trust Region,迭代次数,步长
等等。读者可以查看 Options 的定义,看看有哪些优化方法可选,当然默认的配置已
经可以用在很广泛的问题上了。

我这样理解：我们最终要求得是函数这个式子的最小值，其中y代表的是在x取值的情况下实际随机生成的点的y的值（生成y是有高斯噪声的，也就是有误差的），我们代码中生成的100数据中每个x都对应一个具有误差的y值。要对整个式子求最小，其中x,y是一系列已知的值，a,b,c是变量，所以对a,b,c进行求导，也就是待优化的值，所以维度为<1,3>。

代码思路：我们要通过ceres来拟合，即设置abc的值分别为1 2 1。我们再通过产生的100点产生的值加上随机产生高斯噪声生成100个y，从而生成了100对(x,y)，当然这些x,y都是在这个图像附近的（因为噪声的存在）。随后我们通过构建这100个点的值构建最小二乘：，从而来求出最接近1，2，1的abc的值。如果sigma噪声的值更小，那么拟合也会更加准确。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    // 残差的计算
    template <typename T>
    bool operator() (
        const T* const abc,     // 模型参数，有3维
        T* residual ) const     // 残差
    {
        residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
        return true;
    }
    const double _x, _y;    // x,y数据
};

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 abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
        problem.AddResidualBlock (     // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
                new CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,            // 核函数，这里不使用，为空
            abc                 // 待估计参数
        );
    }

    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填，比如迭代次数、高斯牛顿、LM等，下面一行设置了linear_solver_type的类型
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解，这里使用的是QR方法，还有其他：cholesky、功能梯度法等等
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve ( options, &problem, &summary );  // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出结果
    cout<<summary.BriefReport() <<endl;
    cout<<"estimated a,b,c = ";
    for ( auto a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

g2o安装

首先对g2o进行编译安装：g2o安装_zdb呀的博客-优快云博客_g2o安装

然后将slambook_master中的g2o_curve_fitting中的代码复制出来打开，记得cmake_modules一并复制，因为里面的.cmake文件需要寻找依赖。

-----------作业------------

1图像去畸变

cmake_minimum_required(VERSION 3.2)
project(ch4)

#注意opencv支持c++11以上
set(CMAKE_CXX_FLAGS "-std=c++11")

find_package(OpenCV 3.2 REQUIRED)
include_directories(${OPENCV_INCLUDE_DIRS})
add_executable(ch4 undistort_image.cpp)
target_link_libraries(ch4 ${OpenCV_LIBS})

//
// Created by 高翔 on 2017/12/15.
//

#include <opencv2/opencv.hpp>
#include <string>
#include "cmath"
using namespace std;

string image_file = "/home/gzy/ROS/slam/ch4/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
            double x_rescale_one=(u-cx)/fx;
            double y_rescale_one=(v-cy)/fy;
            double r=sqrt(pow(x_rescale_one,2)+ pow(y_rescale_one,2));
            double x_distort=x_rescale_one*(1+k1*pow(r,2)+k2*pow(r,4))
                             +2*p1*x_rescale_one*y_rescale_one
                             +p2*(pow(r,2)+2*pow(x_rescale_one,2));
            double y_distort=y_rescale_one*(1+k1*pow(r,2)+k2*pow(r,4))
                             +p1*(pow(r,2)+2*pow(y_rescale_one,2))
                             +2*p2*x_rescale_one*y_rescale_one;
            u_distorted=fx*x_distort+cx;
            v_distorted=fy*y_distort+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;
}

2双目视差

这里也使用了vscode试了试：具体操作看第一节的笔记

cmake_minimum_required(VERSION 3.2)
project(ch4_2)

set(CMAKE_CXX_STANDARD 14)

#Eigen
include_directories(usr/include/Eigen)

#opencv
find_package(OpenCV 3 REQUIRED)
include_directories(${OPENCV_INCLUDE_DIRS})

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

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

 这部分代码中：书写部分在start  your code 到end your code

//
// Created by 高翔 on 2017/12/15.
//

#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/gzy/ROS/slam/ch4-2-vs/left.png";
string right_file = "/home/gzy/ROS/slam/ch4-2-vs/right.png";
string disparity_file = "/home/gzy/ROS/slam/ch4-2-vs/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++)
        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)
            // 根据双目模型计算 point 的位置
            double x=(u-cx)/fx;
            double y=(v-cy)/fy;
            double dispar=disparity.at<uchar>(v,u);  //读出来(v,u)这点的视差dispar
            double depth=fx*b/dispar;   //这里使用的是z=fb/d的这个公式，这里公式的z就对应着depth


            //还原真实坐标，只需将归一化坐标的xy乘以depth即可，z方向的就是depth
            point[0]=x*depth;
            point[1]=y*depth;
            point[2]=depth;
            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;
}

4高斯牛顿法