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

Merge remote-tracking branch 'origin/develop' into feature/ExpressionsBALExample

release/4.3a0
dellaert 2015-02-11 11:28:31 +01:00
parent f45ad82940 6a212c1c2d
commit 1d6fb887e8
44 changed files with 1530 additions and 410 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -584,6 +584,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -591,6 +592,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -638,6 +640,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -645,6 +648,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -652,6 +656,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -667,6 +672,7 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1098,6 +1104,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1327,6 +1334,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1409,7 +1456,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1449,7 +1495,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1457,7 +1502,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1471,46 +1515,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1776,6 +1780,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1783,6 +1788,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1790,6 +1796,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1797,6 +1804,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2169,6 +2177,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2675,6 +2691,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2682,6 +2699,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2689,6 +2707,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3288,7 +3307,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

79
gtsam.h
View File

@ -629,28 +629,13 @@ class Cal3_S2 {
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
class Cal3DS2 {
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
Cal3DS2(Vector v);
Cal3DS2_Base();
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// TODO: D2d functions that start with an uppercase letter
// Standard Interface
double fx() const;
@ -658,14 +643,66 @@ class Cal3DS2 {
double skew() const;
double px() const;
double py() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
double k1() const;
double k2() const;
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
Cal3DS2(Vector v);
// Testable
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3Unified.h>
virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3Unified();
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
Cal3Unified(Vector v);
// Testable
bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
// Standard Interface
double xi() const;
gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3Unified retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
class Cal3_S2Stereo {
// Standard Constructors
Cal3_S2Stereo();

15
gtsam/base/testLie.h
View File

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

14
gtsam/geometry/Cal3DS2.h
View File

@ -68,7 +68,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@ -89,10 +89,20 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Base> clone() const {
return boost::shared_ptr<Base>(new Cal3DS2(*this));
}
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{

28
gtsam/geometry/Cal3DS2_Base.h
View File

@ -45,9 +45,6 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
Matrix3 K() const ;
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
Vector9 vector() const ;
/// @name Standard Constructors
/// @{
@ -59,6 +56,8 @@ public:
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
virtual ~Cal3DS2_Base() {}
/// @}
/// @name Advanced Constructors
/// @{
@ -70,7 +69,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
@ -106,6 +105,15 @@ public:
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/// return calibration matrix -- not really applicable
Matrix3 K() const;
/// return distortion parameter vector
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
/// Return all parameters as a vector
Vector9 vector() const;
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
@ -126,9 +134,19 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix29 D2d_calibration(const Point2& p) const ;
private:
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
}
/// @}
private:
/// @name Advanced Interface
/// @{

10
gtsam/geometry/Cal3Unified.h
View File

@ -50,9 +50,8 @@ private:
double xi_; // mirror parameter
public:
enum { dimension = 10 };
Vector10 vector() const ;
enum { dimension = 10 };
/// @name Standard Constructors
/// @{
@ -77,7 +76,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
@ -125,6 +124,11 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 10; } //TODO: make a final dimension variable
/// Return all parameters as a vector
Vector10 vector() const ;
/// @}
private:
/** Serialization function */

93
gtsam/geometry/Quaternion.h
View File

@ -15,8 +15,11 @@
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -37,19 +40,6 @@ struct traits<QUATERNION_TYPE> {
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
/// @{
@ -62,41 +52,36 @@ struct traits<QUATERNION_TYPE> {
/// @}
/// @name Lie group traits
/// @{
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
if (Hg)
*Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? )
static Q Compose(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
if (Hg) *Hg = h.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3;
return g * h;
}
static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
static Q Between(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
Q d = g.inverse() * h;
if (Hg)
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) )
if (Hg) *Hg = -d.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3;
return d;
}
static Q Inverse(const Q &g, ChartJacobian H) {
if (H)
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
static Q Inverse(const Q &g,
ChartJacobian H = boost::none) {
if (H) *H = -g.toRotationMatrix();
return g.inverse();
}
/// Exponential map, simply be converting omega to axis/angle representation
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) {
if (omega.isZero())
return Q::Identity();
if(H) *H = SO3::ExpmapDerivative(omega);
if (omega.isZero()) return Q::Identity();
else {
_Scalar angle = omega.norm();
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
@ -106,43 +91,55 @@ struct traits<QUATERNION_TYPE> {
// define these compile time constants to avoid std::abs:
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();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
//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) {
// Taylor expansion of (angle / s) at -1
//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 {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
return (angle / s) * q.vec();
angle += twoPi;
omega = (angle / s) * q.vec();
}
if (H) CONCEPT_NOT_IMPLEMENTED;
if(H) *H = SO3::LogmapDerivative(omega);
return omega;
}
/// @}
/// @name Manifold traits
/// @{
static TangentVector Local(const Q& origin, const Q& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return Logmap(Between(origin, other, Horigin, Hother));
// TODO: incorporate Jacobian of Logmap
static TangentVector Local(const Q& g, const Q& h,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
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) {
return Compose(origin, Expmap(v), Horigin, Hv);
// TODO : incorporate Jacobian of Expmap
static Q Retract(const Q& g, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
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 = "") {
if (str.size() == 0)
std::cout << "Eigen::Quaternion: ";
std::cout << "Eigen::Quaternion: ";
else
std::cout << str << " ";
std::cout << str << " ";
std::cout << q.vec().transpose() << std::endl;
}
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/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
@ -149,57 +150,13 @@ Vector Rot3::quaternion() const {
}
/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
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::ExpmapDerivative(const Vector3& x) {
return SO3::ExpmapDerivative(x);
}
/* ************************************************************************* */
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
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
return SO3::LogmapDerivative(x);
}
/* ************************************************************************* */

3
gtsam/geometry/Rot3.h
View File

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

62
gtsam/geometry/Rot3M.cpp
View File

@ -23,6 +23,7 @@
#ifndef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
@ -118,25 +119,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
// get components of axis \omega
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);
return SO3::Rodrigues(w,theta);
}
/* ************************************************************************* */
@ -163,46 +146,7 @@ Point3 Rot3::rotate(const Point3& p,
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> 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;
return SO3::Logmap(R.rot_,H);
}
/* ************************************************************************* */

40
gtsam/geometry/Rot3Q.cpp
View File

@ -85,28 +85,13 @@ namespace gtsam {
/* ************************************************************************* */
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 {
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
// here we create an intermediate value by calling matrix()
@ -115,14 +100,6 @@ namespace gtsam {
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,
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) {
if(H) *H = Rot3::LogmapDerivative(thetaR);
return QuaternionChart::Logmap(R.quaternion_);
return traits<Quaternion>::Logmap(R.quaternion_, H);
}
/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
return compose(Expmap(omega));
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
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 {
return Logmap(between(t2));
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
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 <cmath>
using namespace std;
namespace gtsam {
SO3 Rodrigues(const double& theta, const Vector3& axis) {
/* ************************************************************************* */
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
using std::cos;
using std::sin;
@ -46,27 +49,25 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
}
/// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Eigen::Ref<const Vector3>& omega,
SO3 SO3::Expmap(const Vector3& omega,
ChartJacobian H) {
if (H)
CONCEPT_NOT_IMPLEMENTED;
*H = ExpmapDerivative(omega);
if (omega.isZero())
return SO3::Identity();
return Identity();
else {
double angle = omega.norm();
return Rodrigues(angle, omega / angle);
return Rodrigues(omega / angle, angle);
}
}
/* ************************************************************************* */
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
using std::sqrt;
using std::sin;
if (H)
CONCEPT_NOT_IMPLEMENTED;
// note switch to base 1
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);
@ -75,16 +76,18 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// Get trace(R)
double tr = R.trace();
Vector3 omega;
// 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(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)
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
// 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 {
double magnitude;
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)
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

74
gtsam/geometry/SO3.h
View File

@ -30,15 +30,21 @@ namespace gtsam {
* We guarantee (all but first) constructors only generate from sub-manifold.
* 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:
public:
enum { dimension=3 };
enum {
dimension = 3
};
/// @name Constructors
/// @{
/// Constructor from AngleAxisd
SO3() : Matrix3(I_3x3) {
SO3() :
Matrix3(I_3x3) {
}
/// Constructor from Eigen Matrix
@ -52,6 +58,13 @@ public:
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 {
std::cout << s << *this << std::endl;
}
@ -60,32 +73,67 @@ public:
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);
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
struct ChartAtOrigin {
static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) {
return Expmap(v,H);
static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
return Expmap(omega, H);
}
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<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
struct traits<SO3> : public internal::LieGroupTraits<SO3> {
};
template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {};
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
};
} // 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
namespace screw {
namespace screwPose2 {
double w=0.3;
Vector xi = (Vector(3) << 0.0, w, w).finished();
Rot2 expectedR = Rot2::fromAngle(w);
@ -155,9 +155,9 @@ namespace screw {
TEST(Pose2, expmap_c)
{
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
EXPECT(assert_equal(screwPose2::expected, expm<Pose2>(screwPose2::xi),1e-6));
EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),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
namespace screw {
namespace screwPose3 {
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();
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
TEST(Pose3, expmap_c_full)
{
EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screwPose3::expected, expm<Pose3>(screwPose3::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))
TEST(Pose3, Adjoint_full)
{
Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse();
Vector xiprime = T.Adjoint(screw::xi);
Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse();
Vector xiprime = T.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse();
Vector xiprime2 = T2.Adjoint(screw::xi);
Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse();
Vector xiprime2 = T2.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse();
Vector xiprime3 = T3.Adjoint(screw::xi);
Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse();
Vector xiprime3 = T3.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
}
@ -634,9 +634,9 @@ TEST( Pose3, unicycle )
/* ************************************************************************* */
TEST( Pose3, adjointMap) {
Matrix res = Pose3::adjointMap(screw::xi);
Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2));
Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5));
Matrix res = Pose3::adjointMap(screwPose3::xi);
Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
Matrix Z3 = zeros(3,3);
Matrix6 expected;
expected << wh, Z3, vh, wh;
@ -704,13 +704,13 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
}
TEST( Pose3, adjoint) {
Vector expected = testDerivAdjoint(screw::xi, screw::xi);
Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi);
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>(
testDerivAdjoint, screw::xi, screw::xi, 1e-5);
testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5));
@ -775,13 +775,16 @@ TEST(Pose3 , LieGroupDerivatives) {
//******************************************************************************
TEST(Pose3 , ChartDerivatives) {
Pose3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T3);
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
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/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -37,14 +39,6 @@ TEST(Quaternion , Constructor) {
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) {
Vector3 z_axis(0, 0, 1);
@ -74,47 +68,62 @@ TEST(Quaternion , Compose) {
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1 * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Compose(q1, q2, actualH1, actualH2);
Q actual = traits<Q>::Compose(q1, q2);
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) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.2, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1.inverse() * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Between(q1, q2, actualH1, actualH2);
Q actual = traits<Q>::Between(q1, q2);
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) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.1, z_axis));
Q expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH;
Q actual = traits<Q>::Inverse(q1, actualH);
Q actual = traits<Q>::Inverse(q1);
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(T2,id);
check_group_invariants(T2,T1);
check_group_invariants(T1,T2);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
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,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T1,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Rot3 , ChartDerivatives) {
Rot3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T1,T2);
CHECK_CHART_DERIVATIVES(T2,T1);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
// TestResult 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(R2,id);
check_manifold_invariants(R2,R1);
}
//******************************************************************************
//TEST(SO3 , 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(SO3 , 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(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);
/* ************************************************************************* */
TEST (Serialization, text_geometry) {
TEST_DISABLED (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
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::Pose2>(Pose2(1.0, 2.0, 0.3)));
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::Pose2>(Pose2(1.0, 2.0, 0.3)));
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);
// 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();
// 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 {
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 {
double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();

4
gtsam/navigation/AHRSFactor.h
View File

@ -26,7 +26,7 @@
namespace gtsam {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
public:
/**
@ -36,7 +36,7 @@ public:
* Can be built incrementally so as to avoid costly integration at time of
* factor construction.
*/
class PreintegratedMeasurements : public PreintegratedRotation {
class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation {
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>(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
if(F_test){ // This in only for testing
(*F_test) << F;
}
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
if(!use2ndOrderIntegration())
F_pos_noiseacc = Z_3x3;
// 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) {}
/// methods to access class variables
const bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;}
const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;}
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
@ -149,8 +150,14 @@ public:
if(F){
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
*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, Z_3x3, F_angles_angles;// angle
}
@ -353,7 +360,7 @@ public:
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
D_fR_fRrot * ( I_3x3 ), Z_3x3;
D_fR_fRrot, Z_3x3;
}
if(H4) {
H4->resize(9,3);

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

@ -56,7 +56,7 @@ Vector updatePreintegratedMeasurementsTest(
if(!use2ndOrderIntegration){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}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;
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_){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}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;
@ -90,9 +90,9 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
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(),
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity());
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -107,8 +107,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
const list<double>& deltaTs){
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij();
}
@ -117,8 +116,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
const list<double>& deltaTs)
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij();
@ -128,10 +126,9 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
const list<double>& deltaTs){
return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
measuredAccs, measuredOmegas, deltaTs).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
@ -479,21 +476,21 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
// Actual preintegrated values
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
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 expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
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 expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
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 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));
deltaTs.push_back(0.01);
}
bool use2ndOrderIntegration = false;
// Actual preintegrated values
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
// Now we add a new measurement and ask for Jacobians
@ -547,7 +545,126 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
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

