12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

SLERP with Zhaoyang, not really part of BAD, but here it originated :-)

release/4.3a0
dellaert 2014-10-23 19:11:44 +02:00
parent e7ec6b3fa5
commit 79efd2f3fc
3 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Rot3.cpp
View File

@ -261,6 +261,15 @@ Point3 Rot3::unrotate(const Point3& p) const {
return Point3(transpose_*p.vector()); // q = Rt*p
}
/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
// amazingly simple in GTSAM :-)
assert(t>=0 && t<=1);
cout << "slerp" << endl;
Vector3 omega = localCoordinates(other, Rot3::EXPMAP);
return retract(t * omega, Rot3::EXPMAP);
}
/* ************************************************************************* */
} // namespace gtsam

7
gtsam/geometry/Rot3.h
View File

@ -445,6 +445,13 @@ namespace gtsam {
*/
Vector quaternion() const;
/**
* @brief Spherical Linear intERPolation between *this and other
* @param s a value between 0 and 1
* @param other final point of iterpolation geodesic on manifold
*/
Rot3 slerp(double t, const Rot3& other) const;
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);

12
gtsam/geometry/tests/testRot3.cpp
View File

@ -574,6 +574,18 @@ TEST( Rot3, stream)
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
}
/* ************************************************************************* */
TEST( Rot3, slerp)
{
// A first simple test
Rot3 R1 = Rot3::Rz(1), R2 = Rot3::Rz(2), R3 = Rot3::Rz(1.5);
EXPECT(assert_equal(R1, R1.slerp(0.0,R2)));
EXPECT(assert_equal(R2, R1.slerp(1.0,R2)));
EXPECT(assert_equal(R3, R1.slerp(0.5,R2)));
// Make sure other can be *this
EXPECT(assert_equal(R1, R1.slerp(0.5,R1)));
}
/* ************************************************************************* */
int main() {
TestResult tr;