Integrate artificial landmarks with RTAB-Map

[KamalDSOberoi]

Hello Mathieu,

I want to localize artificial landmarks in the point cloud. I get the pose of the landmarks using which I calculate the pose of the robot. Therefore, I have the robot pose using rtabmap (kinect) as well as using artificial landmarks.

Anyway, I visualize the landmarks in the point cloud using the landmark pose information. When the robot moves, it assembles a new point cloud. But the landmarks are not visualized in the new point cloud. They still are in the old point cloud.

I have two questions:
1. Is visualization (of tf world --> landmarks) and localization of the landmarks in RVIZ the same thing?
2. Do you have any idea which can help me in visualizing/localizing the landmarks in the latest point cloud?

I am not sure if the explanation of the problem is clear, but I would really appreciate any guidance. Thanks in advance !!

Cheers,
Kamal  
         

[KamalDSOberoi]

I optimized the graph using graph optimization inside rtabmap node as well as using map_optimizer node.

Using the rtabmap node, when the new point cloud is assembled, the landmarks are not localized in it. Instead they are localized in the first point cloud that was assembled.

However, when I use map_optimizer node, it localizes the landmarks in the latest point cloud. The problem is that since map_optimizer node crashes a lot of times (not at startup but after sometime), I have to use the graph optimization inside rtabmap node.

Hence I want to know how can I localize the landmarks in the latest point cloud just using rtabmap node?

Thanks for any help!!      

[matlabbe]

Hi,

If you disabled map optimization in rtabmap node, the point cloud you have is the odometry one. I am not sure what you mean by old and new point clouds. Do you mean not optimized and optimized point clouds?

To track down where map_optimizer is crashing, you could set output="screen" in the node tag of map_optimizer in your launch file. Also, add this at the beginning of the "main" function in map_optimizer to see more logs:

ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kDebug);

For rtabmap node, optimizing with added landmarks in the graph, deep modifications would be required. So I would stick with the map_optimizer approach.

cheers

[KamalDSOberoi]

Hi,

When everything is started, a point cloud is acquired. After that, when the robot moves  a new point cloud is acquired. This is what I meant by old and new point clouds.

I am not sure if the point cloud displayed in rviz is already optimized or not by default. I have disabled the map optimization in rtabmap node and using map_optimizer node with added landmarks.

Is the point cloud shown in rviz already optimized?

And regarding the crashing of map_optimizer node, thanks for the idea. I will implement it and see what happens.

Regards    

 

[KamalDSOberoi]

Hi,

So I made some tests and I found out why map_optimizer is crashing. In map_optimizer node, when the mapData is received and the callback function is executed, I verify the msg->graph.links.size() and msg->nodes.size(), which is received in the mapData.

Sometimes it so happens that even when the robot is stationary, the msg->nodes.size() = 2 and msg->graph.links.size() = 0. This doesn't give any error but the crashes the node. Actually this should be msg->nodes.size() = 1 and msg->graph.links.size() = 0. When the robot moves and new mapData is received, then msg->nodes.size() = 2 and msg->graph.links.size() = 1.

What do you think about this?

Regards  

[matlabbe]

In RVIZ, make sure you set fixed_frame to /map in the global option, otherwise, point clouds will just be added on over the other.

Do you have this error shown when poses=2 and links=0?

ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
					   "not be null if there are edges, and edges must be null if poses <= 1)",
					  (int)poses.size(), (int)constraints.size());

[KamalDSOberoi]

Hi,

Yes I used to get this error but I used an error handler to remove the extra node if the edge has not been added. I think that this stopped the error but the map_optimizer node started crashing. Why does this error occur (even if the robot is not moving)? And is there any work around?

Thanks!!

Regards

[matlabbe]

If you keep map_optimizer code without modification, is it still crashing? This is just to see if I can reproduce the crash with the current code, that would help to see the problem.

[KamalDSOberoi]

Hi,

I checked with the unmodified version of the map_optimizer, it doesn't crash. Also when I use my code and localize the landmarks in the point cloud, if I rotate the robot by some angle, the new point cloud acquired doesn't combine with the previous point cloud properly. Also the "odom" frame sometimes moves and/or rotates. Does this mean that it detects the loop closure even if there is no loop?    

Regards

[matlabbe]

Hi,

If the transform between /map and /odom changes, that means there is a loop closure or a proximity link added to map. A "loop closure" is just a term saying that rtabmap founds 2 similar images and link them together, adding a new constraint in the graph.

 if I rotate the robot by some angle, the new point cloud acquired doesn't combine with the previous point cloud properly

Odometry can drift, which may cause that consecutive point clouds won't exactly superpose together. Depending on your sensor and and the noise in its depth image, the amount of odometry drift may vary depending on the distance between the camera and the objects.

cheers,
Mathieu

[KamalDSOberoi]

Hi,

Now I have removed the error handler which I wrote and I am getting this error. To find a solution I need to understand how the poses and links are created.

In map_optimizer node, as soon as I receive the mapData and the mapDataCallback() function is executed, I check the number of nodes and links present in the mapData. There itself I see that poses=2 and links=0, hence the error.

So where are the nodes and links created and added to the mapData and mapGraph, which is received in map_optimizer node? In CoreWrapper.cpp, there is a callback function publishMapCallback(), in which I thought that nodes and links are created. But this callback is never executed. Instead, in my opinion, publishStats() publishes the mapData and mapGraph. Am I right?

Where should I check for the root of this error? The landmark nodes for the artificial landmarks are created in map_optimizer node. I dont think that this error is because of that. What do you say?

Thanks for the help!!

Regards  

 

[matlabbe]

Hi,

I don't remember where we discussed that, but when I tried the original map_optimizer I had this error too at the beginning. However, then I started moving, the error didn't show up anymore. In reality, I think this is because rtabmap node changed slightly what is published: always sending last data even when not moving, thus even when there are no edges are added to graph. The error can be safely ignored when poses != 0.

cheers

[KamalDSOberoi]

Hi,

So is there any work around to avoid this error?

Regards

[matlabbe]

I updated the if: https://github.com/introlab/rtabmap_ros/commit/0930c816ba214031b6f4787e8ea7391620c3d71c

cheers

[KamalDSOberoi]

Hi,

After this update, I dont get the error as before. But it hasn't solved the problem. Still in the mapData received in map_optimizer node, a second node has been added but the link between the previous and new node is not added and I get the assertion error. Is there any way that I can check that where in the code the nodes and links are added in the mapData, which is received in the map_optimizer node?

Thanks and regards  

[matlabbe]

I didn't have the assert problem when I tested the other time... Well, the mapData is published here.

[KamalDSOberoi]

Hi,

It works for me now. Thanks a lot.

Regards