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

Fixed Dynamics Factor and added debug cout statements to help fix indeterminent linear system exception

release/4.3a0
krunalchande 2014-10-14 18:08:26 -04:00
parent cb016fe405
commit cf4374563b
15 changed files with 405 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
.cproject
View File

@ -584,7 +584,6 @@
</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>
@ -592,7 +591,6 @@
</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>
@ -640,7 +638,6 @@
</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>
@ -648,7 +645,6 @@
</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>
@ -656,7 +652,6 @@
</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>
@ -672,7 +667,6 @@
</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>
@ -1024,7 +1018,6 @@
</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>
@ -1254,46 +1247,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="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>
@ -1376,6 +1329,7 @@
</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>
@ -1415,6 +1369,7 @@
</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>
@ -1422,6 +1377,7 @@
</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>
@ -1435,6 +1391,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="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>
@ -1692,7 +1688,6 @@
</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>
@ -1700,7 +1695,6 @@
</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>
@ -1708,7 +1702,6 @@
</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>
@ -1716,7 +1709,6 @@
</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>
@ -1929,6 +1921,22 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testDroneDynamicsFactor.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j8</buildArguments>
<buildTarget>testDroneDynamicsFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDroneDynamicsVelXYFactor.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j8</buildArguments>
<buildTarget>testDroneDynamicsVelXYFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2443,7 +2451,6 @@
</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>
@ -2451,7 +2458,6 @@
</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>
@ -2459,7 +2465,6 @@
</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>
@ -2923,6 +2928,7 @@
</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>

67
gtsam.h
View File