6
gtsam/slam/RegularHessianFactor.h
View File

@ -55,6 +55,12 @@ public:
typedef Eigen::Matrix<double, D, 1> DVector;
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 */
void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
// 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 */
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);
this->fillH();
}
@ -78,7 +78,7 @@ namespace gtsam {
/** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
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(model->dim() == (size_t) prior.size());
this->fillH();

4
matlab/+gtsam/covarianceEllipse.m
View File

@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k)
% it is assumed x and y are the first two components of state x
% k is scaling for std deviations, defaults to 1 std
hold on
[e,s] = eig(P(1:2,1:2));
s1 = s(1,1);
s2 = s(2,2);
@ -32,4 +34,4 @@ h = line(ex,ey,'color',color);
y = c(2) + points(2,:);
end
end
end

8
matlab/+gtsam/covarianceEllipse3D.m
View File

@ -1,4 +1,4 @@
function covarianceEllipse3D(c,P)
function sc = covarianceEllipse3D(c,P,scale)
% covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
% Based on Maybeck Vol 1, page 366
% k=2.296 corresponds to 1 std, 68.26% of all probability
@ -6,10 +6,16 @@ function covarianceEllipse3D(c,P)
%
% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
hold on
[e,s] = svd(P);
k = 11.82;
radii = k*sqrt(diag(s));
if exist('scale', 'var')
radii = radii * scale;
end
% generate data for "unrotated" ellipsoid
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);

89
matlab/+gtsam/cylinderSampleProjection.m Normal file
View File

@ -0,0 +1,89 @@
function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders)
% Input:
% Output:
% visiblePoints: data{k} 3D Point in overal point clouds with index k
% Z{k} 2D measurements in overal point clouds with index k
% index {i}{j}
% i: the cylinder index;
% j: the point index on the cylinder;
%
% @Description: Project sampled points on cylinder to camera frame
% @Authors: Zhaoyang Lv
import gtsam.*
camera = SimpleCamera(pose, K);
%% memory allocation
cylinderNum = length(cylinders);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
% Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z <= 0
continue;
end
Z2d = camera.project(sampledPoint3);
% ignore points not visible in the scene
if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y
continue;
end
% ignore points occluded
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
continue;
else
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z2d;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end
end
end

93
matlab/+gtsam/cylinderSampleProjectionStereo.m Normal file
View File

@ -0,0 +1,93 @@
function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
import gtsam.*
%% memory allocation
cylinderNum = length(cylinders);
visiblePoints.data = cell(1);
visiblePoints.Z = cell(1);
visiblePoints.cylinderIdx = cell(1);
visiblePoints.overallIdx = cell(1);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
% For Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z < 0
continue;
end
% measurements
Z.du = K.fx() * K.baseline() / sampledPoint3local.z;
Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
Z.uR = Z.uL + Z.du;
Z.v = K.fy() / sampledPoint3local.z + K.py();
% ignore points not visible in the scene
if Z.uL < 0 || Z.uL >= imageSize.x || ...
Z.uR < 0 || Z.uR >= imageSize.x || ...
Z.v < 0 || Z.v >= imageSize.y
continue;
end
% too small disparity may call indeterminant system exception
if Z.du < 0.6
continue;
end
% ignore points occluded
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
continue;
else
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end
end
end

26
matlab/+gtsam/cylinderSampling.m Normal file
View File

@ -0,0 +1,26 @@
function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
%
% @author: Zhaoyang Lv
import gtsam.*
% calculate the cylinder area
area = 2 * pi * radius * height;
pointsNum = round(area * density);
points3 = cell(pointsNum, 1);
% sample the points
for i = 1:pointsNum
theta = 2 * pi * rand;
x = radius * cos(theta) + baseCentroid.x;
y = radius * sin(theta) + baseCentroid.y;
z = height * rand;
points3{i,1} = Point3([x,y,z]');
end
cylinder.area = area;
cylinder.radius = radius;
cylinder.height = height;
cylinder.Points = points3;
cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
end

10
matlab/+gtsam/plotCamera.m
View File

@ -1,18 +1,20 @@
function plotCamera(pose, axisLength)
hold on
C = pose.translation().vector();
R = pose.rotation().matrix();
xAxis = C+R(:,1)*axisLength;
L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r');
h_x = line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+R(:,2)*axisLength;
L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g');
h_y = line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+R(:,3)*axisLength;
L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b');
h_z = line(L(:,1),L(:,2),L(:,3),'Color','b');
axis equal
end
end

35
matlab/+gtsam/plotCylinderSamples.m Normal file
View File

@ -0,0 +1,35 @@
function plotCylinderSamples(cylinders, options, figID)
% plot the cylinders on the given field
% @author: Zhaoyang Lv
figure(figID);
holdstate = ishold;
hold on
num = size(cylinders, 1);
sampleDensity = 120;
for i = 1:num
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
Z = Z * cylinders{i}.height;
cylinderHandle = surf(X,Y,Z);
set(cylinderHandle, 'FaceAlpha', 0.5);
hold on
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
grid on
if ~holdstate
hold off
end
end

177
matlab/+gtsam/plotFlyingResults.m Normal file
View File

@ -0,0 +1,177 @@
function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
% plot the visible points on the cylinders and trajectories
%
% author: Zhaoyang Lv
import gtsam.*
figID = 1;
figure(figID);
set(gcf, 'Position', [80,1,1800,1000]);
%% plot all the cylinders and sampled points
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Height (m)');
h = cameratoolbar('Show');
if options.camera.IS_MONO
h_title = title('Quadrotor Flight Simulation with Monocular Camera');
else
h_title = title('Quadrotor Flight Simulation with Stereo Camera');
end
text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed))
view([30, 30]);
hlight = camlight('headlight');
lighting gouraud
if(options.writeVideo)
videoObj = VideoWriter('Camera_Flying_Example.avi');
videoObj.Quality = 100;
videoObj.FrameRate = options.camera.fps;
open(videoObj);
end
sampleDensity = 120;
cylinderNum = length(cylinders);
h_cylinder = cell(cylinderNum);
for i = 1:cylinderNum
hold on
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
Z = Z * cylinders{i}.height;
h_cylinder{i} = surf(X,Y,Z);
set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
h_cylinder{i}.AmbientStrength = 0.8;
end
%% plot trajectories and points
posesSize = length(poses);
pointSize = length(pts3d);
for i = 1:posesSize
if i > 1
hold on
plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
end
if exist('h_pose_cov', 'var')
delete(h_pose_cov);
end
%plotCamera(poses{i}, 3);
gRp = poses{i}.rotation().matrix(); % rotation from pose to global
C = poses{i}.translation().vector();
axisLength = 3;
xAxis = C+gRp(:,1)*axisLength;
L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+gRp(:,2)*axisLength;
L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+gRp(:,3)*axisLength;
L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b');
pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame
gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale);
if exist('h_point', 'var')
for j = 1:pointSize
delete(h_point{j});
end
end
if exist('h_point_cov', 'var')
for j = 1:pointSize
delete(h_point_cov{j});
end
end
h_point = cell(pointSize, 1);
h_point_cov = cell(pointSize, 1);
for j = 1:pointSize
if ~isempty(pts3d{j}.cov{i})
hold on
h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
pts3d{j}.cov{i}, options.plot.covarianceScale);
end
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
drawnow
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
end
if exist('h_pose_cov', 'var')
delete(h_pose_cov);
end
% wait for two seconds
pause(2);
%% change views angle
for i = 30 : i : 90
view([30, i]);
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
drawnow
end
% changing perspective
%% camera flying through video
camzoom(0.8);
for i = 1 : posesSize
hold on
campos([poses{i}.x, poses{i}.y, poses{i}.z]);
camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
camlight(hlight, 'headlight');
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
drawnow
end
%%close video
if(options.writeVideo)
close(videoObj);
end
end

113
matlab/+gtsam/points2DTrackMonocular.m Normal file
View File

@ -0,0 +1,113 @@
function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization
% @author: Zhaoyang Lv
import gtsam.*
%% create graph
graph = NonlinearFactorGraph;
%% create the noise factors
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
measurementNoiseSigma = 1.0;
measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
points3d = cell(0);
for i = 1:cylinderNum
cylinderPointsNum = length(cylinders{i}.Points);
pointsNum = pointsNum + cylinderPointsNum;
for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0);
points3d{end}.camConstraintIdx = cell(0);
points3d{end}.added = cell(0);
points3d{end}.visiblity = false;
points3d{end}.cov = cell(cameraPosesNum);
end
end
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end
pts3d = cell(cameraPosesNum, 1);
cameraPosesCov = cell(cameraPosesNum, 1);
marginals = Values;
for i = 1:cameraPosesNum
cameraPose = cameraPoses{i};
pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.camConstraintIdx{end+1} = i;
points3d{index}.added{end+1} = false;
if length(points3d{index}.Z) < 2
continue;
else
for k = 1:length(points3d{index}.Z)
if ~points3d{index}.added{k}
graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ...
measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ...
symbol('p', index), K) );
points3d{index}.added{k} = true;
end
end
end
points3d{index}.visiblity = true;
end
pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
initialEstimate.insert(symbol('x', i), pose_i);
marginals = Marginals(graph, initialEstimate);
for j = 1:pointsNum
if points3d{j}.visiblity
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j));
end
end
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i));
end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% Plot the result
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
%% get all the points track information
for i = 1:pointsNum
if ~points3d{i}.visiblity
continue;
end
pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
pts2dTracksMono.Z{end+1} = points3d{i}.Z;
if length(points3d{i}.Z) == 1
%pts2dTracksMono.cov{i} singular matrix
else
pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
end
end

101
matlab/+gtsam/points2DTrackStereo.m Normal file
View File

@ -0,0 +1,101 @@
function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization
%
% @author: Zhaoyang Lv
import gtsam.*
%% create graph
graph = NonlinearFactorGraph;
%% create the noise factors
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05);
cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
points3d = cell(0);
for i = 1:cylinderNum
cylinderPointsNum = length(cylinders{i}.Points);
pointsNum = pointsNum + length(cylinders{i}.Points);
for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0);
points3d{end}.cameraConstraint = cell(0);
points3d{end}.visiblity = false;
points3d{end}.cov = cell(cameraPosesNum);
end
end
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.05*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end
pts3d = cell(cameraPosesNum, 1);
cameraPosesCov = cell(cameraPosesNum, 1);
for i = 1:cameraPosesNum
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
if isempty(pts3d{i}.Z)
continue;
end
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.cameraConstraint{end+1} = i;
points3d{index}.visiblity = true;
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
stereoNoise, symbol('x', i), symbol('p', index), K));
end
pose_i = cameraPoses{i}.retract(poseNoiseSigmas);
initialEstimate.insert(symbol('x', i), pose_i);
%% linearize the graph
marginals = Marginals(graph, initialEstimate);
for j = 1:pointsNum
if points3d{j}.visiblity
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j));
end
end
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));
end
%% Plot the result
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
%% get all the 2d points track information
pts2dTracksStereo.pt3d = cell(0);
pts2dTracksStereo.Z = cell(0);
pts2dTracksStereo.cov = cell(0);
for i = 1:pointsNum
if ~points3d{i}.visiblity
continue;
end
pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
%
% %% plot the result with covariance ellipses
% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
end

179
matlab/gtsam_examples/CameraFlyingExample.m Normal file
View File

@ -0,0 +1,179 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief A camera flying example through a field of cylinder landmarks
% @author Zhaoyang Lv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
clf;
import gtsam.*
% test or run
options.enableTests = false;
%% cylinder options
% the number of cylinders in the field
options.cylinder.cylinderNum = 15; % pls be smaller than 20
% cylinder size
options.cylinder.radius = 3; % pls be smaller than 5
options.cylinder.height = 10;
% point density on cylinder
options.cylinder.pointDensity = 0.1;
%% camera options
% parameters set according to the stereo camera:
% http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
% set up monocular camera or stereo camera
options.camera.IS_MONO = false;
% the field of view of camera
options.camera.fov = 120;
% fps for image
options.camera.fps = 25;
% camera pixel resolution
options.camera.resolution = Point2(752, 480);
% camera horizon
options.camera.horizon = 60;
% camera baseline
options.camera.baseline = 0.05;
% camera focal length
options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
options.camera.fov);
% camera focal baseline
options.camera.fB = options.camera.f * options.camera.baseline;
% camera disparity
options.camera.disparity = options.camera.fB / options.camera.horizon;
% Monocular Camera Calibration
options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2);
% Stereo Camera Calibration
options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
% write video output
options.writeVideo = true;
% the testing field size (unit: meter)
options.fieldSize = Point2([100, 100]');
% camera flying speed (unit: meter / second)
options.speed = 20;
% camera flying height
options.height = 30;
%% ploting options
% display covariance scaling factor, default to be 1.
% The covariance visualization default models 99% of all probablity
options.plot.covarianceScale = 1;
% plot the trajectory covariance
options.plot.DISP_TRAJ_COV = true;
% plot points covariance
options.plot.POINTS_COV = true;
%% This is for tests
if options.enableTests
% test1: visibility test in monocular camera
cylinders{1}.centroid = Point3(30, 50, 5);
cylinders{2}.centroid = Point3(50, 50, 5);
cylinders{3}.centroid = Point3(70, 50, 5);
for i = 1:3
cylinders{i}.radius = 5;
cylinders{i}.height = 10;
cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
end
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK);
pose = camera.pose;
prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
options.camera.resolution, cylinders);
% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
pose, options.camera.resolution, cylinders);
end
%% generate a set of cylinders and point samples on cylinders
cylinderNum = options.cylinder.cylinderNum;
cylinders = cell(cylinderNum, 1);
baseCentroid = cell(cylinderNum, 1);
theta = 0;
i = 1;
while i <= cylinderNum
theta = theta + 2*pi/10;
x = 40 * rand * cos(theta) + options.fieldSize.x/2;
y = 20 * rand * sin(theta) + options.fieldSize.y/2;
baseCentroid{i} = Point2([x, y]');
% prevent two cylinders interact with each other
regenerate = false;
for j = 1:i-1
if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
regenerate = true;
break;
end
end
if regenerate
continue;
end
cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
options.cylinder.height, options.cylinder.pointDensity);
i = i+1;
end
%% generate ground truth camera trajectories: a line
KMono = Cal3_S2(525,525,0,320,240);
cameraPoses = cell(0);
theta = 0;
t = Point3(5, 5, options.height);
i = 0;
while 1
i = i+1;
distance = options.speed / options.camera.fps;
angle = 0.1*pi*(rand-0.5);
inc_t = Point3(distance * cos(angle), ...
distance * sin(angle), 0);
t = t.compose(inc_t);
if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
break;
end
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
% 15, 10]');
camera = SimpleCamera.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.camera.monoK);
cameraPoses{end+1} = camera.pose;
end
%% set up camera and get measurements
if options.camera.IS_MONO
% use Monocular Camera
pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
options.camera.resolution, cylinders);
else
% use Stereo Camera
pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ...
cameraPoses, options, cylinders);
end

7
matlab/gtsam_tests/testCal3Unified.m Normal file
View File

@ -0,0 +1,7 @@
% test Cal3Unified
import gtsam.*;
K = Cal3Unified;
EXPECT('fx',K.fx()==1);
EXPECT('fy',K.fy()==1);

21
matlab/gtsam_tests/test_gtsam.m
View File

@ -1,17 +1,25 @@
% Test runner script - runs each test
% display 'Starting: testPriorFactor'
% testPriorFactor
%% geometry
display 'Starting: testCal3Unified'
testCal3Unified
display 'Starting: testValues'
testValues
%% linear
display 'Starting: testKalmanFilter'
testKalmanFilter
display 'Starting: testJacobianFactor'
testJacobianFactor
display 'Starting: testKalmanFilter'
testKalmanFilter
%% nonlinear
display 'Starting: testValues'
testValues
%% SLAM
display 'Starting: testPriorFactor'
testPriorFactor
%% examples
display 'Starting: testLocalizationExample'
testLocalizationExample
@ -36,6 +44,7 @@ testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
%% MATLAB specific
display 'Starting: testUtilities'
testUtilities

1
matlab/unstable_examples/.gitignore vendored
View File

@ -1 +1,2 @@
*.m~
*.avi

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( PCGSolver, dummy )