First PC requested from driver (and end of any 'largest PC') is all 0's

Issue #36 resolved
Patrick Beeson
created an issue

For some reason, the very first point cloud requested on an S21 is all 0's. The size of the point cloud is very large (the typical ~250,000 points that I'm seeing in my setup) but all the data is 0. This causes many issues with existing software including PCL. I can get around this by throwing away the first PC received in all of my processing nodes, but I don't htink I should have to do this. This is obviously something weird in your ROS driver when it gets it's first ever point cloud to deliver.

I'm using 3.4.1

Comments (6)

  1. Patrick Beeson reporter

    I found this bug.

    In camera.cpp publishPointCloud() you guys call cloud.data.reserve() to try and do a single memory allocation that will cover the size of all data. But reserve() DOES NOT change the size() of the vector, only the capacity(). Thus, when using reserve() you MUST use push_back() or insert() in order to increase the size() counter. Because you guys are copying stright into the vector using pointers, you don't use push_back. So, when you later call resize() right before the actual publish, STL is resizing from a zero sized vector (at least on the first time) to a validPoints*point_step sized vector and zeroing out everything from 0 to that new size, thus publishing all zeros the first time.

    However, this bug continues to be an issue because anytime that a larger point cloud is seen than the previous largest one published, as all values between the last largest sized cloud and the new cloud will also get initialized to 0.

    The simplest fix is to simple replace the serve() call with a resize(), which not only allocates memory (which it should only do on the first call) but also sets the size() of the vector so that you can resize() later without losing data since you aren't using push_back().

  2. Patrick Beeson reporter
    diff -r f73e23c60002 multisense_ros/src/camera.cpp
    --- a/multisense_ros/src/camera.cpp Tue Dec 30 13:49:22 2014 -0500
    +++ b/multisense_ros/src/camera.cpp Wed Feb 11 11:44:35 2015 -0500
    @@ -162,7 +162,7 @@
         if (points.size() != imageSize)
             return false;
    
    -    cloud.data.reserve(imageSize * cloudStep);
    +    cloud.data.resize(imageSize * cloudStep);
    
         uint8_t       *cloudP      = reinterpret_cast<uint8_t*>(&cloud.data[0]);
         const uint32_t pointSize   = 3 * sizeof(float); // x, y, z
    
  3. Log in to comment