深度图像+彩色图像=点云图像



http://blog.csdn.net/yongshengsilingsa/article/details/37935975

http://blog.csdn.net/kh1445291129/article/details/44636685

http://blog.csdn.net/morewindows/article/details/8239560

#include
#include
#include
#include
#include

using namespace std;
#pragma comment(linker, "/subsystem:\"windows\" /entry:\"mainCRTStartup\"")

int user_data;
const double u0 = 319.52883;
const double v0 = 271.61749;
const double fx = 528.57523;
const double fy = 527.57387;

void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
 viewer.setBackgroundColor(0.0, 0.0, 0.0);
}

int main()
{
 pcl::PointCloud cloud_a;
 pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

 cv::Mat color = cv::imread("co.jpg");
 cv::Mat depth1 = cv::imread("de.jpg");

 //add
 cv::Size dsize = cv::Size(color.cols, color.rows);
 cv::Mat depth = cv::Mat(dsize, CV_8UC3);
 resize(depth1,depth,dsize);
 //add

 int rowNumber = color.rows;
 int colNumber = color.cols;

 cloud_a.height = rowNumber;
 cloud_a.width = colNumber;
 cloud_a.points.resize(cloud_a.width * cloud_a.height);

 for (unsigned int u = 0; u < rowNumber; ++u)
 {
  for (unsigned int v = 0; v < colNumber; ++v)
  {
   unsigned int num = u*colNumber + v;
   double Xw = 0, Yw = 0, Zw = 0;

   //Zw = ((double)depth.at(u, v)) / 255.0 * 10001.0;
   //Xw = (u - u0) * Zw / fx;
   //Yw = (v - v0) * Zw / fy;
   Zw = ((double)depth.at(u, v));
   Xw = u;
      Yw = v;

   cloud_a.points[num].b = color.at(u, v)[0];
   cloud_a.points[num].g = color.at(u, v)[1];
   cloud_a.points[num].r = color.at(u, v)[2];

   cloud_a.points[num].x = Xw;
   cloud_a.points[num].y = Yw;
   cloud_a.points[num].z = Zw;
  }
 }

 *cloud = cloud_a;

 pcl::visualization::CloudViewer viewer("Cloud Viewer");

 viewer.showCloud(cloud);

 viewer.runOnVisualizationThreadOnce(viewerOneOff);

 while (!viewer.wasStopped())
 {
  user_data = 9;
 }

 return 0;
}

//要把深度图像转换成彩色图像一样的大小，不然会显示内存异常