Generating occupancy map on iOS app?

[Li357]

I'm trying to generate an occupancy map using the iOS example on the iPad with LIDAR. I'm not sure what I am doing wrong but I am getting these weird lines and lots of empty space:

This is the 3D scan I'm generating the grid from (I guess there's a slight resemblance?):

I eventually want an output similar to what is found here where an occupancy map is created in a pgm format. Following tools/Reprocess which seems to generate a pgm given a database, https://github.com/introlab/rtabmap/blob/master/tools/Reprocess/main.cpp#L688-L731, I wrote in jni/RTABMapApp.cpp:

void RTABMapApp::addToGrid(const rtabmap::Statistics& stats) {
      if (grid_.addedNodes().find(stats.getLastSignatureData().id()) == grid_.addedNodes().end()) {
          if (stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f) {
              cv::Mat groundCells, obstacleCells, emptyCells;
              stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells);
              grid_.addToCache(stats.getLastSignatureData().id(), groundCells, obstacleCells, emptyCells);
          }
      }

      if (grid_.addedNodes().size() || grid_.cacheSize()) {
          grid_.update(stats.poses());
      }
  }

where grid_ is an rtabmap::OccupancyGrid with default parameters. Then I call this method in handleEvent:

if(event->getClassName().compare("RtabmapEvent") == 0)
{
    LOGI("Received RtabmapEvent event! status=%d", status_.first);
    if(status_.first == rtabmap::RtabmapEventInit::kInitialized)
    {
        boost::mutex::scoped_lock lock(rtabmapMutex_);
        rtabmap::RtabmapEvent *rtabmapEvent = (rtabmap::RtabmapEvent *)event;
        rtabmapEvents_.push_back(rtabmapEvent);
        addToGrid(rtabmapEvent->getStats()); // <-- here
        return true;
    }
    else
    {
         LOGW("Received RtabmapEvent event but ignoring it while we are initializing...status=%d", status_.first);
    }
}

Then when saving the map I have this code to print the map:

float xMin, yMin;
cv::Mat map = grid_.getMap(xMin, yMin);
cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);

But I am something similar to the image above. I also tried looping through poses after saving the database and generating the grid that way with the same image result. Is there something extra (parameter-wise) I need to generate a full map when using LIDAR/RGBD data? How would I correctly generate a 2D occupancy grid in the iPad example?

[matlabbe]

Hi,

This code: if (stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f) { will be always false in iOS App because we don't create occupancy grids by default. You would have to enable local occupancy grids generation somewhere here: parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), std::string("true")));

You would have to update also the Grid parameters to create something like below (see parameters on left I used on this scan):

cheers, Mathieu