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

78 lines
2.0 KiB
C++
Raw Normal View History

1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
/*
* testGraph.cpp
*
* Created on: Jan 12, 2010
* Author: nikai
* Description: unit test for graph-inl.h
*/
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
#include <CppUnitLite/TestHarness.h>
NOISE MODEL. This is a big edit but with no templates involed, so it should not be a big deal. New namespace gtsam::noiseModel collects all noise models, which provide efficient whitening and chain-rule implementation needed for optimization. The class hierarchy gives us the ability to use models from full covariances to i.i.d. unit variance noise with a single interface, where the latter will be much cheaper. From now on, all non-linear factors take a shared_ptr to a Gaussian noise model. This is done through the parameter (const sharedGaussian& model). The use of a shared pointer allows us to share one noise models for thousands of factors, if applicable. Just like Richard's Symbol change, there is a compile flag GTSAM_MAGIC_GAUSSIAN which allows you to use doubles, vectors, or matrices to created noise models on the fly. You have to set it to the correct dimension. Use of this is *not* encouraged and the flag will disappear after some good soul fixed all unit tests.
2010-01-18 13:38:53 +08:00
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 3
2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Graph Pose2Graph;
2010-01-17 02:01:16 +08:00
#include "pose2SLAM.h"
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
2010-01-18 03:34:57 +08:00
#include "TupleConfig-inl.h"
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
#include "graph-inl.h"
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
2010-01-18 03:34:57 +08:00
#include "FactorGraph-inl.h"
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
using namespace std;
using namespace boost;
using namespace gtsam;
/* ************************************************************************* */
ordering is fixed
2010-01-14 11:21:07 +08:00
TEST( Graph, predecessorMap2Graph )
{
typedef SGraph<string>::Vertex SVertex;
SGraph<string> graph;
SVertex root;
map<string, SVertex> key2vertex;
PredecessorMap<string> p_map;
Changed signature of tree insert
2010-01-15 00:05:04 +08:00
p_map.insert("x1", "x2");
p_map.insert("x2", "x2");
p_map.insert("x3", "x2");
ordering is fixed
2010-01-14 11:21:07 +08:00
tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<string>, SVertex, string>(p_map);
LONGS_EQUAL(3, boost::num_vertices(graph));
CHECK(root == key2vertex["x2"]);
}
/* ************************************************************************* */
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
TEST( Graph, composePoses )
{
Pose2Graph graph;
Matrix cov = eye(3);
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
graph.addConstraint(1,2, Pose2(2.0, 0.0, 0.0), cov);
graph.addConstraint(2,3, Pose2(3.0, 0.0, 0.0), cov);
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
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
PredecessorMap<Pose2Config::Key> tree;
Changed signature of tree insert
2010-01-15 00:05:04 +08:00
tree.insert(1,2);
tree.insert(2,2);
tree.insert(3,2);
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
Pose2 rootPose(3.0, 0.0, 0.0);
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
boost::shared_ptr<Pose2Config> actual = composePoses<Pose2Graph, Pose2Factor,
Pose2, Pose2Config> (graph, tree, rootPose);
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
Pose2Config 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(1, Pose2(1.0, 0.0, 0.0));
expected.insert(2, Pose2(3.0, 0.0, 0.0));
expected.insert(3, Pose2(6.0, 0.0, 0.0));
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
return shared_ptr in composePoses
2010-01-13 10:09:16 +08:00
LONGS_EQUAL(3, actual->size());
CHECK(assert_equal(expected, *actual));
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */