search

cartographer-project / cartographer_ros

Provides ROS integration for Cartographer.
Apache License 2.0
1.65k stars 1.2k forks source link

Getting 3D data #720

Closed nitramek closed 6 years ago

nitramek commented 6 years ago

Hello,

I would like to make obstacle checker (via raycasting or something like that probably) for cartographer in 3D. I know you dont publicize OccupancyGridMap in 3D (via #330 ) I have tried to call submaps by their ids from SubmapList, but it seems like there are only low and high resolution slices.

My question is where I can get the 3D data from cartographer? Either in ROS way or within cartographer itself.

Is it even possible to do now? Thanks.

gaschler commented 6 years ago

Currently, cartographer_ros does not publish 3D maps and there is no high-level interface.

Cartographer itself gives you access to all submap data with PoseGraphInterface::GetAllSubmapData. Return data includes the protobuf data cartographer.mapping.proto.Submap3D which has mapping_3d.proto.HybridGrid high_resolution_hybrid_grid.

For offline processing, there is a nicer solution. You can use the asset writer to write a point cloud from the entire map.

(Closing as answered.)

andrea-pallini commented 6 years ago

Hello...

There is perhaps an update on this topic: "Publishment of a 3D global map - online".

I am diving into the code to collect a global map and publish it continuously at a very low frequency and update it after pose optimization. I was curious if anyone else is already working on this issue.

Thank you!

Sanderi44 commented 6 years ago

Hi @andrecmilano, have you have made any progress on this? I would love to see what you have done! Getting a 3D map in real-time would be perfect for my application.

Thanks!

andrea-pallini commented 6 years ago

Hello Sanderi,

I am working part-time on this project indeed the progress is quite slow. Anyway, I am still struggling with this issue I am evaluating how useful it may be. At the end may not be necessary to publish the 3D map online, but I still have to figure it out how it is saved; if it is contained in the PoseGraph class or in MapBuilder.

It may require few more weeks of tests and debug.

Feel free to help me with this ;) I guess you are also working on SLAM with MAVs :)

Cheers

nitramek commented 6 years ago

If this is any use to you I have created my fork or cartographer_ros and made similar service to SubmapQuery I call it SubmapCloudQuery and it fetches 3D data from Cartographer itself. I think it is not very efficient but maybe it will be useful to you guys somehow

https://github.com/nitramek/cartographer_ros/tree/submap_cloud_query

I publish one submap as PointCloud2 which can be used for visualization in RViz in boxes or it can be processed with PCL anyway you want and because it is a service you can update that whenever you want.

Service signature: SubmapCloudQuery.srv Relavant code

Also I publish the submaps it in fixed frame it was more useful for my use case. You need to transform it to global map location yourself.

andrea-pallini commented 6 years ago

Thank nitramek, I will check it! ;)

andrea-pallini commented 6 years ago

Hello @nitramek

Finally I had time to test your code. When I run your service I receive this output:

submap_version: 42 resolution: 0.0 finished: False cloud: header: seq: 0 stamp: secs: 1528732114 nsecs: 852897343 frame_id: "map" height: 1 width: 0 fields:

  • name: "x" offset: 0 datatype: 7 count: 1

  • name: "y" offset: 4 datatype: 7 count: 1

  • name: "z" offset: 8 datatype: 7 count: 1

  • name: "intensity" offset: 12 datatype: 7 count: 1 is_bigendian: False point_step: 16 row_step: 0 data: [] is_dense: Trueo.

I structured the request using these parameters:

rosservice call /submap_cloud_query "trajectory_id: 0 submap_index: 1 min_probability: 0.0 high_resolution: true"

Before to call your service I also called _/finishedtrajectory.

Unfortunatly I noticed that the "hybrid_grid.values_size()" returns zero, because cloud.width is zero.

Do I make any mistake? Do I miss something?

andrea-pallini commented 6 years ago

Update: Sorry... now I can read the data. I debugged a little and now it works, changed small things.

ojura commented 6 years ago

@nitramek Nice work. It would be great if something like this was merged into the Submaps plugin.

nitramek commented 6 years ago

Thanks @ojura, although I am not sure if this approach is good enough to be considered for merging. Also at the time I made it making it polished enough for PR was a last thing to think about.

ojura commented 6 years ago

Even if you think it's not currently the best approach, it's still better than nothing :) I actually saw 3D submaps for the first time yesterday thanks to your hack. It really gives you a better insight into what's going on inside Cartographer.

Duke-Allen commented 5 years ago

@nitramek Hi, I have been running 3D cartographer successfully with no error by a 16-lines LIDAR and a MPU6050. But the track is inconsistent with the real movement, like this: rviz_screenshot_2019_03_16-10_51_11 The result of bag is:

Here is my bag. The account: 15838868117 Password: 123456789 demo_rslidar_3d.launch rslidar_3d.launch rslidar_scan_3d.lua mydemo_3d.rviz rslidar_2d.urdf

Any suggestions would be highly appreciated!