Revert "Simulation: enable returning of organized point clouds"

PointCloudLibrary · Aug 22, 2016 · 41efbd2 · 41efbd2
1 parent 5d19050
commit 41efbd2
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 25 deletions.
diff --git a/simulation/include/pcl/simulation/range_likelihood.h b/simulation/include/pcl/simulation/range_likelihood.h
@@ -117,7 +117,7 @@ namespace pcl
         // global=false - PointCloud is as would be captured by an RGB-D camera [default]
         // global=true  - PointCloud is transformed into the model/world frame using the camera pose
         void getPointCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
-              bool make_global, const Eigen::Isometry3d & pose, bool organized = false);
+              bool make_global, const Eigen::Isometry3d & pose);
 
         // Convenience function to return RangeImagePlanar containing
         // simulated RGB-D:

diff --git a/simulation/src/range_likelihood.cpp b/simulation/src/range_likelihood.cpp
@@ -650,8 +650,7 @@ pcl::simulation::RangeLikelihood::computeScores (float* reference,
 void
 pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
   bool make_global,
-  const Eigen::Isometry3d & pose,
+  const Eigen::Isometry3d & pose)
-  bool organized)
 {
   // TODO: check if this works for for rows/cols >1  and for width&height != 640x480
   // i.e. multiple tiled images
@@ -661,7 +660,7 @@ pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRG
   //pc->width    = camera_width_;
   //pc->height   = camera_height_;
 
-  pc->is_dense = true;
+  pc->is_dense = false;
   pc->points.resize (pc->width*pc->height);
 
   int points_added = 0;
@@ -681,12 +680,8 @@ pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRG
     for (int x = 0; x < col_width_ ; ++x)  // camera_width_
     {
       // Find XYZ from normalized 0->1 mapped disparity
-      int idx;
+      int idx = points_added; // y*camera_width_ + x;
-      if (organized) idx = y*col_width_ + x;
-      else idx = points_added; // y*camera_width_ + x;
-
       float d = depth_buffer_[y*camera_width_ + x] ;
-
       if (d < 1.0) // only add points with depth buffer less than max (20m) range
       {
         float z = zf*zn/((zf-zn)*(d - zf/(zf-zn)));
@@ -701,29 +696,17 @@ pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRG
         pc->points[idx].x = (static_cast<float> (x)-camera_cx_) * z * (-camera_fx_reciprocal_);
         pc->points[idx].y = (static_cast<float> (y)-camera_cy_) * z * (-camera_fy_reciprocal_);
 
-        int rgb_idx = y*col_width_ + x;  //camera_width_
+	int rgb_idx = y*col_width_ + x;  //camera_width_
         pc->points[idx].b = color_buffer[rgb_idx*3+2]; // blue
         pc->points[idx].g = color_buffer[rgb_idx*3+1]; // green
         pc->points[idx].r = color_buffer[rgb_idx*3]; // red
         points_added++;
       }
-      else if (organized)
-      {
-        pc->is_dense = false;
-        pc->points[idx].z = std::numeric_limits<float>::quiet_NaN ();
-        pc->points[idx].x = std::numeric_limits<float>::quiet_NaN ();
-        pc->points[idx].y = std::numeric_limits<float>::quiet_NaN ();
-        pc->points[idx].rgba = 0;
-      }
     }
   }
-
+  pc->width    = 1;
-  if (!organized)
+  pc->height   = points_added;
-  {
+  pc->points.resize (points_added);
-    pc->width    = 1;
-    pc->height   = points_added;
-    pc->points.resize (points_added);
-  }
 
   if (make_global)
   {