luisenp
commented
1 year ago Hi @codeaddict00. Sorry for the delay in getting back to you.
This should be relatively easy to implement in 2D. The following functions should be useful:
- Need to write a wrapper in your cost function for converting either Point2 or SE2 into an xy-vector, and return its jacobian. For Point2 this is trivial, and for SE2 you can use xy.
- Create the error implementation that returns e = p1_xy - p2_xy (the error is weighted and squared automatically by theseus elsewhere).
- Create the jacobian implementation. For this case, the jacobian is just the jacobian of
xy()operation wrapper, multiplied by the correct sign for each variable. - Make sure you register the variables as shown here.
- You can test your jacobians with this function.
For 3D the process is similar, but we don't have an implementation for SE3.xyz(), so we would need to add that first.
codeaddict00
🚀 Feature
A range measurement model between a Pose and a Point. Similar to what exists in GTSAM: https://github.com/borglab/gtsam/blob/develop/gtsam/sam/RangeFactor.h It would be good to have this along with the between factor.
Motivation
Makes Range-based SLAM and Proximity-based loop closing possible, and many more applications.
Alternatives
If you can't implement it. Could you give as much context and pointers as possible to help with the developpement of a Range factor in Theseus.
Thanks in advance for your time!