MapOptimizerNode: mapCorrection

[KamalDSOberoi]

Hi,

The problem which I think I told you about that when the robot rotates, the new point cloud is not combined with the old point cloud properly, is because after optimization the /map --> /odom tf is not zero. (False loop closure detection)

In map_optimizer node, the mapToOdom variable takes the value of mapCorrection. So my question is that what is the logic behind calculating mapCorrection? And how can this value be improved?

Thanks and Regards

[matlabbe]

Hi,

The /map -> /odom transform contains the transform that correct odometry when a relocalization with the map occurs (loop closure detection). Can you share rtabmap.db so we can see if there is a wrong loop closure detected?

[KamalDSOberoi]

Hi,

I tried to save the data in the rtabmap.db file, but was not able to view
it using rtabmap-databaseViewer. So I have attached the screenshots of the
problem. In the "info" field in rviz, we can see that it says that no loop
closure has been detected, but the /map -> /odom tf is not zero. When I
rotated the robot, /base_link rotated as expected. But after optimization,
the /map -> /odom became non zero, the point cloud was not combined
properly and /base_link became straight but /odom rotated. This is the
problem which I am facing.

Also could you please explain how we have to use the data recorder and
database viewer tools? When we have to start recording the data and how can
it be seen in the database viewer?

Regards,
Kamal

[KamalDSOberoi]

Hi,

I tried to save the data in the rtabmap.db file, but was not able to view it using rtabmap-databaseViewer. So I have attached the screenshots of the problem. In the "info" field in rviz, we can see that it says that no loop closure has been detected, but the /map -> /odom tf is not zero. When I rotated the robot, /base_link rotated as expected. But after optimization, the /map -> /odom became non zero, the point cloud was not combined properly and /base_link became straight but /odom rotated. This is the problem which I am facing.  

Also could you please explain how we have to use the data recorder and database viewer tools? When we have to start recording the data and how can it be seen in the database viewer?

Regards,
Kamal

[KamalDSOberoi]

And in the images, tf tree is as follows:

/map -> /odom -> /base_link
/map -> landmarks

(just for more clarity)

Regards

[matlabbe]

Hi,

If no loop closures are detected, it is the optimization side the problem (it is map_optimizer that publishes /map->/odom correction). Also, to which MapCloud are you subscribed? It seems you have 2 MapCloud plugins enabled in RVIZ. If you are optimizing outside rtabmap node, like with map_optimizer node, you have to subscribe to mapData_optimized instead of mapData.

Can you compare MapCloud output from rtabmap node (/rtabmap/mapData) and MapCloud output from map_optimizer (/rtabmap/mapData_optimized)?

cheers

[KamalDSOberoi]

Hi,

Thanks for the idea. I compared both the outputs and you are right. Because of the error in calculating mapCorrection in map_optimizer node, this problem is happening.

So my question is that what is the logic behind calculating the mapCorrection? What I understand is:

mapCorrection = (optimized robot pose(in /map frame)) * ((un-optimized robot pose(in /odom frame).inverse)(in /base_link frame))

Is this correct? If yes, then what could be the reason that I get the wrong result?

Thanks and Regards,
Kamal

[matlabbe]

Hi,

With TF /map -> /odom -> /base_link, we should get:
/map->/base_link = /map->/odom * /odom->/base_link
or
PoseInMapFrame = mapCorrection * PoseInOdomFrame

so

mapCorrection = PoseInMapFrame * inverse(PoseInOdomFrame)

In the code, we compute the error of the last pose in the graph (assuming that graph is optimized from the first pose):

// mapCorrection = PoseInMapFrame * inverse(PoseInOdomFrame)
mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();

cheers

[KamalDSOberoi]

Hi,

Yes, I am doing the same thing.

But I am using the function optimize() defined in OptimizerG2O.cpp for optimization. In an older version of OptimizerG2O.cpp, there was a function called optimizeWithLandmarks(). If I use that function (since I have older version), it seems to work a little better.

What is the difference between their working and which one would you recommend?

Thanks and Regards,
Kamal

[matlabbe]

Can you link the version of the file, I don't remember optimizeWithLandmarks() method, so I cannot compare. Maybe it is on a fork of someone else.

[KamalDSOberoi]

Hi,

The definition of the optimizeWithLandmarks() function is here.


Thanks and regards,
Kamal

[KamalDSOberoi]

Hi,

I just found out that this function was created by the engineer who was working here before me. So there is no need to check that link. Sorry for the confusion.

Thanks and Regards,
Kamal