RGB-D SLAM——点云拼接篇(二)

1.OpenCv旋转向量平移向量转换为Eigen变换矩阵

在上一节中我们已经知道如何求出俩张图片的旋转向量和平移向量，那么接下来我们需要将旋转向量和平移向量转换成平移矩阵，可以使用Eigen工具来实现：

Eigen::Isometry3d cvMat2Eigen(cv::Mat &rvec,cv::Mat &tvec){    cv::Mat R;    cv::Rodrigues(rvec,R);//罗德里格旋转公式，将3x1的旋转向量转换成3x3的旋转矩阵    Eigen::Matrix3d r; //the matrix are 3x3，类型为double    cv::cv2eigen(R,r)；//将cv格式的矩阵(Mat类型的)转换成Eigen类型的Matrix    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//旋转和平移的集合,4*4的矩阵    Eigen::AngleAxisd angle(r);     Eigen::Translation<double,3> trans(tvec.at<double>(0,0),tvec.at<double>(0,1),tvec.at<double>(0,2));    T=angle;  //完成旋转矩阵的存储    T(0,3)=tvec.at<double>(0,0);    T(1,3)=tvec.at<double>(0,1);    T(2,3)=tvec.at<double>(0,2);    return T;}

这段程序需要的知识：
1.其中的Rodrigues（）函数：
原型为（详细可见该API）：

void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian=noArray())Parameters:  src – 输入旋转向量(3x1/1x3)或者旋转矩阵（3x3） dst – 分别输出旋转矩阵（3x3）或者（3x1/1x3） jacobian – 可选择的输出雅克比矩阵，为3x9的或者9x3的,简单来说就是输出矩阵对输入矩阵的分别的局部派生关系 2.这是cv旋转向量和平移向量的转换，之所以只要这么转换，主要是后面pcl点云拼接的时候会用到Eigen的矩阵，因为pcl用了第三方的Eigen来进行矩阵的运算。

2.拼接点云，俩帧点云

当有了每帧图像的点云，又有了俩帧相对的转换关系之后，这时候我们就可以进行点云的拼接了，点云拼接的具体函数如下所示：

void jointPointCloud(FRAME &originalframe,FRAME &newframe,Eigen::Isometry3d &T) //{   PointCloud::Ptr originalcloud=image2PointCloud(originalframe.rgb,originaleframe.depth);   PointCloud::Ptr newCloud=image2PointCloud(newframe.rgb,newframe.depth);   PointCloud::Ptr cloud_output(new PointCloud());   pcl::transformPointCloud(*originalcloud,*cloud_output,T.matrix());   *cloud_output += *newCloud;//降采样滤波，如果在这里使用了#include <pcl/filters/voxel_grid.h>,记得把CMakeLists里的set(CMAKE_CXX_FLAGS "-std=c++11 ")字眼去掉  static pcl::VoxelGrid<PointT> voxel;  voxel.setLeafSize(0.01f,0.01f,0.01f);  voxel.setInputCloud(cloud_output);  PointCloud::Ptr tmp(new PointCloud());  voxel.filter(*tmp);   pcl::io::savePCLFile("jointcloud",*cloud_output);   pcl::visualization::CloudViewer viewer("jointcloud");   viewer.showCloud(cloud_output)}

则得到以下效果：

3.多帧图片进行点云拼接

为完成多点云拼接，需要不断的向里面导入新的点云，然后通过T不断的组装起来，在下面展示如果不断的导入帧并实现把帧给融合起来。

int main(int argc, char** argv){    vector<FRAME> frame;    vector<Eigen::Isometry3d> Trans;//导入帧，使构成的地图更加完整    for(int i=1;i<300;i++)    {        FRAME frame_single;        string path_rgb("/home/hansry/Pose_part/data/rgb_png/");        string path_depth("/home/hansry/Pose_part/data/depth_png/");        string suffix(".png");        stringstream str_rgb,str_depth;        str_rgb<<path_rgb<<i<<suffix;        str_depth<<path_depth<<i<<suffix;        frame_single.rgb=imread(str_rgb.str());        frame_single.depth=imread(str_depth.str(),-1);        frame.push_back(frame_single);    }    cout<<frame.size()<<endl;    for(int k=0;k<(frame.size()-1);k++)    {        computeKeyPointsAndDesp(frame[k]);        computeKeyPointsAndDesp(frame[k+1]);        RESULT_OF_PNP result=estimateMotion(frame[k],frame[k+1]);        if ( result.inliers < 5 ) //inliers不够，放弃该帧            continue;        double norm = normofTransform(result.rvec, result.tvec);                cout<<"norm = "<<norm<<endl;                if ( norm >= 0.3)            continue;        Eigen::Isometry3d T=cvMat2Eigen(result.rvec,result.tvec);        Trans.push_back(T);        cout<<Trans.size()<<endl;    }    //生成第一帧的点云    PointCloud::Ptr original_cloud=image2PointCloud(frame[0].rgb,frame[0].depth);    for(int j=1;j<frame.size();j++)    {        PointCloud::Ptr output=jointPointCloud(original_cloud,frame[j],Trans[j-1]);        original_cloud=output;    }    pcl::io::savePCDFile("/home/hansry/Pose_part/data/outputcloud.pcd",*original_cloud);    waitKey(0);    return  0;}

程序运行结果如下：

有点心塞，滤波虽然能很好的提升速度，但是好像失真得好厉害啊啊啊啊啊！！！！

最后，就只剩下优化啦！！！

参考：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/blob/master/part%20VI/include/slamBase.h