PointcloudMap_chrono

引言

ch5.

1.joinMap

joinMap.cpp

#include <iostream>
#include <fstream>
#include <cstdlib>//命令行调用
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>    // 变换矩阵　Ｔ
#include <boost/format.hpp>  // 格式化字符串 for formating strings 处理图像文件格式
//点云数据处理
#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 
#include <pcl/visualization/pcl_visualizer.h>
/********************************************************
 现实世界物体坐标　—(外参数 变换矩阵Ｔ变换)—>  相机坐标系　—(同/Z)—>归一化平面坐标系——>径向和切向畸变纠正——>(内参数平移　Cx Cy 缩放焦距Fx Fy)
 ——> 图像坐标系下　像素坐标
 u=Fx *X/Z + Cx 　　像素列位置坐标　
 v=Fy *Y/Z + Cy 　　像素列位置坐标　
 
 反过来
 X=(u- Cx)*Z/Fx
 Y=(u- Cy)*Z/Fy
 Z轴归一化
 X=(u- Cx)*Z/Fx/depthScale
 Y=(u- Cy)*Z/Fy/depthScale
 Z=Z/depthScale
 
外参数　T
世界坐标　
pointWorld = T*[X Y Z]
 
 ********************************************************/


int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;   //  Eigen库数据结构内存对齐问题  相机位姿  转换矩阵

    /**************************************************
     * 当调用 Eigen库 成员 时，一下情况需要注意
     Eigen库中的数据结构作为自定义的结构体或者类中的成员;
     STL容器含有Eigen的数据结构
     Eigen数据结构作为函数的参数
     
    
     1:数据结构使用 Eigen库 成员
     class Foo
        {
         ...
        Eigen::Vector2d v;//
         ...
     public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW //不加  会提示 对其错误
        }

     2.STL Containers 标准容器vector<> 中使用 Eigen库 成员
     vector<Eigen::Matrix4d>;//会提示出错
     vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>;//aligned_allocator管理C++中的各种数据类型的内存方法是一样的,但是在Eigen中不一样
     
     3.函数参数 调用  Eigen库 成员
     FramedTransformation( int id, Eigen::Matrix4d t );//出错  error C2719: 't': formal parameter with __declspec(align('16')) won't be aligned
     FramedTransformation( int id, const Eigen::Matrix4d& t );// 把参数t的类型稍加变化即可
     **************************************************/
    
    /**************************************************
     pose.txt
     x,y,z,Qx,Qy,Qz,Qw  Qw为四元数实部
     -0.228993 0.00645704 0.0287837 -0.0004327 -0.113131 -0.0326832 0.993042
     -0.50237 -0.0661803 0.322012 -0.00152174 -0.32441 -0.0783827 0.942662
     -0.970912 -0.185889 0.872353 -0.00662576 -0.278681 -0.0736078 0.957536
     -1.41952 -0.279885 1.43657 -0.00926933 -0.222761 -0.0567118 0.973178
     -1.55819 -0.301094 1.6215 -0.02707 -0.250946 -0.0412848 0.966741
     分别为五张图相机的位置和姿态
     **************************************************/

    ifstream fin("../pose.txt");
    //ifstream fin(argv[1]);//命令行加入参数  ../pose.txt
    if (!fin)
    {
        cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
        return 1;
    }
    
    for ( int i=0; i<5; i++ )//5行数据
    {
        boost::format fmt( "./%s/%d.%s" ); //图像文件格式
        //colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));//彩色图color/1.png~5.png
        //depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 ));//深度图depth/1.pgm~5.pgm 使用-1读取原始图像
        colorImgs.push_back( cv::imread( (fmt%"../color"%(i+1)%"png").str() ));//彩色图color/1.png~5.png
        depthImgs.push_back( cv::imread( (fmt%"../depth"%(i+1)%"pgm").str(), -1 ));//深度图depth/1.pgm~5.pgm 使用-1读取原始图像
	    // 相机位姿数据
        double data[7] = {0};//每一行7个数据
        for ( auto& d:data )//txt文件的每一行数据
            fin>>d;
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );//data[6]为四元数实部
        Eigen::Isometry3d T(q);//变换矩阵 按四元数旋转
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//加上平移
        poses.push_back( T );//保存每一行的旋转平移　变换矩阵T
    }
    
    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;//图像像素　原点平移
    double cy = 253.5;
    double fx = 518.0;//焦距和缩放  等效
    double fy = 519.0;
    double depthScale = 1000.0;//将mm转换为m,单位转换因子
    
    cout<<"正在将图像转换为点云..."<<endl;
    
    // 定义点云使用的格式：这里用的是XYZRGB　即　空间位置和RGB色彩像素对
    typedef pcl::PointXYZRGB PointT; //点云中的点对象  位置和像素值
    typedef pcl::PointCloud<PointT> PointCloud;//整个点云对象
    
    // 新建一个点云 对象
    PointCloud::Ptr pointCloud( new PointCloud ); 
    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]; //每个图像对应的摄像机位姿
        
        //对每个像素值对应的点 转换到现实世界
        for ( int v=0; v<color.rows; v++ )      //每一行
            for ( int u=0; u<color.cols; u++ )  //每一列
            {
	            //内参数　转换
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值 指针访问 像素值 行 列,注意单位，这里读出来的是mm要转换为m
                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;//位于世界坐标系中的实际位置  x,y,z

                PointT p ;          //点云 XYZRGB
                p.x = pointWorld[0];//现实世界中的位置坐标
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];//注意opencv彩色图像通道的顺序为 bgr
                p.g = color.data[ v*color.step+u*color.channels()+1 ];//opencv当中的 .step参数表示的实际像素的宽度 .channel()表示的是通道的提取
                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 );
    //可在命令行内使用pcl_viewer map.pcd 查看点云数据
    //C语言有一个system函数（在<stdlib.h>头中，C++则为<cstdlib>头），可以用来调用终端命令。
    system("pcl_viewer map.pcd");
    return 0;
}

