diff --git a/.cproject b/.cproject index 8891df378..7d8fa61b6 100644 --- a/.cproject +++ b/.cproject @@ -1,5 +1,7 @@ - + + + @@ -315,14 +317,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -349,6 +343,7 @@ make + tests/testBayesTree.run true false @@ -356,6 +351,7 @@ make + testBinaryBayesNet.run true false @@ -403,6 +399,7 @@ make + testSymbolicBayesNet.run true false @@ -410,6 +407,7 @@ make + tests/testSymbolicFactor.run true false @@ -417,6 +415,7 @@ make + testSymbolicFactorGraph.run true false @@ -432,11 +431,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -525,22 +533,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -557,6 +549,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -581,42 +589,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -661,26 +653,42 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run true true true @@ -1071,6 +1079,7 @@ make + testGraph.run true false @@ -1078,6 +1087,7 @@ make + testJunctionTree.run true false @@ -1085,6 +1095,7 @@ make + testSymbolicBayesNetB.run true false @@ -1252,6 +1263,7 @@ make + testErrors.run true false @@ -1297,14 +1309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1385,6 +1389,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1691,7 +1703,6 @@ make - testSimulated2DOriented.run true false @@ -1731,7 +1742,6 @@ make - testSimulated2D.run true false @@ -1739,7 +1749,6 @@ make - testSimulated3D.run true false @@ -1849,14 +1858,6 @@ true true - - make - -j5 - testGaussianBayesNetUnordered.run - true - true - true - make -j2 @@ -1953,6 +1954,38 @@ true true + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + VisualSLAMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + make -j4 @@ -1971,7 +2004,6 @@ make - tests/testGaussianISAM2 true false @@ -1993,110 +2025,6 @@ true true - - make - -j5 - testSymbolicBayesTree.run - true - true - true - - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2298,6 +2226,7 @@ cpack + -G DEB true false @@ -2305,6 +2234,7 @@ cpack + -G RPM true false @@ -2312,6 +2242,7 @@ cpack + -G TGZ true false @@ -2319,6 +2250,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2484,42 +2416,98 @@ true true - + make - -j5 - check.symbolic + -j2 + testRot3.run true true true - + make - -j5 - testSpirit.run + -j2 + testRot2.run true true true - + make - -j5 - testWrap.run + -j2 + testPose3.run true true true - + make - -j5 - check.wrap + -j2 + timeRot3.run true true true - + make - -j5 - wrap + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2563,6 +2551,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 50574a0a8..f4ae59183 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -22,14 +22,11 @@ * - The robot rotates around the landmarks, always facing towards the cube */ -// As this is a full 3D problem, we will use Pose3 variables to represent the camera -// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// For loading the data +#include "visualSLAMdata.h" + // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -// We will also need a camera object to hold calibration information and perform projections. -#include -#include #include -#include // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). @@ -67,28 +64,10 @@ int main(int argc, char* argv[]) { noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks - std::vector points; - points.push_back(gtsam::Point3(10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + std::vector points = createPoints(); // Create the set of ground-truth poses - std::vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - gtsam::Point3 up(0,0,1); - gtsam::Point3 target(0,0,0); - for(; i < 8; ++i, theta += 2*M_PI/8) { - gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); - gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); - } + std::vector poses = createPoses(); // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 43d0ae273..bb79fd27c 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -22,14 +22,11 @@ * - The robot rotates around the landmarks, always facing towards the cube */ -// As this is a full 3D problem, we will use Pose3 variables to represent the camera -// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// For loading the data +#include "visualSLAMdata.h" + // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -// We will also need a camera object to hold calibration information and perform projections. -#include -#include #include -#include // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). @@ -67,28 +64,10 @@ int main(int argc, char* argv[]) { noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks - std::vector points; - points.push_back(gtsam::Point3(10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + std::vector points = createPoints(); // Create the set of ground-truth poses - std::vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - gtsam::Point3 up(0,0,1); - gtsam::Point3 target(0,0,0); - for(; i < 8; ++i, theta += 2*M_PI/8) { - gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); - gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); - } + std::vector poses = createPoses(); // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates int relinearizeInterval = 3; diff --git a/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp index 4a07e201b..0bf54f1ba 100644 --- a/examples/VisualSLAMExample.cpp +++ b/examples/VisualSLAMExample.cpp @@ -21,14 +21,11 @@ * - The robot rotates around the landmarks, always facing towards the cube */ -// As this is a full 3D problem, we will use Pose3 variables to represent the camera -// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// For loading the data +#include "visualSLAMdata.h" + // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -// We will also need a camera object to hold calibration information and perform projections. -#include -#include #include -#include // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). @@ -74,28 +71,10 @@ int main(int argc, char* argv[]) { noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks - std::vector points; - points.push_back(gtsam::Point3(10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + std::vector points = createPoints(); // Create the set of ground-truth poses - std::vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - gtsam::Point3 up(0,0,1); - gtsam::Point3 target(0,0,0); - for(; i < 8; ++i, theta += 2*M_PI/8) { - gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); - gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); - } + std::vector poses = createPoses(); // Create a factor graph NonlinearFactorGraph graph; diff --git a/examples/visualSLAMdata.h b/examples/visualSLAMdata.h new file mode 100644 index 000000000..f481e8b6a --- /dev/null +++ b/examples/visualSLAMdata.h @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file visualSLAMdata.h + * @brief Simple visual SLAM example for the structure-from-motion problems + * @author Duy-Nguyen Ta + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include + +// We will also need a camera object to hold calibration information and perform projections. +#include + +/* ************************************************************************* */ +std::vector createPoints() { + + // Create the set of ground-truth landmarks + std::vector points; + points.push_back(gtsam::Point3(10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + + return points; +} + +/* ************************************************************************* */ +std::vector createPoses() { + + // Create the set of ground-truth poses + std::vector poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 8; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); + } + return poses; +} +/* ************************************************************************* */