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

Added missing header

release/4.3a0
Frank Dellaert 2012-05-22 12:27:34 +00:00
parent 10456a153c
commit 18e23c20df
2 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -16,6 +16,7 @@
#include <gtsam/slam/pose2SLAM.h> #include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
using namespace gtsam; using namespace gtsam;
@ -145,7 +146,7 @@ TEST( Pose2SLAM, linearization )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2Graph, optimize) { TEST(Pose2SLAM, optimize) {
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg; pose2SLAM::Graph fg;
@ -171,11 +172,14 @@ TEST(Pose2Graph, optimize) {
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// Check marginals
Marginals marginals = fg.marginals(actual);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// test optimization with 3 poses // test optimization with 3 poses
TEST(Pose2Graph, optimizeThreePoses) { TEST(Pose2SLAM, optimizeThreePoses) {
// Create a hexagon of poses // Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
@ -289,7 +293,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2Graph, optimize2) { TEST(Pose2SLAM, optimize2) {
// Pose2SLAMOptimizer myOptimizer("100"); // Pose2SLAMOptimizer myOptimizer("100");
// Matrix A1 = myOptimizer.a1(); // Matrix A1 = myOptimizer.a1();
// Matrix A2 = myOptimizer.a2(); // Matrix A2 = myOptimizer.a2();

27
tests/testNonlinearOptimizer.cpp
View File

@ -15,26 +15,25 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <iostream> #include <gtsam/slam/smallExample.h>
using namespace std; #include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <boost/assign/std/list.hpp> // for operator += #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
using namespace boost::assign; #include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/Matrix.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
using namespace boost; #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/Matrix.h> #include <iostream>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
using namespace std;
using namespace gtsam; using namespace gtsam;
const double tol = 1e-5; const double tol = 1e-5;