视觉slam第五讲笔记：相机模型、Opencv、点云处理

视觉SLAM第五讲笔记

一.相机模型

参照博客：Step1：模型 16个相机参数（内参、外参、畸变参数） 。
参照PPT：等什么时候有空了再上传

---

去畸变处理方法：
1.先对整张图片去畸变，在讨论图像上的点的空间位置。
2.先考虑图像中的某个点，按照去畸变方法讨论去去畸变后的空间位置。

---

单目相机成像过程：
1.{world}下有一固定点P，坐标为$P_w$
2.P点在{camera}下的相机坐标$\widetilde{P_c}=R{P}_w+t$
3.此时$\widetilde{P_c}$仍有三个量XYZ，归一化到Z=1平面，得$P_c=[X/Z,Y/Z,1{]}^T$
4.经过内参得到像素坐标$P_{uv}=K{P}_c$

---

深度相机：
在测量深度后，获得彩色图和深度图，在同一个图像位置，读取到色彩信息和距离信息，计算像素3D点坐标，生成点云

---

如果x轴表示列数，y轴表示行数，则像素坐标（x,y）在程序中的位置为：

usigend char pixel image[y][x]； 

注意：像素索引行在前，列在后。

---

二.Opencv处理图片

CMakeLists.txt:

cmake_minimum_required(VERSION 3.14)
project(opencvimg)
set(CMAKE_CXX_STANDARD 14)

find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
add_executable(opencvimg main.cpp)
target_link_libraries( opencvimg ${OpenCV_LIBS} )

遍历像素、图像克隆、图像区域覆盖

#include 
#include 
#include 

using namespace std;

int main(int argc, char** argv) {
    cv::Mat image;
    image = cv::imread(argv[1]);
    if (image.data == nullptr)
    {
        cerr<<"文件"<<argv[1]<<"不存在"<<endl;
        return 0;
    }

    cout << "图像宽和高分别为："<<image.cols<<","<<image.rows<<endl;
    cout <<"图像通道数：" <<image.channels()<<endl;
    cv::imshow("image",image);
    cv::waitKey(0);

    if( image.type() != CV_8UC1 && image.type() != CV_8UC3 )
    {
        cout<<"请输入彩色图或灰度图"<<endl;
        return 0;
    }

    for( size_t y = 0; y<image.rows; y++)
    {
        for (size_t x = 0; x <image.cols ; x++)
        {
            //row_ptr是第y行的头指针
            unsigned char* row_ptr = image.ptr<unsigned char> (y);
            //data_prt指向待访问的像素数据
            unsigned  char* data_ptr = &row_ptr[x*image.channels()];

            //输出像素的每个通道，如果是灰度图只有一个通道
            for (int c = 0; c != image.channels() ; c++)
            {
                unsigned char data = data_ptr[c];
            }
        }
    }

    /*直接赋值不会赋值数据，在image_another上修改会引起image的改变*/
    cv::Mat image_another = image;
    //将左上角100*100的块置为0
    image_another(cv::Rect(0,0,100,100)).setTo(0);
    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("clone",image_clone);
    cv::waitKey(0);
    cv::destroyAllWindows();
    
    return 0;
}

三.chrono计时

#include 
#include 

using namespace std;

int main(int argc, char** argv) {
    //使用std::chrono计时
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for (int i = 0; i < 5000; ++i);
    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;

    return 0;
}

四.拼接点云PCL库

安装PCL库 :

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
sudo apt-get update
sudo apt-get install libpcl-dev

已知数据 :5张RGB彩色图，5张对应的深度图，及每对图像的内外参
其中，外参：[平移向量31，旋转四元数41] 。
可知内容 :每个像素在{camera}下的位置；每个像素在{world}下的位置。
任务：
1.根据内参计算一对RGB-D图像对应的点云
2.根据各张图的外参，叠加所有点云，组成地图
用到的库：Eigen、Opencv 、PCL

CMakeLists.txt：IDE用的是Clion，找到的库目录有点问题，在调用PCL头文件的时候会提示无法找到，所以先把目录放里边

cmake_minimum_required(VERSION 3.14)
project(opencvimg)

set(CMAKE_CXX_STANDARD 14)

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

# Eigen
include_directories(/usr/include/eigen3/Eigen)

# PCL
include_directories(/usr/include/pcl-1.9/pcl)
find_package( PCL REQUIRED COMPONENT common io )
add_definitions(${PCL_DEFINITIONS})

add_executable(opencvimg main.cpp)
target_link_libraries( opencvimg ${OpenCV_LIBS} ${PCL_LIBRARIES} )

#include 
#include 
using namespace std;
#include 
#include 
#include 
#include  //for formating strings
#include 
#include 
#include 
#include 

using namespace std;

int main(int argc, char** argv) {
    vector<cv::Mat> colorImgs,depthImgs;
    /*Allocator 负责提供 vector 需要用到的动态内存,Allocator参数有默认值，一般的使用不需要指定这个参数。
     * 但有时对内存有特殊需求，就需要提供自己定义的内存管理类,总共有5张图片 所以对应着5个位姿矩阵。*/
    vector<Eigen::Isometry3d> poses; //, Eigen::aligned_allocator
    ifstream fin("./pose.txt");
    if(!fin)
    {
        cerr << "在有pose.txt的目录下运行该程序" << endl;
        return 1;
    }

    for (int i = 0; i < 5; ++i)
    {

        /****************读取彩色图和深度图*************/
        boost::format fmt( "./%s/%d.%s" );  //图像文件的格式
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 ));

        /****************读取外参*************/
        double data[7]={0};
        // auto表示自动根据后面的元素 获得符合要求的类型*/
        for( auto& d:data) fin>>d;
        Eigen::Quaterniond q ( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T( q );
        T.pretranslate(Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
    }

    /***************设置内参************/
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;

    /***************点云计算************/
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;//设置点云格式为 XYZBGR

    //新建点云
    PointCloud::Ptr pointCloud( new PointCloud );
    //开始计算
    for (int i = 0; i < 5; i++)
    {
        cout<<"转换图像中： " <<i+1<<endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for (int v = 0; v < color.rows; v++)
        {
            for (int u = 0; u < color.cols; u++)
            {
                unsigned int d = depth.at< unsigned short >(v,u);//读取深度值
                if (d==0) continue;//深度为0表示没测量到
                Eigen::Vector3d point;
                //根据内参，从像素获得相机坐标系的坐标
                point[2] = double(d)/depthScale;//Z坐标归一化
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                //根据外参，从相机坐标系转到世界坐标系
                Eigen::Vector3d pointworld = T * point;


                PointT p;
                //赋值世界坐标
                p.x = pointworld[0];
                p.y = pointworld[1];
                p.z = pointworld[2];
                //开始上色
                p.b = color.data[ v*color.step + u*color.channels() ];
                p.g = color.data[ v*color.step + u*color.channels()+1 ];
                p.r = color.data[ v*color.step + u*color.channels()+2 ];
                //放成品到点云堆中
                pointCloud->points.push_back( p );
            }
        }

        pointCloud->is_dense = false;
        cout << "共有点云：" << pointCloud->size() <<endl;
        //保存点云文件
        pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    }
    return 0;
}

编译后，在终端输入pcl_viewer map.pcd即可看到3d点云。

---

遇到的问题：
报错：段错误
调试：用gdb调试程序，gdb ./opencvimg，run
报错信息：
原因：深度图未正确读入。
解决：文件格式打错了，惨兮兮。

参考

[1]: Xiang Gao, Tao Zhang, Yi Liu, Qinrui Yan, 14 Lectures on Visual SLAM: From Theory to Practice, Publishing House of Electronics Industry, 2017.

强烈安利《视觉slam十四讲》，这是极好的一本书