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

switch to Rodrigues everywhere

release/4.3a0
Frank Dellaert 2015-07-05 16:11:04 -07:00
parent ecfa459590
commit 377b90941b
19 changed files with 67 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SFMExample.cpp
View File

@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
// Intentionally initialize the variables off from the ground truth // Intentionally initialize the variables off from the ground truth
Values initialEstimate; Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i) for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j) for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.print("Initial Estimates:\n"); initialEstimate.print("Initial Estimates:\n");

2
examples/SFMExampleExpressions.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
// Create perturbed initial // Create perturbed initial
Values initial; Values initial;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i) for (size_t i = 0; i < poses.size(); ++i)
initial.insert(Symbol('x', i), poses[i].compose(delta)); initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j) for (size_t j = 0; j < points.size(); ++j)

2
examples/SFMExample_SmartFactor.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
// Create the initial estimate to the solution // Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth // Intentionally initialize the variables off from the ground truth
Values initialEstimate; Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i) for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, Camera(poses[i].compose(delta), K)); initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
initialEstimate.print("Initial Estimates:\n"); initialEstimate.print("Initial Estimates:\n");

2
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -81,7 +81,7 @@ int main(int argc, char* argv[]) {
// Create the initial estimate to the solution // Create the initial estimate to the solution
Values initialEstimate; Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i) for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, Camera(poses[i].compose(delta),K)); initialEstimate.insert(i, Camera(poses[i].compose(delta),K));

2
examples/SelfCalibrationExample.cpp
View File

@ -82,7 +82,7 @@ int main(int argc, char* argv[]) {
Values initialEstimate; Values initialEstimate;
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0)); initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
for (size_t i = 0; i < poses.size(); ++i) for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j) for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));

2
examples/VisualISAM2Example.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
// Add an initial guess for the current pose // Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth // Intentionally initialize the variables off from the ground truth
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
// If this is the first iteration, add a prior on the first pose to set the coordinate frame // If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale // and a prior on the first landmark to set the scale

2
examples/VisualISAMExample.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
} }
// Intentionally initialize the variables off from the ground truth // Intentionally initialize the variables off from the ground truth
Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise); Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose // Add an initial guess for the current pose

2
gtsam.h
View File

@ -438,7 +438,7 @@ class Rot3 {
static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft)
static gtsam::Rot3 ypr(double y, double p, double r); static gtsam::Rot3 ypr(double y, double p, double r);
static gtsam::Rot3 quaternion(double w, double x, double y, double z); static gtsam::Rot3 quaternion(double w, double x, double y, double z);
static gtsam::Rot3 rodriguez(Vector v); static gtsam::Rot3 Rodrigues(Vector v);
// Testable // Testable
void print(string s) const; void print(string s) const;

26
gtsam/geometry/tests/testPose3.cpp
View File

@ -32,10 +32,10 @@ GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3)
static Point3 P(0.2,0.7,-2); static Point3 P(0.2,0.7,-2);
static Rot3 R = Rot3::rodriguez(0.3,0,0); static Rot3 R = Rot3::Rodrigues(0.3,0,0);
static Pose3 T(R,Point3(3.5,-8.2,4.2)); static Pose3 T(R,Point3(3.5,-8.2,4.2));
static Pose3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); static Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2));
static Pose3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3)); static Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
const double tol=1e-5; const double tol=1e-5;
/* ************************************************************************* */ /* ************************************************************************* */
@ -50,7 +50,7 @@ TEST( Pose3, equals)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, constructors) TEST( Pose3, constructors)
{ {
Pose3 expected(Rot3::rodriguez(0,0,3),Point3(1,2,0)); Pose3 expected(Rot3::Rodrigues(0,0,3),Point3(1,2,0));
Pose2 pose2(1,2,3); Pose2 pose2(1,2,3);
EXPECT(assert_equal(expected,Pose3(pose2))); EXPECT(assert_equal(expected,Pose3(pose2)));
} }
@ -103,7 +103,7 @@ TEST(Pose3, expmap_b)
{ {
Pose3 p1(Rot3(), Point3(100, 0, 0)); Pose3 p1(Rot3(), Point3(100, 0, 0));
Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0).finished()); Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0).finished());
Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); Pose3 expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
EXPECT(assert_equal(expected, p2,1e-2)); EXPECT(assert_equal(expected, p2,1e-2));
} }
@ -266,7 +266,7 @@ TEST( Pose3, inverse)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, inverseDerivatives2) TEST( Pose3, inverseDerivatives2)
{ {
Rot3 R = Rot3::rodriguez(0.3,0.4,-0.5); Rot3 R = Rot3::Rodrigues(0.3,0.4,-0.5);
Point3 t(3.5,-8.2,4.2); Point3 t(3.5,-8.2,4.2);
Pose3 T(R,t); Pose3 T(R,t);
@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to_rotate) TEST( Pose3, transform_to_rotate)
{ {
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3()); Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3());
Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 actual = transform.transform_to(Point3(2,1,10));
Point3 expected(-1,2,10); Point3 expected(-1,2,10);
EXPECT(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
@ -397,7 +397,7 @@ TEST( Pose3, transform_to_rotate)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to) TEST( Pose3, transform_to)
{ {
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0)); Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(2,4, 0));
Point3 actual = transform.transform_to(Point3(3,2,10)); Point3 actual = transform.transform_to(Point3(3,2,10));
Point3 expected(2,1,10); Point3 expected(2,1,10);
EXPECT(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
@ -439,7 +439,7 @@ TEST( Pose3, transformPose_to_itself)
TEST( Pose3, transformPose_to_translation) TEST( Pose3, transformPose_to_translation)
{ {
// transform translation only // transform translation only
Rot3 r = Rot3::rodriguez(-1.570796,0,0); Rot3 r = Rot3::Rodrigues(-1.570796,0,0);
Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 pose2(r, Point3(21.,32.,13.));
Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
Pose3 expected(r, Point3(20.,30.,10.)); Pose3 expected(r, Point3(20.,30.,10.));
@ -450,7 +450,7 @@ TEST( Pose3, transformPose_to_translation)
TEST( Pose3, transformPose_to_simple_rotate) TEST( Pose3, transformPose_to_simple_rotate)
{ {
// transform translation only // transform translation only
Rot3 r = Rot3::rodriguez(0,0,-1.570796); Rot3 r = Rot3::Rodrigues(0,0,-1.570796);
Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 pose2(r, Point3(21.,32.,13.));
Pose3 transform(r, Point3(1,2,3)); Pose3 transform(r, Point3(1,2,3));
Pose3 actual = pose2.transform_to(transform); Pose3 actual = pose2.transform_to(transform);
@ -462,12 +462,12 @@ TEST( Pose3, transformPose_to_simple_rotate)
TEST( Pose3, transformPose_to) TEST( Pose3, transformPose_to)
{ {
// transform to // transform to
Rot3 r = Rot3::rodriguez(0,0,-1.570796); //-90 degree yaw Rot3 r = Rot3::Rodrigues(0,0,-1.570796); //-90 degree yaw
Rot3 r2 = Rot3::rodriguez(0,0,0.698131701); //40 degree yaw Rot3 r2 = Rot3::Rodrigues(0,0,0.698131701); //40 degree yaw
Pose3 pose2(r2, Point3(21.,32.,13.)); Pose3 pose2(r2, Point3(21.,32.,13.));
Pose3 transform(r, Point3(1,2,3)); Pose3 transform(r, Point3(1,2,3));
Pose3 actual = pose2.transform_to(transform); Pose3 actual = pose2.transform_to(transform);
Pose3 expected(Rot3::rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); Pose3 expected(Rot3::Rodrigues(0,0,2.26892803), Point3(-30.,20.,10.));
EXPECT(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
} }

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

@ -33,7 +33,7 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot3) GTSAM_CONCEPT_TESTABLE_INST(Rot3)
GTSAM_CONCEPT_LIE_INST(Rot3) GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0); static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001; static double error = 1e-9, epsilon = 0.001;
@ -104,9 +104,9 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez) TEST( Rot3, Rodrigues)
{ {
Rot3 R1 = Rot3::rodriguez(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_rodriguez(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
@ -117,7 +117,7 @@ TEST( Rot3, rodriguez2)
{ {
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::rodriguez(axis, angle); Rot3 actual = Rot3::Rodrigues(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);
@ -128,7 +128,7 @@ TEST( Rot3, rodriguez2)
TEST( Rot3, rodriguez3) TEST( Rot3, rodriguez3)
{ {
Vector w = Vector3(0.1, 0.2, 0.3); Vector w = Vector3(0.1, 0.2, 0.3);
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R1 = Rot3::Rodrigues(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w); Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
} }
@ -138,7 +138,7 @@ TEST( Rot3, rodriguez4)
{ {
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::rodriguez(axis, angle); Rot3 actual = Rot3::Rodrigues(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,
@ -168,7 +168,7 @@ TEST(Rot3, log)
#define CHECK_OMEGA(X,Y,Z) \ #define CHECK_OMEGA(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
R = Rot3::rodriguez(w); \ R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
// Check zero // Check zero
@ -201,7 +201,7 @@ TEST(Rot3, log)
// Windows and Linux have flipped sign in quaternion mode // Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) #if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x*PI, y*PI, z*PI).finished(); w = (Vector(3) << x*PI, y*PI, z*PI).finished();
R = Rot3::rodriguez(w); R = Rot3::Rodrigues(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
#else #else
CHECK_OMEGA(x*PI,y*PI,z*PI) CHECK_OMEGA(x*PI,y*PI,z*PI)
@ -210,7 +210,7 @@ TEST(Rot3, log)
// Check 360 degree rotations // Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \ #define CHECK_OMEGA_ZERO(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
R = Rot3::rodriguez(w); \ R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
@ -312,8 +312,8 @@ TEST( Rot3, jacobianLogmap )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, manifold_expmap) TEST(Rot3, manifold_expmap)
{ {
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7);
Rot3 origin; Rot3 origin;
// log behaves correctly // log behaves correctly
@ -399,8 +399,8 @@ TEST( Rot3, unrotate)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, compose ) TEST( Rot3, compose )
{ {
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
Rot3 expected = R1 * R2; Rot3 expected = R1 * R2;
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
@ -419,7 +419,7 @@ TEST( Rot3, compose )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, inverse ) TEST( Rot3, inverse )
{ {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
Rot3 I; Rot3 I;
Matrix3 actualH; Matrix3 actualH;
@ -444,13 +444,13 @@ TEST( Rot3, between )
0.0, 0.0, 1.0).finished(); 0.0, 0.0, 1.0).finished();
EXPECT(assert_equal(expectedr1, r1.matrix())); EXPECT(assert_equal(expectedr1, r1.matrix()));
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2);
Rot3 origin; Rot3 origin;
EXPECT(assert_equal(R, origin.between(R))); EXPECT(assert_equal(R, origin.between(R)));
EXPECT(assert_equal(R.inverse(), R.between(origin))); EXPECT(assert_equal(R.inverse(), R.between(origin)));
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
Rot3 expected = R1.inverse() * R2; Rot3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
@ -652,8 +652,8 @@ TEST( Rot3, slerp)
} }
//****************************************************************************** //******************************************************************************
Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1)); Rot3 T1(Rot3::Rodrigues(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); Rot3 T2(Rot3::Rodrigues(Vector3(0, 1, 0), 2));
//****************************************************************************** //******************************************************************************
TEST(Rot3 , Invariants) { TEST(Rot3 , Invariants) {

6
gtsam/geometry/tests/testRot3M.cpp
View File

@ -28,14 +28,14 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot3) GTSAM_CONCEPT_TESTABLE_INST(Rot3)
GTSAM_CONCEPT_LIE_INST(Rot3) GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0); static Point3 P(0.2, 0.7, -2.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, manifold_cayley) TEST(Rot3, manifold_cayley)
{ {
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7);
Rot3 origin; Rot3 origin;
// log behaves correctly // log behaves correctly

4
gtsam/slam/dataset.cpp
View File

@ -720,10 +720,10 @@ bool readBAL(const string& filename, SfM_data &data) {
// Get the information for the camera poses // Get the information for the camera poses
for (size_t i = 0; i < nrPoses; i++) { for (size_t i = 0; i < nrPoses; i++) {
// Get the rodriguez vector // Get the Rodrigues vector
float wx, wy, wz; float wx, wy, wz;
is >> wx >> wy >> wz; is >> wx >> wy >> wz;
Rot3 R = Rot3::rodriguez(wx, wy, wz); // BAL-OpenGL rotation matrix Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix
// Get the translation vector // Get the translation vector
float tx, ty, tz; float tx, ty, tz;

6
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -19,9 +19,9 @@ using namespace gtsam::noiseModel;
* This TEST should fail. If you want it to pass, change noise to 0. * This TEST should fail. If you want it to pass, change noise to 0.
*/ */
TEST(BetweenFactor, Rot3) { TEST(BetweenFactor, Rot3) {
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.4, 0.5, 0.6); Rot3 R2 = Rot3::Rodrigues(0.4, 0.5, 0.6);
Rot3 noise = Rot3(); // Rot3::rodriguez(0.01, 0.01, 0.01); // Uncomment to make unit test fail Rot3 noise = Rot3(); // Rot3::Rodrigues(0.01, 0.01, 0.01); // Uncomment to make unit test fail
Rot3 measured = R1.between(R2)*noise ; Rot3 measured = R1.between(R2)*noise ;
BetweenFactor<Rot3> factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05)); BetweenFactor<Rot3> factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05));

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::rodriguez(p1, 1), // Rot3 i1Ri2 = Rot3::Rodrigues(p1, 1), //
i2Ri3 = Rot3::rodriguez(p2, 1), // i2Ri3 = Rot3::Rodrigues(p2, 1), //
i3Ri4 = Rot3::rodriguez(p3, 1); i3Ri4 = Rot3::Rodrigues(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::rodriguez(z1, 1))); EXPECT(assert_equal(c1Zc2, Rot3::Rodrigues(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1))); EXPECT(assert_equal(c2Zc3, Rot3::Rodrigues(z2, 1)));
EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1))); EXPECT(assert_equal(c3Zc4, Rot3::Rodrigues(z3, 1)));
} }
//************************************************************************* //*************************************************************************

2
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -179,7 +179,7 @@ PoseRTV transformed_from_proxy(const PoseRTV& a, const Pose3& trans) {
return a.transformed_from(trans); return a.transformed_from(trans);
} }
TEST( testPoseRTV, transformed_from_1 ) { TEST( testPoseRTV, transformed_from_1 ) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
Point3 T(1.0, 2.0, 3.0); Point3 T(1.0, 2.0, 3.0);
Velocity3 V(2.0, 3.0, 4.0); Velocity3 V(2.0, 3.0, 4.0);
PoseRTV start(R, T, V); PoseRTV start(R, T, V);

8
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -36,10 +36,10 @@ using symbol_shorthand::X;
GTSAM_CONCEPT_TESTABLE_INST(Similarity3) GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
static Point3 P(0.2,0.7,-2); static Point3 P(0.2,0.7,-2);
static Rot3 R = Rot3::rodriguez(0.3,0,0); static Rot3 R = Rot3::Rodrigues(0.3,0,0);
static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); static Similarity3 T(R,Point3(3.5,-8.2,4.2),1);
static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); static Similarity3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1);
static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); static Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
//****************************************************************************** //******************************************************************************
TEST(Similarity3, Constructors) { TEST(Similarity3, Constructors) {
@ -125,7 +125,7 @@ TEST(Similarity3, Manifold) {
EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1); Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1);
Rot3 R = Rot3::rodriguez(0.3,0,0); Rot3 R = Rot3::Rodrigues(0.3,0,0);
Vector vlocal2 = sim.localCoordinates(other2); Vector vlocal2 = sim.localCoordinates(other2);

4
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -69,7 +69,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const {
Vector3 rho = sub(dx, 0, 3); Vector3 rho = sub(dx, 0, 3);
Rot3 delta_nRn = Rot3::rodriguez(rho); Rot3 delta_nRn = Rot3::Rodrigues(rho);
Rot3 bRn = bRn_ * delta_nRn; Rot3 bRn = bRn_ * delta_nRn;
Vector3 x_g = x_g_ + sub(dx, 3, 6); Vector3 x_g = x_g_ + sub(dx, 3, 6);
@ -104,7 +104,7 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u,
Vector3 n_omega_bn = (nRb*b_omega_bn).vector(); Vector3 n_omega_bn = (nRb*b_omega_bn).vector();
// integrate bRn using exponential map, assuming constant over dt // integrate bRn using exponential map, assuming constant over dt
Rot3 delta_nRn = Rot3::rodriguez(n_omega_bn*dt); Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt);
Rot3 bRn = bRn_.compose(delta_nRn); Rot3 bRn = bRn_.compose(delta_nRn);
// implicit updating of biases (variables just do not change) // implicit updating of biases (variables just do not change)

6
gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
View File

@ -21,7 +21,7 @@ using symbol_shorthand::B;
TEST(BiasedGPSFactor, errorNoiseless) { TEST(BiasedGPSFactor, errorNoiseless) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2); Point3 t(1.0, 0.5, 0.2);
Pose3 pose(R,t); Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0); Point3 bias(0.0,0.0,0.0);
@ -36,7 +36,7 @@ TEST(BiasedGPSFactor, errorNoiseless) {
TEST(BiasedGPSFactor, errorNoisy) { TEST(BiasedGPSFactor, errorNoisy) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2); Point3 t(1.0, 0.5, 0.2);
Pose3 pose(R,t); Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0); Point3 bias(0.0,0.0,0.0);
@ -51,7 +51,7 @@ TEST(BiasedGPSFactor, errorNoisy) {
TEST(BiasedGPSFactor, jacobian) { TEST(BiasedGPSFactor, jacobian) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2); Point3 t(1.0, 0.5, 0.2);
Pose3 pose(R,t); Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0); Point3 bias(0.0,0.0,0.0);

6
timing/timeRot3.cpp
View File

@ -40,10 +40,10 @@ int main()
double norm=sqrt(1.0+16.0+4.0); double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm; double x=1.0/norm, y=4.0/norm, z=2.0/norm;
Vector v = (Vector(3) << x, y, z).finished(); Vector v = (Vector(3) << x, y, z).finished();
Rot3 R = Rot3::rodriguez(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::rodriguez(v,0.001)) TEST("Rodriguez formula given axis angle", Rot3::Rodrigues(v,0.001))
TEST("Rodriguez formula given canonical coordinates", Rot3::rodriguez(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))
TEST("Logmap", Rot3::Logmap(R.between(R2))) TEST("Logmap", Rot3::Logmap(R.between(R2)))