introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.78k stars 787 forks source link

Getting point cloud data from .db file #962

Open takahiko-hasegawa-hdjp opened 1 year ago

takahiko-hasegawa-hdjp commented 1 year ago

I am currently trying to build my debug GUI tool for rtabmap. IM trying to get all point cloud data from .db map file and vizualise it in 2D.

Im looking at the table called Node and pose tag to get the poses first but for some reason it seems like what Im getting is just the data BEFORE loop closure happened.

How do we properly extract the point cloud data and the graph data from the data base?

matlabbe commented 1 year ago

You can get the optimized poses from Admin/opt_poses: https://github.com/introlab/rtabmap/blob/ff61266430017eb4924605b832cd688c8739af18/corelib/src/resources/DatabaseSchema.sql.in#L124

To generate the point cloud, you will need the corresponding depth image from Data table for each node with the calibration data. The depth image is in PNG format (which can be decompressed with opencv cv::imdecode()), and the calibration format depends on rtabmap version, but latest one uses this deserialization function: https://github.com/introlab/rtabmap/blob/ff61266430017eb4924605b832cd688c8739af18/corelib/src/CameraModel.cpp#L539

Are you using C++? If so, I recommend to use dependency on find_package(RTABMap REQUIRED) and use the DBDriver class to access the database.

Open database: https://github.com/introlab/rtabmap/blob/ff61266430017eb4924605b832cd688c8739af18/corelib/include/rtabmap/core/DBDriver.h#L132

Get optimized poses: https://github.com/introlab/rtabmap/blob/ff61266430017eb4924605b832cd688c8739af18/corelib/include/rtabmap/core/DBDriver.h#L108

Get node data: https://github.com/introlab/rtabmap/blob/ff61266430017eb4924605b832cd688c8739af18/corelib/include/rtabmap/core/DBDriver.h#L169

See also rtabmap-export tool to see how point clouds are created: https://github.com/introlab/rtabmap/blob/ff61266430017eb4924605b832cd688c8739af18/tools/Export/main.cpp#L1104-L1115

PS: Did you try rtabmap-databaseViewer tool? That is the official tool to debug/visualize rtabmap's databases.

takahiko-hasegawa-hdjp commented 1 year ago

Thanks for that! that helped a lot!

If i understand this correctly, you cannot just read the depth and node information from the database and directly use those values to vizualize the data? So you would have to optimize once you read those data from the database?

Also, what is the calibration data? is that needed if we want to get point cloud data? Can we not use depthx,y,z in feature table>

rtabmap-databaseViewer yes I have been using that but unfortunately I need to develop a 2D pointcloud vizualization application using c#

matlabbe commented 1 year ago

The Node table in database contains only raw odometry poses. You need opt_poses or you would have to re-optimize the graph yourself.

To convert a depth image to a point cloud, you need also the intrinsics parameter of the camera. With the depth image, you only know the Z in meters or mm, to know X and Y, you need the focal distance of the camera (or matrix K of the calibration data). You may also need the local transform between base_link and camera_optical_link frames, which is included in calibration data.

The Feature table is used to store 3D points of the visual features extracted from the images.