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

PreintegratedRotation works for AHRS

release/4.3a0
dellaert 2014-12-05 12:12:54 +01:00
parent 37e6b796ec
commit 507979c526
3 changed files with 165 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
gtsam/navigation/AHRSFactor.cpp
View File

@ -18,9 +18,9 @@
**/
#include <gtsam/navigation/AHRSFactor.h>
#include <iostream>
/* External or standard includes */
#include <ostream>
using namespace std;
namespace gtsam {
@ -29,47 +29,38 @@ namespace gtsam {
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
biasHat_(bias), deltaTij_(0.0) {
biasHat_(bias) {
measurementCovariance_ << measuredOmegaCovariance;
delRdelBiasOmega_.setZero();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
biasHat_(Vector3()), deltaTij_(0.0) {
biasHat_(Vector3()) {
measurementCovariance_.setZero();
delRdelBiasOmega_.setZero();
delRdelBiasOmega_.setZero();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const {
std::cout << s << std::endl;
std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl;
deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
<< std::endl;
std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl;
void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
PreintegratedRotation::print(s);
cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl;
cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool AHRSFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& other, double tol) const {
return equal_with_abs_tol(biasHat_, other.biasHat_, tol)
return PreintegratedRotation::equals(other, tol)
&& equal_with_abs_tol(biasHat_, other.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_,
other.measurementCovariance_, tol)
&& deltaRij_.equals(other.deltaRij_, tol)
&& std::fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
other.measurementCovariance_, tol);
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_.setZero();
PreintegratedRotation::resetIntegration();
preintMeasCov_.setZero();
}
@ -95,23 +86,23 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3 theta_incr = correctedOmega * deltaT;
// rotation increment computed from the current rotation rate measurement
const Rot3 incrR = Rot3::Expmap(theta_incr);
const Rot3 incrR = Rot3::Expmap(theta_incr); // expensive !!
const Matrix3 incrRt = incrR.transpose();
// Right Jacobian computed at theta_incr
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
// Update Jacobians
// ---------------------------------------------------------------------------
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
// Update Jacobian
update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT);
// Update preintegrated measurements covariance
// ---------------------------------------------------------------------------
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3)
const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
Rot3 Rot_j = deltaRij_ * incrR;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3)
// Update rotation and deltaTij. TODO Frank moved from end of this function !!!
updateIntegratedRotationAndDeltaT(incrR, deltaT);
const Vector3 theta_j = thetaRij(); // super-expensive, , Parameterization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
// Update preintegrated measurements covariance: as in [2] we consider a first
@ -129,28 +120,13 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
+ measurementCovariance_ * deltaT;
// Update preintegrated measurements
// ---------------------------------------------------------------------------
deltaRij_ = deltaRij_ * incrR;
deltaTij_ += deltaT;
}
//------------------------------------------------------------------------------
Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
boost::optional<Matrix&> H) const {
const Vector3 biasOmegaIncr = bias - biasHat_;
Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
const Rot3 deltaRij_biascorrected = deltaRij_.retract(
delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) {
const Matrix3 Jrinv_theta_bc = //
Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
}
return theta_biascorrected;
return biascorrectedThetaRij(biasOmegaIncr, H);
}
//------------------------------------------------------------------------------
Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
@ -192,13 +168,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
}
//------------------------------------------------------------------------------
void AHRSFactor::print(const std::string& s,
void AHRSFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::endl;
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
noiseModel_->print(" noise model: ");
if (body_P_sensor_)
body_P_sensor_->print(" sensor pose in body frame: ");

41
gtsam/navigation/AHRSFactor.h
View File

@ -20,6 +20,7 @@
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
@ -35,7 +36,7 @@ public:
* Can be built incrementally so as to avoid costly integration at time of
* factor construction.
*/
class PreintegratedMeasurements {
class PreintegratedMeasurements : public PreintegratedRotation {
friend class AHRSFactor;
@ -43,9 +44,6 @@ public:
Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
public:
@ -61,31 +59,22 @@ public:
PreintegratedMeasurements(const Vector3& bias,
const Matrix3& measuredOmegaCovariance);
const Matrix3& measurementCovariance() const {
return measurementCovariance_;
}
Vector3 biasHat() const {
return biasHat_;
}
const Matrix3& preintMeasCov() const {
return preintMeasCov_;
}
/// print
void print(const std::string& s = "Preintegrated Measurements: ") const;
/// equals
bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const;
const Matrix3& measurementCovariance() const {
return measurementCovariance_;
}
Matrix3 deltaRij() const {
return deltaRij_.matrix();
}
double deltaTij() const {
return deltaTij_;
}
Vector3 biasHat() const {
return biasHat_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const Matrix3& preintMeasCov() const {
return preintMeasCov_;
}
/// TODO: Document
void resetIntegration();
@ -106,7 +95,7 @@ public:
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i,
const Vector3& omegaCoriolis) const {
return rot_i.transpose() * omegaCoriolis * deltaTij_;
return rot_i.transpose() * omegaCoriolis * deltaTij();
}
// This function is only used for test purposes
@ -120,11 +109,9 @@ public:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};

126
gtsam/navigation/PreintegratedRotation.h Normal file
View File

@ -0,0 +1,126 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PreintegratedRotation.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/geometry/Rot3.h>
namespace gtsam {
/**
* PreintegratedRotation is the base class for all PreintegratedMeasurements
* classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
* It includes the definitions of the preintegrated rotation.
*/
class PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
/// Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_;
public:
/**
* Default constructor, initializes the variables in the base class
*/
PreintegratedRotation() :
deltaRij_(Rot3()), deltaTij_(0.0),
delRdelBiasOmega_(Z_3x3) {}
/// methods to access class variables
Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive
Vector3 thetaRij() const {return Rot3::Logmap(deltaRij_);} // super-expensive
const double& deltaTij() const{return deltaTij_;}
const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;}
/// Needed for testable
void print(const std::string& s) const {
std::cout << s << std::endl;
deltaRij_.print(" deltaRij ");
}
/// Needed for testable
bool equals(const PreintegratedRotation& expected, double tol) const {
return deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
}
/// Re-initialize PreintegratedMeasurements
void resetIntegration(){
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_ = Z_3x3;
}
/// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, double deltaT){
deltaRij_ = deltaRij_ * incrR;
deltaTij_ += deltaT;
}
/**
* Update Jacobians to be used during preintegration
* TODO: explain arguments
*/
void update_delRdelBiasOmega(const Matrix3& Jr_theta_incr, const Rot3& incrR,
double deltaT) {
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
}
/// Return a bias corrected version of the integrated rotation - expensive
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const {
return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
}
/// Get so<3> version of bias corrected rotation, with optional Jacobian
Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
boost::optional<Matrix&> H) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// This was done via an expmap, now we go *back* to so<3>
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) {
// We then do a very expensive Jacobian calculation. TODO Right Duy ?
const Matrix3 Jrinv_theta_bc = //
Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
}
return theta_biascorrected;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};
} /// namespace gtsam