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

Added missing Jacobians

release/4.3a0
Frank Dellaert 2015-12-30 00:15:02 -08:00
parent 6f8b05c0d0
commit b3ffc6d824
2 changed files with 70 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/ImuBias.h
View File

@ -78,19 +78,19 @@ public:
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement, Vector3 correctAccelerometer(const Vector3& measurement,
OptionalJacobian<3, 6> H = boost::none) const { OptionalJacobian<3, 6> H1 = boost::none,
if (H) { OptionalJacobian<3, 3> H2 = boost::none) const {
(*H) << -I_3x3, Z_3x3; if (H1) (*H1) << -I_3x3, Z_3x3;
} if (H2) (*H2) << I_3x3;
return measurement - biasAcc_; return measurement - biasAcc_;
} }
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement, Vector3 correctGyroscope(const Vector3& measurement,
OptionalJacobian<3, 6> H = boost::none) const { OptionalJacobian<3, 6> H1 = boost::none,
if (H) { OptionalJacobian<3, 3> H2 = boost::none) const {
(*H) << Z_3x3, -I_3x3; if (H1) (*H1) << Z_3x3, -I_3x3;
} if (H2) (*H2) << I_3x3;
return measurement - biasGyro_; return measurement - biasGyro_;
} }

102
gtsam/navigation/tests/testImuBias.cpp
View File

@ -16,113 +16,135 @@
*/ */
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef imuBias::ConstantBias Bias;
Vector biasAcc1(Vector3(0.2, -0.1, 0)); Vector biasAcc1(Vector3(0.2, -0.1, 0));
Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); Vector biasGyro1(Vector3(0.1, -0.3, -0.2));
imuBias::ConstantBias bias1(biasAcc1, biasGyro1); Bias bias1(biasAcc1, biasGyro1);
Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); Vector biasAcc2(Vector3(0.1, 0.2, 0.04));
Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); Vector biasGyro2(Vector3(-0.002, 0.005, 0.03));
imuBias::ConstantBias bias2(biasAcc2, biasGyro2); Bias bias2(biasAcc2, biasGyro2);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, Constructor) { TEST(ImuBias, Constructor) {
// Default Constructor // Default Constructor
imuBias::ConstantBias bias1; Bias bias1;
// Acc + Gyro Constructor // Acc + Gyro Constructor
imuBias::ConstantBias bias2(biasAcc2, biasGyro2); Bias bias2(biasAcc2, biasGyro2);
// Copy Constructor // Copy Constructor
imuBias::ConstantBias bias3(bias2); Bias bias3(bias2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, inverse) { TEST(ImuBias, inverse) {
imuBias::ConstantBias biasActual = bias1.inverse(); Bias biasActual = bias1.inverse();
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, Bias biasExpected = Bias(-biasAcc1, -biasGyro1);
-biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, compose) { TEST(ImuBias, compose) {
imuBias::ConstantBias biasActual = bias2.compose(bias1); Bias biasActual = bias2.compose(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias( Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, between) { TEST(ImuBias, between) {
// p.between(q) == q - p // p.between(q) == q - p
imuBias::ConstantBias biasActual = bias2.between(bias1); Bias biasActual = bias2.between(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias( Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, localCoordinates) { TEST(ImuBias, localCoordinates) {
Vector deltaActual = Vector(bias2.localCoordinates(bias1)); Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, Vector deltaExpected =
biasGyro1 - biasGyro2)).vector(); (Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector();
EXPECT(assert_equal(deltaExpected, deltaActual)); EXPECT(assert_equal(deltaExpected, deltaActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, retract) { TEST(ImuBias, retract) {
Vector6 delta; Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.retract(delta); Bias biasActual = bias2.retract(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias( Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0),
biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); biasGyro2 + delta.block<3, 1>(3, 0));
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, Logmap) { TEST(ImuBias, Logmap) {
Vector deltaActual = bias2.Logmap(bias1); Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector(); Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual)); EXPECT(assert_equal(deltaExpected, deltaActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, Expmap) { TEST(ImuBias, Expmap) {
Vector6 delta; Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.Expmap(delta); Bias biasActual = bias2.Expmap(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); Bias biasExpected = Bias(delta);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, operatorSub) { TEST(ImuBias, operatorSub) {
imuBias::ConstantBias biasActual = -bias1; Bias biasActual = -bias1;
imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); Bias biasExpected(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, operatorAdd) { TEST(ImuBias, operatorAdd) {
imuBias::ConstantBias biasActual = bias2 + bias1; Bias biasActual = bias2 + bias1;
imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1);
biasGyro2 + biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, operatorSubB) { TEST(ImuBias, operatorSubB) {
imuBias::ConstantBias biasActual = bias2 - bias1; Bias biasActual = bias2 - bias1;
imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1);
biasGyro2 - biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */
TEST(ImuBias, Correct1) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f = boost::bind(
&Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
bias1.correctAccelerometer(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
}
/* ************************************************************************* */
TEST(ImuBias, Correct2) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f =
boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
bias1.correctGyroscope(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;