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

Added loop

release/4.3a0
Frank 2015-12-22 11:47:37 -08:00
parent f1fa66e9c1
commit 40bc3149ad
2 changed files with 33 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,17 +36,31 @@ TEST(Scenario, Forward) {
/* ************************************************************************* */
TEST(Scenario, 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));
// Forward velocity 2m/s, angular velocity 6 degree/sec around Z
const double v = 2, w = 6 * degree;
Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
// R = v/omega, so test if circle is of right size
const double R = v / omega;
// R = v/w, so test if circle is of right size
const double R = v / w;
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));
}
/* ************************************************************************* */
TEST(Scenario, Loop) {
// Forward velocity 2m/s
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
const double v = 2, w = 6 * degree;
Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
// R = v/w, so test if loop crests at 2*R
const double R = v / w;
const Pose3 T30 = loop.pose(30);
EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9));
EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -106,7 +106,7 @@ TEST(ScenarioRunner, Forward) {
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.01);
Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0));
ScenarioRunner runner(circle);
const double T = 15; // seconds
@ -114,6 +114,19 @@ TEST(ScenarioRunner, Circle) {
EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1));
}
/* ************************************************************************* */
TEST(ScenarioRunner, Loop) {
// Forward velocity 2m/s
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
const double v = 2, w = 6 * degree;
Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
ScenarioRunner runner(loop);
const double T = 30; // seconds
ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
EXPECT(assert_equal(loop.pose(T), runner.mean(integrated), 0.1));
}
/* ************************************************************************* */
int main() {
TestResult tr;