Apriltag Pose Estimation support

[fenjalien]

Hi, I'm really excited to use this crate but it doesn't appear to have pose estimation like the original library does. Apprently there is a way to use opencv but I need to look further into it. However it would be nice to have it builtin.

Amazing work btw :)

[fenjalien]

I've had a crack at trying to write the bindings myself however it straight up segfaults and I can't tell why. Could someone take a look?

https://github.com/fenjalien/apriltag-sys https://github.com/fenjalien/apriltag-rust

[jerry73204]

Thanks! The changes in apriltag-sys looks good and may go to crates.io first.

For apriltag-rust, the estimate_tag_pose() returns a dangling pointer that causes the segfault. The sys::apriltag_pose_t is allocated on rust side, initialized by C, and returns the pointer. Note that unlike reference, pointer does not have lifetime, and dereferencing is unsafe. I would let Pose keeps an owned sys::apriltag_pose_t instead of a pointer. Apart from above, the API should change to fit into Rust's convention.

Could you open PRs respectively for apriltag-{sys,rust} that we can edit together?

[jerry73204]

@fenjalien My changes goes to sys and rust crates. You can review if it works for you, or opens your PRs.

[fenjalien]

Hi, sorry for the late reply. It looks rly promising however: I had to clone a local copy the pose-modified branch so I could point it at the modified sys crate. I also modified the example code to print the pose estitmation. Unfortunatly when I run the command cargo run -- ./img.jpg it spits out:

# image ./img.jpg
apriltag_test: /home/fenjalien/robotics/apriltag/common/matd.c:1254: matd_svd_tall: Assertion `maxi >= 0' failed.
Aborted

I've tried fiddling with the focal point values and using different images but to no avail.

[jerry73204]

It seems libapriltag panics under specific camera parameters, for example, fx=0.000001 fy=1.0. I rewrite the whole pose estimation API and now integrated to the example code.

You can run the example program with additional parameters and see if it works for you.

cargo run --features full --example detector -- \
    --family tag36h11 \
    --tag-params 1,2.1,2.2,4,5 \
    input.jpg

[jerry73204]

To use the crate from git repo, I personally write the following in Cargo.toml.

[dependencies]
apriltag = { git = "https://github.com/jerry73204/apriltag-rust.git", branch = "pose-modified" }

[patch.crates-io]
apriltag-sys = { git = "https://github.com/jerry73204/apriltag-sys.git", branch = "pose-modified" }

[fenjalien]

I think you've forgotten to push src/common.rs, cargo is throwing up a lot of errors of things being out of scope.

[jerry73204]

It's my mistake. Fixed now.

[fenjalien]

Awesome! Works really nicely, tysm. One final suggestion would be possibly convert the rotation and translation matrices to nalgebra structs? I think this is the right code but i'm not sure where would be the right place

let rotation = Rotation3::from_matrix(&Matrix3::from_row_slice(pose.rotation());
let translate = Translation3::from_vector(Vector3::from_row_slice(pose.translation().data()));

[jerry73204]

I made an attempt to add nalgebra conversion, but I dare not merge into master since there are serous bugs in apriltag (AprilRobotics/apriltag#104). There are chances that it produces improper rotation matrix. It causes Rotation3::from_matrix to hang indefinitely (with unbounded iterations).

I still push the nalgebra support to this branch that can be found at Pose::to_isometry() I prefer to block the changes until the upstream fix the issue (otherwise it encourages more incorrect implementations). You can read struct Isometry doc to know the usage in nalgebra.

[jerry73204]

Apart from that, I discovered an error in matd to nalgebra matrix conversion because they assume distinct memory order (row vs column major). I merge the fix and pose API to new v0.3 release except nalgebra support.