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

69 lines
2.3 KiB
C++
Raw Normal View History

Created Pose3Config, Pose3Graph, and tested loop closure optimization.
2010-01-11 02:21:20 +08:00
/**
* @file testPose3Config.cpp
* @authors Frank Dellaert
**/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
#include "pose3SLAM.h"
Created Pose3Config, Pose3Graph, and tested loop closure optimization.
2010-01-11 02:21:20 +08:00
using namespace std;
using namespace gtsam;
// The world coordinate system has z pointing up, y north, x east
// The vehicle has X forward, Y right, Z down
Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1));
Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1));
Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1));
Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1));
/* ************************************************************************* */
TEST( Pose3Config, pose3Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Pose3Config expected;
Large gtsam refactoring To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors. The following are the biggest changes: 1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function. 2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like if (H) *H = Matrix_(3,6,....); 3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'> 4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
Created Pose3Config, Pose3Graph, and tested loop closure optimization.
2010-01-11 02:21:20 +08:00
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
Pose3Config actual = pose3SLAM::circle(4,1.0);
Created Pose3Config, Pose3Graph, and tested loop closure optimization.
2010-01-11 02:21:20 +08:00
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( Pose3Config, expmap )
{
Pose3Config expected;
I reverted back to the old expmap in Pose3 :-( It does not work well for Urban, and now it turns out Yong-Dian has problems, too. It seems there is still something that we do not quite understand about the whole Lie group optimization business. If you want to run with the new expmap, define CORRECT_POSE3_EXPMAP
2010-03-13 03:16:53 +08:00
#ifdef CORRECT_POSE3_EXMAP
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
#else
MAJOR: I made the correct Pose3 expmap the default. It turns out that all but the transform_to derivatives were in fact correct (and I fixed transform_to based on new math in math.lyx), but there was still a wrong expmap and logmap in the header file that I forgot about. I now moved those the te cpp file. The new code, which executes a correct "screw motion" for the exponential map, is now linked in by default. If you want the old, incorrect behavior, configure with FASTER_BUT_INCORRECT_EXPMAP defined. Note that the old code is faster per expmap, but *converges* faster (and to a deeper minimum) and hence is faster overall in many cases.
2010-03-12 05:52:24 +08:00
// expected is circle shifted to East
Large gtsam refactoring To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors. The following are the biggest changes: 1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function. 2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like if (H) *H = Matrix_(3,6,....); 3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'> 4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0)));
MAJOR: I made the correct Pose3 expmap the default. It turns out that all but the transform_to derivatives were in fact correct (and I fixed transform_to based on new math in math.lyx), but there was still a wrong expmap and logmap in the header file that I forgot about. I now moved those the te cpp file. The new code, which executes a correct "screw motion" for the exponential map, is now linked in by default. If you want the old, incorrect behavior, configure with FASTER_BUT_INCORRECT_EXPMAP defined. Note that the old code is faster per expmap, but *converges* faster (and to a deeper minimum) and hence is faster overall in many cases.
2010-03-12 05:52:24 +08:00
#endif
Created Pose3Config, Pose3Graph, and tested loop closure optimization.
2010-01-11 02:21:20 +08:00
// Note expmap coordinates are in global coordinates with non-compose expmap
// so shifting to East requires little thought, different from with Pose2 !!!
Vector delta = Vector_(24,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0);
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
Pose3Config actual = expmap(pose3SLAM::circle(4,1.0),delta);
Created Pose3Config, Pose3Graph, and tested loop closure optimization.
2010-01-11 02:21:20 +08:00
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */