Marker pose few cm off in TIAGo simulation
Closed this issue · 20 comments
Hi,
I am trying to implement a grasp routine in the TIAGo simulation using the Aruco cube. I have determined that the size of the aruco marker on the cube is 4.5 cm, and I have placed it on a table in front of the robot.
While the marker gets almost perfectly detected in the image, there is something wrong in the transformation to base_footprint. As can be seen in the image below, the marker is off by almost 1 cm in the x-direction and about 2-3 cm in the y-direction. This causes significant issues later in the pipeline.
I have confirmed the reference and camera frames (xtion_optical_frame and base_footprint) and checked their tf which is [0.244, -0.013, 1.143]. What is interesting is that the y-direction of the aruco marker is off by twice the y component of the tf. Could it be that the 'wrong eye' of tiago is considered as xtion_optical_frame? If the tf of the other eye would be used then the error in y-direction would almost disappear. Could this be the case or am I on the wrong path?
Thanks in advance!
Just to confirm, the 4.5cm size of the aruco you mention, is just up to the black border, or the whole white border as well? It should be just from end to end of the black part.
Yes, it is end-to-end on the black part. I loaded the aruco image the white padding that is stuck on top of the cube in gimp and the black part is 450 pixels wide, the whole image is 500 pixels. The cube is 5 cm wide so I reasoned that the black part in gazebo must be 4.5 cm wide.
On further inspection, my part about the tf was wrong, I noticed that the head was panned and tilted which caused the shift in y-direction. Placing the cube at eye-height and shifting it in the y-direction shows that the error decreases as I shift the cube to the left (from TIAGo's perspective) and increases when moving it to the right. The error increases slightly with increasing distance, too.
I also tried using different cameras, as the simulation keeps mentioning xtion frames I figured it used the asus_xtion camera intrinsics, while the simulation actually uses the orbbec-astra camera by default. Sadly, this didn't solve the issue either.
The xtion thing is a legacy name that we didn't change to avoid breaking all the code. Should work regardless of camera.
I'm having trouble understanding the errors that you are having. In the pictures you shared I don't see much of an error. Maybe I see on the first image a ~0.5cm error in y, but that could be perspective.
Thank you for your response.
In the image, in the terminal and in gazebo on the right, compare the y-positions of the aruco_cube object and the marker. In gazebo the object has a y value of -0.082 but the aruco_ros detects the marker at y = 0.106, which is a difference of about 2.5 cm. The difference in x is not that much, and the difference in z is due to the marker being on top of the object, but the difference in y is problematic.
Ah, the aruco position from gazebo is in the world frame of reference, the origin of gazebo world.
The one you see in the topic, is in the /base_footprint of the robot. Is a different frame of reference, if the robot is not exactly at the world's 0,0,0, coordinates, you will see this.
I just checked, and since I am not moving the robot, it is still at 0,0,0 with 0,0,0,1 orientation. I just tried it with a fresh simulation install and it is still wrong.
Steps to reproduce:
- Install tiago dual simualtion: http://wiki.ros.org/Robots/TIAGo%2B%2B/Tutorials/Installation/Tiago%2B%2BSimulation
- Launch simulation:
roslaunch tiago_dual_gazebo tiago_dual_gazebo.launch public_sim:=true end_effector_left:=pal-gripper end_effector_right:=pal-gripper- In gazebo, place aruco cube on its side on a 1m table so that TIAGo can see the marker
- Run aruco marker detector:
rosrun aruco_ros marker_publisher /camera_info:=/xtion/rgb/camera_info /image:=/xtion/rgb/image_rect_color _marker_id:=582 _marker_size:=0.045 _marker_frame:=/aruco_frame _camera_frame:=/xtion_optical_frame _image_is_rectified:=true _reference_frame:=/base_footprint- Echo detected markers
rostopic echo /aruco_marker_publisher/markersI have again confirmed that base_footprint is at 0,0,0. In any case, even if the robot has moved, when I create collision objects with moveit at the exact location of the detected marker (same base_footprint reference frame) it is misplaced with the same errors.
Bumping this issue.
Could you let me know if you can recreate it using the steps above? Thanks!
I'm going to have to bump this again. The issue still persists. At this point I am quite sure that the issue is not with my system as multiple of our students using your TIAGo++ simulation run into it. Please let me know if there is anything I can try to solve it.
Is this on Melodic, with OpenCV 3.2.x?
According to this comment on this MoveIt Calibration PR, OpenCV 3.2 doesn't play nice with Aruco markers.
A warning specifically for that version of OpenCV was added in moveit/moveit_calibration#84 (readme will also be updated in ros-planning/moveit_calibration#85).
Could be something to check.
@gavanderhoorn Yes, I am running Melodic with OpenCV 3.2.0.
I found your answer on ros answers regarding OpenCV versions in melodic and it seems like using a different version is a pretty involved task.
I am going to assume that this is the reason for my issue as well. Thanks for the response!
It's not that involved, but you will have to compile some things from source, yes.
OpenCV 3.2 doesn't play nice with Aruco markers.
@gavanderhoorn I am not sure if the MoveIt issue is referencing this package or the OpenCV version. I started this ROS package with a copy of the aruco library back then but refrained from migrating to the mainlined OpenCV implementation (which followed in a few years) as it not only had runtime issues but also lacked marker generation, etc. AFAIK this package still has a copy of the aruco library so it may not be suffering from the same issue as the OpenCV implementation. Just a guess though...
If aruco_ros does not use OpenCV, then I don't believe the mentioned issues would be relevant.
Looking at the code a bit more closely it indeed seems like it is not using OpenCV, just a cv bridge. So, the bug is then somewhere in this package or the simulation that uses it. As the marker gets detected very accurately in the RGB image, I suspect it has something to do with the transformation from 2D to 3D. Afaik this could be either the camera intrinsics or the mathematical operations?
According to this comment on this MoveIt Calibration PR, OpenCV
3.2doesn't play nice with Aruco markers.
Just to be clear, what I've witnessed is that OpenCV 3.2 doesn't play nice with ArUco boards (arrays of several ArUco markers). That said, I'm not super-confident in the pose estimation of a single ArUco marker in any version of OpenCV, just because it's only using the four corners of the marker (I believe), so you're estimating a 6 DoF quantity with input that has just 8 DoF.
We're using the ArUco library in OpenCV, not aruco_ros.
Just to give a quick update:
I found out that the issue is not present when using the single aruco detection script (with the single.launch file). The issue is present with the marker_publisher script, which I prefer to use since I want to have a list of all the detected markers instead of just one with a specific ID.
I looked a bit at the code and found that there are already differences in the markers detected with mDetector.detect(args).
I fixed it.
in the TIAGo++ tutorials, we are asked to run
rosrun aruco_ros single /camera_info:=/xtion/rgb/camera_info /image:=/xtion/rgb/image_rect_color _marker_id:=584 _marker_size:=0.10 _marker_frame:=/aruco_frame _camera_frame:=/xtion_optical_frame _image_is_rectified:=true _reference_frame:=/base_footprintThe important part here is: _camera_frame:=/xtion_optical_frame. This is wrong, the camera frame should actually be /xtion_rgb_optical_frame.