初学《视觉SLAM 十四讲》（高翔）---第五讲 笔记

笔记部分

实践部分（代码注释）

ch5 _ imageBasics.cpp

先安装opencv

#include 
#include 
using namespace std;

#include 
#include 

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

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

ch5_ joinMap.cpp

安装pcl：

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

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

int main( int argc, char** argv )
{
    /*彩色图和灰度图各5张，所以用容器来存储*/
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图

    /*vector有两个参数，后面的参数一般是默认的，这里用适合Eigen库的对齐方式来初始化容器，总共有5张图片 所以对应着5个位姿矩阵*/
    vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses;   // 相机位姿
    
    ifstream fin("./pose.txt");
    if (fin.bad())//如果没有打开 那么提示错误！
    {
        cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
        return 1;
    }

    /*循环读取图像*/
    for ( int i=0; i<5; i++ )
    {
        /*用boost中的format格式类，来循环读取图片，否则单张读取图片就会有问题
         * 当在命令行中执行的时候这里必须要为../  在当前ide中执行的时候要修改为./ */
        boost::format fmt( "./%s/%d.%s" ); //图像文件格式   "../%s/%d.%s"   ../ 表示可执行文件在build中,图像在上一个目录,所以用../
        /*这里的%对应./ color对应%s  下面的符号就是与上面一致对应的 */
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

        /*基于范围的for循环，表示从data数组的第一项开始 循环遍历  auto表示自动根据后面的元素 获得符合要求的类型*/
        double data[7] = {0};
        for ( auto& d:data )//auto自动类型转换
            fin>>d;//文件流类型的变量fin将pose.txt中的数据给了d数组
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //四元数 data[6]是实数 但是coeffis输出的是先虚数后实数
        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;//
    
    cout<<"正在将图像转换为点云..."<<endl;
    
    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 新建一个点云//PointCoud::Ptr是一个智能指针类 通过构造函数初始化指针指向的申请的空间
    /*Ptr是一个智能指针，返回一个PointCloud 其中PointT是pcl::PointXYZRGB类型。它重载了->  返回了指向PointCloud的指针
     *Ptr是下面类型 boost::shared_ptr > */
    /*pointCloud 是一个智能指针类型的对象 具体可以参考http://blog.csdn.net/worldwindjp/article/details/18843087*/
    PointCloud::Ptr pointCloud( new PointCloud );
//    pcl::PointCloud::Ptr pointCloud( new pcl::PointCloud );

    /*将5张图片 像素坐标转换到相机坐标 之后转换到世界坐标存储到点云格式的变量中 for循环之后用pcl的相关函数将点云转换到pcl能够显示的格式*/
    for ( int i=0; i<5; i++ )//转换5张图像
    {
        cout<<"转换图像中: "<<i+1<<endl; 
        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];

        /*           插入部分
         * //color.at(471,537)[0] = 12;//修改图像上的对应像素位置的值
        //color.ptr(471)[537][0] = 12;//与上面的效果一样

        //测试像素的输出效果,这里无法通过cout<(471,537)[0] 这种方式来输出第一个通道的值,因为每个通道的像素占了8位而unsigned char
        // 表示ascii码 所以输出的时候不是正确的数字，可以通过下面的方式强制转化为int类型（或者用自带的类型转换方式进行显示转换），就可以看到内部的值了
        //需要注意的一点是 cout页无法输出char类型的变量的地址，也是需要强制转换成void *类型的指针才能正常输出char类型变量的地址信息。
        if(colorImgs[i].channels() == 3) {
             std::cout << "测试1结果 " << color.ptr(471)[537] << "正确的结果:  "
                       << (char) color.at(471, 537)[0] << std::endl;
             std::cout << depth.ptr(471)[537] << std::endl;
             std::cout << colorImgs[i].at(471, 537) << std::endl;
        }
                     插入部分结束
         */

        /*对图像像素进行坐标转换，将图像的坐标通过内参矩阵K转换为相机坐标系下的坐标，之后通过外参矩阵T 转化为世界坐标系下的坐标*/
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                /*通过用Mat中的ptr模板函数 返回一个unsigned short类型的指针。v表示行 根据内部计算返回data头指针 + 偏移量来计算v行的头指针
                 * 图像为单通道的   depth.ptr ( v ) 来获取行指针*/
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值16位存储 一般color图像像素中每一个通道是8位

                 /*             单通道遍历图像的方式总结:
                 *  注意深度图像的像素占16位 与普通图片每个通道的像素为8位不同
                 * 1、同样是用上面的双层for循环，遍历图像 用at方式
                 * for ( int v=0; v(v,u);
                 *
                 *
                 * 2、使用迭代器进行图像的遍历
                 * 不是基于for循环了
                 * cv::MatIterator_ begin,end;
                 * for( begin =depth.begin(), end = depth.end(); begin != end; ){}
                 *
                 * 3、使用指针的方式 如本实验的结果
                 * */

                //迭代器的参数是通道数，因为深度图是单通道的，每个像素的值是unsigned short，所以参数是unsigned short
                //begin代表像素的开始地方
                if ( d==0 ) continue;                // 为0表示没有测量到 然后继续进行for循环那么跳过这个像素继续执行 在后面形成点云时需要设置is_dense为假
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;    //对实际尺度的一个缩放
                point[0] = (u-cx)*point[2]/fx;      //根据书上5.5式子---86页
                point[1] = (v-cy)*point[2]/fy; 
                Eigen::Vector3d pointWorld = T*point;   //将相机坐标系转换为世界坐标系
                
                PointT p ;
                p.x = pointWorld[0];    //将世界坐标系下的坐标用pcl专门的点云格式存储起来
                p.y = pointWorld[1];
                p.z = pointWorld[2];

                /*  color.step 虽然是一个类，但是它内部有一个转换操作符 operator size_t() const;
                 * 此时的color.size编译器就会把它当做size_t类型的变量,这个值的大小是1920 这个是随着图像的读入MAT类中会有自动转换然后存储的buf[]中 */
                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 ];

                /*  -> 是智能指针重载的 然后返回的类型就是输入的类型 可以看上面Ptr的解释说明 */
                pointCloud->points.push_back( p );//存储格式好的点
            }
    }
    std::cout<<"点云的列和行为 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
    //这里有可能深度图中某些像素没有深度信息，那么就是包含无效的像素，所以先置为假，但是如果设置成true的话 也没有看出来有什么不一样的地方
    pointCloud->is_dense = false;
    std::cout<<"点云的列和行为 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;

    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//获取pointCloud指向的对象 这个就当做获取普通指针指向的对象来理解，这个对象是在定义的时候new出来的一段内存空间。
    return 0;

}
/*                                            备注： 3通道的图像的遍历方式总结
 * 对于单通道来说 每个像素占8位 3通道则是每个矩阵元素是一个Vec3b 即一个三维的向量 向量内部元素为8位数的unsigned char类型
 * 1、使用at遍历图像
 * for(v)row
 *  for(u)col
 *      image.at（v,u）[0] 表示第一个通道的像素的值
 *      image.at(v,u)[1]
 *      image.at(v,u)[2]
 * 2、使用迭代器方式 (实际上就是一个指针指向了 cv::Mat矩阵元素)
 * cv::MatIterator_begin,end;
 * for( begin = image.begin(), end = image.end() ; begin != end;  )
 *      (*begin)[0] = ...
 *      (*begin)[1] = ...
 *      (*begin)[2] = ...
 *
 * 3、用指针的方式操作
 * for(v)
 *  for(u)
 *      image.ptr(v)[u][0] 表示第一个通道
 *      image.ptr(v)[u][0] 表示第二通道
 *              .
 *              .
 *              .
 * */

编译执行：

mkdir build
cd build
cmake ..
make
cd ..
pcl_viewer map.pcd

这里不知道有没有cd …(倒数第二步），因为还没有编译成功，可能是opencv安装的问题。
过了几个月在回来看到这句话，感觉好傻，cd .. 就是回到上一级目录，因为map.pcd是在主目录下的，所以要回到上一级，不想回的话也可以，pcl_viewer ../map.pcd