optimizedPoses in MapOptimizerNode.cpp

[KamalDSOberoi]

Hello,

I am trying to integrate the pose of artificial landmarks with this package. I am able to get the landmark pose in /odom frame and I pass it to MapOptimizerNode.cpp using the user data in NodeData.msg.

After I compute the connected graph, I get the pose of the robot and the pose of landmarks as it is, but after optimizing in MapOptimizerNode.cpp, the pose of the landmarks increases. That is, it shows the landmarks to be a bit far from the robot in RVIZ, than they actually are.

I think that this is a problem in a way the graph is optimized. I checked the process of creating the graph and I think that it is correct. But optimization process is a bit confusing.

Could you please help me in understanding where the problem might lie? Thanks!!

Regards,
Kamal

[KamalDSOberoi]

To make the problem more clear:
In MapOptimizerNode.cpp:

optimizer_->getConnectedGraph(fromId, poses, constraints, posesOut, linksOut);
After executing this line, I get the pose of the robot as well as the landmarks in posesOut and I get corresponding links in linksOut.

optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
After executing this line, optimizedPoses contains the pose of the robot and landmarks. Robot poses are correct but landmark pose is increased. This is the problem.

Please guide!!  
 

[matlabbe]

Hi,

It depends on the links between poses and landmarks. On optimization, the optimizer will use link's constraint to update the landmark poses. You may want to look at the difference between the link constraints and the resulting distance between the optimizedPoses.

Another way to debug: you could export the graph in g2o format for example, then try g2o app to optimize the graph. You can also change manually values in the file to see their effect on optimization.

#include <rtabmap/core/Graph.h>

// 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
rtabmap::graph::exportPoses("graph.g2o", 4, posesOut, linksOut);

cheers

[KamalDSOberoi]

Hi,

Thanks for the insight. This means that the problem lies in the way the graph has been constructed. However, there are few variables names in map_optimizer node which are a bit confusing.

For example, what is the difference between std::multimap<int, Link> newConstraints and rtabmap::Link link other than the datatypes? I mean do they signify the same thing, that is, links between the nodes or do they signify something completely different? Also there is edgeAlreadyAdded variable. What is the difference between links, constraints and edges, in general?

Also I have similar doubt regarding poses and nodes. Don't these two refer to same entity?

Thanks a lot !!

Regards      

[KamalDSOberoi]

Also, I am creating the link between the robot and landmark pose as follows:

rtabmap::Link link;
link.setFrom(robot pose id);
link.setTo(landmark pose id);
link.setType(type of the link);
link.setTransform(transform between /odom --> /landmarks);

I have following questions:
1. Is this the right way?
2. How do we define the constraint between robot pose and landmark pose?
3. What is the difference between different types of link types?
4. In link.setTransform(), the tf is correct or should it be /base_link --> /landmarks?

Regards  

[matlabbe]

Hi,

links/edges/constraints mean all the same thing. The type of the link is ignored by the optimizer (so all types are all equally used). I think it is because the terminology in graph optimization, they use constraints or edges. rtabmap uses links.

The transform should be between the node referenced by robot pose id and the landmark pose, not /odom --> /landmark. So if you have the pose of the node in the graph and the associated landmark:

Transform t = robotPose.inv() * landmarkPose;
link.setTransform(t);

[KamalDSOberoi]

Hi,

Could you please tell me that after optimization, the optimizedPoses received are according to which reference frame? Is there any way to verify this?

Thanks and regards,
Kamal

[matlabbe]

They are in /map frame.