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

Added a 1D manifold implementation of 2D rotations. The new representation stores (cos theta, sin theta) rather than theta itself, ensuring that (a) rotate and unrotate do not call cos/sin, (b) same for all derivatives of rotate and unrotate, (c) when you call angle(), you always get the standardized answer given by atan2. The idea is that we will use this new type in Pose2 instead of angle.

release/4.3a0
Frank Dellaert 2009-12-09 21:50:27 +00:00
parent 91370a9d7a
commit 7d4de3ec20
5 changed files with 296 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cpp/Makefile.am
View File

@ -147,9 +147,10 @@ testPose2Constraint_SOURCES = $(example) testPose2Constraint.cpp
testPose2Constraint_LDADD = libgtsam.la
# geometry
sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2
testPoint2_SOURCES = testPoint2.cpp
testRot2_SOURCES = testRot2.cpp
testPose2_SOURCES = testPose2.cpp
testPoint3_SOURCES = testPoint3.cpp
testRot3_SOURCES = testRot3.cpp
@ -157,6 +158,7 @@ testPose3_SOURCES = testPose3.cpp
testCal3_S2_SOURCES = testCal3_S2.cpp
testPoint2_LDADD = libgtsam.la
testRot2_LDADD = libgtsam.la
testPose2_LDADD = libgtsam.la
testPoint3_LDADD = libgtsam.la
testRot3_LDADD = libgtsam.la

70
cpp/Rot2.cpp Normal file
View File

@ -0,0 +1,70 @@
/*
* Rot2.cpp
*
* Created on: Dec 9, 2009
* Author: Frank Dellaert
*/
#include "Rot2.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void Rot2::print(const string& s) const {
cout << s << ":" << angle() << endl;
}
/* ************************************************************************* */
bool Rot2::equals(const Rot2& R, double tol) const {
return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
}
/* ************************************************************************* */
Rot2 Rot2::exmap(const Vector& v) const {
if (zero(v)) return (*this);
return Rot2(v(0)) * (*this);
}
/* ************************************************************************* */
Rot2 exmap(const Rot2& R, const Vector& v) {
return R.exmap(v);
}
/* ************************************************************************* */
Point2 rotate(const Rot2& R, const Point2& p) {
return R * p;
}
/* ************************************************************************* */
// see libraries/caml/geometry/math.ml
Matrix Drotate1(const Rot2& R, const Point2& p) {
Point2 q = R*p;
return Matrix_(2, 1, -q.y(), q.x());
}
/* ************************************************************************* */
Matrix Drotate2(const Rot2& R) {
return R.matrix();
}
/* ************************************************************************* */
Point2 unrotate(const Rot2& R, const Point2& p) {
return R.unrotate(p);
}
/* ************************************************************************* */
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */
/* ************************************************************************* */
Matrix Dunrotate1(const Rot2 & R, const Point2 & p) {
Point2 q = R.unrotate(p);
return Matrix_(2, 1, q.y(), -q.x());
}
/* ************************************************************************* */
Matrix Dunrotate2(const Rot2 & R) {
return R.transpose();
}
} // gtsam

123
cpp/Rot2.h Normal file
View File

@ -0,0 +1,123 @@
/*
* Rot2.h
*
* Created on: Dec 9, 2009
* Author: Frank Dellaert
*/
#ifndef ROT2_H_
#define ROT2_H_
#include "Testable.h"
#include "Point2.h"
#include "Matrix.h"
namespace gtsam {
/* Rotation matrix */
class Rot2: Testable<Rot2> {
private:
/** we store cos(theta) and sin(theta) */
double c_, s_;
/** private constructor from cos/sin */
Rot2(double c, double s) :
c_(c), s_(s) {
}
public:
/** default constructor, zero rotation */
Rot2() : c_(1.0), s_(0.0) {}
/** constructor from angle */
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
/** return angle */
inline double angle() const { return atan2(s_, c_); }
/** print */
void print(const std::string& s = "theta") const;
/** equals with an tolerance */
bool equals(const Rot2& R, double tol = 1e-9) const;
/** return DOF, dimensionality of tangent space */
inline size_t dim() const { return 1;}
/** Given 1-dim tangent vector, create new rotation */
Rot2 exmap(const Vector& d) const;
/** return vectorized form (column-wise)*/
inline Vector vector() const { return Vector_(2,c_,s_);}
/** return 2*2 rotation matrix */
Matrix matrix() const {
double r[] = { c_, -s_, s_, c_ };
return Matrix_(2, 2, r);
}
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const {
double r[] = { c_, s_, -s_, c_ };
return Matrix_(2, 2, r);
}
/** inverse transformation */
Rot2 inverse() const { return Rot2(c_, -s_);}
/** composition via sum and difference formulas */
inline Rot2 operator*(const Rot2& R) const {
return Rot2(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/** rotate from rotated to world, syntactic sugar = R*p */
inline Point2 operator*(const Point2& p) const {
return Point2(c_ * p.x() - s_ * p.y(), s_ * p.x() + c_ * p.y());
}
/** rotate from world to rotated = R'*p */
Point2 unrotate(const Point2& p) const {
return Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
}
/** friends */
friend Matrix Dunrotate1(const Rot2& R, const Point2& p);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(s_);
}
};
/**
* Update Rotation with incremental rotation
* @param v a vector of incremental angle
* @param R a 2D rotation
* @return incremental rotation matrix
*/
Rot2 exmap(const Rot2& R, const Vector& v);
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point2 rotate(const Rot2& R, const Point2& p);
Matrix Drotate1(const Rot2& R, const Point2& p);
Matrix Drotate2(const Rot2& R); // does not depend on p !
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point2 unrotate(const Rot2& R, const Point2& p);
Matrix Dunrotate1(const Rot2& R, const Point2& p);
Matrix Dunrotate2(const Rot2& R); // does not depend on p !
} // gtsam
#endif /* ROT2_H_ */

14
cpp/Rot3.cpp
View File

@ -17,6 +17,12 @@ namespace gtsam {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Rot3 Rot3::exmap(const Vector& v) const {
if (zero(v)) return (*this);
return rodriguez(v) * (*this);
}
/* ************************************************************************* */
/** faster than below ? */
/* ************************************************************************* */
@ -52,13 +58,7 @@ namespace gtsam {
/* ************************************************************************* */
Rot3 exmap(const Rot3& R, const Vector& v) {
return rodriguez(v) * R;
}
/* ************************************************************************* */
Rot3 Rot3::exmap(const Vector& v) const {
if (zero(v)) return (*this);
return rodriguez(v) * (*this);
return R.exmap(v);
}
/* ************************************************************************* */

92
cpp/testRot2.cpp Normal file
View File

@ -0,0 +1,92 @@
/**
* @file testRot2.cpp
* @brief Unit tests for Rot2 class
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include "numericalDerivative.h"
#include "Rot2.h"
using namespace gtsam;
Rot2 R(0.1);
Point2 P(0.2, 0.7);
/* ************************************************************************* */
TEST( Rot2, angle)
{
DOUBLES_EQUAL(0.1,R.angle(),1e-9);
}
/* ************************************************************************* */
TEST( Rot2, transpose)
{
CHECK(assert_equal(R.inverse().matrix(),R.transpose()));
}
/* ************************************************************************* */
TEST( Rot2, equals)
{
CHECK(R.equals(R));
Rot2 zero;
CHECK(!R.equals(zero));
}
/* ************************************************************************* */
TEST( Rot2, exmap)
{
Vector v = zero(1);
CHECK(assert_equal(R.exmap(v), R));
}
/* ************************************************************************* */
// rotate derivatives
TEST( Rot2, Drotate1)
{
Matrix computed = Drotate1(R, P);
Matrix numerical = numericalDerivative21(rotate, R, P);
CHECK(assert_equal(numerical,computed));
}
TEST( Rot2, Drotate2_DNrotate2)
{
Matrix computed = Drotate2(R);
Matrix numerical = numericalDerivative22(rotate, R, P);
CHECK(assert_equal(numerical,computed));
}
/* ************************************************************************* */
// unrotate
TEST( Rot2, unrotate)
{
Point2 w = R * P;
CHECK(assert_equal(unrotate(R,w),P));
}
/* ************************************************************************* */
// unrotate derivatives
TEST( Rot2, Dunrotate1)
{
Matrix computed = Dunrotate1(R, P);
Matrix numerical = numericalDerivative21(unrotate, R, P);
CHECK(assert_equal(numerical,computed));
}
TEST( Rot2, Dunrotate2_DNunrotate2)
{
Matrix computed = Dunrotate2(R);
Matrix numerical = numericalDerivative22(unrotate, R, P);
CHECK(assert_equal(numerical,computed));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */