半闲居士视觉SLAM十四讲笔记（5）相机与图像 - part 2 图像及 OpenCV 操作、点云拼接

本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接：http://blog.csdn.net/youngpan1101/article/details/71088154
作者：宋洋鹏（youngpan1101）
邮箱： [email protected]

---

该讲详细资料下载链接 【Baidu Yun】【Video】【Code】

若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】

---

相机与图像

图像

计算机中图像的表示

（ps: 引自视觉SLAM十四讲 图5-8 图像坐标示意图）

• 像素坐标原点位于图像的左上角， X 轴（列数，宽度）向右， Y 轴（行数，高度）向下。
• 一个灰度像素可以用八位整数记录，像素的深度值采用十六位整数来记录。
• 对于每个像素，对应有 R,G,B 三个数值，每个数值就称为一个通道，所以一个像素占据了 24 位存储空间。
• 如果还想表达图像的透明度，可以使用 R,G,B,A 四个通道来表示它。

---

——————————– 分割线<< 家有小武，如有一母 >>分割线 ——————————–

---

实践：图像的存取与访问

安装 OpenCV

• OpenCV 提供了大量图像开源算法，是计算机视觉中使用极广的图像处理算法库。
• 安装步骤可参考另一篇博文：【Ubuntu 14.04 安装 OpenCV-3.2.0】

OpenCV 操作实例

• CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( imageBasics )

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

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

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

• imageBasics.cpp

#include 
#include 
using namespace std;

#include 
#include 

int main ( int argc, char** argv )
{
    // 读取指定的图像
    cv::Mat image;
    std::string img_path = "../ubuntu.png";
    // cv::imread函数读取指定路径下的图像
    image = cv::imread ( img_path );     
    //image = cv::imread ( argv[1] ); /* cv::imread函数读取 argv[1] 指定的图像 */

    // 判断图像文件是否正确读取
    if ( image.data == nullptr ) //数据不存在,可能是文件不存在
    {
        cerr << "文件" << argv[1] << "不存在." <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; yfor ( size_t x=0; x// 访问位于 x,y 处的像素
            // 用cv::Mat::ptr获得图像的行指针
        /* row_ptr是第y行的头指针 */
            unsigned char* row_ptr = image.ptr<unsigned char> ( y );  
        /* data_ptr 指向待访问的像素数据 */
            unsigned char* data_ptr = &row_ptr[ x*image.channels() ];
            // 输出该像素的每个通道,如果是灰度图就只有一个通道
            for ( int c = 0; c != image.channels(); c++ )
            {
          // data为I(x,y)第c个通道的值
          unsigned char data = data_ptr[c]; 
            }
        }
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_castdouble>>( 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;
}

---

——————————– 分割线<< 家有小武，如有一母 >>分割线 ——————————–

---

实践：拼接点云

• 本程序 5 对相互对应的彩图和深度图，根据 RGB-D 图像和相机内参，可以计算任何一个像素在相机坐标系下的位置，根据相机位姿（ Twc ），又能计算这些像素在世界坐标系下的位置。将所有像素的空间坐标计算出来，相当于构建类似地图的东西。

• PCL 库的安装 【PCL 官网】

  ubuntu 14.04:
  $ sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
  $ sudo apt-get update
  $ sudo apt-get install libpcl-all
  
  ubuntu 16.04:
  $ sudo apt-get install libpcl-dev pcl-tools

• CMakeLists.txt

  cmake_minimum_required( VERSION 2.8 )
  project( joinMap )
  
  set( CMAKE_BUILD_TYPE Release )
  set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
  
   # opencv 
  find_package( OpenCV REQUIRED )
  include_directories( ${OpenCV_INCLUDE_DIRS} )
  
   # eigen 
  include_directories( "/usr/include/eigen3/" )
  
   # pcl 
  find_package( PCL REQUIRED COMPONENT common io )
   # list (REMOVE_ITEM PCL_LIBRARIES "vtkproj4")  # use this in Ubuntu 16.04 链接到一个不存在的库，这里将其去掉
  include_directories( ${PCL_INCLUDE_DIRS} )
  add_definitions( ${PCL_DEFINITIONS} )
  
  add_executable( joinMap joinMap.cpp )
  target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )

• joinMap.cpp

   #include 
   #include 
   using namespace std;
   #include 
   #include 
   #include  
   #include   // for formating strings
   #include  
   #include  
   #include 
  
  int main( int argc, char** argv )
  {
      vector colorImgs, depthImgs;    // 彩色图和深度图
      vector poses;         // 相机位姿
  
      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 )); // 使用-1读取原始图像
  
          double data[7] = {0};
          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;
  
      cout << "正在将图像转换为点云..." << endl;
  
      // 定义点云使用的格式：这里用的是XYZRGB
      typedef pcl::PointXYZRGB PointT; 
      typedef pcl::PointCloud PointCloud;
  
      // 新建一个点云
      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; vfor ( int u=0; uunsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                  if ( d==0 ) continue; // 为0表示没有测量到
                  Eigen::Vector3d point; 
                  point[2] = double(d)/depthScale; 
                  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;
  }

• 查看最后合成的 pcd 点云文件：

  $ pcl_viewer map.pcd