Pose3 interpolateRt method #647
Conversation
varunagrawal
added
bugfix
There was a problem hiding this comment.
Actually, the old code is correct. The interpolate function is defined as doing interpolation in tangent space. If the rotations of both endpoints are different, there is no expectation that the translation linearly interpolates.
If the rotations are the same, I think it does linearly interpolate translation (but I'm not even sure of that).
I though about that at first, that perhaps the interpolation of the translations needn't be linear betweem them if the rotations are different, but after looking it up and checking the implementation of other libraries, it seems that the translations are indeed linear even if the rotations are different. This is one of the reasons I put that example from Peter Corke's video lecture as a unit test. It is based off his Robotics Toolbox for Matlab. |
gtsam/geometry/tests/testPose3.cpp
Outdated
| // This interpolation is easy to calculate by hand. | ||
| double t = 0.5; | ||
| Pose3 expected0(Rot3::Rz(M_PI_4), Point3(0.5, 0, 0)); | ||
| EXPECT(assert_equal(expected0, interpolate(start, end, t))); |
There was a problem hiding this comment.
@dellaert I added this simple test to verify since it is easy to visualize and imagine: a body turning about Z as it moves from the origin to (1, 0, 0).
The old code fails with this message:
Not equal:
expected:
R: [
0.707107, -0.707107, 0;
0.707107, 0.707107, 0;
0, 0, 1
]
t: 0.5 0 0
actual:
R: [
0.707107, -0.707107, 0;
0.707107, 0.707107, 0;
0, 0, 1
]
t: 0.5 -0.207107 0
It doesn't make any sense why the body would off the X axis.
There was a problem hiding this comment.
This is because the interpolation is in tangent space. Straight lines in tangent space correspond to screw motions.
There was a problem hiding this comment.
Can you please provide a reference I can read up on?
There was a problem hiding this comment.
https://en.wikipedia.org/wiki/Slerp
The reference is the documentation of "interpolate". I think you expected something else, which is fine. I emailed with a different approach (interpolateRt). You can document the difference with interpolate there.
|
I see. By shortest distance I meant straight line. Will update. |
varunagrawal
changed the title
New interpolateRt method that might be more intuitive than minimizing geodesic distance.