@ -1779,6 +1779,9 @@ class KeyGroupMap {
class Marginals { class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph, Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution); const gtsam::Values& solution);
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution, string str);
void print(string s) const; void print(string s) const;
Matrix marginalCovariance(size_t variable) const; Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const; Matrix marginalInformation(size_t variable) const;
@ -2136,6 +2139,16 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimple
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera; typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera; typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/DroneDynamicsFactor.h>
virtual class DroneDynamicsFactor : gtsam::NoiseModelFactor {
DroneDynamicsFactor(size_t key1, size_t key2, const gtsam::LieVector& measured, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/DroneDynamicsVelXYFactor.h>
virtual class DroneDynamicsVelXYFactor : gtsam::NoiseModelFactor {
DroneDynamicsVelXYFactor(size_t key1, size_t key2, size_t key3, Vector motors, Vector acc, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/BearingFactor.h> #include <gtsam/slam/BearingFactor.h>
template<POSE, POINT, ROTATION> template<POSE, POINT, ROTATION>
@ -2335,6 +2348,13 @@ class ImuFactorPreintegratedMeasurements {
// Testable // Testable
void print(string s) const; void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
Matrix MeasurementCovariance() const;
Matrix getDeltaRij() const;
double getDeltaTij() const;
Vector getDeltaPij() const;
Vector getDeltaVij() const;
Vector getBiasHat() const;
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
@ -2368,11 +2388,16 @@ class AHRSFactorPreintegratedMeasurements {
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data // get Data
Matrix MeasurementCovariance(); Matrix MeasurementCovariance() const;
Matrix MeasurementCovariance() const;
Matrix deltaRij_() const;
double deltaTij_() const;
Vector biasHat_() const;
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
void resetIntegration() ;
}; };
virtual class AHRSFactor : gtsam::NonlinearFactor { virtual class AHRSFactor : gtsam::NonlinearFactor {
@ -2384,6 +2409,8 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
const gtsam::imuBias::ConstantBias& bias) const;
void Predict(const gtsam::Rot3& rot_i, gtsam::Rot3& rot_j, void Predict(const gtsam::Rot3& rot_i, gtsam::Rot3& rot_j,
const gtsam::imuBias::ConstantBias& bias, const gtsam::imuBias::ConstantBias& bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
@ -2419,6 +2446,12 @@ class CombinedImuFactorPreintegratedMeasurements {
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
Matrix getDeltaRij() const;
double getDeltaTij() const;
Vector getDeltaPij() const;
Vector getDeltaVij() const;
Vector getBiasHat() const;
}; };
virtual class CombinedImuFactor : gtsam::NonlinearFactor { virtual class CombinedImuFactor : gtsam::NonlinearFactor {
@ -2427,10 +2460,27 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
static void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis);
static gtsam::imuBias::ConstantBias PredictImuBias(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
static gtsam::LieVector PredictVelocity(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
static gtsam::Pose3 PredictPose(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
}; };
#include <gtsam/navigation/AttitudeFactor.h> #include <gtsam/navigation/AttitudeFactor.h>
@ -2449,6 +2499,17 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
gtsam::Unit3 bRef() const; gtsam::Unit3 bRef() const;
}; };
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
//************************************************************************* //*************************************************************************
// Utilities // Utilities
//************************************************************************* //*************************************************************************

10
gtsam/linear/GaussianConditional.cpp
View File

@ -129,7 +129,10 @@ namespace gtsam {
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); if(soln.hasNaN()) {
std::cout << "GaussianConditional::solve failed: solution has NaN!!" << std::endl;
throw IndeterminantLinearSystemException(keys().front());
}
// TODO, do we not have to scale by sigmas here? Copy/paste bug // TODO, do we not have to scale by sigmas here? Copy/paste bug
@ -180,7 +183,10 @@ namespace gtsam {
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
// Check for indeterminant solution // Check for indeterminant solution
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); if (frontalVec.hasNaN()) {
std::cout << "GaussianConditional::solveTransposeInPlace failed: frontalVec has NaN!!" << std::endl;
throw IndeterminantLinearSystemException(this->keys().front());
}
for (const_iterator it = beginParents(); it!= endParents(); it++) for (const_iterator it = beginParents(); it!= endParents(); it++)
gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]); gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);

1
gtsam/linear/HessianFactor.cpp
View File

@ -640,6 +640,7 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
// Erase the eliminated keys in the remaining factor // Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size()); jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size());
} catch(CholeskyFailed&) { } catch(CholeskyFailed&) {
std::cout << "EliminateCholesky failed!" << std::endl;
throw IndeterminantLinearSystemException(keys.front()); throw IndeterminantLinearSystemException(keys.front());
} }

8
gtsam/linear/JacobianFactor.cpp
View File

@ -116,8 +116,10 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) :
boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
// Check for indefinite system // Check for indefinite system
if (!success) if (!success) {
std::cout << "JacobianFactor constructor from HessianFactor failed!" << std::endl;
throw IndeterminantLinearSystemException(factor.keys().front()); throw IndeterminantLinearSystemException(factor.keys().front());
}
// Zero out lower triangle // Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() = Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
@ -691,8 +693,10 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
Ab_.rowEnd() = Ab_.rowStart() + frontalDim; Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
SharedDiagonal conditionalNoiseModel; SharedDiagonal conditionalNoiseModel;
if (model_) { if (model_) {
if ((DenseIndex) model_->dim() < frontalDim) if ((DenseIndex) model_->dim() < frontalDim) {
std::cout << "Jacobian::splitConditional failed" << std::endl;
throw IndeterminantLinearSystemException(this->keys().front()); throw IndeterminantLinearSystemException(this->keys().front());
}
conditionalNoiseModel = noiseModel::Diagonal::Sigmas( conditionalNoiseModel = noiseModel::Diagonal::Sigmas(
model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
} }

6
gtsam/linear/linearAlgorithms-inst.h
View File

@ -86,7 +86,11 @@ namespace gtsam
Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); if(soln.hasNaN()) {
std::cout << "OptimizeClique failed: solution has NaN!" << std::endl;
clique->print("Problematic clique: ");
throw IndeterminantLinearSystemException(c.keys().front());
}
// Insert solution into a VectorValues // Insert solution into a VectorValues
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;

62
gtsam/navigation/AHRSFactor.h
View File

@ -31,19 +31,16 @@ public:
PreintegratedMeasurements(const imuBias::ConstantBias& bias, PreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredOmegaCovariance) : const Matrix3& measuredOmegaCovariance) :
biasHat(bias), measurementCovariance(3,3), delRdelBiasOmega( biasHat(bias), measurementCovariance(3,3), deltaTij(0.0),
Matrix3::Zero()), PreintMeasCov(3,3) { delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(3,3) {
// measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; // measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
measurementCovariance <<measuredOmegaCovariance; measurementCovariance <<measuredOmegaCovariance;
PreintMeasCov = Matrix::Zero(3,3); PreintMeasCov = Matrix::Zero(3,3);
} }
PreintegratedMeasurements() : PreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(3,3), delRdelBiasOmega( biasHat(imuBias::ConstantBias()), measurementCovariance(Matrix::Zero(3,3)), deltaTij(0.0),
Matrix3::Zero()), PreintMeasCov(3,3) { delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(3,3)) {}
measurementCovariance = Matrix::Zero(3,3);
PreintMeasCov = Matrix::Zero(3,3);
}
void print(const std::string& s = "Preintegrated Measurements: ") const { void print(const std::string& s = "Preintegrated Measurements: ") const {
std::cout << s << std::endl; std::cout << s << std::endl;
@ -64,9 +61,19 @@ public:
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega,
tol); tol);
} }
Matrix MeasurementCovariance(){ Matrix MeasurementCovariance() const {
return measurementCovariance; return measurementCovariance;
} }
Matrix deltaRij_() const {
return deltaRij.matrix();
}
double deltaTij_() const {
return deltaTij;
}
Vector biasHat_() const {
return biasHat.vector();
}
void resetIntegration() { void resetIntegration() {
deltaRij = Rot3(); deltaRij = Rot3();
@ -226,48 +233,43 @@ public:
boost::optional<Matrix&> H3 = boost::none) const boost::optional<Matrix&> H3 = boost::none) const
{ {
const double& deltaTij = preintegratedMeasurements_.deltaTij; double deltaTij = preintegratedMeasurements_.deltaTij;
// const Vector3 biasAccIncr = bias.accelerometer()
- preintegratedMeasurements_.biasHat.accelerometer(); Vector3 biasOmegaIncr = bias.gyroscope()
const Vector3 biasOmegaIncr = bias.gyroscope()
- preintegratedMeasurements_.biasHat.gyroscope(); - preintegratedMeasurements_.biasHat.gyroscope();
// we give some shorter name to rotations and translations
const Rot3 Rot_i = rot_i;
const Rot3 Rot_j = rot_j;
// We compute factor's Jacobians // We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = Rot3 deltaRij_biascorrected =
preintegratedMeasurements_.deltaRij.retract( preintegratedMeasurements_.deltaRij.retract(
preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr,
Rot3::EXPMAP); Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected
- Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap( Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap(
theta_biascorrected_corioliscorrected); theta_biascorrected_corioliscorrected);
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between( Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(
Rot_i.between(Rot_j)); rot_i.between(rot_j));
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3( Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(
theta_biascorrected_corioliscorrected); theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc Matrix3 Jtheta = -Jr_theta_bcc
* skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); * skewSymmetric(rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse( Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(
Rot3::Logmap(fRhat)); Rot3::Logmap(fRhat));
if (H1) { if (H1) {
H1->resize(3, 3); H1->resize(3, 3);
(*H1) << // dfR/dRi (*H1) << // dfR/dRi
Jrinv_fRhat Jrinv_fRhat
* (-Rot_j.between(Rot_i).matrix() * (-rot_j.between(rot_i).matrix()
- fRhat.inverse().matrix() * Jtheta); - fRhat.inverse().matrix() * Jtheta);
} }
if(H2) { if(H2) {
@ -280,11 +282,11 @@ public:
if (H3) { if (H3) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse( Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(
theta_biascorrected); theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3( Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(
preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
H3->resize(3, 6); H3->resize(3, 6);
@ -294,7 +296,7 @@ public:
Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega); Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega);
} }
const Vector3 fR = Rot3::Logmap(fRhat); Vector3 fR = Rot3::Logmap(fRhat);
Vector r(3); Vector r(3);
r << fR; r << fR;

6
gtsam/navigation/AttitudeFactor.h
View File

@ -191,6 +191,12 @@ public:
} }
return e; return e;
} }
Unit3 nZ() const {
return nZ_;
}
Unit3 bRef() const {
return bRef_;
}
private: private:

72
gtsam/navigation/CombinedImuFactor.h
View File

@ -299,6 +299,23 @@ namespace gtsam {
return Rot3::Logmap(R_t_to_t0); return Rot3::Logmap(R_t_to_t0);
} }
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
Matrix getDeltaRij() const {
return deltaRij.matrix();
}
double getDeltaTij() const{
return deltaTij;
}
Vector getDeltaPij() const {
return deltaPij;
}
Vector getDeltaVij() const {
return deltaVij;
}
Vector getBiasHat() const {
return biasHat.vector();
}
private: private:
/** Serialization function */ /** Serialization function */
@ -675,6 +692,61 @@ namespace gtsam {
} }
static Pose3 PredictPose(const Pose3& pose_i, const LieVector& vel_i,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) {
Pose3 pose_j = Pose3();
LieVector vel_j = LieVector();
imuBias::ConstantBias bias_j = imuBias::ConstantBias();
Predict(pose_i, vel_i, pose_j, vel_j,
bias_i, bias_j,
preintegratedMeasurements,
gravity, omegaCoriolis, body_P_sensor,
use2ndOrderCoriolis);
return pose_j;
}
static LieVector PredictVelocity(const Pose3& pose_i, const LieVector& vel_i,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) {
Pose3 pose_j = Pose3();
LieVector vel_j = LieVector();
imuBias::ConstantBias bias_j = imuBias::ConstantBias();
Predict(pose_i, vel_i, pose_j, vel_j,
bias_i, bias_j,
preintegratedMeasurements,
gravity, omegaCoriolis, body_P_sensor,
use2ndOrderCoriolis);
return vel_j;
}
static imuBias::ConstantBias PredictImuBias(const Pose3& pose_i, const LieVector& vel_i,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) {
Pose3 pose_j = Pose3();
LieVector vel_j = LieVector();
imuBias::ConstantBias bias_j = imuBias::ConstantBias();
Predict(pose_i, vel_i, pose_j, vel_j,
bias_i, bias_j,
preintegratedMeasurements,
gravity, omegaCoriolis, body_P_sensor,
use2ndOrderCoriolis);
return bias_j;
}
private: private:
/** Serialization function */ /** Serialization function */

