Losing odometry when doing fixed point scanning

[scanboy]

That database is over 500MB but I have uploaded another smaller (link sent in private message) that could be related to the problem I stated above
In this smaller DB there are 3 overlapping scans (little bit over 1 m apart) but I'm not sure what is actually happening to the third scan (and possibly the 2nd too...) after reading the log

if(mRM.getWMSize() != 0 && mRM.getStatistics().loopClosureId() == 0) { printf("Could not merge scan to previous scans!\n"); } Does not print so I assume that processing and adding went well but when looking at the log I see 3 Added pose where the two first are at 0. Equally I see 3 Set map correction

So what has happened to scan 3 did it get added? if so what is its pose
If I do this
mRM.get3DMap(signatures, poses, contraints, true, false);
printf("sign size:%ld\n", signatures.size());
printf("sign size size:%ld\n", poses.size());
printf("constraints size:%ld\n", contraints.size());
for (int i = 0; i < poses.size(); i++)
{
        printf("p:%d %s\n", i, poses[i].prettyPrint().c_str());
}
I get this:
sign size size:3
sign size size:3
constraints size size:2
p:0 xyz=[null] rpy=[null]
p:1 xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
p:2 xyz=-0.037204,-1.111841,0.000000 rpy=0.000000,-0.000000,0.024257
p:3 xyz=0.072560,-0.996759,0.000000 rpy=0.000000,-0.000000,0.044889

But in reality, 3 is not this close to 2.

[matlabbe]

I didn't receive the private msg, can you try again? Nevermind, I've found it.

[matlabbe]

Hi,

between 2 and 3, the motion estimation thinks that the camera didn't move. The environment is quite symmetric:

(two different locations)

As the depth sensor doesn't see far (to see distinctive features in the background), visual features are mostly extracted from the close symmetric area. It this case the visual features are wrongly matched. By changing Vis/MinDepth to 2 meters to ignore some close symmetric features, I am able to get a correct transformation between 2 and 3:
xyz=0.007153,-1.211038,0.000000 rpy=0.000000,0.000000,0.020160

There are some other problems in the scan. There are a lot of artifacts in the depth images:


See the small bright pixels in the depth (meaning closer pixels) along the edges. Were they streamed using JPEG compression? Compress them in PNG instead. If you use ROS, make sure to use compressedDepth image_transport plugin to stream depth images.

The local transform for the top camera seems a little off, causing a second ceiling with offset around 10/15 cm:


cheers,
Mathieu

[scanboy]

Hi,

I managed to get much better transformations setting the MinDepth to 2m too.
EDIT: how can I reject transformations/points that are to close to the previous point (or perhaps any point)? kRGBDLinearUpdate does not reject for me when I'm using it


These bright and white pixels are saturated to 2^16-1=65535 correct?
But I looked at the histogram of several individual depth images and the row of images for the multi-camera and I cannot see any values in that high range. How can this be? The depth images are definitely 16bit PNG directly from the API of the depth camera.

When I put my multi-camera together I can see a "spray" as you show in "a lot of artifacts" originating from the camera center (as seen in the database viewer). Are these artifacts from the single depth images or from the montage of the multi-camera? (like the double ceiling)

Most optimization parameters for e.g. bundle adjustment, transformations in RTABMap are for single camera SensorData, what would you suggest I use for a multi-camera Sensor Data?

Thanks!

[matlabbe]

To reject poses that are overlaying (wrongly), you may check the loop closure link associated with the latest node added. If the transform translation is under a fixed threshold, you can remove the link with rtabmap. rejectLoopClosure(). However the node will still be added to database and next nodes added could link with it (there is a missing public deleteLastNode() method on rtabmap class to do that). The behavior you are looking for (delete last added node if loop closure is too small) is very like the parameter RGBD/LinearUpdate, but it only works with odometry links. For loop closure links, you may have to modify the code. Copy code from there to there (setting rejectedHypothesis to true along smallDisplacement). The latest node should then be removed if transform's translation of the loop closure is under RGBD/LinearUpdate.

Brighter pixels means closer to camera. It is more like an interpolation between 0 and the value on the edge (e.g., 3 meters). Id it is very similar to when we compress in JPEG, the edges are soften, creating false depth values. The artifacts can be found on single depth images, it is not a multi-camera problem.

Having the multi-camera setup correctly calibrated and without the depth artifacts on the edges, it could be possible to refine the transformation with ICP (Reg/Strategy=2 and Icp parameters tuned) by providing in the SensorData a voxelized point cloud as LaserScan. Bundle adjustment could work with a multi-camera setup, it is just not implemented.

cheers,
Mathieu

[scanboy]

For RGBD/Linear update I implemented it using distance instead of checking x || y || z. For me it is much more intuitive as the difference of one of x,y can be close to 0.

To avoid getting non-connected loop closures in the DB I could use the parameter kRtabmapStartNewMapOnLoopClosure=true and could remove triggerNewMap from my own code, correct? If it is so I would help not having to implement the "missing public API", deleteLastNode()

I exposed deleteLocation from Memory in rtabmap class like this to have a general remove function (both for wrongly overlayed points and unwanted for other reasons like low hypothesis):
void Rtabmap::deleteLocation(int id)
{
UDEBUG("deleteLocation=%d", id);
if (_memory)
{
_memory->deleteLocation(id);
}
}

But the database still has the same size as if I don't use it which make me suspect that it is still there. I see this by running the same dataset and deleting in one session but not in another.
What am I missing here?
Memory is not DB per se but I assume in some way that it would be transfered to long term memory aka the DB. I'm yet to fully look into ICP. Thanks!

[matlabbe]

Hi,

All created nodes are saved in the database by default (this is a safety feature when a robot shutdown or crash while mapping to be able to recover the map). When deleting the location after rtabmap has been processed the data, we have to set Db/Sqlite3InMemory to true so that there is no backup already saved in the database. We also need to set Mem/NotLinkedNodesKept to false to delete the node without saving it to database. Rtabmap/StartNewMapOnLoopClosure can be set to true to make sure a new node is always linked to a previous one in the database, otherwise it is discarded.

If I take the example from this post, we add:

1 2 3

detectParams.insert(ParametersPair(Parameters::kRtabmapStartNewMapOnLoopClosure(), "true")); detectParams.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "true")); detectParams.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));

and modify this section to delete nodes that moved less than 10 cm:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24

rtabmap.init(detectParams, "testMultiSession.db"); reader.init(); data = reader.takeImage(); while(data.isValid()) { rtabmap.process(data, Transform::getIdentity()); if(rtabmap.getWMSize() != 0 && rtabmap.getStatistics().loopClosureId() == 0) { printf("Could not merge scan to previous scans!\n"); } else if(rtabmap.getStatistics().loopClosureId()) { float norm = rtabmap.getStatistics().loopClosureTransform().getNorm(); printf("norm=%fm\n", norm); if(norm < 0.1) { rtabmap.deleteLastLocation(); } } rtabmap.triggerNewMap(); data = reader.takeImage(); } rtabmap.close(); printf("Closed testMultiSession.db\n");

Note that above I added Rtabmap::deleteLastLocation() function for convenience in this commit.

cheers,
Mathieu