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
Values initialEstimate;
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)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.print("Initial Estimates:\n");

2
examples/SFMExampleExpressions.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
// Create perturbed 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)
initial.insert(Symbol('x', i), poses[i].compose(delta));
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
// Intentionally initialize the variables off from the ground truth
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)
initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
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
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)
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;
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)
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)
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
// 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
// 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
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);
// 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 ypr(double y, double p, double r);
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
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)
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 T2(Rot3::rodriguez(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 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2));
static Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
const double tol=1e-5;
/* ************************************************************************* */
@ -50,7 +50,7 @@ TEST( Pose3, equals)
/* ************************************************************************* */
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);
EXPECT(assert_equal(expected,Pose3(pose2)));
}
@ -103,7 +103,7 @@ TEST(Pose3, expmap_b)
{
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 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));
}
@ -266,7 +266,7 @@ TEST( Pose3, inverse)
/* ************************************************************************* */
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);
Pose3 T(R,t);
@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate)
/* ************************************************************************* */
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 expected(-1,2,10);
EXPECT(assert_equal(expected, actual, 0.001));
@ -397,7 +397,7 @@ TEST( Pose3, transform_to_rotate)
/* ************************************************************************* */
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 expected(2,1,10);
EXPECT(assert_equal(expected, actual, 0.001));
@ -439,7 +439,7 @@ TEST( Pose3, transformPose_to_itself)
TEST( Pose3, transformPose_to_translation)
{
// 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 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
Pose3 expected(r, Point3(20.,30.,10.));
@ -450,7 +450,7 @@ TEST( Pose3, transformPose_to_translation)
TEST( Pose3, transformPose_to_simple_rotate)
{
// 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 transform(r, Point3(1,2,3));
Pose3 actual = pose2.transform_to(transform);
@ -462,12 +462,12 @@ TEST( Pose3, transformPose_to_simple_rotate)
TEST( Pose3, transformPose_to)
{
// transform to
Rot3 r = Rot3::rodriguez(0,0,-1.570796); //-90 degree yaw
Rot3 r2 = Rot3::rodriguez(0,0,0.698131701); //40 degree yaw
Rot3 r = Rot3::Rodrigues(0,0,-1.570796); //-90 degree yaw
Rot3 r2 = Rot3::Rodrigues(0,0,0.698131701); //40 degree yaw
Pose3 pose2(r2, Point3(21.,32.,13.));
Pose3 transform(r, Point3(1,2,3));
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));
}

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

@ -33,7 +33,7 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_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 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();
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
@ -117,7 +117,7 @@ TEST( Rot3, rodriguez2)
{
Vector axis = Vector3(0., 1., 0.); // rotation around Y
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,
0, 1, 0,
-0.706825, 0, 0.707388);
@ -128,7 +128,7 @@ TEST( Rot3, rodriguez2)
TEST( Rot3, rodriguez3)
{
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);
CHECK(assert_equal(R2,R1));
}
@ -138,7 +138,7 @@ TEST( Rot3, rodriguez4)
{
Vector axis = Vector3(0., 0., 1.); // rotation around Z
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);
Rot3 expected(c,-s, 0,
s, c, 0,
@ -168,7 +168,7 @@ TEST(Rot3, log)
#define CHECK_OMEGA(X,Y,Z) \
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));
// Check zero
@ -201,7 +201,7 @@ TEST(Rot3, log)
// Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
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));
#else
CHECK_OMEGA(x*PI,y*PI,z*PI)
@ -210,7 +210,7 @@ TEST(Rot3, log)
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \
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)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
@ -312,8 +312,8 @@ TEST( Rot3, jacobianLogmap )
/* ************************************************************************* */
TEST(Rot3, manifold_expmap)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7);
Rot3 origin;
// log behaves correctly
@ -399,8 +399,8 @@ TEST( Rot3, unrotate)
/* ************************************************************************* */
TEST( Rot3, compose )
{
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
Rot3 expected = R1 * R2;
Matrix actualH1, actualH2;
@ -419,7 +419,7 @@ TEST( Rot3, compose )
/* ************************************************************************* */
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;
Matrix3 actualH;
@ -444,13 +444,13 @@ TEST( Rot3, between )
0.0, 0.0, 1.0).finished();
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;
EXPECT(assert_equal(R, origin.between(R)));
EXPECT(assert_equal(R.inverse(), R.between(origin)));
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
Rot3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
@ -652,8 +652,8 @@ TEST( Rot3, slerp)
}
//******************************************************************************
Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
Rot3 T1(Rot3::Rodrigues(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::Rodrigues(Vector3(0, 1, 0), 2));
//******************************************************************************
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_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);
/* ************************************************************************* */
TEST(Rot3, manifold_cayley)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7);
Rot3 origin;
// 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
for (size_t i = 0; i < nrPoses; i++) {
// Get the rodriguez vector
// Get the Rodrigues vector
float 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
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.
*/
TEST(BetweenFactor, Rot3) {
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.4, 0.5, 0.6);
Rot3 noise = Rot3(); // Rot3::rodriguez(0.01, 0.01, 0.01); // Uncomment to make unit test fail
Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::Rodrigues(0.4, 0.5, 0.6);
Rot3 noise = Rot3(); // Rot3::Rodrigues(0.01, 0.01, 0.01); // Uncomment to make unit test fail
Rot3 measured = R1.between(R2)*noise ;
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
Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), //
i2Ri3 = Rot3::rodriguez(p2, 1), //
i3Ri4 = Rot3::rodriguez(p3, 1);
Rot3 i1Ri2 = Rot3::Rodrigues(p1, 1), //
i2Ri3 = Rot3::Rodrigues(p2, 1), //
i3Ri4 = Rot3::Rodrigues(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::rodriguez(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1)));
EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1)));
EXPECT(assert_equal(c1Zc2, Rot3::Rodrigues(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::Rodrigues(z2, 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);
}
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);
Velocity3 V(2.0, 3.0, 4.0);
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)
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 T2(Rot3::rodriguez(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 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1);
static Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
//******************************************************************************
TEST(Similarity3, Constructors) {
@ -125,7 +125,7 @@ TEST(Similarity3, Manifold) {
EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
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);

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 {
Vector3 rho = sub(dx, 0, 3);
Rot3 delta_nRn = Rot3::rodriguez(rho);
Rot3 delta_nRn = Rot3::Rodrigues(rho);
Rot3 bRn = bRn_ * delta_nRn;
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();
// 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);
// 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) {
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);
Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0);
@ -36,7 +36,7 @@ TEST(BiasedGPSFactor, errorNoiseless) {
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);
Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0);
@ -51,7 +51,7 @@ TEST(BiasedGPSFactor, errorNoisy) {
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);
Pose3 pose(R,t);
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 x=1.0/norm, y=4.0/norm, z=2.0/norm;
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 canonical coordinates", Rot3::rodriguez(v))
TEST("Rodriguez formula given axis angle", Rot3::Rodrigues(v,0.001))
TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v))
TEST("Expmap", R*Rot3::Expmap(v))
TEST("Retract", R.retract(v))
TEST("Logmap", Rot3::Logmap(R.between(R2)))