(1)RGB-D點雲生成

bin文件夾下為生成的可執行文件generate_cloud,執行時和data文件放在同一文件夾下。

圖像數據來自小覓相機。

 

src下的源碼，包括generatePointCloud.cpp和CMakeLists.txt

// C++ 標准庫
#include <iostream>
#include <string>
//#include <unistd.h>
using namespace std;

// OpenCV 庫
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
//#include <pcl/visualization/cloud_viewer.h>

// PCL 庫
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

// 定義點雲類型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud; 

//pcl::visualization::CloudViewer viewer("pcd viewer");  

// 相機內參
const double camera_factor = 1000;
/*
const double camera_cx = 325.5;
const double camera_cy = 253.5;  //nyuv2數據集：http://cs.nyu.edu/~silberman/datasets/ 
const double camera_fx = 518.0;
const double camera_fy = 519.0;
*/
const double camera_cx = 682.3;
const double camera_cy = 254.9;
const double camera_fx = 979.8;  //小覓
const double camera_fy = 942.8;

// 主函數 
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 || d >= 4096) 
                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 );
        }
/*
//    viewer.showCloud(cloud);
//    sleep(100);////#include <unistd.h>
//    return 0;
*/
    // 設置並保存點雲
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout<<"point cloud size = "<<cloud->points.size()<<endl;
    cloud->is_dense = false;
    pcl::io::savePCDFile( "./pointcloud.pcd", *cloud );
    // 清除數據並退出
    cloud->points.clear();
    cout<<"Point cloud saved."<<endl;
    return 0;
}

 

CMakeLists.txt

# 增加PCL庫的依賴
FIND_PACKAGE( PCL REQUIRED )

list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") # use this in Ubuntu 16.04
# 增加opencv的依賴
FIND_PACKAGE( OpenCV REQUIRED )

# 添加頭文件和庫文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

ADD_EXECUTABLE( generate_cloud generatePointCloud.cpp )
TARGET_LINK_LIBRARIES( generate_cloud ${OpenCV_LIBS} 
    ${PCL_LIBRARIES} )

 

和src文件夾在同一文件夾下的CMakeLists.txt

CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )
PROJECT( slam )

SET(CMAKE_CXX_COMPILER "g++")
SET( CMAKE_BUILD_TYPE Debug  )
SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include )
LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib)

ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src )

 

編譯后可執行文件在bin中。

 

 

學習鏈接： http://www.cnblogs.com/gaoxiang12/p/4652478.html