ICP inserting 3D range observations produces empty map

[alexge233]

Hi,

I'm using mrpt 1.5 on Ubuntu 16.04 from a ppa. The sensors I am using are: a SICK TIM 510, a SICK TIM 551, and a Kinect V2 (Xbox one). The SICK LIDARs seem to work fine, but I can't get the Kinect to work:

1. I populate the data of a mrpt::obs::CObservation3DRangeScan using depth and point clouds
2. I then insert it into an mrpt::slam::CMetricMapBuilderICP

My problem is that the ICP log output produces:

processObservation(): new pose extrapolation failed, using last pose as is.

and

**No map was updated** after inserting an observation of type `CObservation3DRangeScan`

The actual produced ICP map is empty, which AFAIK means the observation is either not inserted, or is empty itself.

To acquire data from the kinect v2 I use freenect2 using the Protonect example in a similar way:

libfreenect2::FrameMap frames;
mrpt::obs::CObservation3DRangeScan obs;
obs.timestamp = mrpt::system::now();
obs.resizePoints3DVectors(512 * 424);
obs.hasPoints3D = true;
obs.rangeImage_setSize(424, 512);
obs.range_is_depth = true;
obs.hasRangeImage = true;

if (!listen->waitForNewFrame(frames, 10)) {
    throw std::runtime_error("timeout");
}
libfreenect2::Frame * rgb = frames[libfreenect2::Frame::Color];
libfreenect2::Frame * depth = frames[libfreenect2::Frame::Depth];
registration->apply(rgb, depth, &undistorted__, &registered__);
const libfreenect2::Frame * undistort = &undistorted__;

for (unsigned int row = 0; row < 424; ++row) {
    for (unsigned int col = 0; col < 512; ++col) {
        float x, y, z;
        registration->getPointXYZ(undistort, row, col, x, y, z);
        unsigned int i = (row * 512) + col;
        obs.points3D_x[i] = x;
        obs.points3D_y[i] = y;
        obs.points3D_z[i] = z;
        obs.points3D_idxs_x[i] = col;
        obs.points3D_idxs_y[i] = row;
        obs.rangeImage(row, col) = depth->data[i];
    }
}

listen->release(frames);
return obs;

And to insert it, I tried the following:

auto obs = read(); // the above function
obs.setSensorPose(__sensor__); // __sensor__ is the pose of the kinect on the robot
__builder__.insert(obs.Create());  // create a pointer to it and send it to ICP

where __builder__ is my class wrapping around mrpt::slam::CMetricMapBuilderICP:

template <class obs_type>
void icp_builder::insert(obs_type observation)
{
    assert(observation);
    static_assert(std::is_base_of<mrpt::obs::CObservationPtr, obs_type>::value, 
                  "`obs_type` must be ABC of `CObservationPtr`");
    __builder__.processObservation(observation);
}

Am I missing something, or am I doing something wrong?

[jlblancoc]

You probably use an occupancy grid as metric map, right?

3Dscans "don't know" how to get inserted into a 2D gridmap directly, hence they are ignored. You could ADD a pointsmap to your multimetric map, and you would have 2 maps: 2D gridmap, and 3D point cloud.

[alexge233]

Hi, thanks for the reply!

So I should create the map which icp will use instead of relying on the default one, add a points map to it, and then insert 2D and 3D observations?

[jlblancoc]

If you use icp-slam, as I think you are doing, take a look and compare the map definition sections of these configuration files: https://github.com/MRPT/mrpt/tree/master/share/mrpt/config_files/icp-slam

Compare the one named "...classic" with the "...gridmap" one. That is the easier way to define metric maps via a configuration file.

The icp-slam engine inserts the observations in all maps automatically as the robot localizes and builds the map.

[alexge233]

@jbriales Hi, thanks for the help! I've seen most of the configuration files, and played around with different map types. I've ended up using an octo map, an occupancy grid map and a point map.

The warning message is now gone, although the following one persists:

processObservation(): new pose extrapolation failed, using last pose as is

The problem is that the gridmap and the 3Dscene file from the octomap are exactly the same, e.g., there is no 3rd dimension, but only 2D. I'm visualizing using the SceneViewer3D, but I am puzzled.

I even opened the *.bt file (Octomap binary tree?) in octovis and I am still looking at a 2D map.

What am I missing here? Ideally, I'd like to build a map similar to Octomap and be able to visualise it and use it for collision avoidance.

edit

I've changed the way I'm inserting 3D observations, and now I am getting an interesting warning:

Ignoring ICP of low quality: 0

Instead of:

mrpt::obs::CObservation3DRangeScan obs = read();
obs.setSensorPose(__sensor__);
__builder__.insert(obs.Create());

I'm trying:

mrpt::obs::CObservation3DRangeScan obs = read();
obs.setSensorPose(__sensor__);
auto ptr = mrpt::obs::CObservationPtr();
ptr.setFromPointerDoNotFreeAtDtor(&obs);
__builder__.insert(ptr);