Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Pose3 interpolateRt method #647

Merged
merged 6 commits into from
Dec 31, 2020
Merged

Pose3 interpolateRt method #647

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
fbca10f
Pose3 interpolate() which correctly interpolates between poses
varunagrawal Dec 30, 2020
64630e1
Pose3 template specialization for interpolate
varunagrawal Dec 31, 2020
bfed358
added easy-to-verify test for Pose3 interpolate
varunagrawal Dec 31, 2020
7523696
added new Pose3 interpolateRt function, updated tests
varunagrawal Dec 31, 2020
2c37a58
update documentation of interpolateRt
varunagrawal Dec 31, 2020
949c398
update docs and tests
varunagrawal Dec 31, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions gtsam/geometry/Pose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -423,4 +423,11 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
return os;
}

/* ************************************************************************* */
template <>
Pose3 interpolate<Pose3>(const Pose3& X, const Pose3& Y, double t) {
return Pose3(interpolate<Rot3>(X.rotation(), Y.rotation(), t),
interpolate<Point3>(X.translation(), Y.translation(), t));
}

} // namespace gtsam
12 changes: 12 additions & 0 deletions gtsam/geometry/Pose3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -356,6 +356,18 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
};
// Pose3 class

/**
* Interpolation between two poses via template specialization.
*
* *NOTE* This should be used instead of gtsam::interpolate.
*
* @param X Start point of interpolation.
* @param Y End point of interpolation.
* @param t A value in [0, 1].
*/
template <>
Pose3 interpolate<Pose3>(const Pose3& X, const Pose3& Y, double t);

/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
Expand Down
27 changes: 27 additions & 0 deletions gtsam/geometry/tests/testPose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1016,6 +1016,33 @@ TEST(Pose3, TransformCovariance6) {
TEST(Pose3, interpolate) {
EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0)));
EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0)));

// Trivial example: start at origin and move to (1, 0, 0) while rotating pi/2
// about z-axis.
Pose3 start;
Pose3 end(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
// 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)));
Copy link
Collaborator Author

@varunagrawal varunagrawal Dec 31, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is because the interpolation is in tangent space. Straight lines in tangent space correspond to screw motions.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you please provide a reference I can read up on?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


// Example from Peter Corke
// https://robotacademy.net.au/lesson/interpolating-pose-in-3d/
t = 0.0759; // corresponds to the 10th element when calling `ctraj` in
// the video
Pose3 O;
Pose3 F(Rot3::Roll(0.6).compose(Rot3::Pitch(0.8)).compose(Rot3::Yaw(1.4)),
Point3(1, 2, 3));

// The expected answer matches the result presented in the video.
Pose3 expected1(interpolate(O.rotation(), F.rotation(), t),
interpolate(O.translation(), F.translation(), t));
EXPECT(assert_equal(expected1, interpolate(O, F, t)));

// Non-trivial interpolation
Pose3 expected2(interpolate(T2.rotation(), T3.rotation(), t),
interpolate(T2.translation(), T3.translation(), t));
EXPECT(assert_equal(expected2, interpolate(T2, T3, t)));
}

/* ************************************************************************* */
Expand Down