search

leggedrobotics / elevation_mapping_cupy

Elevation Mapping on GPU.
MIT License
477 stars 105 forks source link

I want to know if it works well Height drift compensation. #81

Closed kdyy1111 closed 4 months ago

kdyy1111 commented 5 months ago

Thank you so much for sharing your great research.

But, there is a small issue.

The point cloud received in an environment with a d435 camera in front is as shown in the photo below.

222

In a situation where the robot is walking trot in place, a phenomenon occurs where the ground without measured values goes down over time.

11

If I set drift_compensation_alpha very low or set to 0, the phenomenon goes away, but I want to know if this is the right way to solve the problem.

mktk1117 commented 5 months ago

The drift_compensation_alpha works like this. Does your robot's odometry actually drift? or it only appears in the map?

https://github.com/leggedrobotics/elevation_mapping_cupy/blob/6ad593642715c342f16d27e9251b84c356efe370/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py#L350C1-L351C1

kdyy1111 commented 5 months ago

Thank you for your comment. If I understand correctly, in the above simulation, the estimator is not used, but the ground truth of the gazebo is received and used. There is no drift in the odometry of the robot, and only the map drift occurs. Since mean_error also affects cells that do not receive point clouds, is it necessary to modify the code to perform drift compensation only for cells that receive point clouds?

mktk1117 commented 5 months ago

If the state estimation is perfect and still this is happening, there might be some timestamp issue. (Because there should be no drift in the point cloud if the odometry is perfect.) Could you check if the point cloud timestamp and tf time stamps are correct?

kdyy1111 commented 5 months ago

As you said, there seems to be a time stamp problem due to frame settings. Code below is my frame parameters and terminal error. I have no idea how to fix it...

#### Basic parameters ########
map_frame: 'odom'  
base_frame: 'base'  
corrected_map_frame: 'corrected_odom'
#### Initializer ########
initialize_method: 'linear'    
initialize_frame_id: ['odom'] 
initialize_tf_offset: [0.0, 0.0, 0.0, 0.0]  
dilation_size_initialize: 5  
initialize_tf_grid_size: 1  
use_initializer_at_start: true

###########################
NODES
  /
    elevation_mapping (elevation_mapping_cupy/elevation_mapping_node)

ROS_MASTER_URI=http://localhost:11311

process[elevation_mapping-1]: started with pid [1964278]
Start loading plugins...
Loaded plugins are  min_filter smooth_filter inpainting smooth_filter
[ INFO] [1706685008.634074094]: [ElevationMappingCupy] finish initialization
[ERROR] [1706685010.925960943, 144.736000000]: Lookup would require extrapolation 0.492000000s into the past.
Requested time 143.732000000 but the earliest data is at time 144.224000000, when looking up transform from frame [base] to frame [odom]
[ INFO] [1706685011.673086336, 145.479000000]: Clearing map with initializer.
[ INFO] [1706685011.673263730, 145.479000000]: Initializing map with points using linear
mktk1117 commented 5 months ago

Okay, great that you found the issue. I think there's some issue in the point cloud publisher node, or state estimation node. As I don't know about your whole setup, I can only suggest to check carefully the point cloud processing pipeline and odometry pipeline. I recommend to use the original time stamp if you're post-processing point cloud data.