diff --git a/.cproject b/.cproject index afe5aa25d..c685dd0d1 100644 --- a/.cproject +++ b/.cproject @@ -5,45 +5,45 @@ - - - - - - - - - + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -297,483 +297,489 @@ - - - make - -j2 - install - true - true - true - - - make - -j2 - check - true - true - true - - - make - -k - check - true - false - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -f local.mk - testCal3_S2.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - testGaussianConditional.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testGaussianFactorGraph.run - true - true - true - - - make - -j2 - testNonlinearFactorGraph.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - testVectorConfig.run - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - - - make - -j2 - testNonlinearFactor.run - true - true - true - - - make - -j2 - timeGaussianFactor.run - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - - testBayesTree.run - true - false - true - - - make - testSymbolicBayesNet.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testSQP.run - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testSQPOptimizer.run - true - true - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testControlConfig.run - true - true - true - - - make - -j2 - testControlPoint.run - true - true - true - - - make - -j2 - testControlGraph.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - -j2 - testPose2Constraint.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testGaussianBayesTree.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - -j2 - testUrbanGraph.run - true - true - true - - - make - -j2 - testUrbanConfig.run - true - true - true - - - make - -j2 - testUrbanMeasurement.run - true - true - true - - - make - -j2 - testUrbanOdometry.run - true - true - true - - - make - -j2 - testIterative.run - true - true - true - - - make - -j2 - testGaussianISAM2.run - true - true - true - - - make - -j2 - testSubgraphPreconditioner.run - true - true - true - - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - testPose2Graph.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testBearingFactor.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - - + + +make +-j2 +install +true +true +true + + +make +-j2 +check +true +true +true + + +make +-k +check +true +false +true + + +make +-j2 +testSimpleCamera.run +true +true +true + + +make +-f local.mk +testCal3_S2.run +true +true +true + + +make +-j2 +testVSLAMFactor.run +true +true +true + + +make +-j2 +testCalibratedCamera.run +true +true +true + + +make +-j2 +testGaussianConditional.run +true +true +true + + +make +-j2 +testPose2.run +true +true +true + + +make +-j2 +testRot3.run +true +true +true + + +make +-j2 +testNonlinearOptimizer.run +true +true +true + + +make +-j2 +testGaussianFactor.run +true +true +true + + +make +-j2 +testGaussianFactorGraph.run +true +true +true + + +make +-j2 +testNonlinearFactorGraph.run +true +true +true + + +make +-j2 +testPose3.run +true +true +true + + +make +-j2 +testVectorConfig.run +true +true +true + + +make +-j2 +testPoint2.run +true +true +true + + +make +-j2 +testNonlinearFactor.run +true +true +true + + +make +-j2 +timeGaussianFactor.run +true +true +true + + +make +-j2 +timeGaussianFactorGraph.run +true +true +true + + +make +-j2 +testGaussianBayesNet.run +true +true +true + + +make +testBayesTree.run +true +false +true + + +make + +testSymbolicBayesNet.run +true +false +true + + +make +testSymbolicFactorGraph.run +true +false +true + + +make +-j2 +testVector.run +true +true +true + + +make +-j2 +testMatrix.run +true +true +true + + +make +-j2 +testVSLAMGraph.run +true +true +true + + +make +-j2 +testNonlinearEquality.run +true +true +true + + +make +-j2 +testSQP.run +true +true +true + + +make +-j2 +testNonlinearConstraint.run +true +true +true + + +make +-j2 +testSQPOptimizer.run +true +true +true + + +make +-j2 +testVSLAMConfig.run +true +true +true + + +make +-j2 +testControlConfig.run +true +true +true + + +make +-j2 +testControlPoint.run +true +true +true + + +make +-j2 +testControlGraph.run +true +true +true + + +make +-j2 +testOrdering.run +true +true +true + + +make +-j2 +testPose2Constraint.run +true +true +true + + +make +-j2 +testRot2.run +true +true +true + + +make +-j2 +testGaussianBayesTree.run +true +true +true + + +make +-j2 +testPose3Factor.run +true +true +true + + +make +-j2 +testUrbanGraph.run +true +true +true + + +make +-j2 +testUrbanConfig.run +true +true +true + + +make +-j2 +testUrbanMeasurement.run +true +true +true + + +make +-j2 +testUrbanOdometry.run +true +true +true + + +make +-j2 +testIterative.run +true +true +true + + +make +-j2 +testGaussianISAM2.run +true +true +true + + +make +-j2 +testSubgraphPreconditioner.run +true +true +true + + +make +-j2 +testBayesNetPreconditioner.run +true +true +true + + +make +-j2 +testPose2Factor.run +true +true +true + + +make +-j2 +timeRot3.run +true +true +true + + +make +-j2 +install +true +true +true + + +make +-j2 +testPose2Graph.run +true +true +true + + +make +-j2 +testPose2Config.run +true +true +true + + +make +-j2 +testLieConfig.run +true +true +true + + +make +-j2 +testBearingFactor.run +true +true +true + + +make +testGraph.run +true +false +true + + +make +-j2 +testPose3Graph.run +true +true +true + + +make +-j2 +install +true +true +true + + +make +-j2 +clean +true +true +true + + +make +-j2 +check +true +true +true + + + + - - + + diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp index d36bbc1dd..92d926963 100644 --- a/cpp/Pose2Graph.cpp +++ b/cpp/Pose2Graph.cpp @@ -7,6 +7,8 @@ #include "Pose2Graph.h" #include "FactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h" +#include "NonlinearOptimizer-inl.h" +#include "NonlinearEquality.h" #include "graph-inl.h" using namespace std; @@ -14,17 +16,19 @@ using namespace gtsam; namespace gtsam { -// explicit instantiation so all the code is there and we can link with it -template class FactorGraph > ; -template class NonlinearFactorGraph ; -//template class NonlinearOptimizer ; + // explicit instantiation so all the code is there and we can link with it + template class FactorGraph > ; + template class NonlinearFactorGraph ; + template class NonlinearEquality ; + template class NonlinearOptimizer; -/* ************************************************************************* */ + void Pose2Graph::addConstraint(const Pose2Config::Key& key, const Pose2& pose) { + push_back(sharedFactor(new NonlinearEquality (key, pose))); + } -bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { - return false; -} - -/* ************************************************************************* */ + bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { + return false; + } } // namespace gtsam diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h index 3ba246206..31ab5c98d 100644 --- a/cpp/Pose2Graph.h +++ b/cpp/Pose2Graph.h @@ -8,9 +8,7 @@ #pragma once #include "Pose2Factor.h" -#include "Pose2Config.h" #include "NonlinearFactorGraph.h" -#include "NonlinearEquality.h" namespace gtsam { @@ -43,9 +41,7 @@ namespace gtsam { * @param key of pose * @param pose which pose to constrain it to */ - inline void addConstraint(const Pose2Config::Key& key, const Pose2& pose = Pose2()) { - push_back(sharedFactor(new NonlinearEquality(key, pose))); - } + void addConstraint(const Pose2Config::Key& key, const Pose2& pose = Pose2()); private: /** Serialization function */ diff --git a/cpp/Pose3Graph.cpp b/cpp/Pose3Graph.cpp index ad2a52369..1879dac9b 100644 --- a/cpp/Pose3Graph.cpp +++ b/cpp/Pose3Graph.cpp @@ -3,20 +3,34 @@ * @brief A factor graph for the 2D PoseSLAM problem * @authors Frank Dellaert, Viorela Ila */ -//#include "NonlinearOptimizer-inl.h" + +#include "Pose3Graph.h" #include "FactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h" -#include "Pose3Graph.h" +#include "NonlinearOptimizer-inl.h" +#include "NonlinearEquality.h" +#include "graph-inl.h" namespace gtsam { -// explicit instantiation so all the code is there and we can link with it -template class FactorGraph > ; -template class NonlinearFactorGraph ; -//template class NonlinearOptimizer ; + // explicit instantiation so all the code is there and we can link with it + template class FactorGraph > ; + template class NonlinearFactorGraph ; + template class NonlinearEquality ; + template class NonlinearOptimizer ; -bool Pose3Graph::equals(const Pose3Graph& p, double tol) const { - return false; -} + bool Pose3Graph::equals(const Pose3Graph& p, double tol) const { + return false; + } + + /** + * Add an equality constraint on a pose + * @param key of pose + * @param pose which pose to constrain it to + */ + void Pose3Graph::addConstraint(const Pose3Config::Key& key, const Pose3& pose) { + push_back(sharedFactor(new NonlinearEquality (key, pose))); + } } // namespace gtsam diff --git a/cpp/Pose3Graph.h b/cpp/Pose3Graph.h index c1237830c..8bfce2353 100644 --- a/cpp/Pose3Graph.h +++ b/cpp/Pose3Graph.h @@ -8,9 +8,7 @@ #pragma once #include "Pose3Factor.h" -#include "Pose3Config.h" #include "NonlinearFactorGraph.h" -#include "NonlinearEquality.h" namespace gtsam { @@ -43,9 +41,7 @@ namespace gtsam { * @param key of pose * @param pose which pose to constrain it to */ - inline void addConstraint(const Pose3Config::Key& key, const Pose3& pose =Pose3()) { - push_back(sharedFactor(new NonlinearEquality (key, pose))); - } + void addConstraint(const Pose3Config::Key& key, const Pose3& pose =Pose3()); private: /** Serialization function */