Skip to content

Commit

Permalink
Merge pull request #845 from bsyoon/mog
Browse files Browse the repository at this point in the history
Fix multi openni grabber buffer corruption
  • Loading branch information
taketwo committed Aug 13, 2014
2 parents ffaa877 + 02f1aaf commit 28ab3c3
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 36 deletions.
6 changes: 6 additions & 0 deletions io/include/pcl/io/openni_grabber.h
Original file line number Diff line number Diff line change
Expand Up @@ -469,6 +469,12 @@ namespace pcl
openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle;
bool running_;

mutable unsigned rgb_array_size_;
mutable unsigned depth_buffer_size_;
mutable boost::shared_array<unsigned char> rgb_array_;
mutable boost::shared_array<unsigned short> depth_buffer_;
mutable boost::shared_array<unsigned short> ir_buffer_;

/** \brief The RGB image focal length (fx). */
double rgb_focal_length_x_;
/** \brief The RGB image focal length (fy). */
Expand Down
61 changes: 25 additions & 36 deletions io/src/openni_grabber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,8 @@ pcl::OpenNIGrabber::OpenNIGrabber (const std::string& device_id, const Mode& dep
, depth_focal_length_y_ (std::numeric_limits<double>::quiet_NaN ())
, depth_principal_point_x_ (std::numeric_limits<double>::quiet_NaN ())
, depth_principal_point_y_ (std::numeric_limits<double>::quiet_NaN ())
, rgb_array_size_ (0)
, depth_buffer_size_ (0)
{
// initialize driver
onInit (device_id, depth_mode, image_mode);
Expand Down Expand Up @@ -582,16 +584,13 @@ pcl::OpenNIGrabber::convertToXYZPointCloud (const boost::shared_ptr<openni_wrapp
register const unsigned short* depth_map = depth_image->getDepthMetaData ().Data ();
if (depth_image->getWidth() != depth_width_ || depth_image->getHeight () != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer ((unsigned short*)(NULL));

if (buffer_size < depth_width_ * depth_height_)
if (depth_buffer_size_ < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset (new unsigned short [buffer_size]);
depth_buffer_size_ = depth_width_ * depth_height_;
depth_buffer_.reset (new unsigned short [depth_buffer_size_]);
}
depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ());
depth_map = depth_buffer.get ();
depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer_.get ());
depth_map = depth_buffer_.get ();
}

register int depth_idx = 0;
Expand Down Expand Up @@ -627,10 +626,7 @@ template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image,
const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
{
static unsigned rgb_array_size = 0;
static boost::shared_array<unsigned char> rgb_array ((unsigned char*)(NULL));
static unsigned char* rgb_buffer = 0;

unsigned char* rgb_buffer = rgb_array_.get ();
boost::shared_ptr<pcl::PointCloud<PointT> > cloud (new pcl::PointCloud<PointT>);

cloud->header.frame_id = rgb_frame_id_;
Expand Down Expand Up @@ -661,25 +657,22 @@ pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wr
register const XnDepthPixel* depth_map = depth_image->getDepthMetaData ().Data ();
if (depth_image->getWidth () != depth_width_ || depth_image->getHeight() != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer ((unsigned short*)(NULL));

if (buffer_size < depth_width_ * depth_height_)
if (depth_buffer_size_ < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset (new unsigned short [buffer_size]);
depth_buffer_size_ = depth_width_ * depth_height_;
depth_buffer_.reset (new unsigned short [depth_buffer_size_]);
}

depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ());
depth_map = depth_buffer.get ();
depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer_.get ());
depth_map = depth_buffer_.get ();
}

// here we need exact the size of the point cloud for a one-one correspondence!
if (rgb_array_size < image_width_ * image_height_ * 3)
if (rgb_array_size_ < image_width_ * image_height_ * 3)
{
rgb_array_size = image_width_ * image_height_ * 3;
rgb_array.reset (new unsigned char [rgb_array_size]);
rgb_buffer = rgb_array.get ();
rgb_array_size_ = image_width_ * image_height_ * 3;
rgb_array_.reset (new unsigned char [rgb_array_size_]);
rgb_buffer = rgb_array_.get ();
}
image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3);
float bad_point = std::numeric_limits<float>::quiet_NaN ();
Expand Down Expand Up @@ -790,22 +783,18 @@ pcl::OpenNIGrabber::convertToXYZIPointCloud (const boost::shared_ptr<openni_wrap

if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer ((unsigned short*)(NULL));
static boost::shared_array<unsigned short> ir_buffer ((unsigned short*)(NULL));

if (buffer_size < depth_width_ * depth_height_)
if (depth_buffer_size_ < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset (new unsigned short [buffer_size]);
ir_buffer.reset (new unsigned short [buffer_size]);
depth_buffer_size_ = depth_width_ * depth_height_;
depth_buffer_.reset (new unsigned short [depth_buffer_size_]);
ir_buffer_.reset (new unsigned short [depth_buffer_size_]);
}

depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ());
depth_map = depth_buffer.get ();
depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer_.get ());
depth_map = depth_buffer_.get ();

ir_image->fillRaw (depth_width_, depth_height_, ir_buffer.get ());
ir_map = ir_buffer.get ();
ir_image->fillRaw (depth_width_, depth_height_, ir_buffer_.get ());
ir_map = ir_buffer_.get ();
}

register int depth_idx = 0;
Expand Down

0 comments on commit 28ab3c3

Please sign in to comment.