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

Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues

release/4.3a0
dellaert 2015-02-11 12:36:16 +01:00
parent e84ed155fa 6a212c1c2d
commit ee3e096135
26 changed files with 524 additions and 323 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2177,6 +2177,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testQuaternion.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testQuaternion.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

15
gtsam/base/testLie.h
View File

@ -38,22 +38,23 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
// Inverse // Inverse
OJ none; OJ none;
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); EXPECT(assert_equal<G>(t1.inverse(),T::Inverse(t1, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1)); EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1)); EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
// Compose // Compose
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); EXPECT(assert_equal<G>(t1 * t2,T::Compose(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1)); EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2)); EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
// Between // Between
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); EXPECT(assert_equal<G>(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1)); EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2)); EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
} }
// Do a comprehensive test of Lie Group Chart derivatives // Do a comprehensive test of Lie Group Chart derivatives
template<typename G> template<typename G>
void testChartDerivatives(TestResult& result_, const std::string& name_, void testChartDerivatives(TestResult& result_, const std::string& name_,
@ -61,18 +62,18 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
Matrix H1, H2; Matrix H1, H2;
typedef traits<G> T; typedef traits<G> T;
typedef typename G::TangentVector V; typedef typename T::TangentVector V;
typedef OptionalJacobian<T::dimension,T::dimension> OJ; typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Retract // Retract
OJ none; OJ none;
V w12 = T::Local(t1, t2); V w12 = T::Local(t1, t2);
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); EXPECT(assert_equal<G>(t2, T::Retract(t1,w12, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1)); EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2)); EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
// Local // Local
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1)); EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2)); EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
} }

93
gtsam/geometry/Quaternion.h
View File

