MapOptimizerNode: mapCorrection

classic Classic list List threaded Threaded
12 messages Options
Reply | Threaded
Open this post in threaded view
|

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
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

matlabbe
Administrator
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?
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

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 

On Mon, Aug 22, 2016 at 3:24 PM, matlabbe [via Official RTAB-Map Forum] <[hidden email]> wrote:
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?


If you reply to this email, your message will be added to the discussion below:
http://official-rtab-map-forum.67519.x6.nabble.com/MapOptimizerNode-mapCorrection-tp1707p1726.html
To unsubscribe from MapOptimizerNode: mapCorrection, click here.
NAML


test.png (1M) Download Attachment
test1.png (1M) Download Attachment
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

KamalDSOberoi
In reply to this post by 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


Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

KamalDSOberoi
And in the images, tf tree is as follows:

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

(just for more clarity)

Regards
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

matlabbe
Administrator
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
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

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
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

matlabbe
Administrator
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
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

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
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

matlabbe
Administrator
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.
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

KamalDSOberoi
Hi,

The definition of the optimizeWithLandmarks() function is here.


Thanks and regards,
Kamal
Reply | Threaded
Open this post in threaded view
|

Re: MapOptimizerNode: mapCorrection

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