CmakeLists.txt

#版本限制
cmake_minimum_required( VERSION 2.8 )

#工程名
project( joinMap )

#模式
set( CMAKE_BUILD_TYPE Release )
# 添加c++ 11标准支持
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 )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )

#可执行文件
add_executable( joinMap joinMap.cpp )

# 链接OpenCV库 PCL库
target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )

2.chrono时间库说明

chrono是一个time library, 源于boost，现在已经是C++标准。

• 参考链接
• 头文件#include< chrono>，其所有实现均在std::chrono namespace下

chrono是一个模版库，使用简单，功能强大，只需要理解三个概念：duration、time_point、clock

1）.Durations 固定时间间隔**
std::chrono::duration 表示一段时间，如两个小时，12.88秒，半个时辰，一炷香的时间等等，只要能换算成秒即可。 template <class Rep, class Period = ratio<1> > class duration;//其中Rep表示一种数值类型，用来表示Period的数量，比如int float double ,Period是ratio类型，用来表示【用秒表示的时间单位】比如second milisecond.

常用的duration<Rep,Period>已经定义好了，在std::chrono::duration下：

• ratio<3600, 1> hours 1小时
• ratio<60, 1> minutes 1分钟
• ratio<1, 1> seconds 1秒
• ratio<1, 1000> milliseconds 1毫妙 1/1000 秒
• ratio<1, 1000000> microseconds 1微妙 1/1000000秒
• ratio<1, 1000000000> nanosecons 1纳秒 1/1000000000秒

说明一下ratio这个类模版的原型：

template <intmax_t N, intmax_t D = 1> class ratio;//  #include < ratio>

N代表分子，D代表分母，所以ratio表示一个分数值。注意，自己可以定义Period，比如ratio<1, -2>表示单位时间是-0.5秒。

