mzahana / px4_fast_planner

Integration of Fast-Planner with PX4 autopilot for multi-rotor fast navigation with obstacle avoidance
MIT License
208 stars 44 forks source link

Point cloud orientation problem #39

Open bhaskar-glitch opened 2 years ago

bhaskar-glitch commented 2 years ago

Hi, Im using realsense d435i and facing an issue with the point cloud of realsense of topic /camera/depth/color/points in orientation of pointcloud is -y is upward...the problem is -y is facing toward z axis as in this picture IMG-20211217-WA0000

Please help

xxEoD2242 commented 2 years ago

Alright, this is a tricky, tricky problem I spent a massive amount of time trying to fix. I got mine to work but was using AirSim and it was an absolute nightmare. Hopefully this helps.

First, take a look in the launch file at the focal length values and ensure that they are consistent for the d435i. Check the launch file for Fast Planner, which sets the fx and fy values.

Second, look at the pose being output by the TF of the D435i camera. There may be a setting in the RealSense ROS node about orientation. This is the relevant code where it pulls in your camera orientation. SDF Map code. This orientation is used to determine where the individual pixel values captured in the image correspond to.

Third, determine what the overall reference in the message is for the pose of the camera. I think Fast Planner uses "World", which is common across ROS but I'm not sure about the RealSense node.

Good luck and let me know what you find. I have a d455 I use and would like to incorporate this as well.

bhaskar-glitch commented 2 years ago

Alright, this is a tricky, tricky problem I spent a massive amount of time trying to fix. I got mine to work but was using AirSim and it was an absolute nightmare. Hopefully this helps.

First, take a look in the launch file at the focal length values and ensure that they are consistent for the d435i. Check the launch file for Fast Planner, which sets the fx and fy values.

Second, look at the pose being output by the TF of the D435i camera. There may be a setting in the RealSense ROS node about orientation. This is the relevant code where it pulls in your camera orientation. SDF Map code. This orientation is used to determine where the individual pixel values captured in the image correspond to.

Third, determine what the overall reference in the message is for the pose of the camera. I think Fast Planner uses "World", which is common across ROS but I'm not sure about the RealSense node.

Good luck and let me know what you find. I have a d455 I use and would like to incorporate this as well.

Hi @xxEoD2242, Thanks for the reply As you told to try the 3 solutions I tried all of them but nothing helped. The changes I tried is md_.cameraq = Eigen::Quaterniond(1,0,0,0); I tried giving the values of Quaterniond manual but nothing changes in point cloud. also, md_.cameraq = Eigen::Quaterniond(0.7071068, 0, 0.7071068, 0); i tried giving these values in Quaterniond but nothing helped.

Now let me explain you everything I'm using:- 1.) Realsense d435i camera 2.) Vins Fusion for localization 3.)Fast planner for path planning

Also, As Fast planner have 2 options either to use depth image for point cloud or direct point cloud now if i'm using the depth image then the point cloud axis is perfect but the point cloud is having delay of 5-6 seconds so we aren't using it bcoj the delay of 5-6 sec is a big delay for drone and it may crash.Do you know any solution of this delay issue?? THe point cloud by depth image is Screenshot from 2021-12-18 18-09-15

and as i told that when i'm using the point cloud itself then the axis of point cloud is not correct Screenshot from 2021-12-17 19-35-34

Also, as per your third solution, my vins and fast planner both are on map frame,the rqt_tf_tree and rqt_graph of both are same This is the rqt_tf_tree 11

This is the rqt_graph 112gr

Now,Please let me know the possible solutions

Chai8226 commented 1 year ago

I change the intrinsic params of the depth camera in px4_topo_replan.launch , then fix the problem.

sebnem-byte commented 2 months ago

I change the intrinsic params of the depth camera in px4_topo_replan.launch , then fix the problem.

could you share your parameters ?