project 3D point coordinate in the camera frame to map frame

[najiib49]

Hi all,

Required Info
Camera Model	D435i
Firmware Version	05.13.00.50
Operating System & Version	Ubuntu 20.04
ROS2 Version	Galactic
Language	Python

I'm using realsense camera(d435i). I grab a pixel in space and convert it to a 3D point coordinate using the rs2_deproject_pixel_to_point. So far the 3D point is accurate as I've tested it with rs2_project_point_to_pixel which returns the pixel point I've started with.

My question is how do I project this 3D point into the map frame? I have a transformation of these frames map --> odom --> baselink --> camera_link --> camera_aligned_depth_to_color_frame --> camera_color_optical_frame

What I've tried

I use tf2 transform listener from map to camera_color_optical_frame to get the location of the robot relative to the camera. I then add the x,y,z translation of this frame to the 3D point I get from the rs2_deproject_pixel_to_point. The reason I'm adding them is because I have the transformation from map to camera_color_optical_frame and that the 3D point's origin is the left imager as mentioned in this Comment (and the documentation). In order to know the point in the map frame, I assume that it was just a matter of adding these points. However, I've noticed it's not always accurate. The axis of camera_color_optical_frame and the 3D point axis of rs2_deproject_pixel_to_point match i.e. positive x is right, positive y is down and positive z is forward. The axis of the map/base_link frame is positive-x forward, positive-y left and positive-z up. Once I add the xyz translation and 3D point, I try to convert the result in the map frame axis (see side note for this conversion). I've also visualized these axis in RViz.

Side note: converting xyz from realsense/camera_color_optical_frame to map/base_link frame axis

x = z
y = -x
z = -y

An interesting observation I've seen in RViz is that the axis for camera_aligned_depth_to_color_frame and camera_color_optical_frameare different. The axis for camera_aligned_depth_to_color_frame is similar to map/base_link frame and camera_color_optical_frame is similar to the realsense-axis i.e. positive-x forward, positive-y left and positive-z up and positive x is right, positive y is down and positive z is forward respectively. However, When echo the tf2 topic for map --> camera_color_optical_frame and map to camera_aligned_depth_to_color_frame. Both have the same translation but different Rotation in Quaternion. See Terminal result below

------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
ros2 run tf2_ros tf2_echo map camera_color_optical_frame 
[INFO] [1661281979.659275222] [tf2_echo]: Waiting for transform map ->  camera_color_optical_frame: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
At time 1661281980.600193203
- Translation: [0.180, 0.075, 0.182]
- Rotation: in Quaternion [-0.495, 0.503, -0.494, 0.508]
At time 1661281981.600425654
- Translation: [0.180, 0.075, 0.182]
- Rotation: in Quaternion [-0.495, 0.503, -0.494, 0.508]
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
ros2 run tf2_ros tf2_echo map camera_color_optical_frame 
[INFO] [1661281979.659275222] [tf2_echo]: Waiting for transform map ->  camera_color_optical_frame: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
At time 1661281980.600193203
- Translation: [0.180, 0.075, 0.182]
- Rotation: in Quaternion [-0.495, 0.503, -0.494, 0.508]
At time 1661281981.600425654
- Translation: [0.180, 0.075, 0.182]
- Rotation: in Quaternion [-0.495, 0.503, -0.494, 0.508]
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

This shows me that my assumption to simply adding the xyz translation from tf2 listener of map to camera_color_optical_frame to the 3D point and then flipping this x,y,z from one axis to another isn't correct. Is this possible? if not, is there a work around to achieving it? I'd appreciate any help I can get.

Additional info

I'm using the following camera topics

• the pixel is grabbed in the callback where I subscribe the '/camera/color/image_raw'
• the camera intrinsic and depth distance parameters of the rs2_deproject_pixel_to_point function are from '/camera/aligned_depth_to_color/camera_info' and '/camera/aligned_depth_to_color/image_raw' respectively

[MartyG-RealSense]

Hi @najiib49 If you are aiming to translate a 3D pointcloud in ROS into a format usable in a navigation system, an approach that some RealSense ROS users have tried is a package called pointcloud_to_laserscan as demonstrated in the links below.

https://github.com/introlab/rtabmap_ros/issues/358

https://answers.ros.org/question/396646/problem-using-pointcloud-data-in-nvidia-jetson-xavier-nx/

https://robotics.shanghaitech.edu.cn/static/robotics2020/MoManTu.pdf

---

http://wiki.ros.org/pointcloud_to_laserscan

[najiib49]

Hi @MartyG-RealSense Thanks for your response. I'm actually aiming to translate a 2D pixel from the camera into a format ROS2 Navigation system can use. I'm trying to project that pixel point into a 2D occupancy map grid. is this possible?

--update for more clarification

what i mean by project the pixel point into a 2D occupancy map grid is that I want to tell a robot to navigate to that pixel location using ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose. How would one achieve this?

The route I'm taking to solve this is to change the pixel point into a 3d point coordinate. I change the 3d coordinate to be the same axis as the map frame. I then listen the transform from map to camera_color_optical_frame. Finally, I add the translation x,y,z of the transform to the 3d point. Using send_goal action, I tell the robot to navigate to that location. However, sometimes the final position I get is way off or sometimes it's close. I hope it's clear enough.

[MartyG-RealSense]

As you are using ROS2 Navigation, I will seek the advice of the developer @SteveMacenski as he works on that system.

Hi @SteveMacenski Would it be possible to provide @najiib49 with advice about their question in the comment above, please? Many thanks!

[MartyG-RealSense]

Hi @najiib49 Have you made any progress with your ROS2 Navigation issue, please? Thanks!

[najiib49]

Hi @MartyG-RealSense Thanks for reaching out. I am somewhat progressing on this task. I'm currently using aruco marker's pose instead of a random pixel. Now I'm trying to create a transformation between aruco marker's pose and the map frame using ROS2 Tf2. Please let me know if there's a better way to going about this. I'm using this to achieve it

[MartyG-RealSense]

There is a recently published ROS2 package called Aruco Pose Filter at the link below with a TF2 feature that can "publish a transform from camera link to (marker_link_prefix + marker_id)"

https://github.com/lapo5/ROS2-Aruco-TargetTracking

[najiib49]

@MartyG-RealSense Thank you so much! This was exactly what I was looking for.

[MartyG-RealSense]

That's great to hear, @najiib49 - thanks very much for the update :)

[MartyG-RealSense]

Hi @najiib49 Do you require further assistance with this case, please? Thanks!

[najiib49]

Hi @MartyG-RealSense not anymore. Once again thank you so much for your help.

[MartyG-RealSense]

You are very welcome. I'm pleased that I could help. As you do not require further assistance, I will close this case. Thanks again!