OmniMapper-Demo with Hardware-Acceleration

[skasperski]

Hello,

I am trying to run the OmniMapper-Demo after installing it from Docker and building the Nvidia-Version, unfortunately with no success yet. After I run the demo with the docker/nvidia/run.sh script, RVIZ still gives an error about having no OpenGL available. I have the following docker-images on my machine:

cogrob/omnimapper-nvidia 331.113 559db9bb2188 23 minutes ago 3.276 GB cogrob/omnimapper-nvidia latest 559db9bb2188 23 minutes ago 3.276 GB cogrob/omnimapper-demo latest 12e1913d7242 24 hours ago 2.957 GB cogrob/omnimapper-dox latest ddac6871e880 5 days ago 2.585 GB cogrob/omnimapper-gui latest 55515cd39e2d 5 days ago 2.472 GB cogrob/omnimapper-dev latest 118214defd55 5 days ago 2.309 GB cogrob/omnimapper-dep latest ebb9d1383365 5 days ago 2.183 GB ubuntu 14.04 5ba9dab47459 13 days ago 188.3 MB training/webapp latest 31fa814ba25a 8 months ago 278.8 MB

Did I miss something else?

Thanks in advance, Sebastian

[skasperski]

Ok, I got it running now. Looks like the run.sh never actually started the omnimapper-nvidia container. When I explicitely docker-run this container, it works for me.

[ruffsl]

Hello @skasperski,

If you look at how I call the run script in the wiki bash run.sh cogrob/omnimapper-demo terminator I left the image tag name and command to run in the container as arguments for the user to specify, these arguments are the same format as with the docker run command, as that is what it's simply passing them to.

The run scrip is simply a convent way to specify the numerous arguments needed to config container. Also note that you may different paths for different/additional Nvidia devices. So see the note about that here.

So are you getting native speeds? As this is docker stuff is still a bit beta, I haven't had much of chance to test other systems types other than what I have available my the lab. Let me know if have any further questions/suggestions.

[skasperski]

Just 2 things that come to mind:

• The automated download of the nvidia-driver didn't work for me, so I had to do this manually. (The created URL was not available.)
• I had to add a "RUN sudo apt-get update" in the Docker-file before the "apt-get install"-line

About the performance in the Docker-Environment I am not so sure. RVIZ seems somewhat slow, but I haven't build the omnimapper-package from source to compare it against a natively running system.

Best regards, Sebastian

EDIT: Nevermind, RVIZ runs smooth like a charm. Looks like my notebook is a little busy with the OmniMapper running! ;)

However, the service call to draw_icp_clouds returns "done: False" and produces these kinds of errors:

name: x_188
ERROR: REQUESTED SYMBOL WITH NO POINTS!
Map Tform:     0.999956   0.00942954    1.424e-05       1.0028
 -0.00942954     0.999956  5.88521e-06  -0.00741739
 -1.41839e-05 -6.01922e-06            1  0.000333334
           0            0            0            1
name: x_189
[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.

[ruffsl]

Looks like when your running the service call, some of the pose symbols it iterates over may not have a cloud associated to the pose in the trajectory to render. This may be the case when no depth image was captured around the same time a pose symbol was generated from odometry or a different plugin. Perhaps Alex could chime in if he has a better explanation. @atrevor

[atrevor]

I'm missing context for what exactly you're running, but that service call should cause point clouds recorded by an icp plugin to be rendered to RViz. If you're using the ICP plugin in addition to other plugins, there may not be clouds available for some poses. This isn't a problem -- it will render the available ones. If nothing is displayed, I'd double check that the ICP plugin is receiving point cloud data.

[skasperski]

Okay, thanks for the hints, I got the demo running now and see the map build from pointclouds.

I am currently trying to build a mapper in an ROS independent environment using only the mapper-base with only an ICP plugin. Are there any docs on how to use the bare CPP-API from omnimapper? I am calling icpPlugin->cloudCallback(cloud), but I don't know how to receive the map and the pose of the robot.

Thanks in advance, Sebastian

[ruffsl]

Hmm, I've been interfacing with OmniMapper using the ROS wrapper for majority of my time. But perhaps you should look at sections of the ICP Plugin's callback in the ROS wrapper to get a sense of what to call. Specifically how publishMapToOdom , publishCurrentPose, and add_pose_per_cloud_ are used.

To get a map from the resulting ICP cloud, you'll want to look at the omnimapper_visualizer_rviz.cpp. This is uses ROS messages, so we keep it in the ROS wrapper side. See here where the draw_icp_clouds_ flag is being used to build up the resulting cloud. The important loop show here is the one that loops over each cloud in the plugin to build the a map with the aggregate_cloud from the latest optimized poses every time a service call is made.

[skasperski]

It seems like my clouds are not added to the mapper correctly. Whenever I call

gtsam::Values current_solution = mapperBase->getSolution();

the returned solution has only 1 Value, and this value has no cloud (ERROR: REQUESTED SYMBOL WITH NO POINTS!). Do I have to call anything else when adding a new cloud?

[ruffsl]

Could you enable the debug_ flag in the icp_plugin? I'd like to know how far the ICP plugin is getting at adding factors to the mapper_.

My other thought is that, you are not adding a pose per cloud as show here when using the add_pose_percloud in the cloud callback for the ROS wrapper.

[atrevor]

When using the base directly (not through ROS), you'll need to also call spinOnce() or spin() to commit poses and optimize. If you're just using the ICP plugin alone, you can call spinOnce() immediately after adding a cloud.

[skasperski]

I added the call to spinOnce and set debug=true:

cloud callback current cloud points: 122944 ICPPosePlugin: No sensor to base transform exists! ICP Plugin: Getting symbol for current time: 2015-Feb-20 09:47:46.482913 OmniMapperBase: Making a new entry OmniMapperBase: need new symbol at: 2015-Feb-20 09:47:46.482913 ICP Plugin: current symbol: 15, inserting cloud ICP SYMS: prev3: x12, prev2: x13 prev: x14, curr: x15 Starting icp... Cloud1: 0 Cloud2: 0 Cloud1 stamp: 0 Cloud2 stamp: 0 ICP did not converge! ICPTest: done with cloud! ICPPoseMeasurementPlugin: Added a pose! OMB: spinning... latest: x 0 to commit: x 1 OmniMapper: Commiting... OmniMapper: Tried to commit without any between factors! Waiting for between factor! Node has 0 factors OMB: No new factors to optimize Solution has 1 values. ICPPlugin: In getCloudPtr! ERROR: REQUESTED SYMBOL WITH NO POINTS! Map Tform: 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 name: x_0

[ruffsl]

Looks like ICP did not converge! is the problem here. If this is being printed, then it failing the if case here, and because of that it never gets the chance to add the between factor for the two poses where the clouds where observed.

You could twiddle with the icp_score_threshold argument or the variable that sets it, 'scorethreshold' by making it more tolerant, or bypass that if case entirely just to see if that's your limiting component.

By how much do you expect these point clouds to differ in pose between frames?

Sometimes ICP just fails, and to handle this we have a no_motion_pose_plugin to simply add uncertain identity transform between pose in the pose chain to ensurer we're never left with a disjointed graph.

Another thing from the ICPPosePlugin: No sensor to base transform exists!. We use this sensor to base transform to align the cloud with the base frame of the robot, the point of the robot that the pose chain is being optimized for. Given this we can give ICP a good initial on the alignment of the two clouds from the most recent optimized trajectory.

In the wrapper we specify a sensor_to_base functor to be set to the ICP plugin. This not as necessary though if you are treating the origin of the sensor as the base frame, like for the hand held demo.

[skasperski]

Thank you a lot for taking your time to look into this!

I did some try-and-error and it does not seem to be related to this threshold. Instead registerClouds returns false because there are no points in both clouds. (Also according to this debug-message: Starting icp... Cloud1: 0 Cloud2: 0)

How can this be, when in the beginning the number of points seems ok (current cloud points: 122944)?

[ruffsl]

So, you start at an instance where you have a full cloud given to the callback, those 122944 points, but then you get to a point where you begin to add empty points clouds to the ICP plugin's callback? Does the message of zero sized clouds come up instantly or after a few iterations?

Could you put a check on where your calling the ICP callback, such that is just skips calling the ICP plugin to begin with if the clouds your handling are under a minimum size? That way, zero sized clouds are not accidentally added to ICP plugin's clouds_ map to begin with. I'd like to isolate the issue and tell if it's up stream or down stream from the plugin.

You could also examine the CloudPtr current_cloud_base just before the clouds.insert() function call to peek and see if the cloud has some size before being added to the map.

[skasperski]

All clouds have around 120k points when given to the ICP-plugin. I added a check at the line you mentioned, and the current_cloud_base has always 0 points.

[ruffsl]

I have two ideas.

Check at the beginning the plugin processes by making sure that the ICP plugin's callback is getting a valid cloud to start with. So make sure the global currentcloud is in fact getting loaded with the cloud you gave the callback. Your giving it a proper CloudPtr cloud typedef as defined here, correct?

My other thought is that the pcl::transformPointCloud () function is returning an empty cloud because sensor_to_base is empty as no sensor to base functor was set with the setSensorToBaseFunctor() setup function.

[skasperski]

The typedef in ICPPoseMeasurementPlugin is private, so I am using an equivalent definition (pcl::PointCloud) like it is done in your ros node here.

The second thing really was a problem! When the error message is printed, no action is taken. After I added

current_cloud_base = current_cloud_filtered;

the ICP definitely works. (Odometry transform is calculated and looks very reasonable) However, I still don't see the map_cloud. The map-cloud creation produces one of these error blocks for each PointCloud in the map:

name: x_0
ICPPlugin: In getCloudPtr!
ERROR: REQUESTED SYMBOL WITH NO POINTS!
Map Tform:      0.99999845 -6.23939268e-05   0.00174709689      1.30163693
 6.30492723e-05      0.99999994 -0.000375053642   0.00272959447
 -0.00174707337  0.000375163247     0.999998391    0.0152680278
              0               0               0               1

I put all this code in a very small ROS-Node. If it is okay I can send it to you, maybe you can just see what is missing at a glance. Maybe you want to close this issue, as my original question about the Docker-Setup has been solved anyway.

Best regards, Sebastian

[ruffsl]

Sounds good, I'll close this issue then. Send me a link with your code and I'll take a look for you.