@ -15,8 +15,11 @@
* @author Frank Dellaert * @author Frank Dellaert
**/ **/
#pragma once
#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
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -37,19 +40,6 @@ struct traits<QUATERNION_TYPE> {
return Q::Identity(); return Q::Identity();
} }
static Q Compose(const Q &g, const Q & h) {
return g * h;
}
static Q Between(const Q &g, const Q & h) {
Q d = g.inverse() * h;
return d;
}
static Q Inverse(const Q &g) {
return g.inverse();
}
/// @} /// @}
/// @name Basic manifold traits /// @name Basic manifold traits
/// @{ /// @{
@ -62,41 +52,36 @@ struct traits<QUATERNION_TYPE> {
/// @} /// @}
/// @name Lie group traits /// @name Lie group traits
/// @{ /// @{
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = static Q Compose(const Q &g, const Q & h,
boost::none) { ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
if (Hg) if (Hg) *Hg = h.toRotationMatrix().transpose();
*Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? ) if (Hh) *Hh = I_3x3;
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? )
return g * h; return g * h;
} }
static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = static Q Between(const Q &g, const Q & h,
boost::none) { ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
Q d = g.inverse() * h; Q d = g.inverse() * h;
if (Hg) if (Hg) *Hg = -d.toRotationMatrix().transpose();
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart if (Hh) *Hh = I_3x3;
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) )
return d; return d;
} }
static Q Inverse(const Q &g, ChartJacobian H) { static Q Inverse(const Q &g,
if (H) ChartJacobian H = boost::none) {
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart if (H) *H = -g.toRotationMatrix();
return g.inverse(); return g.inverse();
} }
/// Exponential map, simply be converting omega to axis/angle representation /// Exponential map, simply be converting omega to axis/angle representation
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 (omega.isZero()) if(H) *H = SO3::ExpmapDerivative(omega);
return Q::Identity(); if (omega.isZero()) return Q::Identity();
else { else {
_Scalar angle = omega.norm(); _Scalar angle = omega.norm();
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
} }
if (H) CONCEPT_NOT_IMPLEMENTED;
} }
/// We use our own Logmap, as there is a slight bug in Eigen /// We use our own Logmap, as there is a slight bug in Eigen
@ -106,43 +91,55 @@ struct traits<QUATERNION_TYPE> {
// define these compile time constants to avoid std::abs: // define these compile time constants to avoid std::abs:
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;
const double qw = q.w(); const double qw = q.w();
if (qw > NearlyOne) { if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1 // Taylor expansion of (angle / s) at 1
//return (2 + 2 * (1-qw) / 3) * q.vec(); //return (2 + 2 * (1-qw) / 3) * q.vec();
return (8. / 3. - 2. / 3. * qw) * q.vec(); omega = (8. / 3. - 2. / 3. * qw) * q.vec();
} else if (qw < NearlyNegativeOne) { } else if (qw < NearlyNegativeOne) {
// Taylor expansion of (angle / s) at -1 // Taylor expansion of (angle / s) at -1
//return (-2 - 2 * (1 + qw) / 3) * q.vec(); //return (-2 - 2 * (1 + qw) / 3) * q.vec();
return (-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); double 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;
else if (angle < -M_PI) else if (angle < -M_PI)
angle += twoPi; angle += twoPi;
return (angle / s) * q.vec(); omega = (angle / s) * q.vec();
} }
if (H) CONCEPT_NOT_IMPLEMENTED; if(H) *H = SO3::LogmapDerivative(omega);
return omega;
} }
/// @} /// @}
/// @name Manifold traits /// @name Manifold traits
/// @{ /// @{
static TangentVector Local(const Q& origin, const Q& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { static TangentVector Local(const Q& g, const Q& h,
return Logmap(Between(origin, other, Horigin, Hother)); ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
// TODO: incorporate Jacobian of Logmap Q b = Between(g, h, H1, H2);
Matrix3 D_v_b;
TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
if (H1) *H1 = D_v_b * (*H1);
if (H2) *H2 = D_v_b * (*H2);
return v;
} }
static Q Retract(const Q& origin, const TangentVector& v,
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { static Q Retract(const Q& g, const TangentVector& v,
return Compose(origin, Expmap(v), Horigin, Hv); ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
// TODO : incorporate Jacobian of Expmap Matrix3 D_h_v;
Q b = Expmap(v,H2 ? &D_h_v : 0);
Q h = Compose(g, b, H1, H2);
if (H2) *H2 = (*H2) * D_h_v;
return h;
} }
/// @} /// @}
@ -150,9 +147,9 @@ struct traits<QUATERNION_TYPE> {
/// @{ /// @{
static void Print(const Q& q, const std::string& str = "") { static void Print(const Q& q, const std::string& str = "") {
if (str.size() == 0) if (str.size() == 0)
std::cout << "Eigen::Quaternion: "; std::cout << "Eigen::Quaternion: ";
else else
std::cout << str << " "; std::cout << str << " ";
std::cout << q.vec().transpose() << std::endl; std::cout << q.vec().transpose() << std::endl;
} }
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {

51
gtsam/geometry/Rot3.cpp
View File

@ -19,6 +19,7 @@
*/ */
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <boost/random.hpp> #include <boost/random.hpp>
#include <cmath> #include <cmath>
@ -149,57 +150,13 @@ Vector Rot3::quaternion() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3; return SO3::ExpmapDerivative(x);
double theta = x.norm(); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = x^, normalized by normx
const Matrix3 Y = skewSymmetric(x) / theta;
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::LogmapDerivative(const Vector3& x) { Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3; return SO3::LogmapDerivative(x);
double theta = x.norm();
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */

3
gtsam/geometry/Rot3.h
View File

@ -208,9 +208,10 @@ namespace gtsam {
return Rot3(); return Rot3();
} }
/** compose two rotations */ /// Syntatic sugar for composing two rotations
Rot3 operator*(const Rot3& R2) const; Rot3 operator*(const Rot3& R2) const;
/// inverse of a rotation, TODO should be different for M/Q
Rot3 inverse() const { Rot3 inverse() const {
return Rot3(Matrix3(transpose())); return Rot3(Matrix3(transpose()));
} }

62
gtsam/geometry/Rot3M.cpp
View File

@ -23,6 +23,7 @@
#ifndef GTSAM_USE_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
@ -118,25 +119,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) { Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
// get components of axis \omega return SO3::Rodrigues(w,theta);
double wx = w(0), wy=w(1), wz=w(2);
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG
double l_n = wwTxx + wwTyy + wwTzz;
if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double swx = wx * s, swy = wy * s, swz = wz * s;
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz;
return Rot3(
c + C00, -swz + C01, swy + C02,
swz + C01, c + C11, -swx + C12,
-swy + C02, swx + C12, c + C22);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -163,46 +146,7 @@ Point3 Rot3::rotate(const Point3& p,
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
return SO3::Logmap(R.rot_,H);
static const double PI = boost::math::constants::pi<double>();
const Matrix3& rot = R.rot_;
// Get trace(R)
double tr = rot.trace();
Vector3 thetaR;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr+1.0) < 1e-10) {
if(std::abs(rot(2,2)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(2,2) )) *
Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
else if(std::abs(rot(1,1)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(1,1))) *
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) *
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
} else {
double magnitude;
double tr_3 = tr-3.0; // always negative
if (tr_3<-1e-7) {
double theta = acos((tr-1.0)/2.0);
magnitude = theta/(2.0*sin(theta));
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0;
}
thetaR = magnitude*Vector3(
rot(2,1)-rot(1,2),
rot(0,2)-rot(2,0),
rot(1,0)-rot(0,1));
}
if(H) *H = Rot3::LogmapDerivative(thetaR);
return thetaR;
} }
/* ************************************************************************* */ /* ************************************************************************* */

40
gtsam/geometry/Rot3Q.cpp
View File

@ -85,28 +85,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) { Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return QuaternionChart::Expmap(theta,w); return Quaternion(Eigen::AngleAxis<double>(theta, w));
} }
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const { Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_); return Rot3(quaternion_ * R2.quaternion_);
} }
/* ************************************************************************* */
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse());
}
/* ************************************************************************* */ /* ************************************************************************* */
// TODO: Could we do this? It works in Rot3M but not here, probably because // TODO: Could we do this? It works in Rot3M but not here, probably because
// here we create an intermediate value by calling matrix() // here we create an intermediate value by calling matrix()
@ -115,14 +100,6 @@ namespace gtsam {
return matrix().transpose(); return matrix().transpose();
} }
/* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return inverse() * R2;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p, Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
@ -135,18 +112,21 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if(H) *H = Rot3::LogmapDerivative(thetaR); return traits<Quaternion>::Logmap(R.quaternion_, H);
return QuaternionChart::Logmap(R.quaternion_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
return compose(Expmap(omega)); static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Expmap(omega, H);
else throw std::runtime_error("Rot3::Retract: unknown mode");
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const { Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
return Logmap(between(t2)); static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Logmap(R, H);
else throw std::runtime_error("Rot3::Local: unknown mode");
} }
/* ************************************************************************* */ /* ************************************************************************* */

92
gtsam/geometry/SO3.cpp
View File

@ -20,9 +20,12 @@
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <cmath> #include <cmath>
using namespace std;
namespace gtsam { namespace gtsam {
SO3 Rodrigues(const double& theta, const Vector3& axis) { /* ************************************************************************* */
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
using std::cos; using std::cos;
using std::sin; using std::sin;
@ -46,27 +49,25 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
} }
/// simply convert omega to axis/angle representation /// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Eigen::Ref<const Vector3>& omega, SO3 SO3::Expmap(const Vector3& omega,
ChartJacobian H) { ChartJacobian H) {
if (H) if (H)
CONCEPT_NOT_IMPLEMENTED; *H = ExpmapDerivative(omega);
if (omega.isZero()) if (omega.isZero())
return SO3::Identity(); return Identity();
else { else {
double angle = omega.norm(); double angle = omega.norm();
return Rodrigues(angle, omega / angle); return Rodrigues(omega / angle, angle);
} }
} }
/* ************************************************************************* */
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
using std::sqrt; using std::sqrt;
using std::sin; using std::sin;
if (H)
CONCEPT_NOT_IMPLEMENTED;
// note switch to base 1 // note switch to base 1
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
@ -75,16 +76,18 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// Get trace(R) // Get trace(R)
double tr = R.trace(); double tr = R.trace();
Vector3 omega;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (std::abs(tr + 1.0) < 1e-10) { if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10) if (std::abs(R33 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10) else if (std::abs(R22 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else { } else {
double magnitude; double magnitude;
double tr_3 = tr - 3.0; // always negative double tr_3 = tr - 3.0; // always negative
@ -96,9 +99,74 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0; magnitude = 0.5 - tr_3 * tr_3 / 12.0;
} }
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
} }
if(H) *H = LogmapDerivative(omega);
return omega;
} }
/* ************************************************************************* */
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm(); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = omega^, normalized by normx
const Matrix3 Y = skewSymmetric(omega) / theta;
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
#endif
}
/* ************************************************************************* */
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm();
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
}
/* ************************************************************************* */
} // end namespace gtsam } // end namespace gtsam

74
gtsam/geometry/SO3.h
View File

@ -30,15 +30,21 @@ namespace gtsam {
* We guarantee (all but first) constructors only generate from sub-manifold. * We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it... * However, round-off errors in repeated composition could move off it...
*/ */
class SO3: public Matrix3, public LieGroup<SO3,3> { class GTSAM_EXPORT SO3: public Matrix3, public LieGroup<SO3, 3> {
protected: protected:
public: public:
enum { dimension=3 }; enum {
dimension = 3
};
/// @name Constructors
/// @{
/// Constructor from AngleAxisd /// Constructor from AngleAxisd
SO3() : Matrix3(I_3x3) { SO3() :
Matrix3(I_3x3) {
} }
/// Constructor from Eigen Matrix /// Constructor from Eigen Matrix
@ -52,6 +58,13 @@ public:
Matrix3(angleAxis) { Matrix3(angleAxis) {
} }
/// Static, named constructor TODO think about relation with above
static SO3 Rodrigues(const Vector3& axis, double theta);
/// @}
/// @name Testable
/// @{
void print(const std::string& s) const { void print(const std::string& s) const {
std::cout << s << *this << std::endl; std::cout << s << *this << std::endl;
} }
@ -60,32 +73,67 @@ public:
return equal_with_abs_tol(*this, R, tol); return equal_with_abs_tol(*this, R, tol);
} }
static SO3 identity() { return I_3x3; } /// @}
SO3 inverse() const { return this->Matrix3::inverse(); } /// @name Group
/// @{
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega, ChartJacobian H = boost::none); /// identity rotation for group operation
static SO3 identity() {
return I_3x3;
}
/// inverse of a rotation = transpose
SO3 inverse() const {
return this->Matrix3::inverse();
}
/// @}
/// @name Lie Group
/// @{
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
*/
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
Matrix3 AdjointMap() const { return *this; } /// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& omega);
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& omega);
Matrix3 AdjointMap() const {
return *this;
}
// Chart at origin // Chart at origin
struct ChartAtOrigin { struct ChartAtOrigin {
static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) { static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
return Expmap(v,H); return Expmap(omega, H);
} }
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) { static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
return Logmap(R,H); return Logmap(R, H);
} }
}; };
using LieGroup<SO3,3>::inverse; using LieGroup<SO3, 3>::inverse;
/// @}
}; };
template<> template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {}; struct traits<SO3> : public internal::LieGroupTraits<SO3> {
};
template<> template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {}; struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
};
} // end namespace gtsam } // end namespace gtsam

8
gtsam/geometry/tests/testPose2.cpp
View File

