diff --git a/.cproject b/.cproject index 4801c4641..4d2610e13 100644 --- a/.cproject +++ b/.cproject @@ -584,7 +584,6 @@ make - tests/testBayesTree.run true false @@ -592,7 +591,6 @@ make - testBinaryBayesNet.run true false @@ -640,7 +638,6 @@ make - testSymbolicBayesNet.run true false @@ -648,7 +645,6 @@ make - tests/testSymbolicFactor.run true false @@ -656,7 +652,6 @@ make - testSymbolicFactorGraph.run true false @@ -672,7 +667,6 @@ make - tests/testBayesTree true false @@ -1024,7 +1018,6 @@ make - testErrors.run true false @@ -1254,46 +1247,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1376,6 +1329,7 @@ make + testSimulated2DOriented.run true false @@ -1415,6 +1369,7 @@ make + testSimulated2D.run true false @@ -1422,6 +1377,7 @@ make + testSimulated3D.run true false @@ -1435,6 +1391,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1692,7 +1688,6 @@ cpack - -G DEB true false @@ -1700,7 +1695,6 @@ cpack - -G RPM true false @@ -1708,7 +1702,6 @@ cpack - -G TGZ true false @@ -1716,7 +1709,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1929,6 +1921,22 @@ false true + + make + -j8 + testDroneDynamicsFactor.run + true + true + true + + + make + -j8 + testDroneDynamicsVelXYFactor.run + true + true + true + make -j2 @@ -2443,7 +2451,6 @@ make - testGraph.run true false @@ -2451,7 +2458,6 @@ make - testJunctionTree.run true false @@ -2459,7 +2465,6 @@ make - testSymbolicBayesNetB.run true false @@ -2923,6 +2928,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam.h b/gtsam.h index 9f3d6ef29..561049189 100644 --- a/gtsam.h +++ b/gtsam.h @@ -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 RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; +#include +virtual class DroneDynamicsFactor : gtsam::NoiseModelFactor { + DroneDynamicsFactor(size_t key1, size_t key2, const gtsam::LieVector& measured, const gtsam::noiseModel::Base* noiseModel); +}; + +#include +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 template @@ -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 @@ -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 //************************************************************************* diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index a5c651a44..58cead05a 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -129,7 +129,10 @@ namespace gtsam { Vector soln = get_R().triangularView().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]); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6f40b9bea..6b3596a7e 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -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()); } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a63bbf473..06f6658f9 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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() = @@ -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())); } diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 4722a9b6d..ea049b277 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -86,7 +86,11 @@ namespace gtsam Vector soln = c.get_R().triangularView().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; diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index ada073943..da6341653 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -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 < 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; diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 9338f3dba..132342023 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -191,6 +191,12 @@ public: } return e; } + Unit3 nZ() const { + return nZ_; + } + Unit3 bRef() const { + return bRef_; + } private: diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 725274acc..a97b28d91 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -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 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 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 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 */ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d68af4f8b..2c9827852 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -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(); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index a6894898b..b4faf79a0 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -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 -///* ************************************************************************* */ -//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 +/* ************************************************************************* */ +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_(); +} /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 9248617b5..7dad5eeec 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -115,6 +115,7 @@ template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, FastSet& changed, const FastSet& 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, 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, double thresh vectorPos += parentVector.size(); } } + xS0 = xS; xS = c.getb() - c.get_S() * xS; Vector soln = c.get_R().triangularView().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; diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 0dca18a1f..e70aa300f 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -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"<& 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); } + + + + }; /** diff --git a/gtsam/slam/DroneDynamicsVelXYFactor.h b/gtsam/slam/DroneDynamicsVelXYFactor.h index 1268c1ac9..ad6ca4f03 100644 --- a/gtsam/slam/DroneDynamicsVelXYFactor.h +++ b/gtsam/slam/DroneDynamicsVelXYFactor.h @@ -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; }