20
gtsam/navigation/ImuFactor.h
View File

@ -127,6 +127,26 @@ namespace gtsam {
&& equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol)
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
} }
Matrix MeasurementCovariance() const {
return measurementCovariance;
}
Matrix getDeltaRij() const {
return deltaRij.matrix();
}
double getDeltaTij() const{
return deltaTij;
}
Vector getDeltaPij() const {
return deltaPij;
}
Vector getDeltaVij() const {
return deltaVij;
}
Vector getBiasHat() const {
return biasHat.vector();
}
void resetIntegration(){ void resetIntegration(){
deltaPij = Vector3::Zero(); deltaPij = Vector3::Zero();

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

@ -441,60 +441,60 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
} }
//#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( ImuFactor, LinearizeTiming) TEST( ImuFactor, LinearizeTiming)
//{ {
// // Linearization point // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
//
// // Pre-integrator // Pre-integrator
// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10)); imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
// Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01; Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
//
// // Pre-integrate Measurements // Pre-integrate Measurements
// Vector3 measuredAcc(0.1, 0.0, 0.0); Vector3 measuredAcc(0.1, 0.0, 0.0);
// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
// double deltaT = 0.5; double deltaT = 0.5;
// for(size_t i = 0; i < 50; ++i) { for(size_t i = 0; i < 50; ++i) {
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// } }
//
// // Create factor // Create factor
// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance()); noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance());
// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
//
// Values values; Values values;
// values.insert(X(1), x1); values.insert(X(1), x1);
// values.insert(X(2), x2); values.insert(X(2), x2);
// values.insert(V(1), v1); values.insert(V(1), v1);
// values.insert(V(2), v2); values.insert(V(2), v2);
// values.insert(B(1), bias); values.insert(B(1), bias);
//
// Ordering ordering; Ordering ordering;
// ordering.push_back(X(1)); ordering.push_back(X(1));
// ordering.push_back(V(1)); ordering.push_back(V(1));
// ordering.push_back(X(2)); ordering.push_back(X(2));
// ordering.push_back(V(2)); ordering.push_back(V(2));
// ordering.push_back(B(1)); ordering.push_back(B(1));
//
// GaussianFactorGraph graph; GaussianFactorGraph graph;
// gttic_(LinearizeTiming); gttic_(LinearizeTiming);
// for(size_t i = 0; i < 100000; ++i) { for(size_t i = 0; i < 100000; ++i) {
// GaussianFactor::shared_ptr g = factor.linearize(values, ordering); GaussianFactor::shared_ptr g = factor.linearize(values, ordering);
// graph.push_back(g); graph.push_back(g);
// } }
// gttoc_(LinearizeTiming); gttoc_(LinearizeTiming);
// tictoc_finishedIteration_(); tictoc_finishedIteration_();
// std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl; std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl;
// tictoc_print_(); tictoc_print_();
//} }
/* ************************************************************************* */ /* ************************************************************************* */

14
gtsam/nonlinear/ISAM2-inl.h
View File

@ -115,6 +115,7 @@ template<class CLIQUE>
bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold, bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count) FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count)
{ {
//clique->print("Input clique: ");
// if none of the variables in this clique (frontal and separator!) changed // if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the // significantly, then by the running intersection property, none of the
// cliques in the children need to be processed // cliques in the children need to be processed
@ -169,6 +170,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
GaussianConditional& c = *clique->conditional(); GaussianConditional& c = *clique->conditional();
// Solve matrix // Solve matrix
Vector xS; Vector xS;
Vector xS0; // Duy: for debug only
{ {
// Count dimensions of vector // Count dimensions of vector
DenseIndex dim = 0; DenseIndex dim = 0;
@ -188,11 +190,21 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
vectorPos += parentVector.size(); vectorPos += parentVector.size();
} }
} }
xS0 = xS;
xS = c.getb() - c.get_S() * xS; xS = c.getb() - c.get_S() * xS;
Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); if(soln.hasNaN()) {
std::cout << "iSAM2 failed: solution has NaN!!" << std::endl;
c.print("Clique conditional: ");
std::cout << "R: " << c.get_R() << std::endl;
std::cout << "S: " << c.get_S().transpose() << std::endl;
std::cout << "b: " << c.getb().transpose() << std::endl;
std::cout << "xS0: " << xS0.transpose() << std::endl;
std::cout << "xS: " << xS.transpose() << std::endl;
throw IndeterminantLinearSystemException(c.keys().front());
}
// Insert solution into a VectorValues // Insert solution into a VectorValues
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;