@ -145,7 +145,7 @@ TEST(Pose2, expmap0d) {
/* ************************************************************************* */ /* ************************************************************************* */
// test case for screw motion in the plane // test case for screw motion in the plane
namespace screw { namespace screwPose2 {
double w=0.3; double w=0.3;
Vector xi = (Vector(3) << 0.0, w, w).finished(); Vector xi = (Vector(3) << 0.0, w, w).finished();
Rot2 expectedR = Rot2::fromAngle(w); Rot2 expectedR = Rot2::fromAngle(w);
@ -155,9 +155,9 @@ namespace screw {
TEST(Pose2, expmap_c) TEST(Pose2, expmap_c)
{ {
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6)); EXPECT(assert_equal(screwPose2::expected, expm<Pose2>(screwPose2::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),1e-6));
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6)); EXPECT(assert_equal(screwPose2::xi, Pose2::Logmap(screwPose2::expected),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -109,7 +109,7 @@ TEST(Pose3, expmap_b)
/* ************************************************************************* */ /* ************************************************************************* */
// test case for screw motion in the plane // test case for screw motion in the plane
namespace screw { namespace screwPose3 {
double a=0.3, c=cos(a), s=sin(a), w=0.3; double a=0.3, c=cos(a), s=sin(a), w=0.3;
Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished(); Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished();
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
@ -121,24 +121,24 @@ namespace screw {
// Checks correct exponential map (Expmap) with brute force matrix exponential // Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(Pose3, expmap_c_full) TEST(Pose3, expmap_c_full)
{ {
EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6)); EXPECT(assert_equal(screwPose3::expected, expm<Pose3>(screwPose3::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); EXPECT(assert_equal(screwPose3::expected, Pose3::Expmap(screwPose3::xi),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose3, Adjoint_full) TEST(Pose3, Adjoint_full)
{ {
Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse();
Vector xiprime = T.Adjoint(screw::xi); Vector xiprime = T.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse();
Vector xiprime2 = T2.Adjoint(screw::xi); Vector xiprime2 = T2.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse();
Vector xiprime3 = T3.Adjoint(screw::xi); Vector xiprime3 = T3.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
} }
@ -634,9 +634,9 @@ TEST( Pose3, unicycle )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, adjointMap) { TEST( Pose3, adjointMap) {
Matrix res = Pose3::adjointMap(screw::xi); Matrix res = Pose3::adjointMap(screwPose3::xi);
Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2)); Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5)); Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
Matrix Z3 = zeros(3,3); Matrix Z3 = zeros(3,3);
Matrix6 expected; Matrix6 expected;
expected << wh, Z3, vh, wh; expected << wh, Z3, vh, wh;
@ -704,13 +704,13 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
} }
TEST( Pose3, adjoint) { TEST( Pose3, adjoint) {
Vector expected = testDerivAdjoint(screw::xi, screw::xi); Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi);
Matrix actualH; Matrix actualH;
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH);
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>( Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screw::xi, screw::xi, 1e-5); testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5));
@ -775,13 +775,16 @@ TEST(Pose3 , LieGroupDerivatives) {
//****************************************************************************** //******************************************************************************
TEST(Pose3 , ChartDerivatives) { TEST(Pose3 , ChartDerivatives) {
Pose3 id; Pose3 id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id); CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2); CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id); CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T3); CHECK_CHART_DERIVATIVES(T2,T3);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} int main(){ //TestResult tr; return TestRegistry::runAllTests(tr);}
std::cout<<"testPose3 currently disabled!!" << std::endl;
}
/* ************************************************************************* */ /* ************************************************************************* */

69
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -17,6 +17,8 @@
#include <gtsam/geometry/Quaternion.h> #include <gtsam/geometry/Quaternion.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
@ -37,14 +39,6 @@ TEST(Quaternion , Constructor) {
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
} }
//******************************************************************************
TEST(Quaternion , Invariants) {
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
check_group_invariants(q1, q2);
check_manifold_invariants(q1, q2);
}
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Local) { TEST(Quaternion , Local) {
Vector3 z_axis(0, 0, 1); Vector3 z_axis(0, 0, 1);
@ -74,47 +68,62 @@ TEST(Quaternion , Compose) {
Q q2(Eigen::AngleAxisd(0.1, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1 * q2; Q expected = q1 * q2;
Matrix actualH1, actualH2; Q actual = traits<Q>::Compose(q1, q2);
Q actual = traits<Q>::Compose(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual)); EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
} }
//******************************************************************************
Vector3 z_axis(0, 0, 1);
Q id(Eigen::AngleAxisd(0, z_axis));
Q R1(Eigen::AngleAxisd(1, z_axis));
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Between) { TEST(Quaternion , Between) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.2, z_axis)); Q q1(Eigen::AngleAxisd(0.2, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1.inverse() * q2; Q expected = q1.inverse() * q2;
Matrix actualH1, actualH2; Q actual = traits<Q>::Between(q1, q2);
Q actual = traits<Q>::Between(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual)); EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
} }
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Inverse) { TEST(Quaternion , Inverse) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.1, z_axis)); Q q1(Eigen::AngleAxisd(0.1, z_axis));
Q expected(Eigen::AngleAxisd(-0.1, z_axis)); Q expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH; Q actual = traits<Q>::Inverse(q1);
Q actual = traits<Q>::Inverse(q1, actualH);
EXPECT(traits<Q>::Equals(expected,actual)); EXPECT(traits<Q>::Equals(expected,actual));
}
Matrix numericalH = numericalDerivative11(traits<Q>::Inverse, q1); //******************************************************************************
EXPECT(assert_equal(numericalH,actualH)); TEST(Quaternion , Invariants) {
check_group_invariants(id,id);
check_group_invariants(id,R1);
check_group_invariants(R2,id);
check_group_invariants(R2,R1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,R1);
check_manifold_invariants(R2,id);
check_manifold_invariants(R2,R1);
}
//******************************************************************************
TEST(Quaternion , LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
}
//******************************************************************************
TEST(Quaternion , ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,R2);
CHECK_CHART_DERIVATIVES(R2,id);
CHECK_CHART_DERIVATIVES(R2,R1);
} }
//****************************************************************************** //******************************************************************************

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

