Added another test, fixed a bug in the factor w.r.t initializing measurement covariance.
krunalchande
parent
b721a7ce1f
commit
73ec571f4b
|
|
@ -35,18 +35,19 @@ public:
|
|||
const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance,
|
||||
const bool use2ndOrderIntegration = false) :
|
||||
biasHat(bias), measurementCovariance(9, 9), delRdelBiasOmega(
|
||||
Matrix3::Zero()), PreintMeasCov(9, 9), use2ndOrderIntegration_(
|
||||
biasHat(bias), measurementCovariance(3,3), delRdelBiasOmega(
|
||||
Matrix3::Zero()), PreintMeasCov(3,3), use2ndOrderIntegration_(
|
||||
use2ndOrderIntegration) {
|
||||
measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
|
||||
PreintMeasCov = Matrix::Zero(9, 9);
|
||||
// measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
|
||||
measurementCovariance <<measuredOmegaCovariance;
|
||||
PreintMeasCov = Matrix::Zero(3,3);
|
||||
}
|
||||
|
||||
PreintegratedMeasurements() :
|
||||
biasHat(imuBias::ConstantBias()), measurementCovariance(9, 9), delRdelBiasOmega(
|
||||
Matrix3::Zero()), PreintMeasCov(9, 9) {
|
||||
measurementCovariance = Matrix::Zero(9, 9);
|
||||
PreintMeasCov = Matrix::Zero(9, 9);
|
||||
biasHat(imuBias::ConstantBias()), measurementCovariance(3,3), delRdelBiasOmega(
|
||||
Matrix3::Zero()), PreintMeasCov(3,3) {
|
||||
measurementCovariance = Matrix::Zero(3,3);
|
||||
PreintMeasCov = Matrix::Zero(3,3);
|
||||
}
|
||||
|
||||
void print(const std::string& s = "Preintegrated Measurements: ") const {
|
||||
|
|
@ -302,9 +303,10 @@ public:
|
|||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc
|
||||
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
|
||||
|
||||
H3->resize(3, 3);
|
||||
H3->resize(3, 6);
|
||||
(*H3) <<
|
||||
// dfR/dBias
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -35,38 +35,29 @@ using symbol_shorthand::X;
|
|||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::B;
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
Vector callEvaluateError(const AHRSFactor& factor,
|
||||
const Rot3 rot_i, const Rot3 rot_j,
|
||||
const imuBias::ConstantBias& bias)
|
||||
{
|
||||
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
|
||||
const Rot3 rot_j, const imuBias::ConstantBias& bias) {
|
||||
return factor.evaluateError(rot_i, rot_j, bias);
|
||||
}
|
||||
|
||||
Rot3 evaluateRotationError(const AHRSFactor& factor,
|
||||
const Rot3 rot_i, const Rot3 rot_j,
|
||||
const imuBias::ConstantBias& bias)
|
||||
{
|
||||
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3) ) ;
|
||||
Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
|
||||
const Rot3 rot_j, const imuBias::ConstantBias& bias) {
|
||||
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
|
||||
}
|
||||
|
||||
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
|
||||
)
|
||||
{
|
||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
|
||||
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
|
||||
Matrix3::Identity(), Matrix3::Identity());
|
||||
|
||||
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
|
||||
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
||||
list<double>::const_iterator itDeltaT = deltaTs.begin();
|
||||
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
|
||||
for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
|
||||
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
|
||||
}
|
||||
|
||||
|
|
@ -74,53 +65,48 @@ AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
|||
}
|
||||
|
||||
Rot3 evaluatePreintegratedMeasurementsRotation(
|
||||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij;
|
||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
|
||||
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs, initialRotationRate).deltaRij;
|
||||
}
|
||||
|
||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
|
||||
{
|
||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
|
||||
const double deltaT) {
|
||||
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
|
||||
}
|
||||
|
||||
|
||||
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
|
||||
{
|
||||
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
|
||||
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
|
||||
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
TEST( AHRSFactor, PreintegratedMeasurements )
|
||||
{
|
||||
TEST( AHRSFactor, PreintegratedMeasurements ) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredAcc(0.1, 0.0, 0.0);
|
||||
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
|
||||
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
|
||||
double deltaT = 0.5;
|
||||
|
||||
// Expected preintegrated values
|
||||
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0);
|
||||
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
|
||||
double expectedDeltaT1(0.5);
|
||||
|
||||
bool use2ndOrderIntegration = true;
|
||||
// Actual preintegrated values
|
||||
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
|
||||
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
|
||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6));
|
||||
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6);
|
||||
|
||||
// Integrate again
|
||||
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0);
|
||||
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
|
||||
double expectedDeltaT2(1);
|
||||
|
||||
// Actual preintegrated values
|
||||
|
|
@ -132,30 +118,35 @@ TEST( AHRSFactor, PreintegratedMeasurements )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, Error )
|
||||
{
|
||||
TEST( ImuFactor, Error ) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias; // Bias
|
||||
Rot3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0));
|
||||
Rot3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0));
|
||||
Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
|
||||
Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
|
||||
|
||||
// Measurements
|
||||
Vector3 gravity; gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
|
||||
Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
|
||||
Vector3 gravity;
|
||||
gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis;
|
||||
omegaCoriolis << 0, 0, 0;
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << M_PI / 100, 0, 0;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector();
|
||||
double deltaT = 1.0;
|
||||
bool use2ndOrderIntegration = true;
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis, false);
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis,
|
||||
false);
|
||||
|
||||
Vector errorActual = factor.evaluateError(x1, x2, bias);
|
||||
|
||||
// Expected error
|
||||
Vector errorExpected(3); errorExpected << 0, 0, 0;
|
||||
Vector errorExpected(3);
|
||||
errorExpected << 0, 0, 0;
|
||||
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
|
||||
// Expected Jacobians
|
||||
|
|
@ -176,22 +167,19 @@ TEST( ImuFactor, Error )
|
|||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
|
||||
|
||||
// rotations
|
||||
EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
|
||||
EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
|
||||
|
||||
EXPECT(assert_equal(H2e, H2a, 1e-5));
|
||||
|
||||
// rotations
|
||||
EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
|
||||
EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
|
||||
|
||||
// EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions.
|
||||
EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions.
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, ErrorWithBiases )
|
||||
{
|
||||
TEST( ImuFactor, ErrorWithBiases ) {
|
||||
// Linearization point
|
||||
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
|
||||
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
|
||||
|
|
@ -199,104 +187,112 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.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.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)));
|
||||
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)));
|
||||
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
|
||||
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
|
||||
|
||||
// Measurements
|
||||
Vector3 gravity; gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
|
||||
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
|
||||
Vector3 gravity;
|
||||
gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis;
|
||||
omegaCoriolis << 0, 0.1, 0.1;
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector()
|
||||
+ Vector3(0.2, 0.0, 0.0);
|
||||
double deltaT = 1.0;
|
||||
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
|
||||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
SETDEBUG("ImuFactor evaluateError", false);
|
||||
Vector errorActual = factor.evaluateError(x1, x2, bias);
|
||||
SETDEBUG("ImuFactor evaluateError", false);
|
||||
SETDEBUG("ImuFactor evaluateError", false);
|
||||
Vector errorActual = factor.evaluateError(x1, x2, bias);
|
||||
SETDEBUG("ImuFactor evaluateError", false);
|
||||
|
||||
// Expected error
|
||||
Vector errorExpected(3); errorExpected << 0, 0, 0;
|
||||
// Expected error
|
||||
Vector errorExpected(3);
|
||||
errorExpected << 0, 0, 0;
|
||||
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<imuBias::ConstantBias>(
|
||||
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<imuBias::ConstantBias>(
|
||||
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
|
||||
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3,Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
|
||||
Matrix RH2e = numericalDerivative11<Rot3,Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
|
||||
Matrix RH3e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
|
||||
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
|
||||
Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
|
||||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
|
||||
EXPECT(assert_equal(H1e, H1a));
|
||||
EXPECT(assert_equal(H2e, H2a));
|
||||
// EXPECT(assert_equal(H3e, H3a));
|
||||
EXPECT(assert_equal(H1e, H1a));
|
||||
EXPECT(assert_equal(H2e, H2a));
|
||||
EXPECT(assert_equal(H3e, H3a));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( AHRSFactor, PartialDerivativeExpmap )
|
||||
{
|
||||
TEST( AHRSFactor, PartialDerivativeExpmap ) {
|
||||
// Linearization point
|
||||
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
|
||||
Vector3 biasOmega;
|
||||
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0.1, 0, 0;
|
||||
double deltaT = 0.5;
|
||||
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind(
|
||||
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega));
|
||||
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(
|
||||
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT),
|
||||
LieVector(biasOmega));
|
||||
|
||||
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
|
||||
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
|
||||
(measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
|
||||
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( AHRSFactor, PartialDerivativeLogmap )
|
||||
{
|
||||
TEST( AHRSFactor, PartialDerivativeLogmap ) {
|
||||
// Linearization point
|
||||
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
|
||||
Vector3 thetahat;
|
||||
thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 deltatheta; deltatheta << 0, 0, 0;
|
||||
|
||||
Vector3 deltatheta;
|
||||
deltatheta << 0, 0, 0;
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind(
|
||||
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
|
||||
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(
|
||||
boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
|
||||
|
||||
const Vector3 x = thetahat; // parametrization of so(3)
|
||||
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
|
||||
double normx = norm_2(x);
|
||||
const Matrix3 actualDelFdeltheta = Matrix3::Identity() +
|
||||
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
|
||||
const Matrix3 actualDelFdeltheta = Matrix3::Identity() + 0.5 * X
|
||||
+ (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
|
||||
* X;
|
||||
|
||||
// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
|
||||
// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;
|
||||
|
|
@ -307,133 +303,201 @@ TEST( AHRSFactor, PartialDerivativeLogmap )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( AHRSFactor, fistOrderExponential )
|
||||
{
|
||||
TEST( AHRSFactor, fistOrderExponential ) {
|
||||
// Linearization point
|
||||
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
|
||||
Vector3 biasOmega;
|
||||
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
|
||||
double deltaT = 1.0;
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0.1, 0, 0;
|
||||
double deltaT = 1.0;
|
||||
|
||||
// change w.r.t. linearization point
|
||||
double alpha = 0.0;
|
||||
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
|
||||
// change w.r.t. linearization point
|
||||
double alpha = 0.0;
|
||||
Vector3 deltabiasOmega;
|
||||
deltabiasOmega << alpha, alpha, alpha;
|
||||
|
||||
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
|
||||
(measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
|
||||
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
|
||||
const Matrix expectedRot = Rot3::Expmap(
|
||||
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
|
||||
|
||||
const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
|
||||
const Matrix3 hatRot =
|
||||
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
|
||||
const Matrix3 actualRot = hatRot
|
||||
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
|
||||
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
|
||||
|
||||
const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
|
||||
const Matrix3 actualRot =
|
||||
hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
|
||||
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedRot, actualRot));
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedRot, actualRot));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements )
|
||||
{
|
||||
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
|
||||
|
||||
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
|
||||
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
|
||||
|
||||
// Measurements
|
||||
list<Vector3> measuredAccs, measuredOmegas;
|
||||
list<double> deltaTs;
|
||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
|
||||
deltaTs.push_back(0.01);
|
||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
|
||||
deltaTs.push_back(0.01);
|
||||
for(int i=1;i<100;i++)
|
||||
{
|
||||
for (int i = 1; i < 100; i++) {
|
||||
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
|
||||
measuredOmegas.push_back(
|
||||
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
|
||||
deltaTs.push_back(0.01);
|
||||
}
|
||||
|
||||
// Actual preintegrated values
|
||||
AHRSFactor::PreintegratedMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0));
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
|
||||
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
||||
Matrix expectedDelRdelBias =
|
||||
numericalDerivative11<Rot3, imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
|
||||
measuredAccs, measuredOmegas, deltaTs,
|
||||
Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
|
||||
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
||||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
||||
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
|
||||
EXPECT(
|
||||
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega,
|
||||
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
||||
}
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
||||
{
|
||||
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
|
||||
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)));
|
||||
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)));
|
||||
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
|
||||
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
|
||||
|
||||
// Measurements
|
||||
Vector3 gravity; gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
|
||||
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
|
||||
Vector3 gravity;
|
||||
gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis;
|
||||
omegaCoriolis << 0, 0.1, 0.1;
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector()
|
||||
+ Vector3(0.2, 0.0, 0.0);
|
||||
double deltaT = 1.0;
|
||||
|
||||
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
|
||||
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
|
||||
Point3(1, 0, 0));
|
||||
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
|
||||
// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);
|
||||
|
||||
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
|
||||
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
|
||||
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<imuBias::ConstantBias>(
|
||||
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<imuBias::ConstantBias>(
|
||||
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
|
||||
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3,Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
|
||||
Matrix RH2e = numericalDerivative11<Rot3,Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
|
||||
Matrix RH3e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
|
||||
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
|
||||
Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
|
||||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a;
|
||||
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
|
||||
|
||||
EXPECT(assert_equal(H1e, H1a));
|
||||
EXPECT(assert_equal(H2e, H2a));
|
||||
// EXPECT(assert_equal(H3e, H3a));
|
||||
EXPECT(assert_equal(H1e, H1a));
|
||||
EXPECT(assert_equal(H2e, H2a));
|
||||
EXPECT(assert_equal(H3e, H3a));
|
||||
}
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
|
||||
using namespace std;
|
||||
TEST (AHRSFactor, graphTest) {
|
||||
// linearization point
|
||||
Rot3 x1(Rot3::RzRyRx(0, 0, 0));
|
||||
Rot3 x2(Rot3::RzRyRx(0, M_PI/4, 0));
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
|
||||
// PreIntegrator
|
||||
imuBias::ConstantBias biasHat(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
Vector3 gravity;
|
||||
gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis;
|
||||
omegaCoriolis << 0, 0, 0;
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(biasHat,
|
||||
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
|
||||
|
||||
// Pre-integrate measurements
|
||||
Vector3 measuredAcc(0.0, 0.0, 0.0);
|
||||
Vector3 measuredOmega(0, M_PI/20, 0);
|
||||
double deltaT = 1;
|
||||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create Factor
|
||||
noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov);
|
||||
// cout<<"model: \n"<<noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov)<<endl;
|
||||
// cout<<"pre int measurement cov: \n "<<pre_int_data.PreintMeasCov<<endl;
|
||||
NonlinearFactorGraph graph;
|
||||
Values values;
|
||||
for(size_t i = 0; i < 5; ++i) {
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
}
|
||||
// pre_int_data.print("Pre integrated measurementes");
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
values.insert(X(1), x1);
|
||||
values.insert(X(2), x2);
|
||||
values.insert(B(1), bias);
|
||||
graph.push_back(factor);
|
||||
// values.print();
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values);
|
||||
Values result = optimizer.optimize();
|
||||
// result.print("Final Result:\n");
|
||||
// cout<<"******************************\n";
|
||||
// cout<<"result.at(X(2)): \n"<<result.at<Rot3>(X(2)).ypr()*(180/M_PI);
|
||||
Rot3 expectedRot(Rot3::RzRyRx(0, M_PI/4, 0));
|
||||
EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
|
||||
// Marginals marginals(graph, result);
|
||||
// cout << "x1 covariance:\n" << marginals.marginalCovariance(X(1)) << endl;
|
||||
// cout << "x2 covariance:\n" << marginals.marginalCovariance(X(2)) << endl;
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue