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

117 lines
3.4 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
*/
#include <iostream>
#include <boost/shared_ptr.hpp>
Moved tree stuff to graph, removed ordering-inl.h
2010-01-19 03:11:22 +08:00
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
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
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
Installing headers in subdirectories and converted include directives to match the subdirectory structure (also see email to frankcvs)
2010-08-20 01:23:19 +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
using namespace std;
using namespace gtsam;
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
Key kx(size_t i) { return Symbol('x',i); }
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;
p_map.insert(kx(1),kx(1));
p_map.insert(kx(2),kx(1));
p_map.insert(kx(3),kx(1));
p_map.insert(kx(4),kx(3));
p_map.insert(kx(5),kx(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;
expected += kx(4),kx(5),kx(3),kx(2),kx(1);//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(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;
p_map.insert(kx(1), kx(2));
p_map.insert(kx(2), kx(2));
p_map.insert(kx(3), kx(2));
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));
Int keys with formatter objects, all unit tests pass
2012-02-21 08:53:35 +08:00
CHECK(root == key2vertex[kx(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;
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
Matrix cov = eye(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);
refactored Pose2SLAM
2012-01-28 10:31:44 +08:00
graph.addOdometry(1,2, p12, cov);
graph.addOdometry(2,3, p23, cov);
graph.addOdometry(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;
All unit tests pass with nonlinear factors templated on value instead of key
2012-02-07 12:02:20 +08:00
tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(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;
(2.0_prep branch) Merged in virtual_values branch
2012-02-04 01:18:32 +08:00
expected.insert(pose2SLAM::PoseKey(1), p1);
expected.insert(pose2SLAM::PoseKey(2), p2);
expected.insert(pose2SLAM::PoseKey(3), p3);
expected.insert(pose2SLAM::PoseKey(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);
}
/* ************************************************************************* */