拖了好久才写。。。懒！！！

第五篇就是将第四篇的一些功能整合成函数，然后由两帧扩展成700帧。

先从parameter.txt说起：

# part 5
# 数据相关
# 起始与终止索引
start_index=1
end_index=700
# 数据所在目录
rgb_dir=../inputdata/rgb_png/
rgb_extension=.png
depth_dir=../inputdata/depth_png/
depth_extension=.png
# 点云分辨率
voxel_grid=0.02
# 是否实时可视化
visualize_pointcloud=yes
# 最小匹配数量
min_good_match=10
# 最小内点
min_inliers=5
# 最大运动误差
max_norm=0.3

上面是参数文件中增加的部分。包含循环读取帧所需的路径和扩展名，还有在两帧匹配后，对匹配进行筛选的设定。

再看src/CmakeLists.txt

# 由于需要链接boost库，所以需要find_package和include_directries进 boost库
find_package(Boost REQUIRED COMPONENTS system)
include_directories(${Boost_INCLUDE_DIRS})

# 增加opencv库依赖
FIND_PACKAGE( OpenCV REQUIRED )
INCLUDE_DIRECTORIES( ${OpenCV_INCLUDE_DIRS} )

# 增加PCL库依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization)
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

# 增加库文件slamBase
add_library(slambase slamBase.cpp)
target_link_libraries(slambase
        ${OpenCV_LIBS}
        ${PCL_LIBS})

#[[
# 增加generate_pointcloud可执行程序，并将其链接到依赖库上
ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp )
TARGET_LINK_LIBRARIES( generate_pointcloud
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES} )

# 增加特征点匹配的可执行文件
add_executable(detectfeacures detectFeacures.cpp )
# 这里也是将boost库链接进来
target_link_libraries(detectfeacures ${Boost_LIBRARIES} slambase )

#增加合并点云可执行文件
ADD_EXECUTABLE( joinPointCloud joinPointCloud.cpp)
TARGET_LINK_LIBRARIES( joinPointCloud
        slambase
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
        ${pcl_visualization_libraries} )]]

#  #[[...]]整体注释

add_executable(visualOdometry visualOdometry.cpp)
target_link_libraries(visualOdometry
        slambase
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES})

把前几篇增加的EXE都注释掉。只留slamBase库文件即可，因为需要的功能都整合进去了。
学到的一点就是，在写Cmakelists时，整体注释可以用：

#[[
    ...
    ...
    ...
        ]]

中间部分就被注释掉了。

在slamBase.h中，增加了几个函数：

// 给定index，读取一帧数据
FRAME readFrame( int index, ParameterReader& pd );
// 度量运动的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );

inline static CAMERA_INTRINSIC_PARAMETERS getDefaultCamera()
{
    ParameterReader pd;
    CAMERA_INTRINSIC_PARAMETERS camera;
    camera.fx = atof( pd.getData( "camera.fx" ).c_str());
    camera.fy = atof( pd.getData( "camera.fy" ).c_str());
    camera.cx = atof( pd.getData( "camera.cx" ).c_str());
    camera.cy = atof( pd.getData( "camera.cy" ).c_str());
    camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
    return camera;
}

前两个，一个读取帧，一个计算运动量大小。
第三个是一个写在头文件中的静态内联函数，功能是读取parameter文件中相机内参数据。
参考：
http://blog.csdn.net/u014630623/article/details/50989891

inline内联的作用是将定义就地展开使用，而不用像普通函数一样再去寻找并跳转到定义处。适用短小常用的函数，省去了跳转寻找时间。
static静态函数又称为内部函数。在C/C++中，定义的函数默认都是全局的(相对于多个文件的源程序)。而在函数的前面加上static关键字可以改变函数的作用域，即将函数的作用域限定在含有此函数的定义所在的文件，在其他文件中不可以使用此函数。

另外还增加几个头文件（其实也不是增加，而是把之前的功能整合到库中，所以头文件中需要增加一些）：

#include <opencv2/core/eigen.hpp> //eigen模块,cv::cv2eigen()在里面
#include <pcl/common/transforms.h>  //点云变换模块,pcl::transformPointCloud()函数在其中
#include <pcl/filters/voxel_grid.h>  //滤波器模块，体素网格

因为随着cv::cv2eigen() 与 pcl::transformPointCloud() 转入slamBase.cpp中，所以这些头文件要include进slamBase.h中。

在slamBase.cpp中，增加的主要是需要整合的东西：

//将rvec和tvec整合成T矩阵
Eigen::Isometry3d cvMat2Eigen(cv::Mat rvec, cv::Mat tvec)
{
    cv::Mat R_cv_matrix;
    cv::Rodrigues(rvec, R_cv_matrix);//mat旋转向量->mat旋转矩阵

    Eigen::Matrix3d r_eigen_matrix;
    cv::cv2eigen(R_cv_matrix, r_eigen_matrix);//mat旋转矩阵->eigen旋转矩阵

    //Eigen::AngleAxis是模板类，必须要传入类型，而Eigen::AngleAxisd是定义的double类型！
    Eigen::AngleAxisd angleaxis(r_eigen_matrix); //eigen旋转矩阵->eigen轴角

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    T = angleaxis;  //eigen轴角->eigen变换矩阵旋转部分

    //平移部分直接按位置赋值过去
    T(0, 3) = tvec.at<double>(0, 0);
    T(1, 3) = tvec.at<double>(0, 1);
    T(2, 3) = tvec.at<double>(0, 3);

    return T;
}

PointCloud::Ptr joinPointCloud(PointCloud::Ptr original, FRAME new_frame, Eigen::Isometry3d T,
                                    CAMERA_INTRINSIC_PARAMETERS camera)
{
    PointCloud::Ptr new_pointcloud = image2PointCloud(new_frame.rgb, new_frame.depth, camera);
    PointCloud::Ptr output_pointcloud(new PointCloud());//要用new一个新点云

    pcl::transformPointCloud(*original, *output_pointcloud, T.matrix());  //此函数传入参数是点云变量，并不是点云指针，要解引用得到点云
    //T也要用.matrix()方法获得矩阵类型，因为T是Eigen::Isometry3d类型，而这里需要的是Eigen::Matrix类型，其实并不对等。需要用.matrix()方法获得矩阵

    *new_pointcloud += *output_pointcloud; //+=操作也是对应点云，点云指针不可以

    // Voxel grid 滤波降采样。。。还没看
    static pcl::VoxelGrid<PointT> voxel;
    static ParameterReader pd;
    double gridsize = atof( pd.getData("voxel_grid").c_str() );
    voxel.setLeafSize( gridsize, gridsize, gridsize );
    voxel.setInputCloud( new_pointcloud );
    PointCloud::Ptr tmp( new PointCloud() );
    voxel.filter( *tmp );
    return tmp;
}

//读取帧。参数是data文件中文件的索引，定位是通过 文件名固定部分 与 随动索引部分 组合成文件名，进行文件读取。
FRAME readFrame( int index, ParameterReader& pd )
{
    FRAME f;
    string rgbDir   =   pd.getData("rgb_dir");
    string depthDir =   pd.getData("depth_dir");

    string rgbExt   =   pd.getData("rgb_extension");
    string depthExt =   pd.getData("depth_extension");

    // 字符串流。这个是在std中的，不是在sstream。
    // 用法上跟iostream没啥区别，只不过进入此流的数据全部被转化成字符串，而流出类型也是字符串
    stringstream ss;

    string filename;

    ss<<rgbDir<<index<<rgbExt; //这里可以看出，index是int类型，也可以进入流中。
    ss>>filename;  //然后出流后整一个就成了string类型。
    f.rgb = cv::imread( filename );

    ss.clear();  //使用完要清理一下
    filename.clear();  //string类型也有此clear()函数

    ss<<depthDir<<index<<depthExt;
    ss>>filename;
    f.depth = cv::imread( filename, -1 );

    return f;
}

//
double normofTransform( cv::Mat rvec, cv::Mat tvec )
{
    //这里注意旋转的周期性。旋转向量的模长表示旋转角度，所以旋转角度要取自身和2PI-自身的最小值。要取到劣弧，不要优弧
    return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
}

visualOdometry.cpp:

//
// Created by robin on 18-2-13.
//
#include <iostream>

using namespace std;

#include "slamBase.h"

int main( int argc, char** argv )
{
    //读取参数文件参数
    ParameterReader pd;
    int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
    int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );
    // 是否显示点云，visualize为bool型，这里注意==的用法，是有返回值的，返回bool类型值，可用于赋值。
    bool visualize = pd.getData("visualize_pointcloud")==string("yes");
    int min_inliers = atoi( pd.getData("min_inliers").c_str() );
    double max_norm = atof( pd.getData("max_norm").c_str() );

    // initialize初始化中包含：相机内参，处理第一帧，点云viewer。
    cout<<"Initializing ..."<<endl;

    //相机内参
    CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();

    //处理第一帧
    FRAME lastFrame = readFrame( startIndex, pd ); // 初始化中lastFrame即为第一帧数据
    computeKeyPointsAndDesp( lastFrame );
    PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );

    //点云可视化
    pcl::visualization::CloudViewer viewer("viewer");

    for ( int currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
    {
        cout<<"Reading files "<<currIndex<<endl;
        // 读取currFrame
        FRAME currFrame = readFrame( currIndex,pd );
        // 计算keypoints 和 descriptor。讲道理这一步可以进一步整合到estimateMotion()整个函数输入量帧，相机内参就可以出来运动矩阵。比较干爽～
        computeKeyPointsAndDesp( currFrame );
        // 比较currFrame 和 lastFrame，计算rvec和tvec。
        RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );

        //下面两个continue对结果进行处理，内点不够或者运动过大时，直接continue放弃此帧。
        if ( result.inliers < min_inliers ) //inliers不够，放弃该帧
            continue;
        // 计算运动范围是否太大
        double norm = normofTransform(result.rvec, result.tvec);
        cout<<"norm = "<<norm<<endl;
        if ( norm >= max_norm )
            continue;

        //构造成4*4的T，输出显示
        Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
        cout<<"T="<<T.matrix()<<endl;

        // 这里对点云进行合并。两种方式都可以，第一种是将当前帧的点云加到以前的点云中。所以用到T.inverse()。因为是将最新帧的点云进行反变换，跟老的点云对其坐标系。

        // 而这里直接用T的话，集需要将整个老的点云用T变换到新帧点云所对应的坐标系下。整个过程来看，当老点云非常大的时候，
        // 第一种貌似省力一些，毕竟每一帧能看到的点云是固定的，而老点云随着时间积累会很大，乘以T时需要运算量。缺点就是在点云显示时，老的点云是不动的，导致整个视角一直在老点云上，并不会随动新增加的点云。
        // 第二种虽然计算量大，但由于是岁视角改变整个旧点云的位置，就是在可视化中视角是随着新进来的点云的。有种慢慢扫图的感觉，比较符合直观感受。而可视化的一大重要作用就是看的明白。
        // cloud = joinPointCloud( cloud, currFrame, T.inverse(), camera );
        cloud = joinPointCloud( cloud, currFrame, T, camera );

        //判断一下可视化
        if ( visualize == true )
            viewer.showCloud( cloud );

        //处理完后，将当前帧赋值给上一帧
        lastFrame = currFrame;
    }

    //输出保存整个点云文件
    pcl::io::savePCDFile( "data/result.pcd", *cloud );


    return 0;
}