52
gtsam/nonlinear/Marginals.cpp
View File

@ -38,10 +38,38 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
// Compute BayesTree // Compute BayesTree
factorization_ = factorization; factorization_ = factorization;
if(factorization_ == CHOLESKY) if(factorization_ == CHOLESKY) {
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky); bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky);
else if(factorization_ == QR) std::cout<<"performing Cholesky"<<std::endl;
}
else if(factorization_ == QR) {
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR); bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR);
std::cout<<"performing QR"<<std::endl;
}
}
Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, const std::string& str) {
Factorization factorization;
if (str == "QR") factorization = QR;
else if (str == "CHOLESKY") factorization = CHOLESKY;
gttic(MarginalsConstructor);
// Linearize graph
graph_ = *graph.linearize(solution);
// Store values
values_ = solution;
// Compute BayesTree
factorization_ = factorization;
if(factorization_ == CHOLESKY) {
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky);
std::cout<<"performing Cholesky"<<std::endl;
}
else if(factorization_ == QR) {
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR);
std::cout<<"performing QR"<<std::endl;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -143,4 +171,24 @@ void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) c
cout << ". Use 'at' or 'operator()' to query matrix blocks." << endl; cout << ". Use 'at' or 'operator()' to query matrix blocks." << endl;
} }
Marginals::Factorization Marginals::factorizationTranslator(const std::string& str) const {
// std::string s = str; boost::algorithm::to_upper(s);
if (str == "QR") return Marginals::QR;
if (str == "CHOLESKY") return Marginals::CHOLESKY;
/* default is CHOLESKY */
return Marginals::CHOLESKY;
}
/* ************************************************************************* */
std::string Marginals::factorizationTranslator(const Marginals::Factorization& value) const {
std::string s;
switch (value) {
case Marginals::QR: s = "QR"; break;
case Marginals::CHOLESKY: s = "CHOLESKY"; break;
default: s = "UNDEFINED"; break;
}
return s;
}
} /* namespace gtsam */ } /* namespace gtsam */

11
gtsam/nonlinear/Marginals.h
View File

@ -54,6 +54,7 @@ public:
* @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
*/ */
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
Marginals(const NonlinearFactorGraph& graph, const Values& solution, const std::string& str);
/** print */ /** print */
void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
@ -71,6 +72,16 @@ public:
/** Compute the joint marginal information of several variables */ /** Compute the joint marginal information of several variables */
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const; JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
Factorization factorizationTranslator(const std::string& str) const;
std::string factorizationTranslator(const Marginals::Factorization& value) const;
void setFactorization(const std::string& factorization) { this->factorization_ = factorizationTranslator(factorization); }
}; };
/** /**

6
gtsam/slam/DroneDynamicsVelXYFactor.h
View File

@ -60,9 +60,9 @@ namespace gtsam {
// M = [sum(sqrt(m))ax 1 0 0; 0 0 sum(sqrt(m))ay 1; 0 0 0 0] // M = [sum(sqrt(m))ax 1 0 0; 0 0 sum(sqrt(m))ay 1; 0 0 0 0]
Matrix computeM(const Vector& motors, const Vector& acc) const { Matrix computeM(const Vector& motors, const Vector& acc) const {
Matrix M = zeros(3,4); Matrix M = zeros(3,4);
double sqrtSumMotors = sqrt(motors(0)) + sqrt(motors(1)) + sqrt(motors(2)) + sqrt(motors(3)); double sumMotors = (motors(0)) + (motors(1)) + (motors(2)) + (motors(3));
M(0,0) = sqrtSumMotors*acc(0); M(0, 1) = 1.0; M(0,0) = acc(0)/sumMotors; M(0, 1) = 1.0/sumMotors;
M(1,2) = 1.0; M(1, 3) = sqrtSumMotors*acc(1); M(1,2) = 1.0/sumMotors; M(1, 3) = acc(1)/sumMotors;
return M; return M;
} }