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

Renamed two-argument versions of Rodrigues to AxisAngle

release/4.3a0
Frank Dellaert 2015-07-05 16:33:10 -07:00
parent 377b90941b
commit bb9543f251
8 changed files with 50 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Quaternion.h
View File

@ -74,7 +74,7 @@ struct traits<QUATERNION_TYPE> {
return g.inverse(); return g.inverse();
} }
/// Exponential map, using the inlined code from Eigen's converseion from axis/angle /// Exponential map, using the inlined code from Eigen's conversion from axis/angle
static Q Expmap(const Eigen::Ref<const TangentVector>& omega, static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) { ChartJacobian H = boost::none) {
using std::cos; using std::cos;

8
gtsam/geometry/Rot3.cpp
View File

@ -34,12 +34,12 @@ void Rot3::print(const std::string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Random(boost::mt19937 & rng) { Rot3 Rot3::Random(boost::mt19937& rng) {
// TODO allow any engine without including all of boost :-( // TODO allow any engine without including all of boost :-(
Unit3 w = Unit3::Random(rng); Unit3 axis = Unit3::Random(rng);
boost::uniform_real<double> randomAngle(-M_PI,M_PI); boost::uniform_real<double> randomAngle(-M_PI, M_PI);
double angle = randomAngle(rng); double angle = randomAngle(rng);
return Rodrigues(w,angle); return AxisAngle(axis, angle);
} }
/* ************************************************************************* */ /* ************************************************************************* */

46
gtsam/geometry/Rot3.h
View File

@ -141,7 +141,7 @@ namespace gtsam {
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf.
* Assumes vehicle coordinate frame X forward, Y right, Z down. * Assumes vehicle coordinate frame X forward, Y right, Z down.
*/ */
static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion coefficients */ /** Create from Quaternion coefficients */
static Rot3 quaternion(double w, double x, double y, double z) { static Rot3 quaternion(double w, double x, double y, double z) {
@ -150,54 +150,54 @@ namespace gtsam {
} }
/** /**
* Rodrigues' formula to compute an incremental rotation matrix * Convert from axis/angle representation
* @param w is the rotation axis, unit length * @param axis is the rotation axis, unit length
* @param angle rotation angle * @param angle rotation angle
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 Rodrigues(const Vector3& axis, double angle) { static Rot3 AxisAngle(const Vector3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
return Quaternion(Eigen::AngleAxis<double>(angle, axis)); return Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else #else
return SO3::Rodrigues(axis,angle); return SO3::AxisAngle(axis,angle);
#endif #endif
} }
/** /**
* Rodrigues' formula to compute an incremental rotation matrix * Convert from axis/angle representation
* @param w is the rotation axis, unit length * @param axisw is the rotation axis, unit length
* @param angle rotation angle * @param angle rotation angle
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 Rodrigues(const Point3& axis, double angle) { static Rot3 AxisAngle(const Point3& axis, double angle) {
return Rodrigues(axis.vector(),angle); return AxisAngle(axis.vector(),angle);
} }
/** /**
* Rodrigues' formula to compute an incremental rotation matrix * Convert from axis/angle representation
* @param w is the rotation axis * @param axis is the rotation axis
* @param angle rotation angle * @param angle rotation angle
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 Rodrigues(const Unit3& axis, double angle) { static Rot3 AxisAngle(const Unit3& axis, double angle) {
return Rodrigues(axis.unitVector(),angle); return AxisAngle(axis.unitVector(),angle);
} }
/** /**
* Rodrigues' formula to compute an incremental rotation matrix * Rodrigues' formula to compute an incremental rotation
* @param w a vector of incremental roll,pitch,yaw * @param w a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 Rodrigues(const Vector3& w) { static Rot3 Rodrigues(const Vector3& w) {
return Rot3::Expmap(w); return Rot3::Expmap(w);
} }
/** /**
* Rodrigues' formula to compute an incremental rotation matrix * Rodrigues' formula to compute an incremental rotation
* @param wx Incremental roll (about X) * @param wx Incremental roll (about X)
* @param wy Incremental pitch (about Y) * @param wy Incremental pitch (about Y)
* @param wz Incremental yaw (about Z) * @param wz Incremental yaw (about Z)
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 Rodrigues(double wx, double wy, double wz) { static Rot3 Rodrigues(double wx, double wy, double wz) {
return Rodrigues(Vector3(wx, wy, wz)); return Rodrigues(Vector3(wx, wy, wz));
@ -442,9 +442,9 @@ namespace gtsam {
/// @} /// @}
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
static Rot3 rodriguez(const Vector3& axis, double angle) { return Rodrigues(axis, angle); } static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Point3& axis, double angle) { return Rodrigues(axis, angle); } static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Unit3& axis, double angle) { return Rodrigues(axis, angle); } static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }
static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); }
/// @} /// @}

4
gtsam/geometry/SO3.cpp
View File

@ -39,7 +39,7 @@ static SO3 FirstOrder(const Vector3& omega) {
return R; return R;
} }
SO3 SO3::Rodrigues(const Vector3& axis, double theta) { SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
if (theta*theta > std::numeric_limits<double>::epsilon()) { if (theta*theta > std::numeric_limits<double>::epsilon()) {
using std::cos; using std::cos;
using std::sin; using std::sin;
@ -79,7 +79,7 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
double theta2 = omega.dot(omega); double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) { if (theta2 > std::numeric_limits<double>::epsilon()) {
double theta = std::sqrt(theta2); double theta = std::sqrt(theta2);
return Rodrigues(omega / theta, theta); return AxisAngle(omega / theta, theta);
} else { } else {
return FirstOrder(omega); return FirstOrder(omega);
} }

2
gtsam/geometry/SO3.h
View File

@ -59,7 +59,7 @@ public:
} }
/// Static, named constructor TODO think about relation with above /// Static, named constructor TODO think about relation with above
static SO3 Rodrigues(const Vector3& axis, double theta); static SO3 AxisAngle(const Vector3& axis, double theta);
/// @} /// @}
/// @name Testable /// @name Testable

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

@ -95,7 +95,7 @@ TEST( Rot3, equals)
/* ************************************************************************* */ /* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_rodriguez(const Vector& w) { Rot3 slow_but_correct_Rodrigues(const Vector& w) {
double t = norm_2(w); double t = norm_2(w);
Matrix J = skewSymmetric(w / t); Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3(); if (t < 1e-5) return Rot3();
@ -108,16 +108,16 @@ TEST( Rot3, Rodrigues)
{ {
Rot3 R1 = Rot3::Rodrigues(epsilon, 0, 0); Rot3 R1 = Rot3::Rodrigues(epsilon, 0, 0);
Vector w = (Vector(3) << epsilon, 0., 0.).finished(); Vector w = (Vector(3) << epsilon, 0., 0.).finished();
Rot3 R2 = slow_but_correct_rodriguez(w); Rot3 R2 = slow_but_correct_Rodrigues(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez2) TEST( Rot3, Rodrigues2)
{ {
Vector axis = Vector3(0., 1., 0.); // rotation around Y Vector axis = Vector3(0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0; double angle = 3.14 / 4.0;
Rot3 actual = Rot3::Rodrigues(axis, angle); Rot3 actual = Rot3::AxisAngle(axis, angle);
Rot3 expected(0.707388, 0, 0.706825, Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0, 0, 1, 0,
-0.706825, 0, 0.707388); -0.706825, 0, 0.707388);
@ -125,26 +125,26 @@ TEST( Rot3, rodriguez2)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez3) TEST( Rot3, Rodrigues3)
{ {
Vector w = Vector3(0.1, 0.2, 0.3); Vector w = Vector3(0.1, 0.2, 0.3);
Rot3 R1 = Rot3::Rodrigues(w / norm_2(w), norm_2(w)); Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w); Rot3 R2 = slow_but_correct_Rodrigues(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez4) TEST( Rot3, Rodrigues4)
{ {
Vector axis = Vector3(0., 0., 1.); // rotation around Z Vector axis = Vector3(0., 0., 1.); // rotation around Z
double angle = M_PI/2.0; double angle = M_PI/2.0;
Rot3 actual = Rot3::Rodrigues(axis, angle); Rot3 actual = Rot3::AxisAngle(axis, angle);
double c=cos(angle),s=sin(angle); double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0, Rot3 expected(c,-s, 0,
s, c, 0, s, c, 0,
0, 0, 1); 0, 0, 1);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual)); CHECK(assert_equal(slow_but_correct_Rodrigues(axis*angle),actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -652,8 +652,8 @@ TEST( Rot3, slerp)
} }
//****************************************************************************** //******************************************************************************
Rot3 T1(Rot3::Rodrigues(Vector3(0, 0, 1), 1)); Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::Rodrigues(Vector3(0, 1, 0), 2)); Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2));
//****************************************************************************** //******************************************************************************
TEST(Rot3 , Invariants) { TEST(Rot3 , Invariants) {

12
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -29,9 +29,9 @@ Rot3 iRc(cameraX, cameraY, cameraZ);
// Now, let's create some rotations around IMU frame // Now, let's create some rotations around IMU frame
Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
Rot3 i1Ri2 = Rot3::Rodrigues(p1, 1), // Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), //
i2Ri3 = Rot3::Rodrigues(p2, 1), // i2Ri3 = Rot3::AxisAngle(p2, 1), //
i3Ri4 = Rot3::Rodrigues(p3, 1); i3Ri4 = Rot3::AxisAngle(p3, 1);
// The corresponding rotations in the camera frame // The corresponding rotations in the camera frame
Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
@ -47,9 +47,9 @@ typedef noiseModel::Isotropic::shared_ptr Model;
//************************************************************************* //*************************************************************************
TEST (RotateFactor, checkMath) { TEST (RotateFactor, checkMath) {
EXPECT(assert_equal(c1Zc2, Rot3::Rodrigues(z1, 1))); EXPECT(assert_equal(c1Zc2, Rot3::AxisAngle(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::Rodrigues(z2, 1))); EXPECT(assert_equal(c2Zc3, Rot3::AxisAngle(z2, 1)));
EXPECT(assert_equal(c3Zc4, Rot3::Rodrigues(z3, 1))); EXPECT(assert_equal(c3Zc4, Rot3::AxisAngle(z3, 1)));
} }
//************************************************************************* //*************************************************************************

2
timing/timeRot3.cpp
View File

@ -42,7 +42,7 @@ int main()
Vector v = (Vector(3) << x, y, z).finished(); Vector v = (Vector(3) << x, y, z).finished();
Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v); Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v);
TEST("Rodriguez formula given axis angle", Rot3::Rodrigues(v,0.001)) TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001))
TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v)) TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v))
TEST("Expmap", R*Rot3::Expmap(v)) TEST("Expmap", R*Rot3::Expmap(v))
TEST("Retract", R.retract(v)) TEST("Retract", R.retract(v))