由于各种duration表示不同，chrono库提供了duration_cast类型转换函数。

 template <class ToDuration, class Rep, class Period>
 constexpr ToDuration duration_cast (const duration<Rep,Period>& dtn);
 
 typedef std::chrono::duration<int> seconds_type;                           //1秒
 typedef std::chrono::duration<int,std::milli> milliseconds_type;    //1毫秒
 typedef std::chrono::duration<int,std::ratio<60*60>> hours_type;//1小时

  hours_type h_oneday (24);                  // 24h       1天 24小时
  seconds_type s_oneday (60*60*24);          // 86400s  86400秒
  milliseconds_type ms_oneday (s_oneday);    // 86400000ms 毫秒
  seconds_type s_onehour (60*60);            // 3600s   1小时 3600秒
  //hours_type h_onehour (s_onehour);        // 无效 类型截断  秒转到小时 整数除法 会有截断 NOT VALID (type truncates), use:
  hours_type h_onehour (std::chrono::duration_cast<hours_type>(s_onehour)); // s_onehour 转换成 hours_type 小时类型
  milliseconds_type ms_onehour (s_onehour);   // 3600000ms (ok, no type truncation)  小时 转成 毫秒  整数乘法  无截断 可以直接强转
  std::cout << ms_onehour.count() << "ms in 1h" << std::endl; //输出显示

2）.Time points 固定时间点

std::chrono::time_point 表示一个具体时间，如上个世纪80年代、你的生日、今天下午、火车出发时间等，只要它能用计算机时钟表示。鉴于使用时间的情景不同，这个time point具体到什么程度，由选用的单位决定。一个time point必须有一个clock计时。参见clock的说明。

template <class Clock, class Duration = typename Clock::duration>  class time_point;
system_clock::time_point tp_epoch;  //Epoch指的是一个特定的时间： 1970-01-01 00:00:00 UTC
time_point <system_clock,duration<int>> tp_seconds (duration<int>(1));  //1秒
std::cout << tp.time_since_epoch().count();//
 // 显示时间点
std::time_t tt = system_clock::to_time_t(tp);
std::cout << "time_point tp is: " << ctime(&tt);

time_point有一个函数time_from_eproch()用来获得1970年1月1日 Epoch 到time_point时间经过的duration。举个例子，如果timepoint以天为单位，函数返回的duration就以天为单位。

typedef duration<int,std::ratio<60*60*24>> days_type; // 天 类型
time_point<system_clock,days_type> today = time_point_cast<days_type>(system_clock::now());
std::cout << today.time_since_epoch().count() << " days since epoch" << std::endl;//当前经历的时间天数

3）.Clocks 时钟
std::chrono::system_clock 它表示当前的系统时钟，系统中运行的所有进程使用now()得到的时间是一致的。每一个clock类中都有确定的time_point, duration, Rep, Period类型。
操作有：

now() //当前时间点 time_point
to_time_t()          //time_point转换成time_t秒
from_time_t()   // 从time_t转换成time_point

典型的应用是计算时间日期：

using std::chrono::system_clock;
system_clock::time_point today = system_clock::now();//今天时间点
system_clock::time_point tomorrow = today + one_day;//明天时间点

   tt = system_clock::to_time_t ( today );//转换成time_t秒
   std::cout << "today is: " << ctime(&tt);
   tt = system_clock::to_time_t ( tomorrow );//转换成time_t秒
   std::cout << "tomorrow will be: " << ctime(&tt);


   std::chrono::steady_clock 为了表示稳定的时间间隔，后一次调用now()得到的时间总是比前一次的值大

（这句话的意思其实是，如果中途修改了系统时间，也不影响now()的结果），每次tick都保证过了稳定的时间间隔。

典型的应用是给算法计时：

 // 打印 1000个 *  系统需要的时间
  using namespace std::chrono;
  steady_clock::time_point t1 = steady_clock::now();//此刻时间点
  std::cout << "printing out 1000 stars...\n";
  for (int i=0; i<1000; ++i) std::cout << "*";
  std::cout << std::endl;
  steady_clock::time_point t2 = steady_clock::now();//当前时间点
  duration<double> time_span = duration_cast<duration<double>>(t2 - t1);//计算时间差
  std::cout << "It took me " << time_span.count() << " seconds."; 

最后一个时钟，std::chrono::high_resolution_clock 顾名思义，这是系统可用的最高精度的时钟。 实际上high_resolution_clock只不过是system_clock或者steady_clock的typedef。

3.opencv

imageBasics.cpp

#include <iostream>
#include <chrono>//用于算法计时
// chrono是一个时间库, 源于boost，现在已经是C++标准。

//#include <opencv2/opencv.hpp>//这个头文件包含了 大部分头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp> // cv::cvtColor 函数


using namespace std;
using namespace cv;

//官方介绍文档
// https://docs.opencv.org/3.0-rc1/df/d65/tutorial_table_of_content_introduction.html
/*
 
    1 载入图像     Load an image (using cv::imread )     Mat img = imread(filename, 0);（读成灰度图）
    2 创建窗口     Create a named OpenCV window (using cv::namedWindow )
    3 显示图像     Display an image in an OpenCV window (using cv::imshow )
    4 格式转换     cv::cvtColor( image, gray_image, COLOR_BGR2GRAY );
    5 保存图像     cv:: imwrite( "..//Gray_Image.jpg", gray_image );
    6 拷贝图像     cv::Mat image_clone = image.clone();//复制数据到另一块内存空间
                  cv::Mat image_clone ;  image.copyTo(image_clone ); 也可复制
 
 */

/*
        //CMake file
    cmake_minimum_required(VERSION 2.8)//版本限制
    project( DisplayImage )//工程名
    find_package( OpenCV REQUIRED )//找到安装包位置
    include_directories( ${OpenCV_INCLUDE_DIRS} )//添加头文件
    add_executable( DisplayImage DisplayImage.cpp )//添加可执行文件
    target_link_libraries( DisplayImage ${OpenCV_LIBS} )//添加动态链接库
 */

