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 name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -592,7 +591,6 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -640,7 +638,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -648,7 +645,6 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -656,7 +652,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -672,7 +667,6 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1024,7 +1018,6 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1254,46 +1247,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1376,6 +1329,7 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1415,6 +1369,7 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1422,6 +1377,7 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1435,6 +1391,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1692,7 +1688,6 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1700,7 +1695,6 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1708,7 +1702,6 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1716,7 +1709,6 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1929,6 +1921,22 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2443,7 +2451,6 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2451,7 +2458,6 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2459,7 +2465,6 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2923,6 +2928,7 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

67
gtsam.h
View File

@ -1779,6 +1779,9 @@ class KeyGroupMap {
class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution);
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution, string str);
void print(string s) const;
Matrix marginalCovariance(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::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>
template<POSE, POINT, ROTATION>
@ -2335,6 +2348,13 @@ class ImuFactorPreintegratedMeasurements {
// Testable
void print(string s) const;
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
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
@ -2368,11 +2388,16 @@ class AHRSFactorPreintegratedMeasurements {
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data
Matrix MeasurementCovariance();
Matrix MeasurementCovariance() const;
Matrix MeasurementCovariance() const;
Matrix deltaRij_() const;
double deltaTij_() const;
Vector biasHat_() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
@ -2384,6 +2409,8 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
// Standard Interface
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,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
@ -2419,6 +2446,12 @@ class CombinedImuFactorPreintegratedMeasurements {
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
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 {
@ -2427,10 +2460,27 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface
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::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>
@ -2449,6 +2499,17 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
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
//*************************************************************************

10
gtsam/linear/GaussianConditional.cpp
View File

@ -129,7 +129,10 @@ namespace gtsam {
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// 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
@ -180,7 +183,10 @@ namespace gtsam {
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
// 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++)
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
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size());
} catch(CholeskyFailed&) {
std::cout << "EliminateCholesky failed!" << std::endl;
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());
// Check for indefinite system
if (!success)
if (!success) {
std::cout << "JacobianFactor constructor from HessianFactor failed!" << std::endl;
throw IndeterminantLinearSystemException(factor.keys().front());
}
// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
@ -691,8 +693,10 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
SharedDiagonal conditionalNoiseModel;
if (model_) {
if ((DenseIndex) model_->dim() < frontalDim)
if ((DenseIndex) model_->dim() < frontalDim) {
std::cout << "Jacobian::splitConditional failed" << std::endl;
throw IndeterminantLinearSystemException(this->keys().front());
}
conditionalNoiseModel = noiseModel::Diagonal::Sigmas(
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);
// 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
DenseIndex vectorPosition = 0;

62
gtsam/navigation/AHRSFactor.h
View File

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

6
gtsam/navigation/AttitudeFactor.h
View File

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

72
gtsam/navigation/CombinedImuFactor.h
View File

@ -299,6 +299,23 @@ namespace gtsam {
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:
/** 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:
/** 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(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(){
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
}
//#include <gtsam/linear/GaussianFactorGraph.h>
///* ************************************************************************* */
//TEST( ImuFactor, LinearizeTiming)
//{
// // 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));
// 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));
// 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));
//
// // Pre-integrator
// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
// Vector3 gravity; gravity << 0, 0, 9.81;
// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
//
// // Pre-integrate Measurements
// Vector3 measuredAcc(0.1, 0.0, 0.0);
// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
// double deltaT = 0.5;
// for(size_t i = 0; i < 50; ++i) {
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// }
//
// // Create factor
// 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);
//
// Values values;
// values.insert(X(1), x1);
// values.insert(X(2), x2);
// values.insert(V(1), v1);
// values.insert(V(2), v2);
// values.insert(B(1), bias);
//
// Ordering ordering;
// ordering.push_back(X(1));
// ordering.push_back(V(1));
// ordering.push_back(X(2));
// ordering.push_back(V(2));
// ordering.push_back(B(1));
//
// GaussianFactorGraph graph;
// gttic_(LinearizeTiming);
// for(size_t i = 0; i < 100000; ++i) {
// GaussianFactor::shared_ptr g = factor.linearize(values, ordering);
// graph.push_back(g);
// }
// gttoc_(LinearizeTiming);
// tictoc_finishedIteration_();
// std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl;
// tictoc_print_();
//}
#include <gtsam/linear/GaussianFactorGraph.h>
/* ************************************************************************* */
TEST( ImuFactor, LinearizeTiming)
{
// 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));
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));
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));
// Pre-integrator
imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
// Pre-integrate Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
double deltaT = 0.5;
for(size_t i = 0; i < 50; ++i) {
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}
// Create factor
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);
Values values;
values.insert(X(1), x1);
values.insert(X(2), x2);
values.insert(V(1), v1);
values.insert(V(2), v2);
values.insert(B(1), bias);
Ordering ordering;
ordering.push_back(X(1));
ordering.push_back(V(1));
ordering.push_back(X(2));
ordering.push_back(V(2));
ordering.push_back(B(1));
GaussianFactorGraph graph;
gttic_(LinearizeTiming);
for(size_t i = 0; i < 100000; ++i) {
GaussianFactor::shared_ptr g = factor.linearize(values, ordering);
graph.push_back(g);
}
gttoc_(LinearizeTiming);
tictoc_finishedIteration_();
std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl;
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,
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
// significantly, then by the running intersection property, none of the
// 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();
// Solve matrix
Vector xS;
Vector xS0; // Duy: for debug only
{
// Count dimensions of vector
DenseIndex dim = 0;
@ -188,11 +190,21 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
vectorPos += parentVector.size();
}
}
xS0 = xS;
xS = c.getb() - c.get_S() * xS;
Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
// 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
DenseIndex vectorPosition = 0;

52
gtsam/nonlinear/Marginals.cpp
View File

@ -38,10 +38,38 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
// Compute BayesTree
factorization_ = factorization;
if(factorization_ == CHOLESKY)
if(factorization_ == CHOLESKY) {
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);
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;
}
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 */

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).
*/
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
Marginals(const NonlinearFactorGraph& graph, const Values& solution, const std::string& str);
/** print */
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 */
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]
Matrix computeM(const Vector& motors, const Vector& acc) const {
Matrix M = zeros(3,4);
double sqrtSumMotors = sqrt(motors(0)) + sqrt(motors(1)) + sqrt(motors(2)) + sqrt(motors(3));
M(0,0) = sqrtSumMotors*acc(0); M(0, 1) = 1.0;
M(1,2) = 1.0; M(1, 3) = sqrtSumMotors*acc(1);
double sumMotors = (motors(0)) + (motors(1)) + (motors(2)) + (motors(3));
M(0,0) = acc(0)/sumMotors; M(0, 1) = 1.0/sumMotors;
M(1,2) = 1.0/sumMotors; M(1, 3) = acc(1)/sumMotors;
return M;
}