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

Merged in fix/smallRetracts (pull request #170) 

Cleaned up expmap near identity for Rot3/Pose3
release/4.3a0
Frank Dellaert 2015-07-08 16:30:44 -07:00
parent cf052850dd 66d7c26a11
commit e331fa1b76
31 changed files with 229 additions and 205 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;

2
gtsam/base/ThreadsafeException.h
View File

@ -22,13 +22,13 @@
#include <gtsam/config.h> // for GTSAM_USE_TBB #include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/optional/optional.hpp> #include <boost/optional/optional.hpp>
#include <tbb/scalable_allocator.h>
#include <string> #include <string>
#include <typeinfo> #include <typeinfo>
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
#include <tbb/tbb_allocator.h> #include <tbb/tbb_allocator.h>
#include <tbb/tbb_exception.h> #include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#include <iostream> #include <iostream>
#endif #endif

26
gtsam/geometry/Pose3.cpp
View File

@ -32,7 +32,7 @@ GTSAM_CONCEPT_POSE_INST(Pose3);
/* ************************************************************************* */ /* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) : Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_(
Point3(pose2.x(), pose2.y(), 0)) { Point3(pose2.x(), pose2.y(), 0)) {
} }
@ -112,24 +112,20 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */ /** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) { if (H) *H = ExpmapDerivative(xi);
*H = ExpmapDerivative(xi);
}
// get angular velocity omega and translational velocity v from twist xi // get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
double theta = w.norm(); Rot3 R = Rot3::Expmap(omega.vector());
if (theta < 1e-10) { double theta2 = omega.dot(omega);
static const Rot3 I; if (theta2 > std::numeric_limits<double>::epsilon()) {
return Pose3(I, v); double omega_v = omega.dot(v); // translation parallel to axis
} else { Point3 omega_cross_v = omega.cross(v); // points towards axis
Point3 n(w / theta); // axis unit vector Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2;
Rot3 R = Rot3::rodriguez(n.vector(), theta);
double vn = n.dot(v); // translation parallel to n
Point3 n_cross_v = n.cross(v); // points towards axis
Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n;
return Pose3(R, t); return Pose3(R, t);
} else {
return Pose3(R, v);
} }
} }

29
gtsam/geometry/Quaternion.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives #include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
#include <limits>
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -73,14 +74,22 @@ struct traits<QUATERNION_TYPE> {
return g.inverse(); return g.inverse();
} }
/// Exponential map, simply be converting omega to axis/angle representation /// 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) {
if(H) *H = SO3::ExpmapDerivative(omega); using std::cos;
if (omega.isZero()) return Q::Identity(); using std::sin;
else { if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
_Scalar angle = omega.norm(); _Scalar theta2 = omega.dot(omega);
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
_Scalar theta = std::sqrt(theta2);
_Scalar ha = _Scalar(0.5) * theta;
Vector3 vec = (sin(ha) / theta) * omega;
return Q(cos(ha), vec.x(), vec.y(), vec.z());
} else {
// first order approximation sin(theta/2)/theta = 0.5
Vector3 vec = _Scalar(0.5) * omega;
return Q(1.0, vec.x(), vec.y(), vec.z());
} }
} }
@ -93,9 +102,9 @@ struct traits<QUATERNION_TYPE> {
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10; NearlyNegativeOne = -1.0 + 1e-10;
Vector3 omega; TangentVector omega;
const double qw = q.w(); const _Scalar qw = q.w();
// See Quaternion-Logmap.nb in doc for Taylor expansions // See Quaternion-Logmap.nb in doc for Taylor expansions
if (qw > NearlyOne) { if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1 // Taylor expansion of (angle / s) at 1
@ -107,7 +116,7 @@ struct traits<QUATERNION_TYPE> {
omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else { } else {
// Normal, away from zero case // Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous // Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI) if (angle > M_PI)
angle -= twoPi; angle -= twoPi;
@ -116,7 +125,7 @@ struct traits<QUATERNION_TYPE> {
omega = (angle / s) * q.vec(); omega = (angle / s) * q.vec();
} }
if(H) *H = SO3::LogmapDerivative(omega); if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
return omega; return omega;
} }

21
gtsam/geometry/Rot3.cpp
View File

@ -33,30 +33,13 @@ void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s); gtsam::print((Matrix)matrix(), s);
} }
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
return rodriguez((Vector)w.vector(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Unit3& w, double theta) {
return rodriguez(w.point3(),theta);
}
/* ************************************************************************* */ /* ************************************************************************* */
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 rodriguez(w,angle); return AxisAngle(axis, angle);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w) {
double t = w.norm();
if (t < 1e-10) return Rot3();
return rodriguez(w/t, t);
} }
/* ************************************************************************* */ /* ************************************************************************* */

