search

HKUST-Aerial-Robotics / Fast-Planner

A Robust and Efficient Trajectory Planner for Quadrotors
GNU General Public License v3.0
2.33k stars 658 forks source link

Depth image based obstacle avoidance not working #65

Closed Stavya1993 closed 3 years ago

Stavya1993 commented 3 years ago

I have generated depth images and camera pose in the world frame from a high fidelity simulator and fed them into fast planner. However the planner is unable to detect or identify these images as obstacles it needs to avoid. I ensured the camera specifics across both the systems are in tune and replaced the controller quad dynamics to that of the simulator quadrotor. I can not understand what am I doing wrong!

This is a high priority task and any help will be greatly appreciated.

ZbyLGsc commented 3 years ago

If you are sure that the sensor pose and depth msgs are correctlt fed, may be your images have a different width/height and depth_scale_factor. You can easily validate whether the setup is correct by visualizing the projected point cloud of the depth images in RVIZ.

Stavya1993 commented 3 years ago

Sensor pose and depth messages are correct, I even generated a point cloud with depth_proc_cloud to validate the same and get a fairly reasonable obstacle detection in rviz. I then adjusted the images height and width to 640*480, and the simulator seemed to be using a scaling multiplication of 0.2 , so I changed the k depth scale factor in the launch file to 0.2. I am at a loss on what's really going wrong.

Stavya1993 commented 3 years ago

DepthImage_fail_example

I am sharing a video where the output of the point cloud is provided from the fast planner/ sdf/occupancy vs the point cloud from the depth_image_proc nodelet. Please help !!!!

ZbyLGsc commented 3 years ago

To ensure that point cloud of depth image is corrent in the map update, uncomment the publishDepth() in visCallback() and check the cloud from topic "/sdf_map/depth_cloud".

Stavya1993 commented 3 years ago

I actually figured that just a while ago. The map indeed looks a bit off, even though its similar to the depth_proc_image one when I choose the gradient option I see it doesnt consider the obstacle in front of it as an imminent red. Any clues on how I can fix this. My Image is a 16UC1 image coming from a camera with fov = 90 , depth scale = 0.2, and height =640, width = 480, camera focal length = 240.

Stavya1993 commented 3 years ago

Thanks, managed to fix it. It wasn't so much of an issue with the fast planner scaling factor as much as it was with the scaling metric used for converting rgbd images to 16UC1 format in the simulation itself.