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

Fixed circle example

release/4.3a0
Frank 2015-12-22 11:37:04 -08:00
parent ef5031e33e
commit d3534b2d2b
3 changed files with 29 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/Scenario.h
View File

@ -49,22 +49,30 @@ class Scenario {
}
/// Pose of body in nav frame at time t
Pose3 poseAtTime(double t) const { return Pose3::Expmap(twist_ * t); }
Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
/// Velocity in nav frame at time t
Vector3 velocityAtTime(double t) {
Vector3 velocity(double t) {
const Rot3 nRb = rotAtTime(t);
return nRb * linearVelocityInBody();
}
// acceleration in nav frame
Vector3 accelerationAtTime(double t) const {
const Rot3 nRb = rotAtTime(t);
Vector3 acceleration(double t) const {
const Vector3 centripetalAcceleration =
angularVelocityInBody().cross(linearVelocityInBody());
const Rot3 nRb = rotAtTime(t);
return nRb * centripetalAcceleration - gravity();
}
// acceleration in body frame frame
Vector3 accelerationInBody(double t) const {
const Vector3 centripetalAcceleration =
angularVelocityInBody().cross(linearVelocityInBody());
const Rot3 nRb = rotAtTime(t);
return centripetalAcceleration - nRb.transpose() * gravity();
}
private:
Vector6 twist_;
double imuSampleTime_;

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

@ -29,7 +29,7 @@ TEST(Scenario, Forward) {
const double v = 2; // m/s
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
const Pose3 T15 = forward.poseAtTime(15);
const Pose3 T15 = forward.pose(15);
EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
}
@ -42,7 +42,7 @@ TEST(Scenario, Circle) {
// R = v/omega, so test if circle is of right size
const double R = v / omega;
const Pose3 T15 = circle.poseAtTime(15);
const Pose3 T15 = circle.pose(15);
EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9));
EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
}

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

@ -48,13 +48,19 @@ class ScenarioRunner {
const double deltaT = scenario_.imuSampleTime();
const size_t nrSteps = T / deltaT;
double t = 0;
Vector3 v0 = scenario_.velocity(0);
Vector3 v = Vector3::Zero();
Vector3 p = Vector3::Zero();
for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
std::cout << t << ", " << deltaT << ": ";
const Vector3 measuredAcc = scenario_.accelerationAtTime(t);
p += deltaT * v;
v += deltaT * scenario_.acceleration(t);
const Vector3 measuredAcc = scenario_.accelerationInBody(t);
result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// std::cout << result.deltaRij() << std::endl;
std::cout << " a:" << measuredAcc.transpose();
std::cout << " P:" << result.deltaVij().transpose() << std::endl;
std::cout << " P:" << result.deltaPij().transpose();
std::cout << " p:" << p.transpose();
std::cout << " p0:" << (p + v0 * t).transpose();
std::cout << std::endl;
}
return result;
@ -65,7 +71,7 @@ class ScenarioRunner {
// TODO(frank): allow non-standard
const imuBias::ConstantBias zeroBias;
const Pose3 pose_i = Pose3::identity();
const Vector3 vel_i = scenario_.velocityAtTime(0);
const Vector3 vel_i = scenario_.velocity(0);
const Vector3 omegaCoriolis = Vector3::Zero();
const bool use2ndOrderCoriolis = true;
const PoseVelocityBias prediction =
@ -95,7 +101,7 @@ using namespace gtsam;
static const double degree = M_PI / 180.0;
/* ************************************************************************* *
/* ************************************************************************* */
TEST(ScenarioRunner, Forward) {
const double v = 2; // m/s
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
@ -103,19 +109,19 @@ TEST(ScenarioRunner, Forward) {
ScenarioRunner runner(forward);
const double T = 1; // seconds
ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
EXPECT(assert_equal(forward.poseAtTime(T), runner.mean(integrated), 1e-9));
EXPECT(assert_equal(forward.pose(T), runner.mean(integrated), 1e-9));
}
/* ************************************************************************* */
TEST(ScenarioRunner, Circle) {
// Forward velocity 2m/s, angular velocity 6 degree/sec
const double v = 2, omega = 6 * degree;
Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.1);
Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.01);
ScenarioRunner runner(circle);
const double T = 15; // seconds
ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
EXPECT(assert_equal(circle.poseAtTime(T), runner.mean(integrated), 1e-9));
EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1));
}
/* ************************************************************************* */