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

functors are now obsolete :-(

release/4.3a0
Frank Dellaert 2016-01-17 15:17:43 -08:00
parent d4bbd1f289
commit 9a5a8f7c7a
4 changed files with 13 additions and 341 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/navigation/AggregateImuReadings.cpp
View File

@ -16,7 +16,6 @@
*/
#include <gtsam/navigation/AggregateImuReadings.h>
#include <gtsam/navigation/functors.h>
#include <gtsam/base/numericalDerivative.h>
#include <cmath>
@ -49,9 +48,8 @@ static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
// See extensive discussion in ImuFactor.lyx
Vector9 AggregateImuReadings::UpdateEstimate(
const Vector9& zeta, const Vector3& correctedAcc,
const Vector3& correctedOmega, double dt, bool useExactDexpDerivative,
OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) {
const Vector3& correctedOmega, double dt, OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
using namespace sugar;
const Vector3 a_dt = correctedAcc * dt;
@ -62,20 +60,6 @@ Vector9 AggregateImuReadings::UpdateEstimate(
const auto theta = dR(zeta);
const Matrix3 R = Rot3::Expmap(theta, H).matrix();
Matrix3 D_invHwdt_theta = Z_3x3;
if (A) {
if (useExactDexpDerivative) {
boost::function<Vector3(const Vector3&)> f =
[w_dt](const Vector3& theta) {
return Rot3::ExpmapDerivative(theta).inverse() * w_dt;
};
D_invHwdt_theta = numericalDerivative11<Vector3, Vector3>(f, theta);
} else {
// First order (small angle) approximation of derivative of invH*w*dt:
D_invHwdt_theta = skewSymmetric(-0.5 * w_dt);
}
}
Vector9 zeta_plus;
const double dt2 = 0.5 * dt;
const Vector3 Radt = R * a_dt;
@ -89,7 +73,8 @@ Vector9 AggregateImuReadings::UpdateEstimate(
const Matrix3 D_Radt_theta = R * skewSymmetric(-a_dt) * H;
A->setIdentity();
A->block<3, 3>(0, 0) += D_invHwdt_theta;
// First order (small angle) approximation of derivative of invH*w*dt:
A->block<3, 3>(0, 0) -= skewSymmetric(0.5 * w_dt);
A->block<3, 3>(3, 0) = D_Radt_theta * dt2;
A->block<3, 3>(3, 6) = I_3x3 * dt;
A->block<3, 3>(6, 0) = D_Radt_theta;
@ -102,17 +87,15 @@ Vector9 AggregateImuReadings::UpdateEstimate(
void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt,
bool useExactDexpDerivative) {
double dt) {
// Correct measurements
const Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer();
const Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
// Do exact mean propagation
Matrix9 A;
Matrix93 Ba, Bw;
zeta_ = UpdateEstimate(zeta_, correctedAcc, correctedOmega, dt,
useExactDexpDerivative, A, Ba, Bw);
Matrix93 B, C;
zeta_ = UpdateEstimate(zeta_, correctedAcc, correctedOmega, dt, A, B, C);
// propagate uncertainty
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
@ -120,8 +103,8 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
const Matrix3& a = p_->accelerometerCovariance;
// TODO(frank): use Eigen-tricks for symmetric matrices
cov_ = A * cov_ * A.transpose();
cov_ += Bw * (w / dt) * Bw.transpose();
cov_ += Ba * (a / dt) * Ba.transpose();
cov_ += B * (a / dt) * B.transpose();
cov_ += C * (w / dt) * C.transpose();
// increment counter and time
k_ += 1;

4
gtsam/navigation/AggregateImuReadings.h
View File

@ -60,8 +60,7 @@ class AggregateImuReadings {
* TODO(frank): put useExactDexpDerivative in params
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt,
bool useExactDexpDerivative = false);
const Vector3& measuredOmega, double dt);
/// Predict state at time j
NavState predict(const NavState& state_i, const Bias& estimatedBias_i,
@ -79,7 +78,6 @@ class AggregateImuReadings {
static Vector9 UpdateEstimate(const Vector9& zeta,
const Vector3& correctedAcc,
const Vector3& correctedOmega, double dt,
bool useExactDexpDerivative = false,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);

154
gtsam/navigation/functors.h
View File

@ -1,154 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 functors.h
* @brief Functors for use in Navigation factors
* @author Frank Dellaert
*/
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <cmath>
namespace gtsam {
// Implement Rot3::ExpmapDerivative(omega) * theta, with derivatives
static Vector3 correctWithExpmapDerivative(
const Vector3& omega, const Vector3& theta,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) {
using std::sin;
const double angle2 = omega.dot(omega); // rotation angle, squared
if (angle2 <= std::numeric_limits<double>::epsilon()) {
if (H1) *H1 = 0.5 * skewSymmetric(theta);
if (H2) *H2 = I_3x3;
return theta;
}
const double angle = std::sqrt(angle2); // rotation angle
const double s1 = sin(angle) / angle;
const double s2 = sin(angle / 2.0);
const double a = 2.0 * s2 * s2 / angle2;
const double b = (1.0 - s1) / angle2;
const Vector3 omega_x_theta = omega.cross(theta);
const Vector3 yt = a * omega_x_theta;
const Matrix3 W = skewSymmetric(omega);
const Vector3 omega_x_omega_x_theta = omega.cross(omega_x_theta);
const Vector3 yyt = b * omega_x_omega_x_theta;
if (H1) {
Matrix13 omega_t = omega.transpose();
const Matrix3 T = skewSymmetric(theta);
const double Da = (s1 - 2.0 * a) / angle2;
const double Db = (3.0 * s1 - cos(angle) - 2.0) / angle2 / angle2;
*H1 = (-Da * omega_x_theta + Db * omega_x_omega_x_theta) * omega_t + a * T -
b * skewSymmetric(omega_x_theta) - b * W * T;
}
if (H2) *H2 = I_3x3 - a* W + b* W* W;
return theta - yt + yyt;
}
// theta(k+1) = theta(k) + inverse(H)*omega dt
// omega = (H/dt_)*(theta(k+1) - H*theta(k))
// TODO(frank): make linear expression
class PredictAngularVelocity {
private:
double dt_;
public:
typedef Vector3 result_type;
PredictAngularVelocity(double dt) : dt_(dt) {}
Vector3 operator()(const Vector3& theta, const Vector3& theta_plus,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
// TODO(frank): take into account derivative of ExpmapDerivative
const Vector3 predicted = (theta_plus - theta) / dt_;
Matrix3 D_c_t, D_c_p;
const Vector3 corrected =
correctWithExpmapDerivative(theta, predicted, D_c_t, D_c_p);
if (H1) *H1 = D_c_t - D_c_p / dt_;
if (H2) *H2 = D_c_p / dt_;
return corrected;
}
};
// TODO(frank): make linear expression
static Vector3 averageVelocity(const Vector3& vel, const Vector3& vel_plus,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) {
// TODO(frank): take into account derivative of ExpmapDerivative
if (H1) *H1 = 0.5 * I_3x3;
if (H2) *H2 = 0.5 * I_3x3;
return 0.5 * (vel + vel_plus);
}
// pos(k+1) = pos(k) + average_velocity * dt
// TODO(frank): make linear expression
class PositionDefect {
private:
double dt_;
public:
typedef Vector3 result_type;
PositionDefect(double dt) : dt_(dt) {}
Vector3 operator()(const Vector3& pos, const Vector3& pos_plus,
const Vector3& average_velocity,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 3> H3 = boost::none) const {
// TODO(frank): take into account derivative of ExpmapDerivative
if (H1) *H1 = I_3x3;
if (H2) *H2 = -I_3x3;
if (H3) *H3 = I_3x3* dt_;
return (pos + average_velocity * dt_) - pos_plus;
}
};
// vel(k+1) = vel(k) + Rk * acc * dt
// acc = Rkt * [vel(k+1) - vel(k)]/dt
// TODO(frank): take in Rot3
class PredictAcceleration {
private:
double dt_;
public:
typedef Vector3 result_type;
PredictAcceleration(double dt) : dt_(dt) {}
Vector3 operator()(const Vector3& vel, const Vector3& vel_plus,
const Vector3& theta,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 3> H3 = boost::none) const {
Matrix3 D_R_theta;
// TODO(frank): D_R_theta is ExpmapDerivative (computed again)
Rot3 nRb = Rot3::Expmap(theta, D_R_theta);
Vector3 n_acc = (vel_plus - vel) / dt_;
Matrix3 D_b_R, D_b_n;
Vector3 b_acc = nRb.unrotate(n_acc, D_b_R, D_b_n);
if (H1) *H1 = -D_b_n / dt_;
if (H2) *H2 = D_b_n / dt_;
if (H3) *H3 = D_b_R* D_R_theta;
return b_acc;
}
};
} // namespace gtsam

161
gtsam/navigation/tests/testAggregateImuReadings.cpp
View File

@ -15,7 +15,6 @@
* @author Frank Dellaert
*/
#include <gtsam/navigation/functors.h>
#include <gtsam/navigation/AggregateImuReadings.h>
#include <gtsam/base/numericalDerivative.h>
@ -40,176 +39,22 @@ static boost::shared_ptr<AggregateImuReadings::Params> defaultParams() {
}
/* ************************************************************************* */
TEST(AggregateImuReadings, CorrectWithExpmapDerivative1) {
Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::bind(
correctWithExpmapDerivative, _1, _2, boost::none, boost::none);
for (Vector3 omega : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
for (Vector3 theta :
{Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
Matrix3 H = Rot3::ExpmapDerivative(omega);
Vector3 expected = H * theta;
EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta)));
EXPECT(assert_equal(expected,
correctWithExpmapDerivative(omega, theta, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
EXPECT(assert_equal(H, aH2));
}
}
}
/* ************************************************************************* */
TEST(AggregateImuReadings, CorrectWithExpmapDerivative2) {
Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::bind(
correctWithExpmapDerivative, _1, _2, boost::none, boost::none);
const Vector3 omega(0, 0, 0);
for (Vector3 theta : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
Matrix3 H = Rot3::ExpmapDerivative(omega);
Vector3 expected = H * theta;
EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta)));
EXPECT(assert_equal(expected,
correctWithExpmapDerivative(omega, theta, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
EXPECT(assert_equal(H, aH2));
}
}
/* ************************************************************************* */
TEST(AggregateImuReadings, CorrectWithExpmapDerivative3) {
Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::bind(
correctWithExpmapDerivative, _1, _2, boost::none, boost::none);
const Vector3 omega(0.1, 0.2, 0.3), theta(0.4, 0.3, 0.2);
Matrix3 H = Rot3::ExpmapDerivative(omega);
Vector3 expected = H * theta;
EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta)));
EXPECT(assert_equal(expected,
correctWithExpmapDerivative(omega, theta, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
EXPECT(assert_equal(H, aH2));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, PredictAngularVelocity1) {
Matrix aH1, aH2;
PredictAngularVelocity functor(kDt);
boost::function<Vector3(const Vector3&, const Vector3&)> f =
boost::bind(functor, _1, _2, boost::none, boost::none);
const Vector3 theta(0, 0, 0), theta_plus(0.4, 0.3, 0.2);
EXPECT(assert_equal(Vector3(4, 3, 2), functor(theta, theta_plus, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1));
EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, PredictAngularVelocity2) {
Matrix aH1, aH2;
PredictAngularVelocity functor(kDt);
boost::function<Vector3(const Vector3&, const Vector3&)> f =
boost::bind(functor, _1, _2, boost::none, boost::none);
const Vector3 theta(0.1, 0.2, 0.3), theta_plus(0.4, 0.3, 0.2);
EXPECT(
assert_equal(Vector3(Rot3::ExpmapDerivative(theta) * Vector3(3, 1, -1)),
functor(theta, theta_plus, aH1, aH2), 1e-5));
EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1));
EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, AverageVelocity) {
Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f =
boost::bind(averageVelocity, _1, _2, boost::none, boost::none);
const Vector3 v(1, 2, 3), v_plus(3, 2, 1);
EXPECT(assert_equal(Vector3(2, 2, 2), averageVelocity(v, v_plus, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, v, v_plus), aH1));
EXPECT(assert_equal(numericalDerivative22(f, v, v_plus), aH2));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, PositionDefect) {
Matrix aH1, aH2, aH3;
PositionDefect functor(kDt);
boost::function<Vector3(const Vector3&, const Vector3&, const Vector3&)> f =
boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none);
const Vector3 pos(1, 2, 3), pos_plus(2, 4, 6);
const Vector3 avg(10, 20, 30);
EXPECT(assert_equal(Vector3(0, 0, 0),
functor(pos, pos_plus, avg, aH1, aH2, aH3)));
EXPECT(assert_equal(numericalDerivative31(f, pos, pos_plus, avg), aH1));
EXPECT(assert_equal(numericalDerivative32(f, pos, pos_plus, avg), aH2));
EXPECT(assert_equal(numericalDerivative33(f, pos, pos_plus, avg), aH3));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, PredictAcceleration1) {
Matrix aH1, aH2, aH3;
PredictAcceleration functor(kDt);
boost::function<Vector3(const Vector3&, const Vector3&, const Vector3&)> f =
boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none);
const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6);
const Vector3 theta(0, 0, 0);
EXPECT(assert_equal(Vector3(10, 20, 30),
functor(vel, vel_plus, theta, aH1, aH2, aH3)));
EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1));
EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2));
EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, PredictAcceleration2) {
Matrix aH1, aH2, aH3;
PredictAcceleration functor(kDt);
boost::function<Vector3(const Vector3&, const Vector3&, const Vector3&)> f =
boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none);
const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6);
const Vector3 theta(0.1, 0.2, 0.3);
EXPECT(assert_equal(Vector3(10, 20, 30),
functor(vel, vel_plus, theta, aH1, aH2, aH3)));
EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1));
EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2));
EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, UpdateEstimate1) {
TEST(AggregateImuReadings, UpdateEstimate) {
AggregateImuReadings pim(defaultParams());
Matrix9 aH1;
Matrix93 aH2, aH3;
boost::function<Vector9(const Vector9&, const Vector3&, const Vector3&)> f =
boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, false,
boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt,
boost::none, boost::none, boost::none);
Vector9 zeta;
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
pim.UpdateEstimate(zeta, acc, omega, kDt, false, aH1, aH2, aH3);
pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3);
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-5));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-5));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, UpdateEstimate2) {
// Using exact dexp derivatives is more expensive but much more accurate
AggregateImuReadings pim(defaultParams());
Matrix9 aH1;
Matrix93 aH2, aH3;
boost::function<Vector9(const Vector9&, const Vector3&, const Vector3&)> f =
boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, true,
boost::none, boost::none, boost::none);
Vector9 zeta;
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
pim.UpdateEstimate(zeta, acc, omega, kDt, true, aH1, aH2, aH3);
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-8));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;