Merge remote-tracking branch 'origin/develop' into feature/ExpressionsBALExample
dellaert
commit
1d6fb887e8
106
.cproject
106
.cproject
|
|
@ -584,6 +584,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>tests/testBayesTree.run</buildTarget>
|
<buildTarget>tests/testBayesTree.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -591,6 +592,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -638,6 +640,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -645,6 +648,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -652,6 +656,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -667,6 +672,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>tests/testBayesTree</buildTarget>
|
<buildTarget>tests/testBayesTree</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1098,6 +1104,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testErrors.run</buildTarget>
|
<buildTarget>testErrors.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1327,6 +1334,46 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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">
|
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -1409,7 +1456,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1449,7 +1495,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>testSimulated2D.run</buildTarget>
|
<buildTarget>testSimulated2D.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1457,7 +1502,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>testSimulated3D.run</buildTarget>
|
<buildTarget>testSimulated3D.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1471,46 +1515,6 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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">
|
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
|
|
@ -1776,6 +1780,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>cpack</buildCommand>
|
<buildCommand>cpack</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>-G DEB</buildTarget>
|
<buildTarget>-G DEB</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1783,6 +1788,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>cpack</buildCommand>
|
<buildCommand>cpack</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>-G RPM</buildTarget>
|
<buildTarget>-G RPM</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1790,6 +1796,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>cpack</buildCommand>
|
<buildCommand>cpack</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>-G TGZ</buildTarget>
|
<buildTarget>-G TGZ</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1797,6 +1804,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>cpack</buildCommand>
|
<buildCommand>cpack</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
|
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -2169,6 +2177,14 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="testQuaternion.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j4</buildArguments>
|
||||||
|
<buildTarget>testQuaternion.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -2675,6 +2691,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testGraph.run</buildTarget>
|
<buildTarget>testGraph.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -2682,6 +2699,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testJunctionTree.run</buildTarget>
|
<buildTarget>testJunctionTree.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -2689,6 +2707,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -3288,7 +3307,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
|
||||||
79
gtsam.h
79
gtsam.h
|
|
@ -629,28 +629,13 @@ class Cal3_S2 {
|
||||||
void serialize() const;
|
void serialize() const;
|
||||||
};
|
};
|
||||||
|
|
||||||
#include <gtsam/geometry/Cal3DS2.h>
|
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||||
class Cal3DS2 {
|
virtual class Cal3DS2_Base {
|
||||||
// Standard Constructors
|
// Standard Constructors
|
||||||
Cal3DS2();
|
Cal3DS2_Base();
|
||||||
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
|
|
||||||
Cal3DS2(Vector v);
|
|
||||||
|
|
||||||
// Testable
|
// Testable
|
||||||
void print(string s) const;
|
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
|
// Standard Interface
|
||||||
double fx() const;
|
double fx() const;
|
||||||
|
|
@ -658,14 +643,66 @@ class Cal3DS2 {
|
||||||
double skew() const;
|
double skew() const;
|
||||||
double px() const;
|
double px() const;
|
||||||
double py() const;
|
double py() const;
|
||||||
Vector vector() const;
|
double k1() const;
|
||||||
Vector k() const;
|
double k2() const;
|
||||||
//Matrix K() const; //FIXME: Uppercase
|
|
||||||
|
// 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
|
// enabling serialization functionality
|
||||||
void serialize() const;
|
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 {
|
class Cal3_S2Stereo {
|
||||||
// Standard Constructors
|
// Standard Constructors
|
||||||
Cal3_S2Stereo();
|
Cal3_S2Stereo();
|
||||||
|
|
|
||||||
|
|
@ -38,22 +38,23 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
|
||||||
|
|
||||||
// Inverse
|
// Inverse
|
||||||
OJ none;
|
OJ none;
|
||||||
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
|
EXPECT(assert_equal<G>(t1.inverse(),T::Inverse(t1, H1)));
|
||||||
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
|
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
|
||||||
|
|
||||||
EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1)));
|
EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
|
||||||
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
|
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
|
||||||
|
|
||||||
// Compose
|
// Compose
|
||||||
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
|
EXPECT(assert_equal<G>(t1 * t2,T::Compose(t1, t2, H1, H2)));
|
||||||
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
|
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
|
||||||
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
|
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
|
||||||
|
|
||||||
// Between
|
// Between
|
||||||
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
|
EXPECT(assert_equal<G>(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
|
||||||
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
|
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
|
||||||
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
|
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
|
||||||
}
|
}
|
||||||
|
|
||||||
// Do a comprehensive test of Lie Group Chart derivatives
|
// Do a comprehensive test of Lie Group Chart derivatives
|
||||||
template<typename G>
|
template<typename G>
|
||||||
void testChartDerivatives(TestResult& result_, const std::string& name_,
|
void testChartDerivatives(TestResult& result_, const std::string& name_,
|
||||||
|
|
@ -61,18 +62,18 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
|
||||||
|
|
||||||
Matrix H1, H2;
|
Matrix H1, H2;
|
||||||
typedef traits<G> T;
|
typedef traits<G> T;
|
||||||
typedef typename G::TangentVector V;
|
typedef typename T::TangentVector V;
|
||||||
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
|
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
|
||||||
|
|
||||||
// Retract
|
// Retract
|
||||||
OJ none;
|
OJ none;
|
||||||
V w12 = T::Local(t1, t2);
|
V w12 = T::Local(t1, t2);
|
||||||
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
|
EXPECT(assert_equal<G>(t2, T::Retract(t1,w12, H1, H2)));
|
||||||
EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
|
EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
|
||||||
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
|
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
|
||||||
|
|
||||||
// Local
|
// Local
|
||||||
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
|
EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
|
||||||
EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
|
EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
|
||||||
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
|
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -68,7 +68,7 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// print with optional string
|
/// 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
|
/// assert equality up to a tolerance
|
||||||
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
|
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
|
||||||
|
|
@ -89,10 +89,20 @@ public:
|
||||||
/// Return dimensions of calibration manifold object
|
/// Return dimensions of calibration manifold object
|
||||||
static size_t Dim() { return 9; } //TODO: make a final dimension variable
|
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:
|
private:
|
||||||
|
|
||||||
/// @}
|
|
||||||
/// @name Advanced Interface
|
/// @name Advanced Interface
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -45,9 +45,6 @@ protected:
|
||||||
double p1_, p2_ ; // tangential distortion
|
double p1_, p2_ ; // tangential distortion
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Matrix3 K() const ;
|
|
||||||
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
|
|
||||||
Vector9 vector() const ;
|
|
||||||
|
|
||||||
/// @name Standard Constructors
|
/// @name Standard Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
|
@ -59,6 +56,8 @@ public:
|
||||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
|
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) {}
|
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
|
||||||
|
|
||||||
|
virtual ~Cal3DS2_Base() {}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Advanced Constructors
|
/// @name Advanced Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
|
@ -70,7 +69,7 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// print with optional string
|
/// 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
|
/// assert equality up to a tolerance
|
||||||
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
|
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
|
||||||
|
|
@ -106,6 +105,15 @@ public:
|
||||||
/// Second tangential distortion coefficient
|
/// Second tangential distortion coefficient
|
||||||
inline double p2() const { return p2_;}
|
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
|
* convert intrinsic coordinates xy to (distorted) image coordinates uv
|
||||||
* @param p point in intrinsic coordinates
|
* @param p point in intrinsic coordinates
|
||||||
|
|
@ -126,9 +134,19 @@ public:
|
||||||
/// Derivative of uncalibrate wrpt the calibration parameters
|
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||||
Matrix29 D2d_calibration(const Point2& p) const ;
|
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
|
/// @name Advanced Interface
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -50,9 +50,8 @@ private:
|
||||||
double xi_; // mirror parameter
|
double xi_; // mirror parameter
|
||||||
|
|
||||||
public:
|
public:
|
||||||
enum { dimension = 10 };
|
|
||||||
|
|
||||||
Vector10 vector() const ;
|
enum { dimension = 10 };
|
||||||
|
|
||||||
/// @name Standard Constructors
|
/// @name Standard Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
|
@ -77,7 +76,7 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// print with optional string
|
/// 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
|
/// assert equality up to a tolerance
|
||||||
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
|
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
|
||||||
|
|
@ -125,6 +124,11 @@ public:
|
||||||
/// Return dimensions of calibration manifold object
|
/// Return dimensions of calibration manifold object
|
||||||
static size_t Dim() { return 10; } //TODO: make a final dimension variable
|
static size_t Dim() { return 10; } //TODO: make a final dimension variable
|
||||||
|
|
||||||
|
/// Return all parameters as a vector
|
||||||
|
Vector10 vector() const ;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
|
|
|
||||||
|
|
@ -15,8 +15,11 @@
|
||||||
* @author Frank Dellaert
|
* @author Frank Dellaert
|
||||||
**/
|
**/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/Lie.h>
|
#include <gtsam/base/Lie.h>
|
||||||
#include <gtsam/base/concepts.h>
|
#include <gtsam/base/concepts.h>
|
||||||
|
#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
|
||||||
|
|
||||||
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
|
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
|
||||||
|
|
||||||
|
|
@ -37,19 +40,6 @@ struct traits<QUATERNION_TYPE> {
|
||||||
return Q::Identity();
|
return Q::Identity();
|
||||||
}
|
}
|
||||||
|
|
||||||
static Q Compose(const Q &g, const Q & h) {
|
|
||||||
return g * h;
|
|
||||||
}
|
|
||||||
|
|
||||||
static Q Between(const Q &g, const Q & h) {
|
|
||||||
Q d = g.inverse() * h;
|
|
||||||
return d;
|
|
||||||
}
|
|
||||||
|
|
||||||
static Q Inverse(const Q &g) {
|
|
||||||
return g.inverse();
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Basic manifold traits
|
/// @name Basic manifold traits
|
||||||
/// @{
|
/// @{
|
||||||
|
|
@ -62,41 +52,36 @@ struct traits<QUATERNION_TYPE> {
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Lie group traits
|
/// @name Lie group traits
|
||||||
/// @{
|
/// @{
|
||||||
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
|
static Q Compose(const Q &g, const Q & h,
|
||||||
boost::none) {
|
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
|
||||||
if (Hg)
|
if (Hg) *Hg = h.toRotationMatrix().transpose();
|
||||||
*Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
|
if (Hh) *Hh = I_3x3;
|
||||||
if (Hh)
|
|
||||||
*Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? )
|
|
||||||
return g * h;
|
return g * h;
|
||||||
}
|
}
|
||||||
|
|
||||||
static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
|
static Q Between(const Q &g, const Q & h,
|
||||||
boost::none) {
|
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
|
||||||
Q d = g.inverse() * h;
|
Q d = g.inverse() * h;
|
||||||
if (Hg)
|
if (Hg) *Hg = -d.toRotationMatrix().transpose();
|
||||||
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
|
if (Hh) *Hh = I_3x3;
|
||||||
if (Hh)
|
|
||||||
*Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) )
|
|
||||||
return d;
|
return d;
|
||||||
}
|
}
|
||||||
|
|
||||||
static Q Inverse(const Q &g, ChartJacobian H) {
|
static Q Inverse(const Q &g,
|
||||||
if (H)
|
ChartJacobian H = boost::none) {
|
||||||
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
|
if (H) *H = -g.toRotationMatrix();
|
||||||
return g.inverse();
|
return g.inverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Exponential map, simply be converting omega to axis/angle representation
|
/// Exponential map, simply be converting omega to axis/angle representation
|
||||||
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
|
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
|
||||||
ChartJacobian H = boost::none) {
|
ChartJacobian H = boost::none) {
|
||||||
if (omega.isZero())
|
if(H) *H = SO3::ExpmapDerivative(omega);
|
||||||
return Q::Identity();
|
if (omega.isZero()) return Q::Identity();
|
||||||
else {
|
else {
|
||||||
_Scalar angle = omega.norm();
|
_Scalar angle = omega.norm();
|
||||||
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
|
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
|
||||||
}
|
}
|
||||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// We use our own Logmap, as there is a slight bug in Eigen
|
/// We use our own Logmap, as there is a slight bug in Eigen
|
||||||
|
|
@ -106,43 +91,55 @@ struct traits<QUATERNION_TYPE> {
|
||||||
|
|
||||||
// define these compile time constants to avoid std::abs:
|
// define these compile time constants to avoid std::abs:
|
||||||
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
|
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
|
||||||
NearlyNegativeOne = -1.0 + 1e-10;
|
NearlyNegativeOne = -1.0 + 1e-10;
|
||||||
|
|
||||||
|
Vector3 omega;
|
||||||
|
|
||||||
const double qw = q.w();
|
const double qw = q.w();
|
||||||
if (qw > NearlyOne) {
|
if (qw > NearlyOne) {
|
||||||
// Taylor expansion of (angle / s) at 1
|
// Taylor expansion of (angle / s) at 1
|
||||||
//return (2 + 2 * (1-qw) / 3) * q.vec();
|
//return (2 + 2 * (1-qw) / 3) * q.vec();
|
||||||
return (8. / 3. - 2. / 3. * qw) * q.vec();
|
omega = (8. / 3. - 2. / 3. * qw) * q.vec();
|
||||||
} else if (qw < NearlyNegativeOne) {
|
} else if (qw < NearlyNegativeOne) {
|
||||||
// Taylor expansion of (angle / s) at -1
|
// Taylor expansion of (angle / s) at -1
|
||||||
//return (-2 - 2 * (1 + qw) / 3) * q.vec();
|
//return (-2 - 2 * (1 + qw) / 3) * q.vec();
|
||||||
return (-8. / 3 + 2. / 3 * qw) * q.vec();
|
omega = (-8. / 3 + 2. / 3 * qw) * q.vec();
|
||||||
} else {
|
} else {
|
||||||
// Normal, away from zero case
|
// Normal, away from zero case
|
||||||
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
|
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
|
||||||
// Important: convert to [-pi,pi] to keep error continuous
|
// Important: convert to [-pi,pi] to keep error continuous
|
||||||
if (angle > M_PI)
|
if (angle > M_PI)
|
||||||
angle -= twoPi;
|
angle -= twoPi;
|
||||||
else if (angle < -M_PI)
|
else if (angle < -M_PI)
|
||||||
angle += twoPi;
|
angle += twoPi;
|
||||||
return (angle / s) * q.vec();
|
omega = (angle / s) * q.vec();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
if(H) *H = SO3::LogmapDerivative(omega);
|
||||||
|
return omega;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Manifold traits
|
/// @name Manifold traits
|
||||||
/// @{
|
/// @{
|
||||||
static TangentVector Local(const Q& origin, const Q& other,
|
|
||||||
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
|
static TangentVector Local(const Q& g, const Q& h,
|
||||||
return Logmap(Between(origin, other, Horigin, Hother));
|
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
|
||||||
// TODO: incorporate Jacobian of Logmap
|
Q b = Between(g, h, H1, H2);
|
||||||
|
Matrix3 D_v_b;
|
||||||
|
TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
|
||||||
|
if (H1) *H1 = D_v_b * (*H1);
|
||||||
|
if (H2) *H2 = D_v_b * (*H2);
|
||||||
|
return v;
|
||||||
}
|
}
|
||||||
static Q Retract(const Q& origin, const TangentVector& v,
|
|
||||||
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
|
static Q Retract(const Q& g, const TangentVector& v,
|
||||||
return Compose(origin, Expmap(v), Horigin, Hv);
|
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
|
||||||
// TODO : incorporate Jacobian of Expmap
|
Matrix3 D_h_v;
|
||||||
|
Q b = Expmap(v,H2 ? &D_h_v : 0);
|
||||||
|
Q h = Compose(g, b, H1, H2);
|
||||||
|
if (H2) *H2 = (*H2) * D_h_v;
|
||||||
|
return h;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
@ -150,9 +147,9 @@ struct traits<QUATERNION_TYPE> {
|
||||||
/// @{
|
/// @{
|
||||||
static void Print(const Q& q, const std::string& str = "") {
|
static void Print(const Q& q, const std::string& str = "") {
|
||||||
if (str.size() == 0)
|
if (str.size() == 0)
|
||||||
std::cout << "Eigen::Quaternion: ";
|
std::cout << "Eigen::Quaternion: ";
|
||||||
else
|
else
|
||||||
std::cout << str << " ";
|
std::cout << str << " ";
|
||||||
std::cout << q.vec().transpose() << std::endl;
|
std::cout << q.vec().transpose() << std::endl;
|
||||||
}
|
}
|
||||||
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
|
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
|
||||||
|
|
|
||||||
|
|
@ -19,6 +19,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/geometry/Rot3.h>
|
#include <gtsam/geometry/Rot3.h>
|
||||||
|
#include <gtsam/geometry/SO3.h>
|
||||||
#include <boost/math/constants/constants.hpp>
|
#include <boost/math/constants/constants.hpp>
|
||||||
#include <boost/random.hpp>
|
#include <boost/random.hpp>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
@ -149,57 +150,13 @@ Vector Rot3::quaternion() const {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
|
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
|
||||||
if(zero(x)) return I_3x3;
|
return SO3::ExpmapDerivative(x);
|
||||||
double theta = x.norm(); // rotation angle
|
|
||||||
#ifdef DUY_VERSION
|
|
||||||
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
|
||||||
Matrix3 X = skewSymmetric(x);
|
|
||||||
Matrix3 X2 = X*X;
|
|
||||||
double vi = theta/2.0;
|
|
||||||
double s1 = sin(vi)/vi;
|
|
||||||
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
|
||||||
return I_3x3 - 0.5*s1*s1*X + s2*X2;
|
|
||||||
#else // Luca's version
|
|
||||||
/**
|
|
||||||
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
|
|
||||||
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
|
|
||||||
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
|
|
||||||
* where Jr = ExpmapDerivative(thetahat);
|
|
||||||
* This maps a perturbation in the tangent space (omega) to
|
|
||||||
* a perturbation on the manifold (expmap(Jr * omega))
|
|
||||||
*/
|
|
||||||
// element of Lie algebra so(3): X = x^, normalized by normx
|
|
||||||
const Matrix3 Y = skewSymmetric(x) / theta;
|
|
||||||
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
|
|
||||||
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
|
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
|
||||||
if(zero(x)) return I_3x3;
|
return SO3::LogmapDerivative(x);
|
||||||
double theta = x.norm();
|
|
||||||
#ifdef DUY_VERSION
|
|
||||||
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
|
|
||||||
Matrix3 X = skewSymmetric(x);
|
|
||||||
Matrix3 X2 = X*X;
|
|
||||||
double vi = theta/2.0;
|
|
||||||
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
|
|
||||||
return I_3x3 + 0.5*X - s2*X2;
|
|
||||||
#else // Luca's version
|
|
||||||
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
|
|
||||||
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
|
|
||||||
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
|
|
||||||
* where Jrinv = LogmapDerivative(omega);
|
|
||||||
* This maps a perturbation on the manifold (expmap(omega))
|
|
||||||
* to a perturbation in the tangent space (Jrinv * omega)
|
|
||||||
*/
|
|
||||||
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
|
|
||||||
return I_3x3 + 0.5 * X
|
|
||||||
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
|
|
||||||
* X;
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -208,9 +208,10 @@ namespace gtsam {
|
||||||
return Rot3();
|
return Rot3();
|
||||||
}
|
}
|
||||||
|
|
||||||
/** compose two rotations */
|
/// Syntatic sugar for composing two rotations
|
||||||
Rot3 operator*(const Rot3& R2) const;
|
Rot3 operator*(const Rot3& R2) const;
|
||||||
|
|
||||||
|
/// inverse of a rotation, TODO should be different for M/Q
|
||||||
Rot3 inverse() const {
|
Rot3 inverse() const {
|
||||||
return Rot3(Matrix3(transpose()));
|
return Rot3(Matrix3(transpose()));
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -23,6 +23,7 @@
|
||||||
#ifndef GTSAM_USE_QUATERNIONS
|
#ifndef GTSAM_USE_QUATERNIONS
|
||||||
|
|
||||||
#include <gtsam/geometry/Rot3.h>
|
#include <gtsam/geometry/Rot3.h>
|
||||||
|
#include <gtsam/geometry/SO3.h>
|
||||||
#include <boost/math/constants/constants.hpp>
|
#include <boost/math/constants/constants.hpp>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
|
|
@ -118,25 +119,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
|
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
|
||||||
// get components of axis \omega
|
return SO3::Rodrigues(w,theta);
|
||||||
double wx = w(0), wy=w(1), wz=w(2);
|
|
||||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
|
||||||
#ifndef NDEBUG
|
|
||||||
double l_n = wwTxx + wwTyy + wwTzz;
|
|
||||||
if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
|
||||||
#endif
|
|
||||||
|
|
||||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
|
||||||
|
|
||||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
|
||||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
|
||||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
|
||||||
double C22 = c_1*wwTzz;
|
|
||||||
|
|
||||||
return Rot3(
|
|
||||||
c + C00, -swz + C01, swy + C02,
|
|
||||||
swz + C01, c + C11, -swx + C12,
|
|
||||||
-swy + C02, swx + C12, c + C22);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -163,46 +146,7 @@ Point3 Rot3::rotate(const Point3& p,
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Log map at identity - return the canonical coordinates of this rotation
|
// Log map at identity - return the canonical coordinates of this rotation
|
||||||
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
|
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
|
||||||
|
return SO3::Logmap(R.rot_,H);
|
||||||
static const double PI = boost::math::constants::pi<double>();
|
|
||||||
|
|
||||||
const Matrix3& rot = R.rot_;
|
|
||||||
// Get trace(R)
|
|
||||||
double tr = rot.trace();
|
|
||||||
|
|
||||||
Vector3 thetaR;
|
|
||||||
|
|
||||||
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
|
|
||||||
// we do something special
|
|
||||||
if (std::abs(tr+1.0) < 1e-10) {
|
|
||||||
if(std::abs(rot(2,2)+1.0) > 1e-10)
|
|
||||||
return (PI / sqrt(2.0+2.0*rot(2,2) )) *
|
|
||||||
Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
|
|
||||||
else if(std::abs(rot(1,1)+1.0) > 1e-10)
|
|
||||||
return (PI / sqrt(2.0+2.0*rot(1,1))) *
|
|
||||||
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
|
|
||||||
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
|
|
||||||
thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) *
|
|
||||||
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
|
|
||||||
} else {
|
|
||||||
double magnitude;
|
|
||||||
double tr_3 = tr-3.0; // always negative
|
|
||||||
if (tr_3<-1e-7) {
|
|
||||||
double theta = acos((tr-1.0)/2.0);
|
|
||||||
magnitude = theta/(2.0*sin(theta));
|
|
||||||
} else {
|
|
||||||
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
|
|
||||||
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
|
|
||||||
magnitude = 0.5 - tr_3*tr_3/12.0;
|
|
||||||
}
|
|
||||||
thetaR = magnitude*Vector3(
|
|
||||||
rot(2,1)-rot(1,2),
|
|
||||||
rot(0,2)-rot(2,0),
|
|
||||||
rot(1,0)-rot(0,1));
|
|
||||||
}
|
|
||||||
|
|
||||||
if(H) *H = Rot3::LogmapDerivative(thetaR);
|
|
||||||
return thetaR;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -85,28 +85,13 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
|
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
|
||||||
return QuaternionChart::Expmap(theta,w);
|
return Quaternion(Eigen::AngleAxis<double>(theta, w));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Rot3 Rot3::compose(const Rot3& R2,
|
|
||||||
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
|
|
||||||
if (H1) *H1 = R2.transpose();
|
|
||||||
if (H2) *H2 = I3;
|
|
||||||
return Rot3(quaternion_ * R2.quaternion_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||||
return Rot3(quaternion_ * R2.quaternion_);
|
return Rot3(quaternion_ * R2.quaternion_);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
|
|
||||||
if (H1) *H1 = -matrix();
|
|
||||||
return Rot3(quaternion_.inverse());
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// TODO: Could we do this? It works in Rot3M but not here, probably because
|
// TODO: Could we do this? It works in Rot3M but not here, probably because
|
||||||
// here we create an intermediate value by calling matrix()
|
// here we create an intermediate value by calling matrix()
|
||||||
|
|
@ -115,14 +100,6 @@ namespace gtsam {
|
||||||
return matrix().transpose();
|
return matrix().transpose();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Rot3 Rot3::between(const Rot3& R2,
|
|
||||||
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
|
|
||||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
|
||||||
if (H2) *H2 = I3;
|
|
||||||
return inverse() * R2;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Point3 Rot3::rotate(const Point3& p,
|
Point3 Rot3::rotate(const Point3& p,
|
||||||
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
|
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
|
||||||
|
|
@ -135,18 +112,21 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
|
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
|
||||||
if(H) *H = Rot3::LogmapDerivative(thetaR);
|
return traits<Quaternion>::Logmap(R.quaternion_, H);
|
||||||
return QuaternionChart::Logmap(R.quaternion_);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
|
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
|
||||||
return compose(Expmap(omega));
|
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
|
||||||
|
if (mode == Rot3::EXPMAP) return Expmap(omega, H);
|
||||||
|
else throw std::runtime_error("Rot3::Retract: unknown mode");
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
|
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
|
||||||
return Logmap(between(t2));
|
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
|
||||||
|
if (mode == Rot3::EXPMAP) return Logmap(R, H);
|
||||||
|
else throw std::runtime_error("Rot3::Local: unknown mode");
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -20,9 +20,12 @@
|
||||||
#include <gtsam/base/concepts.h>
|
#include <gtsam/base/concepts.h>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
SO3 Rodrigues(const double& theta, const Vector3& axis) {
|
/* ************************************************************************* */
|
||||||
|
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
|
||||||
using std::cos;
|
using std::cos;
|
||||||
using std::sin;
|
using std::sin;
|
||||||
|
|
||||||
|
|
@ -46,27 +49,25 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/// simply convert omega to axis/angle representation
|
/// simply convert omega to axis/angle representation
|
||||||
SO3 SO3::Expmap(const Eigen::Ref<const Vector3>& omega,
|
SO3 SO3::Expmap(const Vector3& omega,
|
||||||
ChartJacobian H) {
|
ChartJacobian H) {
|
||||||
|
|
||||||
if (H)
|
if (H)
|
||||||
CONCEPT_NOT_IMPLEMENTED;
|
*H = ExpmapDerivative(omega);
|
||||||
|
|
||||||
if (omega.isZero())
|
if (omega.isZero())
|
||||||
return SO3::Identity();
|
return Identity();
|
||||||
else {
|
else {
|
||||||
double angle = omega.norm();
|
double angle = omega.norm();
|
||||||
return Rodrigues(angle, omega / angle);
|
return Rodrigues(omega / angle, angle);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
||||||
using std::sqrt;
|
using std::sqrt;
|
||||||
using std::sin;
|
using std::sin;
|
||||||
|
|
||||||
if (H)
|
|
||||||
CONCEPT_NOT_IMPLEMENTED;
|
|
||||||
|
|
||||||
// note switch to base 1
|
// note switch to base 1
|
||||||
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
|
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
|
||||||
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
|
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
|
||||||
|
|
@ -75,16 +76,18 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
||||||
// Get trace(R)
|
// Get trace(R)
|
||||||
double tr = R.trace();
|
double tr = R.trace();
|
||||||
|
|
||||||
|
Vector3 omega;
|
||||||
|
|
||||||
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
|
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
|
||||||
// we do something special
|
// we do something special
|
||||||
if (std::abs(tr + 1.0) < 1e-10) {
|
if (std::abs(tr + 1.0) < 1e-10) {
|
||||||
if (std::abs(R33 + 1.0) > 1e-10)
|
if (std::abs(R33 + 1.0) > 1e-10)
|
||||||
return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
|
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
|
||||||
else if (std::abs(R22 + 1.0) > 1e-10)
|
else if (std::abs(R22 + 1.0) > 1e-10)
|
||||||
return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
|
omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
|
||||||
else
|
else
|
||||||
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
|
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
|
||||||
return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
|
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
|
||||||
} else {
|
} else {
|
||||||
double magnitude;
|
double magnitude;
|
||||||
double tr_3 = tr - 3.0; // always negative
|
double tr_3 = tr - 3.0; // always negative
|
||||||
|
|
@ -96,9 +99,74 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
||||||
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
|
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
|
||||||
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
|
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
|
||||||
}
|
}
|
||||||
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
|
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if(H) *H = LogmapDerivative(omega);
|
||||||
|
return omega;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||||
|
using std::cos;
|
||||||
|
using std::sin;
|
||||||
|
|
||||||
|
if(zero(omega)) return I_3x3;
|
||||||
|
double theta = omega.norm(); // rotation angle
|
||||||
|
#ifdef DUY_VERSION
|
||||||
|
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
||||||
|
Matrix3 X = skewSymmetric(omega);
|
||||||
|
Matrix3 X2 = X*X;
|
||||||
|
double vi = theta/2.0;
|
||||||
|
double s1 = sin(vi)/vi;
|
||||||
|
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
||||||
|
return I_3x3 - 0.5*s1*s1*X + s2*X2;
|
||||||
|
#else // Luca's version
|
||||||
|
/**
|
||||||
|
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
|
||||||
|
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
|
||||||
|
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
|
||||||
|
* where Jr = ExpmapDerivative(thetahat);
|
||||||
|
* This maps a perturbation in the tangent space (omega) to
|
||||||
|
* a perturbation on the manifold (expmap(Jr * omega))
|
||||||
|
*/
|
||||||
|
// element of Lie algebra so(3): X = omega^, normalized by normx
|
||||||
|
const Matrix3 Y = skewSymmetric(omega) / theta;
|
||||||
|
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
|
||||||
|
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
|
||||||
|
using std::cos;
|
||||||
|
using std::sin;
|
||||||
|
|
||||||
|
if(zero(omega)) return I_3x3;
|
||||||
|
double theta = omega.norm();
|
||||||
|
#ifdef DUY_VERSION
|
||||||
|
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
|
||||||
|
Matrix3 X = skewSymmetric(omega);
|
||||||
|
Matrix3 X2 = X*X;
|
||||||
|
double vi = theta/2.0;
|
||||||
|
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
|
||||||
|
return I_3x3 + 0.5*X - s2*X2;
|
||||||
|
#else // Luca's version
|
||||||
|
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
|
||||||
|
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
|
||||||
|
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
|
||||||
|
* where Jrinv = LogmapDerivative(omega);
|
||||||
|
* This maps a perturbation on the manifold (expmap(omega))
|
||||||
|
* to a perturbation in the tangent space (Jrinv * omega)
|
||||||
|
*/
|
||||||
|
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
|
||||||
|
return I_3x3 + 0.5 * X
|
||||||
|
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
|
||||||
|
* X;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
} // end namespace gtsam
|
} // end namespace gtsam
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -30,15 +30,21 @@ namespace gtsam {
|
||||||
* We guarantee (all but first) constructors only generate from sub-manifold.
|
* We guarantee (all but first) constructors only generate from sub-manifold.
|
||||||
* However, round-off errors in repeated composition could move off it...
|
* However, round-off errors in repeated composition could move off it...
|
||||||
*/
|
*/
|
||||||
class SO3: public Matrix3, public LieGroup<SO3,3> {
|
class GTSAM_EXPORT SO3: public Matrix3, public LieGroup<SO3, 3> {
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
public:
|
public:
|
||||||
enum { dimension=3 };
|
enum {
|
||||||
|
dimension = 3
|
||||||
|
};
|
||||||
|
|
||||||
|
/// @name Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
/// Constructor from AngleAxisd
|
/// Constructor from AngleAxisd
|
||||||
SO3() : Matrix3(I_3x3) {
|
SO3() :
|
||||||
|
Matrix3(I_3x3) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Constructor from Eigen Matrix
|
/// Constructor from Eigen Matrix
|
||||||
|
|
@ -52,6 +58,13 @@ public:
|
||||||
Matrix3(angleAxis) {
|
Matrix3(angleAxis) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Static, named constructor TODO think about relation with above
|
||||||
|
static SO3 Rodrigues(const Vector3& axis, double theta);
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
|
||||||
void print(const std::string& s) const {
|
void print(const std::string& s) const {
|
||||||
std::cout << s << *this << std::endl;
|
std::cout << s << *this << std::endl;
|
||||||
}
|
}
|
||||||
|
|
@ -60,32 +73,67 @@ public:
|
||||||
return equal_with_abs_tol(*this, R, tol);
|
return equal_with_abs_tol(*this, R, tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
static SO3 identity() { return I_3x3; }
|
/// @}
|
||||||
SO3 inverse() const { return this->Matrix3::inverse(); }
|
/// @name Group
|
||||||
|
/// @{
|
||||||
|
|
||||||
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega, ChartJacobian H = boost::none);
|
/// identity rotation for group operation
|
||||||
|
static SO3 identity() {
|
||||||
|
return I_3x3;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// inverse of a rotation = transpose
|
||||||
|
SO3 inverse() const {
|
||||||
|
return this->Matrix3::inverse();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Lie Group
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Exponential map at identity - create a rotation from canonical coordinates
|
||||||
|
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
|
||||||
|
*/
|
||||||
|
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Log map at identity - returns the canonical coordinates
|
||||||
|
* \f$ [R_x,R_y,R_z] \f$ of this rotation
|
||||||
|
*/
|
||||||
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
|
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
|
||||||
|
|
||||||
Matrix3 AdjointMap() const { return *this; }
|
/// Derivative of Expmap
|
||||||
|
static Matrix3 ExpmapDerivative(const Vector3& omega);
|
||||||
|
|
||||||
|
/// Derivative of Logmap
|
||||||
|
static Matrix3 LogmapDerivative(const Vector3& omega);
|
||||||
|
|
||||||
|
Matrix3 AdjointMap() const {
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
// Chart at origin
|
// Chart at origin
|
||||||
struct ChartAtOrigin {
|
struct ChartAtOrigin {
|
||||||
static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) {
|
static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
|
||||||
return Expmap(v,H);
|
return Expmap(omega, H);
|
||||||
}
|
}
|
||||||
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
|
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
|
||||||
return Logmap(R,H);
|
return Logmap(R, H);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
using LieGroup<SO3,3>::inverse;
|
using LieGroup<SO3, 3>::inverse;
|
||||||
|
|
||||||
|
/// @}
|
||||||
};
|
};
|
||||||
|
|
||||||
template<>
|
template<>
|
||||||
struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
|
struct traits<SO3> : public internal::LieGroupTraits<SO3> {
|
||||||
|
};
|
||||||
|
|
||||||
template<>
|
template<>
|
||||||
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {};
|
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
|
||||||
|
};
|
||||||
} // end namespace gtsam
|
} // end namespace gtsam
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -145,7 +145,7 @@ TEST(Pose2, expmap0d) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// test case for screw motion in the plane
|
// test case for screw motion in the plane
|
||||||
namespace screw {
|
namespace screwPose2 {
|
||||||
double w=0.3;
|
double w=0.3;
|
||||||
Vector xi = (Vector(3) << 0.0, w, w).finished();
|
Vector xi = (Vector(3) << 0.0, w, w).finished();
|
||||||
Rot2 expectedR = Rot2::fromAngle(w);
|
Rot2 expectedR = Rot2::fromAngle(w);
|
||||||
|
|
@ -155,9 +155,9 @@ namespace screw {
|
||||||
|
|
||||||
TEST(Pose2, expmap_c)
|
TEST(Pose2, expmap_c)
|
||||||
{
|
{
|
||||||
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
|
EXPECT(assert_equal(screwPose2::expected, expm<Pose2>(screwPose2::xi),1e-6));
|
||||||
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
|
EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),1e-6));
|
||||||
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
|
EXPECT(assert_equal(screwPose2::xi, Pose2::Logmap(screwPose2::expected),1e-6));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -109,7 +109,7 @@ TEST(Pose3, expmap_b)
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// test case for screw motion in the plane
|
// test case for screw motion in the plane
|
||||||
namespace screw {
|
namespace screwPose3 {
|
||||||
double a=0.3, c=cos(a), s=sin(a), w=0.3;
|
double a=0.3, c=cos(a), s=sin(a), w=0.3;
|
||||||
Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished();
|
Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished();
|
||||||
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
|
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
|
||||||
|
|
@ -121,24 +121,24 @@ namespace screw {
|
||||||
// Checks correct exponential map (Expmap) with brute force matrix exponential
|
// Checks correct exponential map (Expmap) with brute force matrix exponential
|
||||||
TEST(Pose3, expmap_c_full)
|
TEST(Pose3, expmap_c_full)
|
||||||
{
|
{
|
||||||
EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
|
EXPECT(assert_equal(screwPose3::expected, expm<Pose3>(screwPose3::xi),1e-6));
|
||||||
EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
|
EXPECT(assert_equal(screwPose3::expected, Pose3::Expmap(screwPose3::xi),1e-6));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
|
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
|
||||||
TEST(Pose3, Adjoint_full)
|
TEST(Pose3, Adjoint_full)
|
||||||
{
|
{
|
||||||
Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse();
|
Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse();
|
||||||
Vector xiprime = T.Adjoint(screw::xi);
|
Vector xiprime = T.Adjoint(screwPose3::xi);
|
||||||
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
|
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
|
||||||
|
|
||||||
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse();
|
Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse();
|
||||||
Vector xiprime2 = T2.Adjoint(screw::xi);
|
Vector xiprime2 = T2.Adjoint(screwPose3::xi);
|
||||||
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
|
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
|
||||||
|
|
||||||
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse();
|
Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse();
|
||||||
Vector xiprime3 = T3.Adjoint(screw::xi);
|
Vector xiprime3 = T3.Adjoint(screwPose3::xi);
|
||||||
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
|
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -634,9 +634,9 @@ TEST( Pose3, unicycle )
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( Pose3, adjointMap) {
|
TEST( Pose3, adjointMap) {
|
||||||
Matrix res = Pose3::adjointMap(screw::xi);
|
Matrix res = Pose3::adjointMap(screwPose3::xi);
|
||||||
Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2));
|
Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
|
||||||
Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5));
|
Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
|
||||||
Matrix Z3 = zeros(3,3);
|
Matrix Z3 = zeros(3,3);
|
||||||
Matrix6 expected;
|
Matrix6 expected;
|
||||||
expected << wh, Z3, vh, wh;
|
expected << wh, Z3, vh, wh;
|
||||||
|
|
@ -704,13 +704,13 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST( Pose3, adjoint) {
|
TEST( Pose3, adjoint) {
|
||||||
Vector expected = testDerivAdjoint(screw::xi, screw::xi);
|
Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi);
|
||||||
|
|
||||||
Matrix actualH;
|
Matrix actualH;
|
||||||
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
|
Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH);
|
||||||
|
|
||||||
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
|
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
|
||||||
testDerivAdjoint, screw::xi, screw::xi, 1e-5);
|
testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);
|
||||||
|
|
||||||
EXPECT(assert_equal(expected,actual,1e-5));
|
EXPECT(assert_equal(expected,actual,1e-5));
|
||||||
EXPECT(assert_equal(numericalH,actualH,1e-5));
|
EXPECT(assert_equal(numericalH,actualH,1e-5));
|
||||||
|
|
@ -775,13 +775,16 @@ TEST(Pose3 , LieGroupDerivatives) {
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(Pose3 , ChartDerivatives) {
|
TEST(Pose3 , ChartDerivatives) {
|
||||||
Pose3 id;
|
Pose3 id;
|
||||||
|
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
|
||||||
CHECK_CHART_DERIVATIVES(id,id);
|
CHECK_CHART_DERIVATIVES(id,id);
|
||||||
CHECK_CHART_DERIVATIVES(id,T2);
|
CHECK_CHART_DERIVATIVES(id,T2);
|
||||||
CHECK_CHART_DERIVATIVES(T2,id);
|
CHECK_CHART_DERIVATIVES(T2,id);
|
||||||
CHECK_CHART_DERIVATIVES(T2,T3);
|
CHECK_CHART_DERIVATIVES(T2,T3);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main(){ TestResult tr; return TestRegistry::runAllTests(tr);}
|
int main(){ //TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
|
std::cout<<"testPose3 currently disabled!!" << std::endl;
|
||||||
|
}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -17,6 +17,8 @@
|
||||||
|
|
||||||
#include <gtsam/geometry/Quaternion.h>
|
#include <gtsam/geometry/Quaternion.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
#include <gtsam/base/testLie.h>
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
@ -37,14 +39,6 @@ TEST(Quaternion , Constructor) {
|
||||||
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
|
||||||
TEST(Quaternion , Invariants) {
|
|
||||||
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
|
||||||
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
|
|
||||||
check_group_invariants(q1, q2);
|
|
||||||
check_manifold_invariants(q1, q2);
|
|
||||||
}
|
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(Quaternion , Local) {
|
TEST(Quaternion , Local) {
|
||||||
Vector3 z_axis(0, 0, 1);
|
Vector3 z_axis(0, 0, 1);
|
||||||
|
|
@ -74,47 +68,62 @@ TEST(Quaternion , Compose) {
|
||||||
Q q2(Eigen::AngleAxisd(0.1, z_axis));
|
Q q2(Eigen::AngleAxisd(0.1, z_axis));
|
||||||
|
|
||||||
Q expected = q1 * q2;
|
Q expected = q1 * q2;
|
||||||
Matrix actualH1, actualH2;
|
Q actual = traits<Q>::Compose(q1, q2);
|
||||||
Q actual = traits<Q>::Compose(q1, q2, actualH1, actualH2);
|
|
||||||
EXPECT(traits<Q>::Equals(expected,actual));
|
EXPECT(traits<Q>::Equals(expected,actual));
|
||||||
|
|
||||||
Matrix numericalH1 = numericalDerivative21(traits<Q>::Compose, q1, q2);
|
|
||||||
EXPECT(assert_equal(numericalH1,actualH1));
|
|
||||||
|
|
||||||
Matrix numericalH2 = numericalDerivative22(traits<Q>::Compose, q1, q2);
|
|
||||||
EXPECT(assert_equal(numericalH2,actualH2));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
Vector3 z_axis(0, 0, 1);
|
||||||
|
Q id(Eigen::AngleAxisd(0, z_axis));
|
||||||
|
Q R1(Eigen::AngleAxisd(1, z_axis));
|
||||||
|
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(Quaternion , Between) {
|
TEST(Quaternion , Between) {
|
||||||
Vector3 z_axis(0, 0, 1);
|
|
||||||
Q q1(Eigen::AngleAxisd(0.2, z_axis));
|
Q q1(Eigen::AngleAxisd(0.2, z_axis));
|
||||||
Q q2(Eigen::AngleAxisd(0.1, z_axis));
|
Q q2(Eigen::AngleAxisd(0.1, z_axis));
|
||||||
|
|
||||||
Q expected = q1.inverse() * q2;
|
Q expected = q1.inverse() * q2;
|
||||||
Matrix actualH1, actualH2;
|
Q actual = traits<Q>::Between(q1, q2);
|
||||||
Q actual = traits<Q>::Between(q1, q2, actualH1, actualH2);
|
|
||||||
EXPECT(traits<Q>::Equals(expected,actual));
|
EXPECT(traits<Q>::Equals(expected,actual));
|
||||||
|
|
||||||
Matrix numericalH1 = numericalDerivative21(traits<Q>::Between, q1, q2);
|
|
||||||
EXPECT(assert_equal(numericalH1,actualH1));
|
|
||||||
|
|
||||||
Matrix numericalH2 = numericalDerivative22(traits<Q>::Between, q1, q2);
|
|
||||||
EXPECT(assert_equal(numericalH2,actualH2));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(Quaternion , Inverse) {
|
TEST(Quaternion , Inverse) {
|
||||||
Vector3 z_axis(0, 0, 1);
|
|
||||||
Q q1(Eigen::AngleAxisd(0.1, z_axis));
|
Q q1(Eigen::AngleAxisd(0.1, z_axis));
|
||||||
Q expected(Eigen::AngleAxisd(-0.1, z_axis));
|
Q expected(Eigen::AngleAxisd(-0.1, z_axis));
|
||||||
|
|
||||||
Matrix actualH;
|
Q actual = traits<Q>::Inverse(q1);
|
||||||
Q actual = traits<Q>::Inverse(q1, actualH);
|
|
||||||
EXPECT(traits<Q>::Equals(expected,actual));
|
EXPECT(traits<Q>::Equals(expected,actual));
|
||||||
|
}
|
||||||
|
|
||||||
Matrix numericalH = numericalDerivative11(traits<Q>::Inverse, q1);
|
//******************************************************************************
|
||||||
EXPECT(assert_equal(numericalH,actualH));
|
TEST(Quaternion , Invariants) {
|
||||||
|
check_group_invariants(id,id);
|
||||||
|
check_group_invariants(id,R1);
|
||||||
|
check_group_invariants(R2,id);
|
||||||
|
check_group_invariants(R2,R1);
|
||||||
|
|
||||||
|
check_manifold_invariants(id,id);
|
||||||
|
check_manifold_invariants(id,R1);
|
||||||
|
check_manifold_invariants(R2,id);
|
||||||
|
check_manifold_invariants(R2,R1);
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(Quaternion , LieGroupDerivatives) {
|
||||||
|
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||||
|
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
|
||||||
|
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
|
||||||
|
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(Quaternion , ChartDerivatives) {
|
||||||
|
CHECK_CHART_DERIVATIVES(id,id);
|
||||||
|
CHECK_CHART_DERIVATIVES(id,R2);
|
||||||
|
CHECK_CHART_DERIVATIVES(R2,id);
|
||||||
|
CHECK_CHART_DERIVATIVES(R2,R1);
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
|
|
|
||||||
|
|
@ -663,12 +663,13 @@ TEST(Rot3 , Invariants) {
|
||||||
check_group_invariants(id,T1);
|
check_group_invariants(id,T1);
|
||||||
check_group_invariants(T2,id);
|
check_group_invariants(T2,id);
|
||||||
check_group_invariants(T2,T1);
|
check_group_invariants(T2,T1);
|
||||||
|
check_group_invariants(T1,T2);
|
||||||
|
|
||||||
check_manifold_invariants(id,id);
|
check_manifold_invariants(id,id);
|
||||||
check_manifold_invariants(id,T1);
|
check_manifold_invariants(id,T1);
|
||||||
check_manifold_invariants(T2,id);
|
check_manifold_invariants(T2,id);
|
||||||
check_manifold_invariants(T2,T1);
|
check_manifold_invariants(T2,T1);
|
||||||
|
check_manifold_invariants(T1,T2);
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
|
|
@ -678,24 +679,27 @@ TEST(Rot3 , LieGroupDerivatives) {
|
||||||
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||||
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
|
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
|
||||||
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
|
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
|
||||||
|
CHECK_LIE_GROUP_DERIVATIVES(T1,T2);
|
||||||
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
|
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(Rot3 , ChartDerivatives) {
|
TEST(Rot3 , ChartDerivatives) {
|
||||||
Rot3 id;
|
Rot3 id;
|
||||||
|
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
|
||||||
CHECK_CHART_DERIVATIVES(id,id);
|
CHECK_CHART_DERIVATIVES(id,id);
|
||||||
CHECK_CHART_DERIVATIVES(id,T2);
|
CHECK_CHART_DERIVATIVES(id,T2);
|
||||||
CHECK_CHART_DERIVATIVES(T2,id);
|
CHECK_CHART_DERIVATIVES(T2,id);
|
||||||
CHECK_CHART_DERIVATIVES(T2,T1);
|
CHECK_CHART_DERIVATIVES(T1,T2);
|
||||||
|
CHECK_CHART_DERIVATIVES(T2,T1);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() {
|
int main() {
|
||||||
TestResult tr;
|
// TestResult tr;
|
||||||
return TestRegistry::runAllTests(tr);
|
// return TestRegistry::runAllTests(tr);
|
||||||
|
std::cout << "testRot3 currently disabled!!" << std::endl;
|
||||||
}
|
}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -66,16 +66,15 @@ TEST(SO3 , Invariants) {
|
||||||
check_manifold_invariants(id,R1);
|
check_manifold_invariants(id,R1);
|
||||||
check_manifold_invariants(R2,id);
|
check_manifold_invariants(R2,id);
|
||||||
check_manifold_invariants(R2,R1);
|
check_manifold_invariants(R2,R1);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
//TEST(SO3 , LieGroupDerivatives) {
|
TEST(SO3 , LieGroupDerivatives) {
|
||||||
// CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||||
// CHECK_LIE_GROUP_DERIVATIVES(id,R2);
|
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
|
||||||
// CHECK_LIE_GROUP_DERIVATIVES(R2,id);
|
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
|
||||||
// CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
|
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
|
||||||
//}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(SO3 , ChartDerivatives) {
|
TEST(SO3 , ChartDerivatives) {
|
||||||
|
|
|
||||||
|
|
@ -59,7 +59,7 @@ static StereoCamera cam2(pose3, cal4ptr);
|
||||||
static StereoPoint2 spt(1.0, 2.0, 3.0);
|
static StereoPoint2 spt(1.0, 2.0, 3.0);
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST (Serialization, text_geometry) {
|
TEST_DISABLED (Serialization, text_geometry) {
|
||||||
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||||
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||||
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||||
|
|
@ -84,7 +84,7 @@ TEST (Serialization, text_geometry) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST (Serialization, xml_geometry) {
|
TEST_DISABLED (Serialization, xml_geometry) {
|
||||||
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
|
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||||
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||||
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||||
|
|
@ -108,7 +108,7 @@ TEST (Serialization, xml_geometry) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST (Serialization, binary_geometry) {
|
TEST_DISABLED (Serialization, binary_geometry) {
|
||||||
EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0)));
|
EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||||
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||||
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||||
|
|
|
||||||
|
|
@ -84,8 +84,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
|
||||||
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
|
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
|
||||||
|
|
||||||
// Make the result as Vector form
|
// Make the result as Vector form
|
||||||
AtAx = vvAtAx.vector();
|
AtAx = vvAtAx.vector(keyInfo_.ordering());
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*****************************************************************************/
|
/*****************************************************************************/
|
||||||
|
|
@ -96,7 +95,7 @@ void GaussianFactorGraphSystem::getb(Vector &b) const {
|
||||||
VectorValues vvb = gfg_.gradientAtZero();
|
VectorValues vvb = gfg_.gradientAtZero();
|
||||||
|
|
||||||
// Make the result as Vector form
|
// Make the result as Vector form
|
||||||
b = -vvb.vector();
|
b = -vvb.vector(keyInfo_.ordering());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**********************************************************************************/
|
/**********************************************************************************/
|
||||||
|
|
|
||||||
|
|
@ -148,7 +148,6 @@ TEST( GaussianBayesNet, DeterminantTest )
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
typedef Eigen::Matrix<double,10,1> Vector10;
|
|
||||||
namespace {
|
namespace {
|
||||||
double computeError(const GaussianBayesNet& gbn, const Vector10& values)
|
double computeError(const GaussianBayesNet& gbn, const Vector10& values)
|
||||||
{
|
{
|
||||||
|
|
|
||||||
|
|
@ -175,7 +175,6 @@ TEST(GaussianBayesTree, complicatedMarginal) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
typedef Eigen::Matrix<double, 10, 1> Vector10;
|
|
||||||
namespace {
|
namespace {
|
||||||
double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
|
double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
|
||||||
pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();
|
pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();
|
||||||
|
|
|
||||||
|
|
@ -26,7 +26,7 @@
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
|
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -36,7 +36,7 @@ public:
|
||||||
* Can be built incrementally so as to avoid costly integration at time of
|
* Can be built incrementally so as to avoid costly integration at time of
|
||||||
* factor construction.
|
* factor construction.
|
||||||
*/
|
*/
|
||||||
class PreintegratedMeasurements : public PreintegratedRotation {
|
class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation {
|
||||||
|
|
||||||
friend class AHRSFactor;
|
friend class AHRSFactor;
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -94,15 +94,27 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
|
||||||
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
|
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
|
||||||
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
|
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
|
||||||
|
|
||||||
|
Matrix3 F_pos_noiseacc;
|
||||||
|
if(use2ndOrderIntegration()){
|
||||||
|
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
|
||||||
|
preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose();
|
||||||
|
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
|
||||||
|
preintMeasCov_.block<3,3>(0,3) += temp;
|
||||||
|
preintMeasCov_.block<3,3>(3,0) += temp.transpose();
|
||||||
|
}
|
||||||
|
|
||||||
// F_test and G_test are given as output for testing purposes and are not needed by the factor
|
// F_test and G_test are given as output for testing purposes and are not needed by the factor
|
||||||
if(F_test){ // This in only for testing
|
if(F_test){ // This in only for testing
|
||||||
(*F_test) << F;
|
(*F_test) << F;
|
||||||
}
|
}
|
||||||
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
|
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
|
||||||
// intNoise accNoise omegaNoise
|
if(!use2ndOrderIntegration())
|
||||||
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
|
F_pos_noiseacc = Z_3x3;
|
||||||
Z_3x3, R_i * deltaT, Z_3x3, // vel
|
|
||||||
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
|
// intNoise accNoise omegaNoise
|
||||||
|
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
|
||||||
|
Z_3x3, R_i * deltaT, Z_3x3, // vel
|
||||||
|
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -83,6 +83,7 @@ public:
|
||||||
integrationCovariance_(integrationErrorCovariance) {}
|
integrationCovariance_(integrationErrorCovariance) {}
|
||||||
|
|
||||||
/// methods to access class variables
|
/// methods to access class variables
|
||||||
|
const bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;}
|
||||||
const Vector3& deltaPij() const {return deltaPij_;}
|
const Vector3& deltaPij() const {return deltaPij_;}
|
||||||
const Vector3& deltaVij() const {return deltaVij_;}
|
const Vector3& deltaVij() const {return deltaVij_;}
|
||||||
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
|
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
|
||||||
|
|
@ -149,8 +150,14 @@ public:
|
||||||
|
|
||||||
if(F){
|
if(F){
|
||||||
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
|
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
|
||||||
|
Matrix3 F_pos_angles;
|
||||||
|
if(use2ndOrderIntegration_)
|
||||||
|
F_pos_angles = 0.5 * F_vel_angles * deltaT;
|
||||||
|
else
|
||||||
|
F_pos_angles = Z_3x3;
|
||||||
|
|
||||||
// pos vel angle
|
// pos vel angle
|
||||||
*F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
|
*F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
|
||||||
Z_3x3, I_3x3, F_vel_angles, // vel
|
Z_3x3, I_3x3, F_vel_angles, // vel
|
||||||
Z_3x3, Z_3x3, F_angles_angles;// angle
|
Z_3x3, Z_3x3, F_angles_angles;// angle
|
||||||
}
|
}
|
||||||
|
|
@ -353,7 +360,7 @@ public:
|
||||||
// dfV/dPosej
|
// dfV/dPosej
|
||||||
Matrix::Zero(3,6),
|
Matrix::Zero(3,6),
|
||||||
// dfR/dPosej
|
// dfR/dPosej
|
||||||
D_fR_fRrot * ( I_3x3 ), Z_3x3;
|
D_fR_fRrot, Z_3x3;
|
||||||
}
|
}
|
||||||
if(H4) {
|
if(H4) {
|
||||||
H4->resize(9,3);
|
H4->resize(9,3);
|
||||||
|
|
|
||||||
|
|
@ -56,7 +56,7 @@ Vector updatePreintegratedMeasurementsTest(
|
||||||
if(!use2ndOrderIntegration){
|
if(!use2ndOrderIntegration){
|
||||||
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
|
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
|
||||||
}else{
|
}else{
|
||||||
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
||||||
}
|
}
|
||||||
Vector3 deltaVij_new = deltaVij_old + temp;
|
Vector3 deltaVij_new = deltaVij_old + temp;
|
||||||
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);
|
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);
|
||||||
|
|
|
||||||
|
|
@ -65,7 +65,7 @@ Vector updatePreintegratedPosVel(
|
||||||
if(!use2ndOrderIntegration_){
|
if(!use2ndOrderIntegration_){
|
||||||
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
|
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
|
||||||
}else{
|
}else{
|
||||||
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
||||||
}
|
}
|
||||||
Vector3 deltaVij_new = deltaVij_old + temp;
|
Vector3 deltaVij_new = deltaVij_old + temp;
|
||||||
|
|
||||||
|
|
@ -90,9 +90,9 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
||||||
const list<Vector3>& measuredAccs,
|
const list<Vector3>& measuredAccs,
|
||||||
const list<Vector3>& measuredOmegas,
|
const list<Vector3>& measuredOmegas,
|
||||||
const list<double>& deltaTs,
|
const list<double>& deltaTs,
|
||||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
const bool use2ndOrderIntegration = false){
|
||||||
ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(),
|
ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(),
|
||||||
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity());
|
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration);
|
||||||
|
|
||||||
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
|
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
|
||||||
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
||||||
|
|
@ -107,8 +107,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
|
||||||
const imuBias::ConstantBias& bias,
|
const imuBias::ConstantBias& bias,
|
||||||
const list<Vector3>& measuredAccs,
|
const list<Vector3>& measuredAccs,
|
||||||
const list<Vector3>& measuredOmegas,
|
const list<Vector3>& measuredOmegas,
|
||||||
const list<double>& deltaTs,
|
const list<double>& deltaTs){
|
||||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
|
||||||
return evaluatePreintegratedMeasurements(bias,
|
return evaluatePreintegratedMeasurements(bias,
|
||||||
measuredAccs, measuredOmegas, deltaTs).deltaPij();
|
measuredAccs, measuredOmegas, deltaTs).deltaPij();
|
||||||
}
|
}
|
||||||
|
|
@ -117,8 +116,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
|
||||||
const imuBias::ConstantBias& bias,
|
const imuBias::ConstantBias& bias,
|
||||||
const list<Vector3>& measuredAccs,
|
const list<Vector3>& measuredAccs,
|
||||||
const list<Vector3>& measuredOmegas,
|
const list<Vector3>& measuredOmegas,
|
||||||
const list<double>& deltaTs,
|
const list<double>& deltaTs)
|
||||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
|
||||||
{
|
{
|
||||||
return evaluatePreintegratedMeasurements(bias,
|
return evaluatePreintegratedMeasurements(bias,
|
||||||
measuredAccs, measuredOmegas, deltaTs).deltaVij();
|
measuredAccs, measuredOmegas, deltaTs).deltaVij();
|
||||||
|
|
@ -128,10 +126,9 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
|
||||||
const imuBias::ConstantBias& bias,
|
const imuBias::ConstantBias& bias,
|
||||||
const list<Vector3>& measuredAccs,
|
const list<Vector3>& measuredAccs,
|
||||||
const list<Vector3>& measuredOmegas,
|
const list<Vector3>& measuredOmegas,
|
||||||
const list<double>& deltaTs,
|
const list<double>& deltaTs){
|
||||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
|
||||||
return Rot3(evaluatePreintegratedMeasurements(bias,
|
return Rot3(evaluatePreintegratedMeasurements(bias,
|
||||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
|
measuredAccs, measuredOmegas, deltaTs).deltaRij());
|
||||||
}
|
}
|
||||||
|
|
||||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
|
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
|
||||||
|
|
@ -479,21 +476,21 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
|
||||||
|
|
||||||
// Actual preintegrated values
|
// Actual preintegrated values
|
||||||
ImuFactor::PreintegratedMeasurements preintegrated =
|
ImuFactor::PreintegratedMeasurements preintegrated =
|
||||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
|
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
|
||||||
|
|
||||||
// Compute numerical derivatives
|
// Compute numerical derivatives
|
||||||
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
|
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
|
||||||
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
|
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias);
|
||||||
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
|
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
|
||||||
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
|
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
|
||||||
|
|
||||||
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
|
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
|
||||||
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
|
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias);
|
||||||
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
|
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
|
||||||
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
|
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
|
||||||
|
|
||||||
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
||||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
|
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias);
|
||||||
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
||||||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||||
|
|
||||||
|
|
@ -528,9 +525,10 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
|
||||||
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
|
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
|
||||||
deltaTs.push_back(0.01);
|
deltaTs.push_back(0.01);
|
||||||
}
|
}
|
||||||
|
bool use2ndOrderIntegration = false;
|
||||||
// Actual preintegrated values
|
// Actual preintegrated values
|
||||||
ImuFactor::PreintegratedMeasurements preintegrated =
|
ImuFactor::PreintegratedMeasurements preintegrated =
|
||||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
|
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
|
||||||
|
|
||||||
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
||||||
// Now we add a new measurement and ask for Jacobians
|
// Now we add a new measurement and ask for Jacobians
|
||||||
|
|
@ -547,7 +545,126 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
|
||||||
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
|
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
|
||||||
body_P_sensor, Factual, Gactual);
|
body_P_sensor, Factual, Gactual);
|
||||||
|
|
||||||
bool use2ndOrderIntegration = false;
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// COMPUTE NUMERICAL DERIVATIVES FOR F
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// Compute expected f_pos_vel wrt positions
|
||||||
|
Matrix dfpv_dpos =
|
||||||
|
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||||
|
_1, deltaVij_old, deltaRij_old,
|
||||||
|
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
|
||||||
|
|
||||||
|
// Compute expected f_pos_vel wrt velocities
|
||||||
|
Matrix dfpv_dvel =
|
||||||
|
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||||
|
deltaPij_old, _1, deltaRij_old,
|
||||||
|
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
|
||||||
|
|
||||||
|
// Compute expected f_pos_vel wrt angles
|
||||||
|
Matrix dfpv_dangle =
|
||||||
|
numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
|
||||||
|
deltaPij_old, deltaVij_old, _1,
|
||||||
|
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
|
||||||
|
|
||||||
|
Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
|
||||||
|
|
||||||
|
// Compute expected f_rot wrt angles
|
||||||
|
Matrix dfr_dangle =
|
||||||
|
numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
|
||||||
|
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
|
||||||
|
|
||||||
|
Matrix FexpectedBottom3(3,9);
|
||||||
|
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
|
||||||
|
Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3;
|
||||||
|
|
||||||
|
EXPECT(assert_equal(Fexpected, Factual));
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// COMPUTE NUMERICAL DERIVATIVES FOR G
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// Compute jacobian wrt integration noise
|
||||||
|
Matrix dgpv_dintNoise(6,3);
|
||||||
|
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
|
||||||
|
|
||||||
|
// Compute jacobian wrt acc noise
|
||||||
|
Matrix dgpv_daccNoise =
|
||||||
|
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||||
|
deltaPij_old, deltaVij_old, deltaRij_old,
|
||||||
|
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
|
||||||
|
|
||||||
|
// Compute expected F wrt gyro noise
|
||||||
|
Matrix dgpv_domegaNoise =
|
||||||
|
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||||
|
deltaPij_old, deltaVij_old, deltaRij_old,
|
||||||
|
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
|
||||||
|
Matrix GexpectedTop6(6,9);
|
||||||
|
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
|
||||||
|
|
||||||
|
// Compute expected f_rot wrt gyro noise
|
||||||
|
Matrix dgr_dangle =
|
||||||
|
numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
|
||||||
|
deltaRij_old, _1, newDeltaT), newMeasuredOmega);
|
||||||
|
|
||||||
|
Matrix GexpectedBottom3(3,9);
|
||||||
|
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
|
||||||
|
Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3;
|
||||||
|
|
||||||
|
EXPECT(assert_equal(Gexpected, Gactual));
|
||||||
|
|
||||||
|
// Check covariance propagation
|
||||||
|
Matrix9 measurementCovariance;
|
||||||
|
measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3,
|
||||||
|
Z_3x3, accNoiseVar*I_3x3, Z_3x3,
|
||||||
|
Z_3x3, Z_3x3, omegaNoiseVar*I_3x3;
|
||||||
|
|
||||||
|
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
|
||||||
|
(1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
|
||||||
|
|
||||||
|
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
|
||||||
|
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
|
||||||
|
{
|
||||||
|
// Linearization point
|
||||||
|
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
|
||||||
|
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
|
||||||
|
|
||||||
|
// Measurements
|
||||||
|
list<Vector3> measuredAccs, measuredOmegas;
|
||||||
|
list<double> deltaTs;
|
||||||
|
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||||
|
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||||
|
deltaTs.push_back(0.01);
|
||||||
|
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||||
|
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||||
|
deltaTs.push_back(0.01);
|
||||||
|
for(int i=1;i<100;i++)
|
||||||
|
{
|
||||||
|
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
|
||||||
|
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
|
||||||
|
deltaTs.push_back(0.01);
|
||||||
|
}
|
||||||
|
bool use2ndOrderIntegration = true;
|
||||||
|
// Actual preintegrated values
|
||||||
|
ImuFactor::PreintegratedMeasurements preintegrated =
|
||||||
|
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
|
||||||
|
|
||||||
|
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
||||||
|
// Now we add a new measurement and ask for Jacobians
|
||||||
|
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
|
||||||
|
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
|
||||||
|
const double newDeltaT = 0.01;
|
||||||
|
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
|
||||||
|
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
|
||||||
|
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
|
||||||
|
|
||||||
|
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
|
||||||
|
|
||||||
|
Matrix Factual, Gactual;
|
||||||
|
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
|
||||||
|
body_P_sensor, Factual, Gactual);
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
// COMPUTE NUMERICAL DERIVATIVES FOR F
|
// COMPUTE NUMERICAL DERIVATIVES FOR F
|
||||||
|
|
|
||||||
|
|
@ -55,6 +55,12 @@ public:
|
||||||
typedef Eigen::Matrix<double, D, 1> DVector;
|
typedef Eigen::Matrix<double, D, 1> DVector;
|
||||||
mutable std::vector<DVector> y;
|
mutable std::vector<DVector> y;
|
||||||
|
|
||||||
|
/** y += alpha * A'*A*x */
|
||||||
|
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{
|
||||||
|
throw std::runtime_error(
|
||||||
|
"RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead");
|
||||||
|
}
|
||||||
|
|
||||||
/** y += alpha * A'*A*x */
|
/** y += alpha * A'*A*x */
|
||||||
void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
|
void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
|
||||||
// Create a vector of temporary y values, corresponding to rows i
|
// Create a vector of temporary y values, corresponding to rows i
|
||||||
|
|
|
||||||
|
|
@ -70,7 +70,7 @@ namespace gtsam {
|
||||||
|
|
||||||
/** Single Element Constructor: acts on a single parameter specified by idx */
|
/** Single Element Constructor: acts on a single parameter specified by idx */
|
||||||
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
|
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
|
||||||
Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::Dim())) {
|
Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) {
|
||||||
assert(model->dim() == 1);
|
assert(model->dim() == 1);
|
||||||
this->fillH();
|
this->fillH();
|
||||||
}
|
}
|
||||||
|
|
@ -78,7 +78,7 @@ namespace gtsam {
|
||||||
/** Indices Constructor: specify the mask with a set of indices */
|
/** Indices Constructor: specify the mask with a set of indices */
|
||||||
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
|
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
|
||||||
const SharedNoiseModel& model) :
|
const SharedNoiseModel& model) :
|
||||||
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
|
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) {
|
||||||
assert((size_t)prior_.size() == mask.size());
|
assert((size_t)prior_.size() == mask.size());
|
||||||
assert(model->dim() == (size_t) prior.size());
|
assert(model->dim() == (size_t) prior.size());
|
||||||
this->fillH();
|
this->fillH();
|
||||||
|
|
|
||||||
|
|
@ -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
|
% it is assumed x and y are the first two components of state x
|
||||||
% k is scaling for std deviations, defaults to 1 std
|
% k is scaling for std deviations, defaults to 1 std
|
||||||
|
|
||||||
|
hold on
|
||||||
|
|
||||||
[e,s] = eig(P(1:2,1:2));
|
[e,s] = eig(P(1:2,1:2));
|
||||||
s1 = s(1,1);
|
s1 = s(1,1);
|
||||||
s2 = s(2,2);
|
s2 = s(2,2);
|
||||||
|
|
@ -32,4 +34,4 @@ h = line(ex,ey,'color',color);
|
||||||
y = c(2) + points(2,:);
|
y = c(2) + points(2,:);
|
||||||
end
|
end
|
||||||
|
|
||||||
end
|
end
|
||||||
|
|
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
function covarianceEllipse3D(c,P)
|
function sc = covarianceEllipse3D(c,P,scale)
|
||||||
% covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
|
% covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
|
||||||
% Based on Maybeck Vol 1, page 366
|
% Based on Maybeck Vol 1, page 366
|
||||||
% k=2.296 corresponds to 1 std, 68.26% of all probability
|
% 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
|
% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
|
||||||
|
|
||||||
|
hold on
|
||||||
|
|
||||||
[e,s] = svd(P);
|
[e,s] = svd(P);
|
||||||
k = 11.82;
|
k = 11.82;
|
||||||
radii = k*sqrt(diag(s));
|
radii = k*sqrt(diag(s));
|
||||||
|
|
||||||
|
if exist('scale', 'var')
|
||||||
|
radii = radii * scale;
|
||||||
|
end
|
||||||
|
|
||||||
% generate data for "unrotated" ellipsoid
|
% generate data for "unrotated" ellipsoid
|
||||||
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);
|
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -1,18 +1,20 @@
|
||||||
function plotCamera(pose, axisLength)
|
function plotCamera(pose, axisLength)
|
||||||
|
hold on
|
||||||
|
|
||||||
C = pose.translation().vector();
|
C = pose.translation().vector();
|
||||||
R = pose.rotation().matrix();
|
R = pose.rotation().matrix();
|
||||||
|
|
||||||
xAxis = C+R(:,1)*axisLength;
|
xAxis = C+R(:,1)*axisLength;
|
||||||
L = [C xAxis]';
|
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;
|
yAxis = C+R(:,2)*axisLength;
|
||||||
L = [C yAxis]';
|
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;
|
zAxis = C+R(:,3)*axisLength;
|
||||||
L = [C zAxis]';
|
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
|
axis equal
|
||||||
end
|
end
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,7 @@
|
||||||
|
% test Cal3Unified
|
||||||
|
import gtsam.*;
|
||||||
|
|
||||||
|
K = Cal3Unified;
|
||||||
|
EXPECT('fx',K.fx()==1);
|
||||||
|
EXPECT('fy',K.fy()==1);
|
||||||
|
|
||||||
|
|
@ -1,17 +1,25 @@
|
||||||
% Test runner script - runs each test
|
% Test runner script - runs each test
|
||||||
|
|
||||||
% display 'Starting: testPriorFactor'
|
%% geometry
|
||||||
% testPriorFactor
|
display 'Starting: testCal3Unified'
|
||||||
|
testCal3Unified
|
||||||
|
|
||||||
display 'Starting: testValues'
|
%% linear
|
||||||
testValues
|
display 'Starting: testKalmanFilter'
|
||||||
|
testKalmanFilter
|
||||||
|
|
||||||
display 'Starting: testJacobianFactor'
|
display 'Starting: testJacobianFactor'
|
||||||
testJacobianFactor
|
testJacobianFactor
|
||||||
|
|
||||||
display 'Starting: testKalmanFilter'
|
%% nonlinear
|
||||||
testKalmanFilter
|
display 'Starting: testValues'
|
||||||
|
testValues
|
||||||
|
|
||||||
|
%% SLAM
|
||||||
|
display 'Starting: testPriorFactor'
|
||||||
|
testPriorFactor
|
||||||
|
|
||||||
|
%% examples
|
||||||
display 'Starting: testLocalizationExample'
|
display 'Starting: testLocalizationExample'
|
||||||
testLocalizationExample
|
testLocalizationExample
|
||||||
|
|
||||||
|
|
@ -36,6 +44,7 @@ testStereoVOExample
|
||||||
display 'Starting: testVisualISAMExample'
|
display 'Starting: testVisualISAMExample'
|
||||||
testVisualISAMExample
|
testVisualISAMExample
|
||||||
|
|
||||||
|
%% MATLAB specific
|
||||||
display 'Starting: testUtilities'
|
display 'Starting: testUtilities'
|
||||||
testUtilities
|
testUtilities
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1 +1,2 @@
|
||||||
*.m~
|
*.m~
|
||||||
|
*.avi
|
||||||
|
|
|
||||||
|
|
@ -91,6 +91,49 @@ TEST( PCGSolver, llt ) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Test GaussianFactorGraphSystem::multiply and getb
|
||||||
|
TEST( GaussianFactorGraphSystem, multiply_getb)
|
||||||
|
{
|
||||||
|
// Create a Gaussian Factor Graph
|
||||||
|
GaussianFactorGraph simpleGFG;
|
||||||
|
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
|
||||||
|
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
|
||||||
|
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
|
||||||
|
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
|
||||||
|
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
|
||||||
|
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
|
||||||
|
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
|
||||||
|
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
|
||||||
|
|
||||||
|
// Create a dummy-preconditioner and a GaussianFactorGraphSystem
|
||||||
|
DummyPreconditioner dummyPreconditioner;
|
||||||
|
KeyInfo keyInfo(simpleGFG);
|
||||||
|
std::map<Key,Vector> lambda;
|
||||||
|
dummyPreconditioner.build(simpleGFG, keyInfo, lambda);
|
||||||
|
GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda);
|
||||||
|
|
||||||
|
// Prepare container for each variable
|
||||||
|
Vector initial, residual, preconditionedResidual, p, actualAp;
|
||||||
|
initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished();
|
||||||
|
|
||||||
|
// Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver
|
||||||
|
gfgs.residual(initial, residual); /* r = b-Ax */
|
||||||
|
gfgs.leftPrecondition(residual, preconditionedResidual); /* pr = L^{-1} (b-Ax) */
|
||||||
|
gfgs.rightPrecondition(preconditionedResidual, p); /* p = L^{-T} pr */
|
||||||
|
gfgs.multiply(p, actualAp); /* A p */
|
||||||
|
|
||||||
|
// Expected value of Ap for the first iteration of this example problem
|
||||||
|
Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
|
||||||
|
EXPECT(assert_equal(expectedAp, actualAp, 1e-3));
|
||||||
|
|
||||||
|
// Expected value of getb
|
||||||
|
Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
|
||||||
|
Vector actualb;
|
||||||
|
gfgs.getb(actualb);
|
||||||
|
EXPECT(assert_equal(expectedb, actualb, 1e-3));
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Test Dummy Preconditioner
|
// Test Dummy Preconditioner
|
||||||
TEST( PCGSolver, dummy )
|
TEST( PCGSolver, dummy )
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue