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

Merge remote-tracking branch 'origin/RSS_ImuFactor' into feature/scenarios

release/4.3a0
Frank Dellaert 2015-12-23 15:05:14 -08:00
parent 25db851a0b f79a9b8d3a
commit 5869b8cb65
3 changed files with 58 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/Scenario.h
View File

@ -67,24 +67,26 @@ class ExpmapScenario : public Scenario {
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
};
/// Accelerating from an arbitrary initial state
/// Accelerating from an arbitrary initial state, with optional rotation
class AcceleratingScenario : public Scenario {
public:
/// Construct scenario with constant twist [w,v]
/// Construct scenario with constant acceleration in navigation frame and
/// optional angular velocity in body: rotating while translating...
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
const Vector3& accelerationInBody)
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {}
const Vector3& a_n,
const Vector3& omega_b = Vector3::Zero())
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
Pose3 pose(double t) const {
return Pose3(nRb_, p0_ + v0_ * t + a_n_ * t * t / 2.0);
return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
}
Vector3 omega_b(double t) const { return Vector3::Zero(); }
Vector3 omega_b(double t) const { return omega_b_; }
Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
Vector3 acceleration_n(double t) const { return a_n_; }
private:
const Rot3 nRb_;
const Vector3 p0_, v0_, a_n_;
const Vector3 p0_, v0_, a_n_, omega_b_;
};
} // namespace gtsam

22
gtsam/navigation/tests/testScenario.cpp
View File

@ -16,7 +16,10 @@
*/
#include <gtsam/navigation/Scenario.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <cmath>
using namespace std;
@ -87,18 +90,25 @@ TEST(Scenario, Accelerating) {
const Point3 P0(10, 20, 0);
const Vector3 V0(50, 0, 0);
const double a_b = 0.2; // m/s^2
const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0));
const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3);
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 3;
const Vector3 A = nRb * Vector3(a_b, 0, 0);
EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
{
// Check acceleration in nav
Matrix expected = numericalDerivative11<Vector3, double>(
boost::bind(&Scenario::velocity_n, scenario, _1), T);
EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
}
const Pose3 T3 = scenario.pose(3);
EXPECT(assert_equal(nRb, T3.rotation(), 1e-9));
EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0),
EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9));
EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0),
T3.translation(), 1e-9));
}

32
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -76,15 +76,20 @@ TEST(ScenarioRunner, Loop) {
}
/* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) {
namespace initial {
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
// going in X. The body itself has Z axis pointing down
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
const Point3 P0(10, 20, 0);
const Vector3 V0(50, 0, 0);
}
const double a_b = 0.2; // m/s^2
const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0));
/* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) {
using namespace initial;
const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0);
const AcceleratingScenario scenario(nRb, P0, V0, A);
const double T = 3; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
@ -92,7 +97,26 @@ TEST(ScenarioRunner, Accelerating) {
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
cout << estimatedCov << endl << endl;
cout << runner.poseCovariance(pim) << endl;
}
/* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating) {
using namespace initial;
const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0), W(0, 0.1, 0);
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 3; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}