Added .cpp file and a print for parameters
dellaert
parent
9af69254b2
commit
887c0d8f59
|
|
@ -0,0 +1,111 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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.cpp
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include "PreintegratedRotation.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
void PreintegratedRotation::Params::print(const string& s) const {
|
||||
cout << s << endl;
|
||||
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
|
||||
if (omegaCoriolis)
|
||||
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")"
|
||||
<< endl;
|
||||
if (body_P_sensor)
|
||||
body_P_sensor->print("body_P_sensor");
|
||||
}
|
||||
|
||||
void PreintegratedRotation::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
deltaRij_ = Rot3();
|
||||
delRdelBiasOmega_ = Z_3x3;
|
||||
}
|
||||
|
||||
void PreintegratedRotation::print(const string& s) const {
|
||||
cout << s << endl;
|
||||
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
||||
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
|
||||
}
|
||||
|
||||
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
|
||||
double tol) const {
|
||||
return deltaRij_.equals(other.deltaRij_, tol)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
||||
Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
|
||||
|
||||
// First we compensate the measurements for the bias
|
||||
Vector3 correctedOmega = measuredOmega - biasHat;
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities
|
||||
// (originally in the IMU frame) into the body frame
|
||||
if (p_->body_P_sensor) {
|
||||
Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
|
||||
// rotation rate vector in the body frame
|
||||
correctedOmega = body_R_sensor * correctedOmega;
|
||||
}
|
||||
|
||||
// rotation vector describing rotation increment computed from the
|
||||
// current rotation rate measurement
|
||||
const Vector3 integratedOmega = correctedOmega * deltaT;
|
||||
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
}
|
||||
|
||||
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
|
||||
Matrix3* F) {
|
||||
|
||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
|
||||
D_incrR_integratedOmega);
|
||||
|
||||
// Update deltaTij and rotation
|
||||
deltaTij_ += deltaT;
|
||||
deltaRij_ = deltaRij_.compose(incrR, F);
|
||||
|
||||
// Update Jacobian
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||
- *D_incrR_integratedOmega * deltaT;
|
||||
}
|
||||
|
||||
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||
OptionalJacobian<3, 3> H) const {
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
|
||||
const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,
|
||||
boost::none, H);
|
||||
if (H)
|
||||
(*H) *= delRdelBiasOmega_;
|
||||
return deltaRij_biascorrected;
|
||||
}
|
||||
|
||||
Vector3 PreintegratedRotation::integrateCoriolis(const Rot3& rot_i) const {
|
||||
if (!p_->omegaCoriolis)
|
||||
return Vector3::Zero();
|
||||
return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -32,7 +32,7 @@ namespace gtsam {
|
|||
* It includes the definitions of the preintegrated rotation.
|
||||
*/
|
||||
class PreintegratedRotation {
|
||||
public:
|
||||
public:
|
||||
|
||||
/// Parameters for pre-integration:
|
||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||
|
|
@ -41,7 +41,11 @@ class PreintegratedRotation {
|
|||
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
|
||||
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
|
||||
|
||||
Params():gyroscopeCovariance(I_3x3) {}
|
||||
Params() :
|
||||
gyroscopeCovariance(I_3x3) {
|
||||
}
|
||||
|
||||
virtual void print(const std::string& s) const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -54,7 +58,7 @@ class PreintegratedRotation {
|
|||
}
|
||||
};
|
||||
|
||||
protected:
|
||||
protected:
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
|
|
@ -63,20 +67,18 @@ class PreintegratedRotation {
|
|||
boost::shared_ptr<Params> p_;
|
||||
|
||||
/// Default constructor for serialization
|
||||
PreintegratedRotation() {}
|
||||
PreintegratedRotation() {
|
||||
}
|
||||
|
||||
public:
|
||||
public:
|
||||
/// Default constructor, resets integration to zero
|
||||
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
|
||||
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) :
|
||||
p_(p) {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
deltaRij_ = Rot3();
|
||||
delRdelBiasOmega_ = Z_3x3;
|
||||
}
|
||||
void resetIntegration();
|
||||
|
||||
/// @name Access instance variables
|
||||
/// @{
|
||||
|
|
@ -94,17 +96,8 @@ class PreintegratedRotation {
|
|||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
void print(const std::string& s) const {
|
||||
std::cout << s << std::endl;
|
||||
std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl;
|
||||
std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl;
|
||||
}
|
||||
|
||||
bool equals(const PreintegratedRotation& other, double tol) const {
|
||||
return deltaRij_.equals(other.deltaRij_, tol) &&
|
||||
fabs(deltaTij_ - other.deltaTij_) < tol &&
|
||||
equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
||||
}
|
||||
void print(const std::string& s) const;
|
||||
bool equals(const PreintegratedRotation& other, double tol) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -112,60 +105,22 @@ class PreintegratedRotation {
|
|||
/// and possibly the sensor pose, and then integrate it forward in time to yield
|
||||
/// an incremental rotation.
|
||||
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat,
|
||||
double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
|
||||
|
||||
// First we compensate the measurements for the bias
|
||||
Vector3 correctedOmega = measuredOmega - biasHat;
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities
|
||||
// (originally in the IMU frame) into the body frame
|
||||
if (p_->body_P_sensor) {
|
||||
Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
|
||||
// rotation rate vector in the body frame
|
||||
correctedOmega = body_R_sensor * correctedOmega;
|
||||
}
|
||||
|
||||
// rotation vector describing rotation increment computed from the
|
||||
// current rotation rate measurement
|
||||
const Vector3 integratedOmega = correctedOmega * deltaT;
|
||||
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
}
|
||||
double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
|
||||
|
||||
/// Calculate an incremental rotation given the gyro measurement and a time interval,
|
||||
/// and update both deltaTij_ and deltaRij_.
|
||||
void integrateMeasurement(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
|
||||
Matrix3* F) {
|
||||
|
||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
|
||||
D_incrR_integratedOmega);
|
||||
|
||||
// Update deltaTij and rotation
|
||||
deltaTij_ += deltaT;
|
||||
deltaRij_ = deltaRij_.compose(incrR, F);
|
||||
|
||||
// Update Jacobian
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||
- *D_incrR_integratedOmega * deltaT;
|
||||
}
|
||||
Matrix3* F);
|
||||
|
||||
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
|
||||
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||
OptionalJacobian<3, 3> H = boost::none) const {
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
|
||||
const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega, boost::none, H);
|
||||
if (H) (*H) *= delRdelBiasOmega_;
|
||||
return deltaRij_biascorrected;
|
||||
}
|
||||
OptionalJacobian<3, 3> H = boost::none) const;
|
||||
|
||||
/// Integrate coriolis correction in body frame rot_i
|
||||
Vector3 integrateCoriolis(const Rot3& rot_i) const {
|
||||
if (!p_->omegaCoriolis) return Vector3::Zero();
|
||||
return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_;
|
||||
}
|
||||
Vector3 integrateCoriolis(const Rot3& rot_i) const;
|
||||
|
||||
private:
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
|
|||
Loading…
Reference in New Issue