Created AHRS factor based on Luca's IMU factor. Has not been tested yet.
krunalchande
parent
c8c25b16ab
commit
f3882cd0d7
|
|
@ -0,0 +1,243 @@
|
|||
/*
|
||||
* ImuFactor.h
|
||||
*
|
||||
* Created on: Jun 29, 2014
|
||||
* Author: krunal
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias>{
|
||||
public:
|
||||
class PreintegratedMeasurements{
|
||||
public:
|
||||
imuBias::ConstantBias biasHat;
|
||||
Matrix measurementCovariance;
|
||||
|
||||
Rot3 deltaRij;
|
||||
double deltaTij;
|
||||
Matrix3 delRdelBiasOmega;
|
||||
Matrix PreintMeasCov;
|
||||
bool use2ndOrderIntegration_;
|
||||
|
||||
|
||||
PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias,
|
||||
const Matrix3& measurementAccCovariance,
|
||||
const Matrix3& measurementGyroCovariance,
|
||||
const Matrix3& integrationErrorCovariance,
|
||||
const bool use2ndOrderIntegration =false
|
||||
): biasHat(bias), measurementCovariance(9,9), delRdelBiasOmega(Matirx3::Zero()), PreintMeasCov(9,9),
|
||||
use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
|
||||
PreintMeasCov = Matrix::Zero(9,9);
|
||||
}
|
||||
|
||||
PreintegratedMeasurements():
|
||||
biasHat(imuBias::ConstantBias()), measurementCovariance(9,9),
|
||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
|
||||
{
|
||||
measurementCovariance = Matrix::Zero(9,9);
|
||||
PreintMeasCov = Matrix::Zero(9,9);
|
||||
}
|
||||
|
||||
void print (const std::string& s = "Preintegrated Measurements: ") const {
|
||||
std::cout<<s<<std::endl;
|
||||
biasHat.print(" biasHat");
|
||||
deltaRij.print(" deltaRij ");
|
||||
std::cout<<" measurementCovariance [" <<measurementCovariance<<" ]"<< std::endl;
|
||||
std::cout<<" PreintMeasCov [ "<<PreintMeasCov << " ]"<< std::endl;
|
||||
}
|
||||
|
||||
bool equals (const PreintegratedMeasurements& expected, double tol = 1e-9) const {
|
||||
return biasHat.equals(expected.biasHat, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance, expected.measurementCovariancem, tol)
|
||||
&& deltaRij.equals(expected.deltaRij, tol)
|
||||
&& std::fabs(deltaTij - expected.deltaTij)<tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
|
||||
}
|
||||
|
||||
void resetIntegration(){
|
||||
deltaRij = Rot3();
|
||||
deltaTij = 0.0;
|
||||
delRdelBiasOmega = Matrix3::Zero();
|
||||
PreintMeasCov = Matrix::Zero(9,9);
|
||||
}
|
||||
|
||||
void integrateMeasurement (
|
||||
const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega,
|
||||
double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none
|
||||
){
|
||||
Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
|
||||
|
||||
if(body_P_sensor) {
|
||||
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
|
||||
correctedOmega = body_R_sensor * correctedOmega;
|
||||
Matrix3 body_omega_body_cross = skewSymmetric(correctedOmega);
|
||||
correctedAcc = body_R_sensor * correctedAcc - body_omega_body_cross * body_omega_body_cross * body_P_sensor->translation.vector();
|
||||
}
|
||||
const Vector3 theta_incr = correctedOmega * deltaT;
|
||||
const Rot3 Rincr = Rot3::Expmap(theta_incr);
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
|
||||
|
||||
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
|
||||
|
||||
Matrix3 Z_3x3 = Matrix::Zero();
|
||||
Matrix3 I_3x3 = Matrix::Identity();
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij);
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
|
||||
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
|
||||
Matrix F(3,3);
|
||||
F<<H_angles_angles;
|
||||
|
||||
PreintMeasCov = F* PreintMeasCov * F.transpose() + measurementCovariance * deltaT;
|
||||
deltaRij = deltaRij * Rincr;
|
||||
deltaTij += deltaT;
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
typedef AHRSFactor This;
|
||||
typedef NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> Base;
|
||||
|
||||
PreintegratedMeasurements preintegratedMeasurements_;
|
||||
Vector3 gravity_;
|
||||
Vector3 omegaCoriolis_;
|
||||
boost::optional<Pose3> body_P_sensor_;
|
||||
bool use2ndOrderCoriolis_;
|
||||
|
||||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
|
||||
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<ImuFactor> shared_ptr;
|
||||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
AHRSFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
|
||||
|
||||
AHRSFactor(
|
||||
Key rot_i,
|
||||
Key rot_j,
|
||||
Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity,
|
||||
const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false
|
||||
):
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), Rot_i, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){}
|
||||
|
||||
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout<<s<<"AHRSFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ","
|
||||
<< keyFormatter(this->key3()) << ",";
|
||||
preintegratedMeasurements_.print(" preintegrated measurements:");
|
||||
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
|
||||
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
|
||||
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
|
||||
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
}
|
||||
/** Access the preintegrated measurements. */
|
||||
const PreintegratedMeasurements& preintegratedMeasurements() const {
|
||||
return preintegratedMeasurements_; }
|
||||
|
||||
const Vector3& gravity() const { return gravity_; }
|
||||
|
||||
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
|
||||
|
||||
|
||||
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, const imuBias::ConstantBias& bias,
|
||||
boost::optional<Matrix&> H1 = boost::none) const
|
||||
{
|
||||
const double& deltaTij = preintegratedMeasurements_.deltaTij;
|
||||
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Rot3 Rot_j = pose_j.rotation();
|
||||
// We compute factor's Jacobians
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const 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
|
||||
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
|
||||
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const 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() - fRhat.inverse().matrix() * Jtheta);
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
|
||||
|
||||
H5->resize(3,3);
|
||||
(*H5) <<
|
||||
// dfR/dBias
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
Vector r(3); r << fR;
|
||||
return r;
|
||||
}
|
||||
}; // AHRSFactor
|
||||
typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
|
||||
} //namespace gtsam
|
||||
Loading…
Reference in New Issue