int main ( int argc, char** argv )
{

    // 读取argv[1]指定的图像
    cv::Mat image;
    image = cv::imread ( "../ubuntu.png" ); //cv::imread函数读取指定路径下的图像

    /*************************************
     备注：
    如果flag>0，返回一个三通道的彩色图像(强转)，flag=0返回一个灰度图像(强转)，flag<0,返回包含Alpha通道的原始图像(不修改通道数)
    用法举例：
        Mat image1 = imread("try,jpg", 2 | 4);//载入无损的原图像  
        Mat image2 = imread("try,jpg", 0);    //载入灰度图像  
        Mat image3 = imread("try,jpg", 199);  //载入三通道彩色图像  
     *************************************/

    // 判断图像文件是否正确读取
    if ( image.data == nullptr ) // 返回空指针   数据不存在,可能是文件不存在
    // if (!image.data )
    // if(image.empty() )
    {
        cerr<<"文件"<<argv[1]<<"不存在."<<endl;//输出到错误流
        return 0;
    }
    
    // 文件顺利读取, 首先输出一些基本信息,行数为高,列数为宽
    //image.cols列数(宽) image.rows行数(高) image.channels()图像通道数
    cout<<"图像宽为"<<image.cols<<",高为"<<image.rows<<",通道数为"<<image.channels()<<endl;
    cv::imshow ( "image", image );      // 前一个参数为,窗口名字
    //namedWindow("显示窗口名子", WINDOW_AUTOSIZE );
    cv::waitKey ( 0 );                  // 暂停程序,等待一个按键输入,随机按键
 
    // 判断image的类型　　CV_8UC1   1通道８位无符号 灰度图　　CV_8UC３   ３通道８位无符号  彩色图
    if ( image.type() != CV_8UC1 && image.type() != CV_8UC3 )
    {
        // 图像类型不符合要求
        cout<<"请输入一张彩色图或灰度图."<<endl;
        return 0;
    }
   
   //彩色图转灰度图
   cv::Mat gray_image;
   cv::cvtColor( image, gray_image, cv::COLOR_BGR2GRAY ); // COLOR_GRAY2BGR
   // 写图像文件
   cv:: imwrite( "..//Gray_Image.jpg", gray_image );
   cv::imshow ( "Gray_Image" , gray_image);//显示原来的图像   被修改了
   
   
   /*******************************
    cv::Mat::isContinuous() 和 Mat::ptr<uchar>(i)  结合可以提速
    这个跟计算机组成有关，关于ip寄存器的。其中一个结论是下面的代码，前面比后面快：
    ******************************/
   
   // 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
   // 1 指针直接访问 对一个对象Mat，通过调用函数  Mat::ptr<uchar>(i)  来得到第i行的指针地址.
   // 使用 std::chrono 来给算法计时

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//当前时间点,steady_clock表示系统时钟,不受影响
    for ( size_t y=0; y<image.rows; y++ )//行坐标为y 
    {
        for ( size_t x=0; x<image.cols; x++ )//l列坐标为x
        {
            // 访问位于 x,y 处的像素
            // 用cv::Mat::ptr获得图像的行指针
            unsigned char* row_ptr = image.ptr<unsigned char> ( y );  // row_ptr是第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;        //最快

    // 2、迭代器访问
    // /*对一个Mat，创建一个Mat::Iterator对象it和itend，通过it=Mat::begin()来的到迭代首地址，itend=Mat::end()来得到尾地址，
    // it != itend来判断是否到尾，it++来得到下一个像素指向，(*it)来得到实际像素  *取地址内的值    */

    Mat_<Vec3b>::iterator it =  image.begin<Vec3b>();  //迭代 首地址
    Mat_<Vec3b>::iterator itend =  image.end<Vec3b>(); //迭代 尾地址
    chrono::steady_clock::time_point t11 = chrono::steady_clock::now();//当前时间点  steady_clock表示系统时钟 不受影响
    while (it != itend)
    {  
            // 输出该像素的每个通道,如果是灰度图就只有一个通道
            for ( int c = 0; c != image.channels(); c++ )
            {
                unsigned char data = (*it)[c]; //  (*it)[0]代表当前像素单位的B位，(*it)[1]代表当前像素单位的G位，(*it)[2]代表当前像素单位的R位
            }
            it++;  
    }
    chrono::steady_clock::time_point t22 = chrono::steady_clock::now();//算法运行后 此刻时间点
    chrono::duration<double> time_used1 = chrono::duration_cast<chrono::duration<double>>( t22-t11 );//时间差
    cout<<"迭代器遍历图像用时："<<time_used1.count()<<" 秒。"<<endl;//较快

    //3、动态访问,用类自带的方法image.at<Vec3b>(i,j)[c] 方便，但效率不高,这种方法是最慢的
    //  对一个mat，可以直接用at函数来得到像素，Mat::at<Vec3b>(i,j)为一个像素点

    int colNum= image.cols;    //列宽
    int rowN=image.rows;       //行高
    chrono::steady_clock::time_point t111 = chrono::steady_clock::now();//当前时间点,steady_clock表示系统时钟,不受影响
    for (int i = 0; i < rowN; i++)
    {  
        for (int j = 0; j < colNum; j++)        //这里colNum要注意，下面说明  
        { 
            // 输出该像素的每个通道,如果是灰度图就只有一个通道
            for ( int c = 0; c != image.channels(); c++ )
            {
                unsigned char data =image.at<Vec3b>(i,j)[c]; //  (*it)[0]代表当前像素单位的B位，(*it)[1]代表当前像素单位的G位，(*it)[2]代表当前像素单位的R位
            }
        }  
    }  
    chrono::steady_clock::time_point t222 = chrono::steady_clock::now();//算法运行后 此刻时间点
    chrono::duration<double> time_used2 = chrono::duration_cast<chrono::duration<double>>( t222-t111 );//时间差
    cout<<"动态访问遍历图像用时："<<time_used2.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 ( argv[1], image );//显示原来的图像,被修改了
    cv::waitKey ( 0 );
    
    // 使用clone函数来拷贝数据
    cv::Mat image_clone = image.clone();//复制数据到另一块内存空间
    // cv::Mat image_clone ;  image.copyTo(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;
}

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} )

# 添加可执行文件
#add_executable( bleblending_two  blending_two_images.cpp)
# 链接OpenCV库
#target_link_libraries( bleblending_two ${OpenCV_LIBS} )