I want just fiducial pose estimation. Should I make it another node called fiducial_localization or find a way to work it into the existing nodes?

[jack-digilabs]

I'm looking at a simpler fiducial node that takes a predefined fiducial map and publishes pose estimates with covariances. The way the current fiducial_slam package is structured, I don't see a good way of bisecting out the Mapping so only Localization is performed without several changes to how the variance propagates. Specifically, the camera pose variance seems to be based on the fiducial's variance in the map and not on the detection error.

This makes me think it would be simpler to just add another node that only does localization. I'd be very interested in hearing your opinions on the best way to proceed. All I'm looking for is a node that...

1. Publishes pose estimate based on fiducials with proper variance for position and orientation.
2. Fiducials are in a pre-defined map and can be given some variance in position and orientation in that map.

There's also a high probability I'm misunderstanding the code so any tips or help would be greatly appreciated.

[rohbotics]

There is a read_only_map parameter, that prevents the map from being updated as new detection come in. Does that suit your needs?

[jack-digilabs]

Sorry for the slow response. I wanted to make sure I was correct in my variance propagation. I'll be referencing PR #153 I just submitted to make it easier to reference parts I am thinking of changing. Feel free to close it.

So right now there are a couple issues fiducial_slam has in being used for just a pose data source.

1. It publishes a tf from map->odom and there's no way to currently turn that off. I think setting tf_publish_interval==0 was intended to do this however this check was missing from the part of the code when fiducials are detected. Was this the intent or did I interpret it incorrectly? Basically, I need a way of turning off fiducial_slam's map->odom publishing so if this isn't correct, what do you recommend? This is related to Issue publish_tf doesn't seem to be used in the example fiducial_slam.launch #148

2.The pose estimate from fiducial_slam (topic /fiducial_pose) have very low covariance (less than 0.001) so when I pass it through the EKF the robot pose jumps around. From what I can tell, the robot pose variance is set by the camera pose which is set by the transform_map2cam, which is the average of all seen fiducial transforms (Lines 369, 403 in map.cpp). I think this goes back to using just objectError (Line 146 in estimator.cpp) as the variance and it doesn't distinguish between pose variance vs orientation variance. This is where I'm thinking of adding a testing different ways of setting the variance like some asymptotic behavior as the aspect ratio gets very small or very large (a very flat fiducial).

This would require adding another member to TransformWithVariance so it distinguishes between pose variance and orientation variance but I think would allow for better integration with other packages in the end. Any guidance would be extremely appreciated. As much as I think I understand the code, I'm sure I've missed things.

[johnjin404]

Hello, I am new to this field, would u please tell me what's the meaning of the "objectError", I don't know why the formula is like this
"(reprojectionError / dist(imagePoints[0], imagePoints[2])) *(norm(tvec) / fiducialLen);"
thank u very much!

[jim-v]

Hi @johnjin404

Thanks for your question. In future, could you please create a new issue instead of responding to an existing one.

The object error is an _estimate+ of how much error (in meters) there is in the object's pose estimate, based on the reprojection error (in pixels). dist(imagePoints[0], imagePoints[2]) is the distance between two image vertices in pixels, fiducialLen is the same in meters. norm(tvec) is the distance from the fiducial to the camera. It is used in this expression, since the error increases the further away the fiducial is from the camera.