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();
}
/// 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,
ChartJacobian H = boost::none) {
using std::cos;

4
gtsam/geometry/Rot3.cpp
View File

@ -36,10 +36,10 @@ void Rot3::print(const std::string& s) const {
/* ************************************************************************* */
Rot3 Rot3::Random(boost::mt19937& rng) {
// 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);
double angle = randomAngle(rng);
return Rodrigues(w,angle);
return AxisAngle(axis, angle);
}
/* ************************************************************************* */

44
gtsam/geometry/Rot3.h
View File

@ -150,54 +150,54 @@ namespace gtsam {
}
/**
* Rodrigues' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* Convert from axis/angle representation
* @param axis is the rotation axis, unit length
* @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
return Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return SO3::Rodrigues(axis,angle);
return SO3::AxisAngle(axis,angle);
#endif
}
/**
* Rodrigues' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* Convert from axis/angle representation
* @param axisw is the rotation axis, unit length
* @param angle rotation angle
* @return incremental rotation matrix
* @return incremental rotation
*/
static Rot3 Rodrigues(const Point3& axis, double angle) {
return Rodrigues(axis.vector(),angle);
static Rot3 AxisAngle(const Point3& axis, double angle) {
return AxisAngle(axis.vector(),angle);
}
/**
* Rodrigues' formula to compute an incremental rotation matrix
* @param w is the rotation axis
* Convert from axis/angle representation
* @param axis is the rotation axis
* @param angle rotation angle
* @return incremental rotation matrix
* @return incremental rotation
*/
static Rot3 Rodrigues(const Unit3& axis, double angle) {
return Rodrigues(axis.unitVector(),angle);
static Rot3 AxisAngle(const Unit3& axis, double 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
* @return incremental rotation matrix
* @return incremental rotation
*/
static Rot3 Rodrigues(const Vector3& 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 wy Incremental pitch (about Y)
* @param wz Incremental yaw (about Z)
* @return incremental rotation matrix
* @return incremental rotation
*/
static Rot3 Rodrigues(double wx, double wy, double wz) {
return Rodrigues(Vector3(wx, wy, wz));
@ -442,9 +442,9 @@ namespace gtsam {
/// @}
/// @name Deprecated
/// @{
static Rot3 rodriguez(const Vector3& axis, double angle) { return Rodrigues(axis, angle); }
static Rot3 rodriguez(const Point3& axis, double angle) { return Rodrigues(axis, angle); }
static Rot3 rodriguez(const Unit3& 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 AxisAngle(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(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;
}
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
if (theta*theta > std::numeric_limits<double>::epsilon()) {
using std::cos;
using std::sin;
@ -79,7 +79,7 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
double theta = std::sqrt(theta2);
return Rodrigues(omega / theta, theta);
return AxisAngle(omega / theta, theta);
} else {
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 SO3 Rodrigues(const Vector3& axis, double theta);
static SO3 AxisAngle(const Vector3& axis, double theta);
/// @}
/// @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 + ....
Rot3 slow_but_correct_rodriguez(const Vector& w) {
Rot3 slow_but_correct_Rodrigues(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
@ -108,16 +108,16 @@ TEST( Rot3, Rodrigues)
{
Rot3 R1 = Rot3::Rodrigues(epsilon, 0, 0);
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));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
TEST( Rot3, Rodrigues2)
{
Vector axis = Vector3(0., 1., 0.); // rotation around Y
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,
0, 1, 0,
-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);
Rot3 R1 = Rot3::Rodrigues(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_Rodrigues(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
TEST( Rot3, Rodrigues4)
{
Vector axis = Vector3(0., 0., 1.); // rotation around Z
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);
Rot3 expected(c,-s, 0,
s, c, 0,
0, 0, 1);
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 T2(Rot3::Rodrigues(Vector3(0, 1, 0), 2));
Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2));
//******************************************************************************
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
Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
Rot3 i1Ri2 = Rot3::Rodrigues(p1, 1), //
i2Ri3 = Rot3::Rodrigues(p2, 1), //
i3Ri4 = Rot3::Rodrigues(p3, 1);
Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), //
i2Ri3 = Rot3::AxisAngle(p2, 1), //
i3Ri4 = Rot3::AxisAngle(p3, 1);
// The corresponding rotations in the camera frame
Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
@ -47,9 +47,9 @@ typedef noiseModel::Isotropic::shared_ptr Model;
//*************************************************************************
TEST (RotateFactor, checkMath) {
EXPECT(assert_equal(c1Zc2, Rot3::Rodrigues(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::Rodrigues(z2, 1)));
EXPECT(assert_equal(c3Zc4, Rot3::Rodrigues(z3, 1)));
EXPECT(assert_equal(c1Zc2, Rot3::AxisAngle(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::AxisAngle(z2, 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();
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("Expmap", R*Rot3::Expmap(v))
TEST("Retract", R.retract(v))