78
gtsam/geometry/Rot3.h
View File

@ -22,8 +22,9 @@
#pragma once #pragma once
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
@ -149,45 +150,58 @@ namespace gtsam {
} }
/** /**
* Rodriguez' 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 theta rotation angle * @param angle rotation angle
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 rodriguez(const Vector3& w, double theta); static Rot3 AxisAngle(const Vector3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return SO3::AxisAngle(axis,angle);
#endif
}
/** /**
* Rodriguez' 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 theta rotation angle * @param angle rotation angle
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 rodriguez(const Point3& w, double theta); static Rot3 AxisAngle(const Point3& axis, double angle) {
return AxisAngle(axis.vector(),angle);
}
/** /**
* Rodriguez' 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 theta rotation angle * @param angle rotation angle
* @return incremental rotation matrix * @return incremental rotation
*/ */
static Rot3 rodriguez(const Unit3& w, double theta); static Rot3 AxisAngle(const Unit3& axis, double angle) {
return AxisAngle(axis.unitVector(),angle);
}
/** /**
* Rodriguez' formula to compute an incremental rotation matrix * Rodrigues' formula to compute an incremental rotation
* @param v 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 rodriguez(const Vector3& v); static Rot3 Rodrigues(const Vector3& w) {
return Rot3::Expmap(w);
}
/** /**
* Rodriguez' 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 rodriguez(double wx, double wy, double wz) static Rot3 Rodrigues(double wx, double wy, double wz) {
{ return rodriguez(Vector3(wx, wy, wz));} return Rodrigues(Vector3(wx, wy, wz));
}
/// @} /// @}
/// @name Testable /// @name Testable
@ -272,11 +286,15 @@ namespace gtsam {
/** /**
* Exponential map at identity - create a rotation from canonical coordinates * Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/ */
static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) {
if(H) *H = Rot3::ExpmapDerivative(v); if(H) *H = Rot3::ExpmapDerivative(v);
if (zero(v)) return Rot3(); else return rodriguez(v); #ifdef GTSAM_USE_QUATERNIONS
return traits<Quaternion>::Expmap(v);
#else
return traits<SO3>::Expmap(v);
#endif
} }
/** /**
@ -422,6 +440,14 @@ namespace gtsam {
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
/// @} /// @}
/// @name Deprecated
/// @{
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); }
/// @}
private: private:

5
gtsam/geometry/Rot3M.cpp
View File

@ -117,11 +117,6 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
); );
} }
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return SO3::Rodrigues(w,theta);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const { Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_)); return Rot3(Matrix3(rot_*R2.rot_));

4
gtsam/geometry/Rot3Q.cpp
View File

@ -83,10 +83,6 @@ namespace gtsam {
Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
} }
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return Quaternion(Eigen::AngleAxis<double>(theta, w));
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const { Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_); return Rot3(quaternion_ * R2.quaternion_);

69
gtsam/geometry/SO3.cpp
View File

@ -19,25 +19,41 @@
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <cmath> #include <cmath>
#include <limits>
using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
SO3 SO3::Rodrigues(const Vector3& axis, double theta) { // Near zero, we just have I + skew(omega)
static SO3 FirstOrder(const Vector3& omega) {
Matrix3 R;
R(0, 0) = 1.;
R(1, 0) = omega.z();
R(2, 0) = -omega.y();
R(0, 1) = -omega.z();
R(1, 1) = 1.;
R(2, 1) = omega.x();
R(0, 2) = omega.y();
R(1, 2) = -omega.x();
R(2, 2) = 1.;
return R;
}
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
if (theta*theta > std::numeric_limits<double>::epsilon()) {
using std::cos; using std::cos;
using std::sin; using std::sin;
// get components of axis \omega // get components of axis \omega, where is a unit vector
double wx = axis(0), wy = axis(1), wz = axis(2); const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta; const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, wz_sintheta = wz * sintheta; const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
wz_sintheta = wz * sintheta;
double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz; const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
double C22 = c_1 * wz * wz; const double C22 = c_1 * wz * wz;
Matrix3 R; Matrix3 R;
R(0, 0) = costheta + C00; R(0, 0) = costheta + C00;
@ -49,22 +65,23 @@ SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
R(0, 2) = wy_sintheta + C02; R(0, 2) = wy_sintheta + C02;
R(1, 2) = -wx_sintheta + C12; R(1, 2) = -wx_sintheta + C12;
R(2, 2) = costheta + C22; R(2, 2) = costheta + C22;
return R; return R;
} else {
return FirstOrder(axis*theta);
}
} }
/// simply convert omega to axis/angle representation /// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Vector3& omega, SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
ChartJacobian H) { if (H) *H = ExpmapDerivative(omega);
if (H) double theta2 = omega.dot(omega);
*H = ExpmapDerivative(omega); if (theta2 > std::numeric_limits<double>::epsilon()) {
double theta = std::sqrt(theta2);
if (omega.isZero()) return AxisAngle(omega / theta, theta);
return Identity(); } else {
else { return FirstOrder(omega);
double angle = omega.norm();
return Rodrigues(omega / angle, angle);
} }
} }
@ -116,8 +133,9 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::cos; using std::cos;
using std::sin; using std::sin;
if(zero(omega)) return I_3x3; double theta2 = omega.dot(omega);
double theta = omega.norm(); // rotation angle if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION #ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega); Matrix3 X = skewSymmetric(omega);
@ -147,8 +165,9 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos; using std::cos;
using std::sin; using std::sin;
if(zero(omega)) return I_3x3; double theta2 = omega.dot(omega);
double theta = omega.norm(); if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION #ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega); Matrix3 X = skewSymmetric(omega);

4
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
@ -93,7 +93,7 @@ public:
/** /**
* Exponential map at identity - create a rotation from canonical coordinates * Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/ */
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);

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));
} }

52
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;
@ -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();
@ -104,20 +104,20 @@ 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_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::rodriguez(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::rodriguez(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::rodriguez(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));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -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::AxisAngle(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); Rot3 T2(Rot3::AxisAngle(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));

2
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -246,7 +246,7 @@ TEST( InitializePose3, initializePoses )
inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel)); inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
Values initial = InitializePose3::initialize(*inputGraph); Values initial = InitializePose3::initialize(*inputGraph);
EXPECT(assert_equal(*expectedValues,initial,1e-4)); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !!
} }

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

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

@ -267,9 +267,9 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams); LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7)); EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-5));
EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-7)); EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-5));
EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-7)); EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-5));
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
// VectorValues delta = GFG->optimize(); // VectorValues delta = GFG->optimize();

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);

2
tests/testGeneralSFMFactorB.cpp
View File

@ -61,7 +61,7 @@ TEST(PinholeCamera, BAL) {
Values actual = lm.optimize(); Values actual = lm.optimize();
double actualError = graph.error(actual); double actualError = graph.error(actual);
EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-7); EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
} }
/* ************************************************************************* */ /* ************************************************************************* */

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::AxisAngle(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)))