@ -663,12 +663,13 @@ TEST(Rot3 , Invariants) {
check_group_invariants(id,T1); check_group_invariants(id,T1);
check_group_invariants(T2,id); check_group_invariants(T2,id);
check_group_invariants(T2,T1); check_group_invariants(T2,T1);
check_group_invariants(T1,T2);
check_manifold_invariants(id,id); check_manifold_invariants(id,id);
check_manifold_invariants(id,T1); check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id); check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1); check_manifold_invariants(T2,T1);
check_manifold_invariants(T1,T2);
} }
//****************************************************************************** //******************************************************************************
@ -678,24 +679,27 @@ TEST(Rot3 , LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,id); CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2); CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id); CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T1,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1); CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
} }
//****************************************************************************** //******************************************************************************
TEST(Rot3 , ChartDerivatives) { TEST(Rot3 , ChartDerivatives) {
Rot3 id; Rot3 id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id); CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2); CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id); CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1); CHECK_CHART_DERIVATIVES(T1,T2);
CHECK_CHART_DERIVATIVES(T2,T1);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; // TestResult tr;
return TestRegistry::runAllTests(tr); // return TestRegistry::runAllTests(tr);
std::cout << "testRot3 currently disabled!!" << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */

13
gtsam/geometry/tests/testSO3.cpp
View File

@ -66,16 +66,15 @@ TEST(SO3 , Invariants) {
check_manifold_invariants(id,R1); check_manifold_invariants(id,R1);
check_manifold_invariants(R2,id); check_manifold_invariants(R2,id);
check_manifold_invariants(R2,R1); check_manifold_invariants(R2,R1);
} }
//****************************************************************************** //******************************************************************************
//TEST(SO3 , LieGroupDerivatives) { TEST(SO3 , LieGroupDerivatives) {
// CHECK_LIE_GROUP_DERIVATIVES(id,id); CHECK_LIE_GROUP_DERIVATIVES(id,id);
// CHECK_LIE_GROUP_DERIVATIVES(id,R2); CHECK_LIE_GROUP_DERIVATIVES(id,R2);
// CHECK_LIE_GROUP_DERIVATIVES(R2,id); CHECK_LIE_GROUP_DERIVATIVES(R2,id);
// CHECK_LIE_GROUP_DERIVATIVES(R2,R1); CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
//} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , ChartDerivatives) { TEST(SO3 , ChartDerivatives) {

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

@ -59,7 +59,7 @@ static StereoCamera cam2(pose3, cal4ptr);
static StereoPoint2 spt(1.0, 2.0, 3.0); static StereoPoint2 spt(1.0, 2.0, 3.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, text_geometry) { TEST_DISABLED (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0))); EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0))); EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
@ -84,7 +84,7 @@ TEST (Serialization, text_geometry) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, xml_geometry) { TEST_DISABLED (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0))); EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0))); EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
@ -108,7 +108,7 @@ TEST (Serialization, xml_geometry) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, binary_geometry) { TEST_DISABLED (Serialization, binary_geometry) {
EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0))); EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0))); EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));

5
gtsam/linear/PCGSolver.cpp
View File

@ -84,8 +84,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
// Make the result as Vector form // Make the result as Vector form
AtAx = vvAtAx.vector(); AtAx = vvAtAx.vector(keyInfo_.ordering());
} }
/*****************************************************************************/ /*****************************************************************************/
@ -96,7 +95,7 @@ void GaussianFactorGraphSystem::getb(Vector &b) const {
VectorValues vvb = gfg_.gradientAtZero(); VectorValues vvb = gfg_.gradientAtZero();
// Make the result as Vector form // Make the result as Vector form
b = -vvb.vector(); b = -vvb.vector(keyInfo_.ordering());
} }
/**********************************************************************************/ /**********************************************************************************/

