Fiducial pose jumps when in image center

[ivan140]

Hi,

I'm using your library for navigation with on the floor attached markers. It works really well, except for one case:
When the fiducial is in the image center it tends to jump and rotate around x-axis for ~180°.
I'm using dictionary 16 with a 0.048 fiducial length. Here are my launch files for both, aruco_detect and fiducial_slam and the map:

aruco_detect.txt
fiducial_slam.txt
map.txt

Here's how it looks like with a slow moving robot:

And also while standing at this special spot:

I'm using a fisheye camera which I calibrated with camera_calibration, here's my calibration yaml file:
camera_calibration.txt

Did you ever experience a similar behavior? If so, how you solved it?
I can also upload a bag file, if needed.

Thank you in advance for any help.

[rohbotics]

@ivan140 That is very weird, we haven't seen that behavior before.

Could you upload a bag file (with the image data) so that we could look a little closer?

Thanks for reporting!

[ivan140]

@rohbotics yes sure, here's a bagfile:
https://drive.google.com/file/d/1h7ZE1oXTL1njr9Sz3Vtkv9TU-OYGVoln/view?usp=sharing

The included topics are TF, pylon-camera-node images and the fiducials. For some reasons the marker is published tilted, with a rotation around cameras X axis. Maybe this is the reason for the jumps.

[mjstn]

Hello ivan140. We have only published a 'general solution' for ceiling nav so far and there was discovered an issue very similar to what you report in some lab tests about 6 months back as well as in some testing. I cannot say it was your exact issue but it sounds in general very similar to what you are reporting.

At that time it was decided to look into ways to minimize the sort of effect you are seeing by attempting to offer less general solutions that may meet many customers needs.

I would like to ask you about your application.

1. Are you trying to navigate using ceiling fiducials (none on walls)?

2. Does the environment of your robot require ramps or non-flat surfaces to be navigated?

Thank you,
mark

[ivan140]

Hello @mjstn ,
Thank you for the fast reply.
To 1: the fiducials are on a flat floor and are detected through a camera located at the bottom of the robot.

To 2: the robot is driving on a flat surface so the distance to the marker should be the same within +-2mm.

In your code you are using OpenCV's solvePnP function to estimate the 3D pose of the marker. My guess would be that for a specific 4 points input it generates a non-unique solution. As a possible solution for cases where the z-distance to the marker is known and remains the same, maybe the solvePnP could be omitted and specified by a z_dist parameter. However, this is just one possible way of handling it.

[mjstn]

Hello ivan140,
This is a very insightful and great reply. We have used the fiducials on ceiling and also very soon will be using them on floor but not directly under the robot, always at an angle.

What you are discussing is actually an extremely common fiducial recognition situation (AKA Amazon most likely).
We have not approached or attempted a solution for that case but your very comment indicates this is a hole in our offering.

I absolutely am very curious into this effect as it may apply to other modes we do have customers using. Yes I agree we likely have a case of a non-unique solution generates a flip or some trig passes through an undefined point and comes out 180 deg off in it's pose after that.

This is then in fact NOT the case I had seen. I had made a test where i rotated a fiducial on a pivot and noted cases where a jump happens. Not what you are reporting.

Also I hinted at a mode we are working on but from your answer it would not apply to your situation.

This is a good input and we very much thank you for your suggested solution.

Because of my timezone I will be signing off now but again, thanks for this entry.

[rohbotics]

@ivan140 a quick question: are you using aruco_detect only, or fiducial_slam as well?

I am going to take a look at your bag file, this might be something involving angle->quaternion conversion. Thanks for sending the bag file btw.

[ivan140]

@rohbotics I'm using the whole fiducials package. The only difference is, that I set the rosparam for read_only_map to true, so that the predefined values in the map do not change. This way I am able to detect the camera (robot) pose even with a single fiducial.

[JanezCim]

I can reproduce the same error on the given bag file, but when i change the input image inside aruco_detect.launch to image_raw instead of already undistorted image_rect with

<arg name="image" default="image_raw"/> <!-- image_rect-->

I manage to get an output without the jumps. Since OpenCV's solvePnP inside aruco_detect.cpp accepts camera matrix and distortion coefficients it might be that it needs the raw image as an input and not the already undistorted ones. This in turn might cause the jumps. Can you try this out and see if it helps?

[ivan140]

@JanezCim on my robot I'm using a fisheye cam and I'm not sure, whether solvePnP can interpret it's camera and distortion matrices correctly. Also, but it's only my opinion, it seems that the rectify libraries on ROS already do the undistortion work, so having an other undistortion action could lead to weird results.

However, also as a reaction to @mjstn : I added a package named qr_detect to your library which implements the zxing library which supports qr and many other fiducials. There's a small constraint of having a constant z distance to the fiducial (floor, ceiling), but except for this the library is a lot faster than the aruco_detect module and can reach approx camera FPS - 5 FPS. If you're interested I can open a push request.

[JanezCim]

@ivan140 Yes, I agree, I think undistorting an already undistorted image is not good. So then if you want to use the already undistorted image image_rect, then you also need to pass the proper camera intrinsics params (that are not for fisheye camera). I would try to do that by creating a new camera_info topic with params for the undistorted image and remapping the launch fille to it.

[JanezCim]

@ivan140 Did you solve this then?

[ivan140]

@JanezCim kind of. As I wrote in the previous comment I added a package without usage of solvePnP and set the z distance to a constant value. If you say, that when using the image_raw topic the jumps dissapeared from my perspective the issue is solved.

[JanezCim]

oh, alright, closing the issue then