2D图像转换点云

Code Hello-SLAM

标签（空格分隔）： 旭 SLAM Program

---

转载：
一起做RGB-D SLAM (1)
一起做RGB-D SLAM (2)
使用opencv以及pcl将2D图像转换为3D点云

1.目标

写一个RGB-D SLAM程序
要用的库：OpenCV, PCL, g2o
系统环境：Ubuntu16.04

2.安装软件

2.1.OpenCV

参见笔记：Vins-Mono环境配置与测试

2.2.PCL

安装PCL：

sudo apt-get install libpcl-dev pcl-tools

3.构建CMAKE程序

新建工程目录/slam ，在目录中创建以下文件夹：

• bin 用来放编译好的可执行二进制文件
• src 用来放源代码
• lib 用来放编译好的库文件
• include 用来放头文件
• data 用来存放数据

将RGB图片、深度图片（rgb.pgn depth.pgn)放入/slam/data目录下;

在/slam目录下创建CMakeLists.txt：

touch CMakeLists.txt

写入以下信息：

CMAKE_MINIMUM_REQUIRED( VERSION 2.8 ) #设定版本
PROJECT( slam ) #设定工程名
SET( CMAKE_CXX_COMPILER "g++") #设定编译器
	
#设定可执行二进制文件的目录
SET( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 
	
#设定存放编译出来的库文件的目录
SET( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 
#并且把该目录设为连接目录
LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib)
	
#设定头文件目录
INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include)
	
#增加子文件夹，也就是进入源代码文件夹继续构建
ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src)

在代码根目录文件夹，即/slam/src中新建一个名字为generatePointCloud.cpp的文件，写入以下内容：

// C++ 标准库
#include <iostream>
#include <string>
using namespace std;
// OpenCV 库
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// PCL 库
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud; 
// 相机内参
const double camera_factor = 1000;
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;
// 主函数 
int main( int argc, char** argv )
{
// 读取./data/rgb.png和./data/depth.png，并转化为点云
// 图像矩阵
cv::Mat rgb, depth;
// 使用cv::imread()来读取图像
// API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
rgb = cv::imread( "./data/rgb.png" );
// rgb 图像是8UC3的彩色图像
// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
depth = cv::imread( "./data/depth.png", -1 );
// 点云变量
// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
PointCloud::Ptr cloud ( new PointCloud );
// 遍历深度图
for (int m = 0; m < depth.rows; m++)
for (int n=0; n < depth.cols; n++)
{
// 获取深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d 可能没有值，若如此，跳过此点
if (d == 0)
continue;
// d 存在值，则向点云增加一个点
PointT p;
// 计算这个点的空间坐标
p.z = double(d) / camera_factor;
p.x = (n - camera_cx) * p.z / camera_fx;
p.y = (m - camera_cy) * p.z / camera_fy;
// 从rgb图像中获取它的颜色
// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
p.b = rgb.ptr<uchar>(m)[n*3];
p.g = rgb.ptr<uchar>(m)[n*3+1];
p.r = rgb.ptr<uchar>(m)[n*3+2];
// 把p加入到点云中
cloud->points.push_back( p );
}
// 设置并保存点云
cloud->height = 1;
cloud->width = cloud->points.size();
cout<<"point cloud size = "<<cloud->points.size()<<endl;
cloud->is_dense = false;
pcl::io::savePCDFile( "./data/pointcloud.pcd", *cloud );
// 清除数据并退出
cloud->points.clear();
cout<<"Point cloud saved."<<endl;
return 0;
}

为了告诉编译器我们需要编译这个程序，故需要在/slam/src/中建立一个名字为CMakeLists.txt文件，并在CMakeLists.txt中写入如下内容：：

# 增加PCL库的依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )
# 增加opencv的依赖
FIND_PACKAGE( OpenCV REQUIRED )
# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS} )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp )
TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS} 
${PCL_LIBRARIES} )

开启终端进入/slam目录，编译工程：

mkdir build
cd build
cmake ..
make

若编译成功，结果如下：

-- Boost version: 1.58.0
-- Found the following Boost libraries:
--   system
--   filesystem
--   thread
--   date_time
--   iostreams
--   serialization
--   chrono
--   atomic
--   regex
-- Checking for module 'libopenni2'
--   No package 'libopenni2' found
-- Could NOT find OpenNI2 (missing:  OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) 
** WARNING ** io features related to openni2 will be disabled
** WARNING ** io features related to pcap will be disabled
** WARNING ** io features related to png will be disabled
-- The imported target "vtkRenderingPythonTkWidgets" references the file
   "/usr/lib/x86_64-linux-gnu/libvtkRenderingPythonTkWidgets.so"
but this file does not exist.  Possible reasons include:
* The file was deleted, renamed, or moved to another location.
* An install or uninstall procedure did not complete successfully.
* The installation package was faulty and contained
   "/usr/lib/cmake/vtk-6.2/VTKTargets.cmake"
but not all the files it references.

-- The imported target "vtk" references the file
   "/usr/bin/vtk"
but this file does not exist.  Possible reasons include:
* The file was deleted, renamed, or moved to another location.
* An install or uninstall procedure did not complete successfully.
* The installation package was faulty and contained
   "/usr/lib/cmake/vtk-6.2/VTKTargets.cmake"
but not all the files it references.

-- looking for PCL_COMMON
-- looking for PCL_OCTREE
-- looking for PCL_IO
-- Configuring done
WARNING: Target "generate_pointcloud" requests linking to directory "/usr/lib/x86_64-linux-gnu".  Targets may link only to libraries.  CMake is dropping the item.
-- Generating done
-- Build files have been written to: /home/wanghuidan/桌面/代码/test/slam/build

[ 50%] Building CXX object src/CMakeFiles/generate_pointcloud.dir/generatePointCloud.cpp.o
[100%] Linking CXX executable ../../bin/generate_pointcloud
[100%] Built target generate_pointcloud
N501JW:~/桌面/代码

如果编译通过的话，就会在/slam/bin/中生成一个名为generate_pointcloud的可执行的二进制文件。
这时候可以执行这个程序：

N501JW:~/slam/build$ cd ..
N501JW:~/slam$ bin/generate_pointcloud

此时如果安装了pcl，就可以通过

N501JW:~/slam$ pcl_viewer data/pointcloud.pcd

查看生成的点云文件是个什么样子了。
终端结果显示为：

The viewer window provides interactive commands; for help, press 'h' or 'H' from within the window.
> Loading data/pointcloud.pcd [done, 
1235 ms : 204186 points]
Available dimensions: x y z rgba

在屏幕上就会弹出一个PCD viewer.