Major updates to CombinedImuFactor to make it Testable as well as serializable
Varun Agrawal
parent
cc2456678f
commit
95b4a49f64
|
|
@ -17,9 +17,11 @@
|
|||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
* @author Varun Agrawal
|
||||
**/
|
||||
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <boost/serialization/export.hpp>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
|
@ -28,6 +30,31 @@ namespace gtsam {
|
|||
|
||||
using namespace std;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Inner class PreintegrationCombinedParams
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationCombinedParams::print(const string& s) const {
|
||||
PreintegrationParams::print(s);
|
||||
cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]"
|
||||
<< endl;
|
||||
cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
|
||||
<< endl;
|
||||
cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
|
||||
<< endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool PreintegrationCombinedParams::equals(const PreintegrationParams& other,
|
||||
double tol) const {
|
||||
auto e = dynamic_cast<const PreintegrationCombinedParams*>(&other);
|
||||
return e != nullptr && PreintegrationParams::equals(other, tol) &&
|
||||
equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance,
|
||||
tol) &&
|
||||
equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
|
||||
tol) &&
|
||||
equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Inner class PreintegratedCombinedMeasurements
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -242,6 +269,13 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
|
|||
return r;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
|
||||
f._PIM_.print("combined preintegrated measurements:\n");
|
||||
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
|
||||
return os;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
CombinedImuFactor::CombinedImuFactor(
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
* @author Varun Agrawal
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -26,6 +27,7 @@
|
|||
#include <gtsam/navigation/TangentPreintegration.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/serialization.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -61,10 +63,19 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
|||
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
||||
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
|
||||
|
||||
/// Default constructor makes unitialized params struct.
|
||||
/// Used for serialization.
|
||||
PreintegrationCombinedParams()
|
||||
: PreintegrationParams(),
|
||||
biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3),
|
||||
biasAccOmegaInt(I_6x6) {}
|
||||
|
||||
/// See two named constructors below for good values of n_gravity in body frame
|
||||
PreintegrationCombinedParams(const Vector3& n_gravity) :
|
||||
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
|
||||
I_3x3), biasAccOmegaInt(I_6x6) {
|
||||
PreintegrationCombinedParams(const Vector3& n_gravity) :
|
||||
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
|
||||
|
||||
}
|
||||
|
||||
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
|
||||
|
|
@ -77,6 +88,9 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
|
|||
return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
|
||||
}
|
||||
|
||||
void print(const std::string& s="") const;
|
||||
bool equals(const PreintegrationParams& other, double tol) const;
|
||||
|
||||
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
|
||||
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
|
||||
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
|
||||
|
|
@ -86,24 +100,25 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
|
|||
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
|
||||
|
||||
private:
|
||||
/// Default constructor makes unitialized params struct
|
||||
PreintegrationCombinedParams() {}
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
|
||||
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
||||
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
||||
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
|
||||
ar & bs::make_nvp("biasAccCovariance",
|
||||
bs::make_array(biasAccCovariance.data(), biasAccCovariance.size()));
|
||||
ar & bs::make_nvp("biasOmegaCovariance",
|
||||
bs::make_array(biasOmegaCovariance.data(), biasOmegaCovariance.size()));
|
||||
ar & bs::make_nvp("biasAccOmegaInt", bs::make_array(biasAccOmegaInt.data(),
|
||||
biasAccOmegaInt.size()));
|
||||
}
|
||||
|
||||
public:
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* PreintegratedCombinedMeasurements integrates the IMU measurements
|
||||
* (rotation rates and accelerations) and the corresponding covariance matrix.
|
||||
|
|
@ -128,7 +143,6 @@ public:
|
|||
*/
|
||||
Eigen::Matrix<double, 15, 15> preintMeasCov_;
|
||||
|
||||
|
||||
friend class CombinedImuFactor;
|
||||
|
||||
public:
|
||||
|
|
@ -136,11 +150,14 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Default constructor only for serialization and Cython wrapper
|
||||
PreintegratedCombinedMeasurements() {}
|
||||
PreintegratedCombinedMeasurements() {
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Default constructor, initializes the class with no measurements
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
* @param p Parameters, typically fixed in a single application
|
||||
* @param biasHat Current estimate of acceleration and rotation rate biases
|
||||
*/
|
||||
PreintegratedCombinedMeasurements(
|
||||
const boost::shared_ptr<Params>& p,
|
||||
|
|
@ -149,6 +166,19 @@ public:
|
|||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct preintegrated directly from members: base class and preintMeasCov
|
||||
* @param base PreintegrationType instance
|
||||
* @param preintMeasCov Covariance matrix used in noise model.
|
||||
*/
|
||||
PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
|
||||
: PreintegrationType(base),
|
||||
preintMeasCov_(preintMeasCov) {
|
||||
}
|
||||
|
||||
/// Virtual destructor
|
||||
virtual ~PreintegratedCombinedMeasurements() {}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Basic utilities
|
||||
|
|
@ -158,20 +188,25 @@ public:
|
|||
void resetIntegration() override;
|
||||
|
||||
/// const reference to params, shadows definition in base class
|
||||
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
|
||||
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
|
||||
/// @}
|
||||
|
||||
/// @name Access instance variables
|
||||
/// @{
|
||||
/// Return pre-integrated measurement covariance
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
/// @}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const override;
|
||||
bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
|
||||
/// equals
|
||||
bool equals(const PreintegratedCombinedMeasurements& expected,
|
||||
double tol = 1e-9) const;
|
||||
/// @}
|
||||
|
||||
|
||||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
|
|
@ -205,8 +240,10 @@ public:
|
|||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
|
||||
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
|
||||
ar& bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(),
|
||||
preintMeasCov_.size()));
|
||||
}
|
||||
|
||||
public:
|
||||
|
|
@ -244,9 +281,6 @@ private:
|
|||
|
||||
PreintegratedCombinedMeasurements _PIM_;
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
CombinedImuFactor() {}
|
||||
|
||||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
|
|
@ -256,6 +290,9 @@ public:
|
|||
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
CombinedImuFactor() {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param pose_i Previous pose key
|
||||
|
|
@ -277,12 +314,17 @@ public:
|
|||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
|
||||
const CombinedImuFactor&);
|
||||
/// print
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const;
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
/// @}
|
||||
|
||||
/** Access the preintegrated measurements. */
|
||||
|
||||
|
|
@ -321,14 +363,12 @@ public:
|
|||
#endif
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor6",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(_PIM_);
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
|
||||
ar& BOOST_SERIALIZATION_NVP(_PIM_);
|
||||
}
|
||||
|
||||
public:
|
||||
|
|
@ -336,4 +376,19 @@ public:
|
|||
};
|
||||
// class CombinedImuFactor
|
||||
|
||||
} /// namespace gtsam
|
||||
template <>
|
||||
struct traits<PreintegrationCombinedParams>
|
||||
: public Testable<PreintegrationCombinedParams> {};
|
||||
|
||||
template <>
|
||||
struct traits<PreintegratedCombinedMeasurements>
|
||||
: public Testable<PreintegratedCombinedMeasurements> {};
|
||||
|
||||
template <>
|
||||
struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
/// Add Boost serialization export for derived class
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::PreintegrationCombinedParams, "gtsam_PreintegrationCombinedParams");
|
||||
// BOOST_CLASS_EXPORT_GUID(gtsam::CombinedImuFactor, "gtsam_CombinedImuFactor");
|
||||
|
|
|
|||
Loading…
Reference in New Issue