单张图片生成三维点云

配置pcl:参照博客http://blog.youkuaiyun.com/chentravelling/article/details/43451589

// 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.hpp>  
//#include <pcl/point_types.h>  


#include <pcl/visualization/cloud_viewer.h>  
#include <iostream>  
#include <pcl/io/io.h>  
#include <pcl/io/pcd_io.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 user_data;  
  
void viewerOneOff (pcl::visualization::PCLVisualizer& viewer) 
{  
    viewer.setBackgroundColor (0, 0, 0);    
   
}  
  
void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)  
{  
    static unsigned count = 0;  
    std::stringstream ss;  
    ss << "Once per viewer loop: " << count++;  
    viewer.removeShape ("text", 0);  
    viewer.addText (ss.str(), 200, 300, "text", 0);  
  
    //FIXME: possible race condition here:  
    user_data++;  
}  
  
  
// 主函数   
int main( int argc, char** argv )  
{  
    // 读取./data/rgb.png和./data/depth.png，并转化为点云  
    //int m=0,n=0;
    // 图像矩阵  
    cv::Mat rgb, depth;  
    // 使用cv::imread()来读取图像  
    
    //for(m;m<301;m++)
//char szName_col[56]={0};
//sprintf(szName_col, "D:\Program Files (x86)\visual studio 2010\Projects\dep_corlor\dep_corlor\deng\1",m);

rgb = cv::imread( "./1/1.jpg" );  
    // rgb 图像是8UC3的彩色图像  
    // depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改  
    depth = cv::imread( "./2/1.jpg", -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 );  
        }  
    pcl::visualization::CloudViewer viewer("Cloud Viewer"); 
    viewer.showCloud(cloud);  
    viewer.runOnVisualizationThreadOnce (viewerOneOff);  
    viewer.runOnVisualizationThread (viewerPsycho);  
    while (!viewer.wasStopped ())  
    { 
        user_data++;  
    }  
    return 0;  

}