Permalink
Browse files

Revert "Simulation: enable returning of organized point clouds"

  • Loading branch information...
1 parent 5d19050 commit 41efbd20062c335316cba2641d7706d202e74c26 @SergioRAgostinho SergioRAgostinho committed on GitHub Aug 22, 2016
Showing with 8 additions and 25 deletions.
  1. +1 −1 simulation/include/pcl/simulation/range_likelihood.h
  2. +7 −24 simulation/src/range_likelihood.cpp
@@ -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:
@@ -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,
- bool organized)
+ const Eigen::Isometry3d & pose)
{
// 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;
- if (organized) idx = y*col_width_ + x;
- else idx = points_added; // y*camera_width_ + x;
-
+ int 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;
- }
}
}
-
- if (!organized)
- {
- pc->width = 1;
- pc->height = points_added;
- pc->points.resize (points_added);
- }
+ pc->width = 1;
+ pc->height = points_added;
+ pc->points.resize (points_added);
if (make_global)
{

0 comments on commit 41efbd2

Please sign in to comment.