ch13/rgbdmapping

ch13/dense_RGBD/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required( VERSION 2.8 )
+
+set( CMAKE_BUILD_TYPE Release )
+set( CMAKE_CXX_FLAGS "-std=c++11" )
+
+# opencv 
+find_package( OpenCV REQUIRED )
+include_directories( ${OpenCV_INCLUDE_DIRS} )
+
+# eigen 
+include_directories( "/usr/include/eigen3/" )
+
+# pcl 
+find_package( PCL REQUIRED COMPONENT common io filters )
+include_directories( ${PCL_INCLUDE_DIRS} )
+add_definitions( ${PCL_DEFINITIONS} )
+
+# octomap 
+find_package( octomap REQUIRED )
+include_directories( ${OCTOMAP_INCLUDE_DIRS} )
+
+add_executable( pointcloud_mapping pointcloud_mapping.cpp )
+target_link_libraries( pointcloud_mapping ${OpenCV_LIBS} ${PCL_LIBRARIES} )
+
+add_executable( octomap_mapping octomap_mapping.cpp )
+target_link_libraries( octomap_mapping ${OpenCV_LIBS} ${PCL_LIBRARIES} ${OCTOMAP_LIBRARIES} )

ch13/dense_RGBD/data/pose.txt

@@ -0,0 +1,5 @@
+-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

ch13/dense_RGBD/octomap_mapping.cpp

@@ -0,0 +1,88 @@
+#include <iostream>
+#include <fstream>
+using namespace std;
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <octomap/octomap.h>    // for octomap 
+
+#include <Eigen/Geometry> 
+#include <boost/format.hpp>  // for formating strings
+
+int main( int argc, char** argv )
+{
+    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
+    vector<Eigen::Isometry3d> poses;         // 相机位姿
+
+    ifstream fin("./data/pose.txt");
+    if (!fin)
+    {
+        cerr<<"cannot find pose file"<<endl;
+        return 1;
+    }
+
+    for ( int i=0; i<5; i++ )
+    {
+        boost::format fmt( "./data/%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 ( int i=0; i<7; i++ )
+        {
+            fin>>data[i];
+        }
+        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<<"正在将图像转换为 Octomap ..."<<endl;
+
+    // octomap tree 
+    octomap::OcTree tree( 0.05 ); // 参数为分辨率
+
+    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];
+
+        octomap::Pointcloud cloud;  // the point cloud in octomap 
+
+        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]; // 深度值
+                if ( d==0 ) continue; // 为0表示没有测量到
+                if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉
+                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;
+                // 将世界坐标系的点放入点云
+                cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] ); 
+            }
+
+        // 将点云存入八叉树地图，给定原点，这样可以计算投射线
+        tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) );     
+    }
+
+    // 更新中间节点的占据信息并写入磁盘
+    tree.updateInnerOccupancy();
+    cout<<"saving octomap ... "<<endl;
+    tree.writeBinary( "octomap.bt" );
+    return 0;
+}

ch13/dense_RGBD/pointcloud_mapping.cpp

@@ -0,0 +1,113 @@
+#include <iostream>
+#include <fstream>
+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/filters/voxel_grid.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+
+int main( int argc, char** argv )
+{
+    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
+    vector<Eigen::Isometry3d> poses;         // 相机位姿
+
+    ifstream fin("./data/pose.txt");
+    if (!fin)
+    {
+        cerr<<"cannot find pose file"<<endl;
+        return 1;
+    }
+
+    for ( int i=0; i<5; i++ )
+    {
+        boost::format fmt( "./data/%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 ( int i=0; i<7; i++ )
+        {
+            fin>>data[i];
+        }
+        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<PointT> PointCloud;
+
+    // 新建一个点云
+    PointCloud::Ptr pointCloud( new PointCloud ); 
+    for ( int i=0; i<5; i++ )
+    {
+        PointCloud::Ptr current( new PointCloud );
+        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]; // 深度值
+                if ( d==0 ) continue; // 为0表示没有测量到
+                if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉
+                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 ];
+                current->points.push_back( p );
+            }
+        // depth filter and statistical removal 
+        PointCloud::Ptr tmp ( new PointCloud );
+        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
+        statistical_filter.setMeanK(50);
+        statistical_filter.setStddevMulThresh(1.0);
+        statistical_filter.setInputCloud(current);
+        statistical_filter.filter( *tmp );
+        (*pointCloud) += *tmp;
+    }
+
+    pointCloud->is_dense = false;
+    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
+
+    // voxel filter 
+    pcl::VoxelGrid<PointT> voxel_filter; 
+    voxel_filter.setLeafSize( 0.01, 0.01, 0.01 );       // resolution 
+    PointCloud::Ptr tmp ( new PointCloud );
+    voxel_filter.setInputCloud( pointCloud );
+    voxel_filter.filter( *tmp );
+    tmp->swap( *pointCloud );
+
+    cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl;
+
+    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
+    return 0;
+}