1
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -148,7 +148,6 @@ TEST( GaussianBayesNet, DeterminantTest )
} }
/* ************************************************************************* */ /* ************************************************************************* */
typedef Eigen::Matrix<double,10,1> Vector10;
namespace { namespace {
double computeError(const GaussianBayesNet& gbn, const Vector10& values) double computeError(const GaussianBayesNet& gbn, const Vector10& values)
{ {

1
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -175,7 +175,6 @@ TEST(GaussianBayesTree, complicatedMarginal) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
typedef Eigen::Matrix<double, 10, 1> Vector10;
namespace { namespace {
double computeError(const GaussianBayesTree& gbt, const Vector10& values) { double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian(); pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();

4
gtsam/navigation/AHRSFactor.h
View File

@ -26,7 +26,7 @@
namespace gtsam { namespace gtsam {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> { class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
public: public:
/** /**
@ -36,7 +36,7 @@ public:
* Can be built incrementally so as to avoid costly integration at time of * Can be built incrementally so as to avoid costly integration at time of
* factor construction. * factor construction.
*/ */
class PreintegratedMeasurements : public PreintegratedRotation { class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation {
friend class AHRSFactor; friend class AHRSFactor;

20
gtsam/navigation/ImuFactor.cpp
View File

@ -94,15 +94,27 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc;
if(use2ndOrderIntegration()){
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
preintMeasCov_.block<3,3>(0,3) += temp;
preintMeasCov_.block<3,3>(3,0) += temp.transpose();
}
// F_test and G_test are given as output for testing purposes and are not needed by the factor // F_test and G_test are given as output for testing purposes and are not needed by the factor
if(F_test){ // This in only for testing if(F_test){ // This in only for testing
(*F_test) << F; (*F_test) << F;
} }
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
// intNoise accNoise omegaNoise if(!use2ndOrderIntegration())
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos F_pos_noiseacc = Z_3x3;
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle // intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
} }
} }

11
gtsam/navigation/PreintegrationBase.h
View File

@ -83,6 +83,7 @@ public:
integrationCovariance_(integrationErrorCovariance) {} integrationCovariance_(integrationErrorCovariance) {}
/// methods to access class variables /// methods to access class variables
const bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;}
const Vector3& deltaPij() const {return deltaPij_;} const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;} const Vector3& deltaVij() const {return deltaVij_;}
const imuBias::ConstantBias& biasHat() const { return biasHat_;} const imuBias::ConstantBias& biasHat() const { return biasHat_;}
@ -149,8 +150,14 @@ public:
if(F){ if(F){
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles;
if(use2ndOrderIntegration_)
F_pos_angles = 0.5 * F_vel_angles * deltaT;
else
F_pos_angles = Z_3x3;
// pos vel angle // pos vel angle
*F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos *F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles;// angle Z_3x3, Z_3x3, F_angles_angles;// angle
} }
@ -353,7 +360,7 @@ public:
// dfV/dPosej // dfV/dPosej
Matrix::Zero(3,6), Matrix::Zero(3,6),
// dfR/dPosej // dfR/dPosej
D_fR_fRrot * ( I_3x3 ), Z_3x3; D_fR_fRrot, Z_3x3;
} }
if(H4) { if(H4) {
H4->resize(9,3); H4->resize(9,3);

2
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -56,7 +56,7 @@ Vector updatePreintegratedMeasurementsTest(
if(!use2ndOrderIntegration){ if(!use2ndOrderIntegration){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT; deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{ }else{
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
} }
Vector3 deltaVij_new = deltaVij_old + temp; Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);

149
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -65,7 +65,7 @@ Vector updatePreintegratedPosVel(
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT; deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{ }else{
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
} }
Vector3 deltaVij_new = deltaVij_old + temp; Vector3 deltaVij_new = deltaVij_old + temp;
@ -90,9 +90,9 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const list<Vector3>& measuredAccs, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ const bool use2ndOrderIntegration = false){
ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(),
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity()); omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration);
list<Vector3>::const_iterator itAcc = measuredAccs.begin(); list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -107,8 +107,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs){
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij(); measuredAccs, measuredOmegas, deltaTs).deltaPij();
} }
@ -117,8 +116,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs)
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij(); measuredAccs, measuredOmegas, deltaTs).deltaVij();
@ -128,10 +126,9 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs){
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
return Rot3(evaluatePreintegratedMeasurements(bias, return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij()); measuredAccs, measuredOmegas, deltaTs).deltaRij());
} }
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
@ -479,21 +476,21 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>( Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>( Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>( Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
@ -528,9 +525,10 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
} }
bool use2ndOrderIntegration = false;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements // so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians // Now we add a new measurement and ask for Jacobians
@ -547,7 +545,126 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual); body_P_sensor, Factual, Gactual);
bool use2ndOrderIntegration = false; //////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
_1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle =
numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
// Compute expected f_rot wrt angles
Matrix dfr_dangle =
numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
Matrix FexpectedBottom3(3,9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3;
EXPECT(assert_equal(Fexpected, Factual));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6,3);
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
Matrix GexpectedTop6(6,9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle =
numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
deltaRij_old, _1, newDeltaT), newMeasuredOmega);
Matrix GexpectedBottom3(3,9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3,
Z_3x3, accNoiseVar*I_3x3, Z_3x3,
Z_3x3, Z_3x3, omegaNoiseVar*I_3x3;
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
(1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
{
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01);
}
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F // COMPUTE NUMERICAL DERIVATIVES FOR F

6
gtsam/slam/RegularHessianFactor.h
View File

@ -55,6 +55,12 @@ public:
typedef Eigen::Matrix<double, D, 1> DVector; typedef Eigen::Matrix<double, D, 1> DVector;
mutable std::vector<DVector> y; mutable std::vector<DVector> y;
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{
throw std::runtime_error(
"RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead");
}
/** y += alpha * A'*A*x */ /** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
// Create a vector of temporary y values, corresponding to rows i // Create a vector of temporary y values, corresponding to rows i

4
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
/** Single Element Constructor: acts on a single parameter specified by idx */ /** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::Dim())) { Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) {
assert(model->dim() == 1); assert(model->dim() == 1);
this->fillH(); this->fillH();
} }
@ -78,7 +78,7 @@ namespace gtsam {
/** Indices Constructor: specify the mask with a set of indices */ /** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior, PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) {
assert((size_t)prior_.size() == mask.size()); assert((size_t)prior_.size() == mask.size());
assert(model->dim() == (size_t) prior.size()); assert(model->dim() == (size_t) prior.size());
this->fillH(); this->fillH();

43
tests/testPCGSolver.cpp
View File

@ -91,6 +91,49 @@ TEST( PCGSolver, llt ) {
} }
/* ************************************************************************* */
// Test GaussianFactorGraphSystem::multiply and getb
TEST( GaussianFactorGraphSystem, multiply_getb)
{
// Create a Gaussian Factor Graph
GaussianFactorGraph simpleGFG;
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
// Create a dummy-preconditioner and a GaussianFactorGraphSystem
DummyPreconditioner dummyPreconditioner;
KeyInfo keyInfo(simpleGFG);
std::map<Key,Vector> lambda;
dummyPreconditioner.build(simpleGFG, keyInfo, lambda);
GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda);
// Prepare container for each variable
Vector initial, residual, preconditionedResidual, p, actualAp;
initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished();
// Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver
gfgs.residual(initial, residual); /* r = b-Ax */
gfgs.leftPrecondition(residual, preconditionedResidual); /* pr = L^{-1} (b-Ax) */
gfgs.rightPrecondition(preconditionedResidual, p); /* p = L^{-T} pr */
gfgs.multiply(p, actualAp); /* A p */
// Expected value of Ap for the first iteration of this example problem
Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
EXPECT(assert_equal(expectedAp, actualAp, 1e-3));
// Expected value of getb
Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
Vector actualb;
gfgs.getb(actualb);
EXPECT(assert_equal(expectedb, actualb, 1e-3));
}
/* ************************************************************************* */ /* ************************************************************************* */
// Test Dummy Preconditioner // Test Dummy Preconditioner
TEST( PCGSolver, dummy ) TEST( PCGSolver, dummy )