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

113 lines
3.0 KiB
C++
Raw Normal View History

prepend license information on all the codes
2010-10-14 12:54:38 +08:00
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
Fixed some Doxygen problems with global replace
2011-10-14 11:23:14 +08:00
/**
* @file testGraph.cpp
* @date Jan 12, 2010
* @author nikai
* @brief unit test for graph-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
*/
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/inference/graph-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
Moved the subdirectories that compile into libgtsam.la into a 'gtsam' subdirectory. Hopefully a good fix for the problem we had trying to make a gtsam distribution, that the source directory may not be called 'gtsam'.
2010-10-26 04:10:33 +08:00
#include <CppUnitLite/TestHarness.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
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
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
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
#include <iostream>
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 gtsam;
Moved tree stuff to graph, removed ordering-inl.h
2010-01-19 03:11:22 +08:00
/* ************************************************************************* */
// x1 -> x2
// -> x3 -> x4
// -> x5
TEST ( Ordering, predecessorMap2Keys ) {
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
PredecessorMap<Key> p_map;
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
p_map.insert(1,1);
p_map.insert(2,1);
p_map.insert(3,1);
p_map.insert(4,3);
p_map.insert(5,1);
All unit tests pass with TypedSymbol removed
2012-02-07 12:58:11 +08:00
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
list<Key> expected;
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
expected += 4,5,3,2,1;
All unit tests pass with TypedSymbol removed
2012-02-07 12:58:11 +08:00
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
list<Key> actual = predecessorMap2Keys<Key>(p_map);
Moved tree stuff to graph, removed ordering-inl.h
2010-01-19 03:11:22 +08:00
LONGS_EQUAL(expected.size(), actual.size());
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
list<Key>::const_iterator it1 = expected.begin();
list<Key>::const_iterator it2 = actual.begin();
Moved tree stuff to graph, removed ordering-inl.h
2010-01-19 03:11:22 +08:00
for(; it1!=expected.end(); it1++, it2++)
CHECK(*it1 == *it2)
}
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
/* ************************************************************************* */
ordering is fixed
2010-01-14 11:21:07 +08:00
TEST( Graph, predecessorMap2Graph )
{
typedef SGraph<string>::Vertex SVertex;
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
SGraph<Key> graph;
ordering is fixed
2010-01-14 11:21:07 +08:00
SVertex root;
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
map<Key, SVertex> key2vertex;
ordering is fixed
2010-01-14 11:21:07 +08:00
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
PredecessorMap<Key> p_map;
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
p_map.insert(1, 2);
p_map.insert(2, 2);
p_map.insert(3, 2);
Fix errors
2012-06-05 02:46:05 +08:00
boost::tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
ordering is fixed
2010-01-14 11:21:07 +08:00
LONGS_EQUAL(3, boost::num_vertices(graph));
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
CHECK(root == key2vertex[2]);
ordering is fixed
2010-01-14 11:21:07 +08:00
}
/* ************************************************************************* */
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 )
{
refactored Pose2SLAM
2012-01-28 10:31:44 +08:00
pose2SLAM::Graph graph;
clean up noise model: Remove Shared[NoiseModel] classes and headers, typedef for backward compatibility in NoiseModel.h. Fix all tests and examples to create shared noise models through static functions in noise model classes. Fix MATLAB wrapper and examples as well. Add tests for MATLAB examples
2012-06-23 03:36:49 +08:00
SharedNoiseModel cov = noiseModel::Unit::Create(3);
Comments, and better test of composePoses
2010-01-27 10:49:58 +08:00
Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
Removed remaining global lie functions on lie objects and configs, switched the Lie base class to a simple concept check function, fixed build script for examples. ISAM2 and MastSLAM verified as compiling.
2010-08-27 03:55:40 +08:00
Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
Streamlining of SLAM namespaces: planarSLAM Values and Graph now derive from Pose3SLAM. visualSLAM Values and Graph now derive from pose3SLAM. Several methods have been renamed to make them consistent through these 4 namespaces: addPrior -> addPosePrior addHardConstraint -> addPoseConstraint addOdometry/addConstraint -> addRelativePose All gtsam and matlab examples/tests run. PS: please don't use the deprecated typedefs in these namespaces.
2012-06-24 10:48:12 +08:00
graph.addRelativePose(1,2, p12, cov);
graph.addRelativePose(2,3, p23, cov);
graph.addRelativePose(4,3, p43, 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
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
PredecessorMap<Key> tree;
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
tree.insert(1,2);
tree.insert(2,2);
tree.insert(3,2);
tree.insert(4,3);
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
Comments, and better test of composePoses
2010-01-27 10:49:58 +08:00
Pose2 rootPose = p2;
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
(2.0_prep branch) Merged in virtual_values branch
2012-02-04 01:18:32 +08:00
boost::shared_ptr<Values> actual = composePoses<pose2SLAM::Graph, pose2SLAM::Odometry,
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
Pose2, Key> (graph, tree, rootPose);
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
Renamed DynamicValues to Values and removed specialized derived Values classes
2012-02-03 00:16:46 +08:00
Values expected;
Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.
2012-06-03 00:18:40 +08:00
expected.insert(1, p1);
expected.insert(2, p2);
expected.insert(3, p3);
expected.insert(4, p4);
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
Comments, and better test of composePoses
2010-01-27 10:49:58 +08:00
LONGS_EQUAL(4, actual->size());
return shared_ptr in composePoses
2010-01-13 10:09:16 +08:00
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);
}
/* ************************************************************************* */