From c298c277938bf09c5a77091bea1c5a0b5022f681 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 14:43:24 -0400 Subject: [PATCH 01/44] implemented and unit tested initial version of GPS factor --- .cproject | 32 +++++---- gtsam/slam/GPSFactor.h | 110 +++++++++++++++++++++++++++++ gtsam/slam/tests/testGPSFactor.cpp | 85 ++++++++++++++++++++++ 3 files changed, 214 insertions(+), 13 deletions(-) create mode 100644 gtsam/slam/GPSFactor.h create mode 100644 gtsam/slam/tests/testGPSFactor.cpp diff --git a/.cproject b/.cproject index 21ac9d913..a0cec2816 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -784,18 +782,18 @@ true true - + make -j5 - testGaussianFactorGraph.run + testGaussianFactorGraphUnordered.run true true true - + make -j5 - testGaussianBayesNet.run + testGaussianBayesNetUnordered.run true true true @@ -2622,6 +2620,14 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + make -j2 diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/GPSFactor.h new file mode 100644 index 000000000..7d720cd15 --- /dev/null +++ b/gtsam/slam/GPSFactor.h @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * 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 GPSFactor.h + * @author Luca Carlone + **/ +#pragma once + +#include + +#include +#include + +namespace gtsam { + + /** + * A class to model GPS measurements, including a bias term which models + * common-mode errors and that can be partially corrected if other sensors are used + * @addtogroup SLAM + */ + class GPSFactor: public NoiseModelFactor2 { + + private: + + typedef GPSFactor This; + typedef NoiseModelFactor2 Base; + + Point3 measured_; /** The measurement */ + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + GPSFactor() {} + + /** Constructor */ + GPSFactor(Key posekey, Key biaskey, const Point3 measured, + const SharedNoiseModel& model) : + Base(model, posekey, biaskey), measured_(measured) { + } + + virtual ~GPSFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "GPSFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + measured_.print(" measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose, const Point3& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + + if (H1 || H2){ + H1->resize(3,6); // jacobian wrt pose + (*H1) << Matrix3::Zero(), pose.rotation().matrix(); + H2->resize(3,3); // jacobian wrt bias + (*H2) << Matrix3::Identity(); + } + return pose.translation().vector() + bias.vector() - measured_.vector(); + } + + /** return the measured */ + const Point3 measured() const { + return measured_; + } + + /** number of variables attached to this factor */ + size_t size() const { + return 2; + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; // \class GPSFactor + +} /// namespace gtsam diff --git a/gtsam/slam/tests/testGPSFactor.cpp b/gtsam/slam/tests/testGPSFactor.cpp new file mode 100644 index 000000000..f501183b3 --- /dev/null +++ b/gtsam/slam/tests/testGPSFactor.cpp @@ -0,0 +1,85 @@ +/** + * @file testGPSFactor.cpp + * @brief + * @author Luca Carlone + * @date July 30, 2014 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace gtsam::noiseModel; +// Convenience for named keys + +using symbol_shorthand::X; +using symbol_shorthand::B; + +TEST(GPSFactor, errorNoiseless) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(GPSFactor, errorNoisy) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(1.0,2.0,3.0); + Point3 measured = t - noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(GPSFactor, jacobian) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + + Matrix actualH1, actualH2; + factor.evaluateError(pose,bias, actualH1, actualH2); + + Matrix numericalH1 = numericalDerivative21( + boost::function(boost::bind( + &GPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); + + Matrix numericalH2 = numericalDerivative22( + boost::function(boost::bind( + &GPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 001f61ce324bd462f90bd7656192f4b6fbc69fd5 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 15:26:48 -0400 Subject: [PATCH 02/44] removed size --- gtsam/slam/GPSFactor.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/GPSFactor.h index 7d720cd15..9add83be0 100644 --- a/gtsam/slam/GPSFactor.h +++ b/gtsam/slam/GPSFactor.h @@ -90,11 +90,6 @@ namespace gtsam { return measured_; } - /** number of variables attached to this factor */ - size_t size() const { - return 2; - } - private: /** Serialization function */ From cedabdce8131922a3ba93848903867807895cd01 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 18:51:12 -0400 Subject: [PATCH 03/44] added first order Gauss Markov model --- .cproject | 8 ++ gtsam/slam/GaussMarkov1stOrderFactor.h | 135 ++++++++++++++++++ .../tests/testGaussMarkov1stOrderFactor.cpp | 122 ++++++++++++++++ 3 files changed, 265 insertions(+) create mode 100644 gtsam/slam/GaussMarkov1stOrderFactor.h create mode 100644 gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp diff --git a/.cproject b/.cproject index a0cec2816..46b623bb9 100644 --- a/.cproject +++ b/.cproject @@ -2628,6 +2628,14 @@ true true + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + make -j2 diff --git a/gtsam/slam/GaussMarkov1stOrderFactor.h b/gtsam/slam/GaussMarkov1stOrderFactor.h new file mode 100644 index 000000000..bb5f68e02 --- /dev/null +++ b/gtsam/slam/GaussMarkov1stOrderFactor.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussMarkov1stOrderFactor.h + * @author Vadim Indelman, Stephen Williams + * @date Jan 17, 2012 + **/ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* + * - The 1st order GaussMarkov factor relates two keys of the same type. This relation is given via + * key_2 = exp(-1/tau*delta_t) * key1 + w_d + * where tau is the time constant and delta_t is the time difference between the two keys. + * w_d is the equivalent discrete noise, whose covariance is calculated from the continuous noise model and delta_t. + * - w_d is approximated as a Gaussian noise. + * - In the multi-dimensional case, tau is a vector, and the above equation is applied on each element + * in the state (represented by keys), using the appropriate time constant in the vector tau. + */ + +/* + * A class for a measurement predicted by "GaussMarkov1stOrderFactor(config[key1],config[key2])" + * KEY1::Value is the Lie Group type + * T is the measurement type, by default the same + */ +template +class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { + +private: + + typedef GaussMarkov1stOrderFactor This; + typedef NoiseModelFactor2 Base; + + double dt_; + Vector tau_; + +public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + GaussMarkov1stOrderFactor() {} + + /** Constructor */ + GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau, + const SharedGaussian& model) : + Base(calc_descrete_noise_model(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) { + } + + virtual ~GaussMarkov1stOrderFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "GaussMarkov1stOrderFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + this->noiseModel_->print(" noise model"); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const VALUE& p1, const VALUE& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + + Vector v1( VALUE::Logmap(p1) ); + Vector v2( VALUE::Logmap(p2) ); + + Vector alpha(tau_.size()); + Vector alpha_v1(tau_.size()); + for(int i=0; i + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(dt_); + ar & BOOST_SERIALIZATION_NVP(tau_); + } + + SharedGaussian calc_descrete_noise_model(const SharedGaussian& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast(model); + SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t))); + return model_d; + } + +}; // \class GaussMarkov1stOrderFactor + +} /// namespace gtsam diff --git a/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp new file mode 100644 index 000000000..d5a3ed8f7 --- /dev/null +++ b/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussMarkov1stOrderFactor.cpp + * @brief Unit tests for the GaussMarkov1stOrder factor + * @author Vadim Indelman + * @date Jan 17, 2012 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//! Factors +typedef GaussMarkov1stOrderFactor GaussMarkovFactor; + +/* ************************************************************************* */ +gtsam::LieVector predictionError(const gtsam::LieVector& v1, const gtsam::LieVector& v2, const GaussMarkovFactor factor) { + return factor.evaluateError(v1, v2); +} + +/* ************************************************************************* */ +TEST( GaussMarkovFactor, equals ) +{ + // Create two identical factors and make sure they're equal + gtsam::Key x1(1); + gtsam::Key x2(2); + double delta_t = 0.10; + Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + + GaussMarkovFactor factor1(x1, x2, delta_t, tau, model); + GaussMarkovFactor factor2(x1, x2, delta_t, tau, model); + + CHECK(gtsam::assert_equal(factor1, factor2)); +} + +/* ************************************************************************* */ +TEST( GaussMarkovFactor, error ) +{ + gtsam::Values linPoint; + gtsam::Key x1(1); + gtsam::Key x2(2); + double delta_t = 0.10; + Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + + gtsam::LieVector v1 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 12.0, 13.0)); + gtsam::LieVector v2 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 15.0, 14.0)); + + // Create two nodes + linPoint.insert(x1, v1); + linPoint.insert(x2, v2); + + GaussMarkovFactor factor(x1, x2, delta_t, tau, model); + gtsam::Vector Err1( factor.evaluateError(v1, v2) ); + + // Manually calculate the error + Vector alpha(tau.size()); + Vector alpha_v1(tau.size()); + for(int i=0; i(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + numerical_H2 = gtsam::numericalDerivative22(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + + // Verify they are equal for this choice of state + CHECK( gtsam::assert_equal(numerical_H1, computed_H1, 1e-9)); + CHECK( gtsam::assert_equal(numerical_H2, computed_H2, 1e-9)); +} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 92b113d1733f834e1911f97706c8fe5cf8e195bc Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 19:01:11 -0400 Subject: [PATCH 04/44] minor --- gtsam/slam/GaussMarkov1stOrderFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/GaussMarkov1stOrderFactor.h b/gtsam/slam/GaussMarkov1stOrderFactor.h index bb5f68e02..01b32b39b 100644 --- a/gtsam/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam/slam/GaussMarkov1stOrderFactor.h @@ -11,7 +11,7 @@ /** * @file GaussMarkov1stOrderFactor.h - * @author Vadim Indelman, Stephen Williams + * @author Vadim Indelman, Stephen Williams, Luca Carlone * @date Jan 17, 2012 **/ #pragma once @@ -63,7 +63,7 @@ public: /** Constructor */ GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau, const SharedGaussian& model) : - Base(calc_descrete_noise_model(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) { + Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) { } virtual ~GaussMarkov1stOrderFactor() {} @@ -120,7 +120,7 @@ private: ar & BOOST_SERIALIZATION_NVP(tau_); } - SharedGaussian calc_descrete_noise_model(const SharedGaussian& model, double delta_t){ + SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){ /* Q_d (approx)= Q * delta_t */ /* In practice, square root of the information matrix is represented, so that: * R_d (approx)= R / sqrt(delta_t) From 41bb99b48af0d565d7f298a19feedbcd99f43f85 Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 6 Sep 2014 10:57:22 -0400 Subject: [PATCH 05/44] fixed gradient --- ...se3SLAMExample_initializePose3Gradient.cpp | 3 +++ gtsam/slam/InitializePose3.cpp | 20 ++++++++++++++----- gtsam/slam/InitializePose3.h | 2 +- 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index f2f549c1a..898986b0a 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -57,6 +57,9 @@ int main(const int argc, const char *argv[]) { Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); std::cout << "done!" << std::endl; + std::cout << "initial error=" <error(*initial)<< std::endl; + std::cout << "initialization error=" <error(initialization)<< std::endl; + if (argc < 3) { initialization.print("initialization"); } else { diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index c0e2ef6d9..3fc082d92 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -184,6 +184,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) + std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; + // gradient iterations size_t it; for(it=0; it < maxIter; it++){ @@ -217,7 +219,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; + // std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -235,7 +237,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const std::cout << "nr of gradient iterations " << it << std::endl; // Return correct rotations - const Rot3& Rref = Rot3(); // inverseRot.at(keyAnchor); + const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior Values estimateRot; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; @@ -284,13 +286,19 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); - if(logRot.norm()<1e-4){ // some tolerance + double th = logRot.norm(); + if(th < 1e-4 || th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); + th = logRot.norm(); } - double th = logRot.norm(); - if (th > 1e-5) + // exclude small or invalid rotations + if (th > 1e-5 || th == th){ // the second case means that th = nan (logRot does not work well for +/-pi) logRot = logRot / th; + }else{ + logRot = Vector3::Zero(); + th = 0.0; + } double fdot = a*b*th*exp(-b*th); return fdot*logRot; @@ -372,6 +380,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b else orientations = computeOrientationsChordal(pose3Graph); +// orientations.print("orientations\n"); + // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 1bb9a8e31..a19933bb5 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -39,7 +39,7 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 1000); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 10000); GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph); From e3ec28ebeb3e557a70375c4778a45a401230a00b Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 6 Sep 2014 18:27:07 -0400 Subject: [PATCH 06/44] added debug info for gradient code --- gtsam/slam/InitializePose3.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 3fc082d92..f1415a8fa 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -185,7 +185,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double stepsize = 2/mu_max; // = 1/(a b dG) std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; - + double maxGrad; // gradient iterations size_t it; for(it=0; it < maxIter; it++){ @@ -193,7 +193,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // compute the gradient at each node //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; - double maxGrad = 0; + maxGrad = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; @@ -234,7 +234,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations - std::cout << "nr of gradient iterations " << it << std::endl; + std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl; // Return correct rotations const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior @@ -287,13 +287,13 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); - if(th < 1e-4 || th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) + if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); th = logRot.norm(); } // exclude small or invalid rotations - if (th > 1e-5 || th == th){ // the second case means that th = nan (logRot does not work well for +/-pi) + if (th > 1e-5 && th == th){ // nonzero valid rotations logRot = logRot / th; }else{ logRot = Vector3::Zero(); @@ -334,7 +334,13 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // Create optimizer GaussNewtonParams params; - params.maxIterations = 1; + bool singleIter = false; + if(singleIter){ + params.maxIterations = 1; + }else{ + std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" < Date: Wed, 10 Sep 2014 09:35:49 -0400 Subject: [PATCH 07/44] added possibility to specify number of iterations --- examples/Pose2SLAMExample_g2o.cpp | 12 +++- examples/Pose3SLAMExample_changeKeys.cpp | 73 ++++++++++++++++++++++++ gtsam/slam/InitializePose3.cpp | 2 +- 3 files changed, 85 insertions(+), 2 deletions(-) create mode 100644 examples/Pose3SLAMExample_changeKeys.cpp diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index b46a53198..8f1a53a66 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -45,11 +45,21 @@ int main(const int argc, const char *argv[]) { noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); + if (argc == 4) { + params.maxIterations = atoi(argv[3]); + std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl; + } + std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graphWithPrior, *initial); + GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; + std::cout << "initial error=" <error(*initial)<< std::endl; + std::cout << "final error=" <error(result)<< std::endl; + if (argc < 3) { result.print("result"); } else { diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp new file mode 100644 index 000000000..1e28c2097 --- /dev/null +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o rewritted.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3example.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + Key firstKey = 8646911284551352320; + + std::cout << "Using reference key: " << firstKey << std::endl; + + if (argc < 3) { + std::cout << "Please provide output file to write " << std::endl; + } else { + const string inputFileRewritten = argv[2]; + std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; + // Additional: rewrite input with simplified keys 0,1,... + Values simpleInitial; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + Key key = key_value.key + firstKey; + simpleInitial.insert(key, initial->at(key_value.key)); + } + NonlinearFactorGraph simpleGraph; + BOOST_FOREACH(const boost::shared_ptr& factor, *graph) { + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between){ + Key key1 = pose3Between->key1() + firstKey; + Key key2 = pose3Between->key2() + firstKey; + NonlinearFactor::shared_ptr simpleFactor( + new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); + simpleGraph.add(simpleFactor); + } + } + writeG2o(simpleGraph, simpleInitial, inputFileRewritten); + } + return 0; +} diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index f1415a8fa..c571cd8e7 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -334,7 +334,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // Create optimizer GaussNewtonParams params; - bool singleIter = false; + bool singleIter = true; if(singleIter){ params.maxIterations = 1; }else{ From db411214ff1b3bdd5700d1ea2713109dbf6db77b Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 10 Sep 2014 15:30:06 -0400 Subject: [PATCH 08/44] renamed GPS factor --- gtsam/slam/{GPSFactor.h => BiasedGPSFactor.h} | 18 ++++++++--------- ...tGPSFactor.cpp => testBiasedGPSFactor.cpp} | 20 +++++++++---------- 2 files changed, 19 insertions(+), 19 deletions(-) rename gtsam/slam/{GPSFactor.h => BiasedGPSFactor.h} (87%) rename gtsam/slam/tests/{testGPSFactor.cpp => testBiasedGPSFactor.cpp} (78%) diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/BiasedGPSFactor.h similarity index 87% rename from gtsam/slam/GPSFactor.h rename to gtsam/slam/BiasedGPSFactor.h index 9add83be0..fc525f52a 100644 --- a/gtsam/slam/GPSFactor.h +++ b/gtsam/slam/BiasedGPSFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GPSFactor.h + * @file BiasedGPSFactor.h * @author Luca Carlone **/ #pragma once @@ -27,11 +27,11 @@ namespace gtsam { * common-mode errors and that can be partially corrected if other sensors are used * @addtogroup SLAM */ - class GPSFactor: public NoiseModelFactor2 { + class BiasedGPSFactor: public NoiseModelFactor2 { private: - typedef GPSFactor This; + typedef BiasedGPSFactor This; typedef NoiseModelFactor2 Base; Point3 measured_; /** The measurement */ @@ -39,24 +39,24 @@ namespace gtsam { public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::shared_ptr shared_ptr; /** default constructor - only use for serialization */ - GPSFactor() {} + BiasedGPSFactor() {} /** Constructor */ - GPSFactor(Key posekey, Key biaskey, const Point3 measured, + BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured, const SharedNoiseModel& model) : Base(model, posekey, biaskey), measured_(measured) { } - virtual ~GPSFactor() {} + virtual ~BiasedGPSFactor() {} /** implement functions needed for Testable */ /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "GPSFactor(" + std::cout << s << "BiasedGPSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; measured_.print(" measured: "); @@ -100,6 +100,6 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } - }; // \class GPSFactor + }; // \class BiasedGPSFactor } /// namespace gtsam diff --git a/gtsam/slam/tests/testGPSFactor.cpp b/gtsam/slam/tests/testBiasedGPSFactor.cpp similarity index 78% rename from gtsam/slam/tests/testGPSFactor.cpp rename to gtsam/slam/tests/testBiasedGPSFactor.cpp index f501183b3..d907852c5 100644 --- a/gtsam/slam/tests/testGPSFactor.cpp +++ b/gtsam/slam/tests/testBiasedGPSFactor.cpp @@ -1,5 +1,5 @@ /** - * @file testGPSFactor.cpp + * @file testBiasedGPSFactor.cpp * @brief * @author Luca Carlone * @date July 30, 2014 @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include using namespace gtsam; @@ -19,7 +19,7 @@ using namespace gtsam::noiseModel; using symbol_shorthand::X; using symbol_shorthand::B; -TEST(GPSFactor, errorNoiseless) { +TEST(BiasedGPSFactor, errorNoiseless) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -28,13 +28,13 @@ TEST(GPSFactor, errorNoiseless) { Point3 noise(0.0,0.0,0.0); Point3 measured = t + noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } -TEST(GPSFactor, errorNoisy) { +TEST(BiasedGPSFactor, errorNoisy) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -43,13 +43,13 @@ TEST(GPSFactor, errorNoisy) { Point3 noise(1.0,2.0,3.0); Point3 measured = t - noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } -TEST(GPSFactor, jacobian) { +TEST(BiasedGPSFactor, jacobian) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -59,20 +59,20 @@ TEST(GPSFactor, jacobian) { Point3 noise(0.0,0.0,0.0); Point3 measured = t + noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Matrix actualH1, actualH2; factor.evaluateError(pose,bias, actualH1, actualH2); Matrix numericalH1 = numericalDerivative21( boost::function(boost::bind( - &GPSFactor::evaluateError, factor, _1, _2, boost::none, + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), pose, bias, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( - &GPSFactor::evaluateError, factor, _1, _2, boost::none, + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), pose, bias, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } From 7322a74bbdf356d7401daaa7558e74eb7f42a848 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Sep 2014 12:05:36 -0400 Subject: [PATCH 09/44] nice and clean unit tests for gradient initialization --- .cproject | 24 ++++++++++-- examples/Data/simpleGraph10gradIter.txt | 11 ++++++ gtsam/slam/InitializePose3.cpp | 9 +++-- gtsam/slam/InitializePose3.h | 3 +- gtsam/slam/tests/testInitializePose3.cpp | 50 ++++++++---------------- 5 files changed, 55 insertions(+), 42 deletions(-) create mode 100644 examples/Data/simpleGraph10gradIter.txt diff --git a/.cproject b/.cproject index 1b4d0b1ec..90e48745b 100644 --- a/.cproject +++ b/.cproject @@ -788,18 +788,18 @@ true true - + make -j5 - testGaussianFactorGraph.run + testGaussianFactorGraphUnordered.run true true true - + make -j5 - testGaussianBayesNet.run + testGaussianBayesNetUnordered.run true true true @@ -2631,6 +2631,22 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + make -j5 diff --git a/examples/Data/simpleGraph10gradIter.txt b/examples/Data/simpleGraph10gradIter.txt new file mode 100644 index 000000000..c50171468 --- /dev/null +++ b/examples/Data/simpleGraph10gradIter.txt @@ -0,0 +1,11 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816 +VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026 +VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610 +VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180 +VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502 +EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index c571cd8e7..97c8a541e 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -147,7 +147,7 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { /* ************************************************************************* */ // Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter) { +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -219,7 +219,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - // std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; + //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -243,7 +243,10 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Key key = key_value.key; if (key != keyAnchor) { const Rot3& R = inverseRot.at(key); - estimateRot.insert(key, Rref.compose(R.inverse())); + if(setRefFrame) + estimateRot.insert(key, Rref.compose(R.inverse())); + else + estimateRot.insert(key, R.inverse()); } } return estimateRot; diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index a19933bb5..dba5ceac3 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -39,7 +39,8 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 10000); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, + const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph); diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 76e278f28..0cea2cead 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -153,18 +153,12 @@ TEST( InitializePose3, iterationGradient ) { givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); size_t maxIter = 1; // test gradient at the first iteration - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter); + bool setRefFrame = false; + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); - // const Key keyAnchor = symbol('Z', 9999999); - // Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984, - // 0.002460082752984, 0.999993962808392, -0.002454045561375, - // -0.002454045561375, 0.002460082752984, 0.999993962808392); - // Rot3 RzExpected = Rot3(Mz); - // EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-6)); - - Matrix M0 = (Matrix(3,3) << 0.999344848920642, -0.036021919324717, 0.003506317718352, - 0.036032601656108, 0.999346013522419, -0.003032634950127, - -0.003394783302464, 0.003156989865691, 0.999989254372924); + Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, + 0.033572116359134, 0.999436104312325, -0.000621610948719, + -0.000983333645009, 0.000654992453817, 0.999999302019670); Rot3 R0Expected = Rot3(M0); EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-5)); @@ -199,42 +193,30 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); // do 10 gradient iterations - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10); + bool setRefFrame = false; + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // const Key keyAnchor = symbol('Z', 9999999); // givenPoses.insert(keyAnchor,simple::pose0); // string g2oFile = "/home/aspn/Desktop/toyExample.g2o"; // writeG2o(pose3Graph, givenPoses, g2oFile); - // const Key keyAnchor = symbol('Z', 9999999); - // Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948, - // 0.181688570817424, 0.983350839452522, -0.003223318529546, - // -0.003987804220587, 0.004014645856811, 0.999983989889910); - // Rot3 RzExpected = Rot3(Mz); - // EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-4)); + const string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter"); + NonlinearFactorGraph::shared_ptr matlabGraph; + Values::shared_ptr matlabValues; + bool is3D = true; + boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D); - Matrix M0 = (Matrix(3,3) << 0.946965375724015, -0.321288672646614, 0.005492359133630, - 0.321308000189570, 0.946969747977338, -0.003076593882320, - -0.004212623179851, 0.004678166811270, 0.999980184084280); - Rot3 R0Expected = Rot3(M0); + Rot3 R0Expected = matlabValues->at(1).rotation(); EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-4)); - Matrix M1 = (Matrix(3,3) << 0.998716651908197, -0.050557818750367, 0.002992685163299, - 0.050577444118653, 0.998696360370342, -0.006892164352146, - -0.002640330984207, 0.007034681672788, 0.999971770554577); - Rot3 R1Expected = Rot3(M1); + Rot3 R1Expected = matlabValues->at(2).rotation(); EXPECT(assert_equal(R1Expected, orientations.at(x1), 1e-4)); - Matrix M2 = (Matrix(3,3) << 0.837033946147607, 0.547150466557869, 0.000734807505930, - -0.547150098128722, 0.837029489230720, 0.002899012916604, - 0.000971140718506, -0.002828622220494, 0.999995527881019); - Rot3 R2Expected = Rot3(M2); + Rot3 R2Expected = matlabValues->at(3).rotation(); EXPECT(assert_equal(R2Expected, orientations.at(x2), 1e-3)); - Matrix M3 = (Matrix(3,3) << 0.999422151199602, -0.033471815469964, 0.005916186331164, - 0.033474965560969, 0.999439461971174, -0.000434206705656, - -0.005898336397012, 0.000631999933520, 0.999982404947123); - Rot3 R3Expected = Rot3(M3); + Rot3 R3Expected = matlabValues->at(4).rotation(); EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-4)); } From 661862f3e38f41dde28b093fddcbe576f728feec Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Sep 2014 12:08:42 -0400 Subject: [PATCH 10/44] small comment --- examples/Pose3SLAMExample_g2o.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index ddb900901..c2acb8091 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) { std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; - params.setVerbosity("TERMINATION"); + params.setVerbosity("TERMINATION"); // this will show info about stopping conditions GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; From 3ad83e6394d4293008d87e1ae292376512ac4f66 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Sep 2014 20:54:46 -0400 Subject: [PATCH 11/44] small improvements --- examples/Pose3SLAMExample_changeKeys.cpp | 25 ++++++++++++++++--- ...ose3SLAMExample_initializePose3Chordal.cpp | 25 ------------------- ...se3SLAMExample_initializePose3Gradient.cpp | 2 +- 3 files changed, 22 insertions(+), 30 deletions(-) diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 1e28c2097..ae4ee5a8c 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -12,7 +12,7 @@ /** * @file Pose3SLAMExample_initializePose3.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o rewritted.g2o + * Syntax for the script is ./Pose3SLAMExample_changeKeys input.g2o rewritted.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -40,9 +40,15 @@ int main(const int argc, const char *argv[]) { bool is3D = true; boost::tie(graph, initial) = readG2o(g2oFile, is3D); + bool add = false; Key firstKey = 8646911284551352320; std::cout << "Using reference key: " << firstKey << std::endl; + if(add) + std::cout << "adding key " << std::endl; + else + std::cout << "subtracting key " << std::endl; + if (argc < 3) { std::cout << "Please provide output file to write " << std::endl; @@ -52,7 +58,12 @@ int main(const int argc, const char *argv[]) { // Additional: rewrite input with simplified keys 0,1,... Values simpleInitial; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { - Key key = key_value.key + firstKey; + Key key; + if(add) + key = key_value.key + firstKey; + else + key = key_value.key - firstKey; + simpleInitial.insert(key, initial->at(key_value.key)); } NonlinearFactorGraph simpleGraph; @@ -60,8 +71,14 @@ int main(const int argc, const char *argv[]) { boost::shared_ptr > pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ - Key key1 = pose3Between->key1() + firstKey; - Key key2 = pose3Between->key2() + firstKey; + Key key1, key2; + if(add){ + key1 = pose3Between->key1() + firstKey; + key2 = pose3Between->key2() + firstKey; + }else{ + key1 = pose3Between->key1() - firstKey; + key2 = pose3Between->key2() - firstKey; + } NonlinearFactor::shared_ptr simpleFactor( new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); simpleGraph.add(simpleFactor); diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index e52534b93..afc66ea1e 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -63,31 +63,6 @@ int main(const int argc, const char *argv[]) { std::cout << "Writing results to file: " << outputFile << std::endl; writeG2o(*graph, initialization, outputFile); std::cout << "done! " << std::endl; - - // This should be deleted: only for debug - // if (argc == 4){ - // const string inputFileRewritten = argv[3]; - // std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; - // // Additional: rewrite input with simplified keys 0,1,... - // Values simpleInitial; - // BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { - // Key key = key_value.key - firstKey; - // simpleInitial.insert(key, initial->at(key_value.key)); - // } - // NonlinearFactorGraph simpleGraph; - // BOOST_FOREACH(const boost::shared_ptr& factor, *graph) { - // boost::shared_ptr > pose3Between = - // boost::dynamic_pointer_cast >(factor); - // if (pose3Between){ - // Key key1 = pose3Between->key1() - firstKey; - // Key key2 = pose3Between->key2() - firstKey; - // NonlinearFactor::shared_ptr simpleFactor( - // new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); - // simpleGraph.add(simpleFactor); - // } - // } - // writeG2o(simpleGraph, simpleInitial, inputFileRewritten); - // } } return 0; } diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 898986b0a..9dc410692 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the first key NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 0.1, 0.1, 0.1, 0.01, 0.01, 0.01)); + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); Key firstKey = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { std::cout << "Adding prior to g2o file " << std::endl; From 7a4f3073422b898dfb9c49df6e9a66ef33cce92d Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 6 Oct 2014 13:11:27 -0400 Subject: [PATCH 12/44] moved biasedGPS and GaussMarkov to unstable --- {gtsam => gtsam_unstable}/slam/BiasedGPSFactor.h | 0 {gtsam => gtsam_unstable}/slam/GaussMarkov1stOrderFactor.h | 0 {gtsam => gtsam_unstable}/slam/tests/testBiasedGPSFactor.cpp | 2 +- .../slam/tests/testGaussMarkov1stOrderFactor.cpp | 2 +- 4 files changed, 2 insertions(+), 2 deletions(-) rename {gtsam => gtsam_unstable}/slam/BiasedGPSFactor.h (100%) rename {gtsam => gtsam_unstable}/slam/GaussMarkov1stOrderFactor.h (100%) rename {gtsam => gtsam_unstable}/slam/tests/testBiasedGPSFactor.cpp (98%) rename {gtsam => gtsam_unstable}/slam/tests/testGaussMarkov1stOrderFactor.cpp (98%) diff --git a/gtsam/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h similarity index 100% rename from gtsam/slam/BiasedGPSFactor.h rename to gtsam_unstable/slam/BiasedGPSFactor.h diff --git a/gtsam/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h similarity index 100% rename from gtsam/slam/GaussMarkov1stOrderFactor.h rename to gtsam_unstable/slam/GaussMarkov1stOrderFactor.h diff --git a/gtsam/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp similarity index 98% rename from gtsam/slam/tests/testBiasedGPSFactor.cpp rename to gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index d907852c5..38e8a1466 100644 --- a/gtsam/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include using namespace gtsam; diff --git a/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp similarity index 98% rename from gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp rename to gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index d5a3ed8f7..c708c1949 100644 --- a/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include From 7c8716f746a235ec61192e1f1057612b09507247 Mon Sep 17 00:00:00 2001 From: Lachlan Toohey Date: Tue, 7 Oct 2014 11:29:09 +1100 Subject: [PATCH 13/44] Fixed PoseRotationPrior error calculation. Use measured_.localCoordinates not difference between Logmap values to handle wraparound properly. --- gtsam/slam/PoseRotationPrior.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 7834f235f..7d31e2e2c 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -77,7 +77,7 @@ public: (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); } - return Rotation::Logmap(newR) - Rotation::Logmap(measured_); + return measured_.localCoordinates(newR); } private: From 3c42fbacb97a07182d470c0157d599abf49fcfe0 Mon Sep 17 00:00:00 2001 From: Lachlan Toohey Date: Thu, 9 Oct 2014 14:13:24 +1100 Subject: [PATCH 14/44] Update tests for testPoseRotationPrior Adds new test for wraparound handling in Rot2. Removes tests that are not quite equal for Rot3. --- gtsam/slam/tests/testPoseRotationPrior.cpp | 24 +++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index b40da2e2a..987f8619b 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -35,6 +35,7 @@ const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3) // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); +const Rot2 rot2C = Rot2::fromAngle(M_PI-0.01), rot2D = Rot2::fromAngle(M_PI+0.01); /* ************************************************************************* */ LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { @@ -61,11 +62,17 @@ TEST( testPoseRotationFactor, level3_zero_error ) { TEST( testPoseRotationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3C, model3); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) Matrix actH1; - EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << 0.1,0.2,0.3), factor.evaluateError(pose1, actH1))); + // the derivative is more complex, but is close to the identity for Rot3 around the origin + /* Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); - EXPECT(assert_equal(expH1, actH1, tol)); + boost::bind(evalFactorError3, factor, _1), pose1, 1e-2); + EXPECT(assert_equal(expH1, actH1, tol));*/ +#else + // If not using true expmap will be close, but not exact around the origin +#endif } /* ************************************************************************* */ @@ -90,6 +97,17 @@ TEST( testPoseRotationFactor, level2_error ) { EXPECT(assert_equal(expH1, actH1, tol)); } +/* ************************************************************************* */ +TEST( testPoseRotationFactor, level2_error_wrap ) { + Pose2 pose1(rot2C, point2A); + Pose2RotationPrior factor(poseKey, rot2D, model1); + Matrix actH1; + EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From fdf1dfffa85f1b795cc556a06a23962dd120aecb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 12 Oct 2014 18:21:17 +0200 Subject: [PATCH 15/44] Now in nanoseconds, and added timing of localCoordinates. --- timing/timePinholeCamera.cpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 8e3e12e22..2113ad56d 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -26,7 +26,7 @@ using namespace gtsam; int main() { - int n = 1000000; + int n = 1e6; const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., @@ -35,8 +35,6 @@ int main() ), Point3(0,0,0.5)); -// static Cal3_S2 K(500, 100, 0.1, 320, 240); -// static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); static Cal3Bundler K(500, 1e-3, 2.0*1e-3); const PinholeCamera camera(pose1,K); const Point3 point1(-0.08,-0.08, 0.0); @@ -63,8 +61,18 @@ int main() camera.project(point1); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; - cout << ((double)n/seconds) << " calls/second" << endl; - cout << ((double)seconds*1000000/n) << " musecs/call" << endl; + cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; + } + + // Oct 12 2014, Macbook Air + { + long timeLog = clock(); + Point2 measurement(0,0); + for(int i = 0; i < n; i++) + measurement.localCoordinates(camera.project(point1)); + long timeLog2 = clock(); + double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; } // Oct 12 2013, iMac 3.06GHz Core i3 @@ -84,8 +92,7 @@ int main() camera.project(point1, Dpose, Dpoint); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; - cout << ((double)n/seconds) << " calls/second" << endl; - cout << ((double)seconds*1000000/n) << " musecs/call" << endl; + cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; } // Oct 12 2013, iMac 3.06GHz Core i3 @@ -97,7 +104,7 @@ int main() // Cal3Bundler fix: 2.0946 musecs/call // June 24 2014, Macbook Pro 2.3GHz Core i7 // GTSAM 3.1: 0.2294 musecs/call - // After project fix: 0.2093 musecs/call + // After project fix: 0.2093 nanosecs/call { Matrix Dpose, Dpoint, Dcal; long timeLog = clock(); @@ -105,8 +112,7 @@ int main() camera.project(point1, Dpose, Dpoint, Dcal); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; - cout << ((double)n/seconds) << " calls/second" << endl; - cout << ((double)seconds*1000000/n) << " musecs/call" << endl; + cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; } return 0; From 5c68e9e755eed220ec9caddcda905fb95f4ca572 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 13 Oct 2014 15:48:17 -0400 Subject: [PATCH 16/44] silence unused variable warning on Mac --- gtsam/geometry/Unit3.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 3a07b8b98..4e643fde9 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -21,7 +21,16 @@ #include #include #include + +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wunused-variable" +#endif #include +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + #include #include From 2560ddef990bfe5ee2699f7adb78c612356568bc Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 14 Oct 2014 00:04:28 -0400 Subject: [PATCH 17/44] Fix building with MKL on Mac OS --- cmake/FindMKL.cmake | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index 44681f7b8..8fb3be25d 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -137,13 +137,16 @@ ELSE() # UNIX and macOS ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} ${MKL_ROOT_DIR}/lib/ ) - - FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY - mkl_gnu_thread - PATHS - ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} - ${MKL_ROOT_DIR}/lib/ - ) + + # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above) + IF(NOT APPLE) + FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY + mkl_gnu_thread + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() # Intel Libraries IF("${MKL_ARCH_DIR}" STREQUAL "32") @@ -227,7 +230,12 @@ ELSE() # UNIX and macOS endforeach() endforeach() - SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) + IF(APPLE) + SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES}) + ELSE() + SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) + ENDIF() + MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY) ENDIF() From fcc49bd22ae925f9ae22581830a6fb345e51cf25 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 14 Oct 2014 16:30:13 -0400 Subject: [PATCH 18/44] Fix for nasty intermittent TBB crashes in testEssentialMatrixFactor and testGeneralSFMFactor. Fixes issue 93 --- gtsam/geometry/Unit3.cpp | 12 ++++++------ gtsam/geometry/Unit3.h | 7 +++++-- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 4e643fde9..82ba979fd 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -67,11 +67,11 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -const Matrix& Unit3::basis() const { +const Unit3::Matrix32& Unit3::basis() const { // Return cached version if exists - if (B_.rows() == 3) - return B_; + if (B_) + return *B_; // Get the axis of rotation with the minimum projected length of the point Point3 axis; @@ -92,9 +92,9 @@ const Matrix& Unit3::basis() const { b2 = b2 / b2.norm(); // Create the basis matrix - B_ = Matrix(3, 2); - B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - return B_; + B_.reset(Unit3::Matrix32()); + (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + return *B_; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8d2c024c0..23dc535e8 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -23,6 +23,7 @@ #include #include #include +#include namespace gtsam { @@ -31,8 +32,10 @@ class GTSAM_EXPORT Unit3: public DerivedValue { private: + typedef Eigen::Matrix Matrix32; + Point3 p_; ///< The location of the point on the unit sphere - mutable Matrix B_; ///< Cached basis + mutable boost::optional B_; ///< Cached basis public: @@ -84,7 +87,7 @@ public: * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions * tangent to the sphere at the current direction. */ - const Matrix& basis() const; + const Matrix32& basis() const; /// Return skew-symmetric associated with 3D point on unit sphere Matrix skew() const; From 799beec7e28737c73b2932dac77c4fe44ad126e8 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 14 Oct 2014 17:00:03 -0400 Subject: [PATCH 19/44] fix signed/unsigned warnings --- gtsam/slam/dataset.cpp | 4 ++-- gtsam/slam/dataset.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index db85c65bf..607819cc7 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -163,7 +163,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, } /* ************************************************************************* */ -GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, +GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { @@ -211,7 +211,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, } // Parse the pose constraints - int id1, id2; + Key id1, id2; bool haveLandmark = false; while (!is.eof()) { if (!(is >> tag)) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index c27419a6e..09a5baac7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -94,7 +94,7 @@ GTSAM_EXPORT GraphAndValues load2D( * @return graph and initial values */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise = + SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise = false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); From b4c62969d160f6c7b7174816d84cc39ab2be9e3f Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 13:30:43 -0400 Subject: [PATCH 20/44] move calculation of Cal3DS2 to base class, all unit tests passed and hopefully fix issue of DerivedValue --- gtsam/geometry/Cal3DS2.cpp | 146 +------------------------ gtsam/geometry/Cal3DS2.h | 82 ++------------ gtsam/geometry/Cal3DS2_Base.cpp | 187 ++++++++++++++++++++++++++++++++ gtsam/geometry/Cal3DS2_Base.h | 158 +++++++++++++++++++++++++++ gtsam/geometry/Cal3Unified.h | 12 +- 5 files changed, 361 insertions(+), 224 deletions(-) create mode 100644 gtsam/geometry/Cal3DS2_Base.cpp create mode 100644 gtsam/geometry/Cal3DS2_Base.h diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index c75eff022..044d47de1 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -23,24 +23,9 @@ namespace gtsam { -/* ************************************************************************* */ -Cal3DS2::Cal3DS2(const Vector &v): - fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} - -/* ************************************************************************* */ -Matrix Cal3DS2::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); -} - -/* ************************************************************************* */ -Vector Cal3DS2::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); -} - /* ************************************************************************* */ void Cal3DS2::print(const std::string& s_) const { - gtsam::print(K(), s_ + ".K"); - gtsam::print(Vector(k()), s_ + ".k"); + Base::print(s_); } /* ************************************************************************* */ @@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { return true; } -/* ************************************************************************* */ -static Eigen::Matrix D2dcalibration(double x, double y, double xx, - double yy, double xy, double rr, double r4, double pnx, double pny, - const Eigen::Matrix& DK) { - Eigen::Matrix DR1; - DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; - Eigen::Matrix DR2; - DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // - y * rr, y * r4, rr + 2 * yy, 2 * xy; - Eigen::Matrix D; - D << DR1, DK * DR2; - return D; -} - -/* ************************************************************************* */ -static Eigen::Matrix D2dintrinsic(double x, double y, double rr, - double g, double k1, double k2, double p1, double p2, - const Eigen::Matrix& DK) { - const double drdx = 2. * x; - const double drdy = 2. * y; - const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; - const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; - - // Dx = 2*p1*xy + p2*(rr+2*xx); - // Dy = 2*p2*xy + p1*(rr+2*yy); - const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); - const double dDxdy = 2. * p1 * x + p2 * drdy; - const double dDydx = 2. * p2 * y + p1 * drdx; - const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - - Eigen::Matrix DR; - DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // - y * dgdx + dDydx, g + y * dgdy + dDydy; - - return DK * DR; -} - -/* ************************************************************************* */ -Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { - - // rr = x^2 + y^2; - // g = (1 + k(1)*rr + k(2)*rr^2); - // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; - // pi(:,i) = g * pn(:,i) + dp; - const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; - const double rr = xx + yy; - const double r4 = rr * rr; - const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor - - // tangential component - const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); - const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); - - // Radial and tangential distortion applied - const double pnx = g * x + dx; - const double pny = g * y + dy; - - Eigen::Matrix DK; - if (H1 || H2) DK << fx_, s_, 0.0, fy_; - - // Derivative for calibration - if (H1) - *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); - - // Derivative for points - if (H2) - *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); - - // Regular uncalibrate after distortion - return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { - // Use the following fixed point iteration to invert the radial distortion. - // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) - - const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), - (1 / fy_) * (pi.y() - v0_)); - - // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; - - // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 10; - int iteration; - for (iteration = 0; iteration < maxIterations; ++iteration) { - if (uncalibrate(pn).distance(pi) <= tol) break; - const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); - const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); - pn = (invKPi - Point2(dx, dy)) / g; - } - - if ( iteration >= maxIterations ) - throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); - - return pn; -} - -/* ************************************************************************* */ -Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; - const double rr = xx + yy; - const double r4 = rr * rr; - const double g = (1 + k1_ * rr + k2_ * r4); - Eigen::Matrix DK; - DK << fx_, s_, 0.0, fy_; - return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); -} - -/* ************************************************************************* */ -Matrix Cal3DS2::D2d_calibration(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; - const double rr = xx + yy; - const double r4 = rr * rr; - const double g = (1 + k1_ * rr + k2_ * r4); - const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); - const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); - const double pnx = g * x + dx; - const double pny = g * y + dy; - Eigen::Matrix DK; - DK << fx_, s_, 0.0, fy_; - return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); -} - /* ************************************************************************* */ Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 51fe958d6..ad8080fa3 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -37,34 +37,27 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3DS2 : public DerivedValue { +class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue { -protected: - - double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point - double k1_, k2_ ; // radial 2nd-order and 4th-order - double p1_, p2_ ; // tangential distortion + typedef Cal3DS2_Base Base; public: - Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } - Vector vector() const ; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} + Cal3DS2() : Base() {} Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1 = 0.0, double p2 = 0.0) : - fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {} /// @} /// @name Advanced Constructors /// @{ - Cal3DS2(const Vector &v) ; + Cal3DS2(const Vector &v) : Base(v) {} /// @} /// @name Testable @@ -76,57 +69,6 @@ public: /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; - /// @} - /// @name Standard Interface - /// @{ - - /// focal length x - inline double fx() const { return fx_;} - - /// focal length x - inline double fy() const { return fy_;} - - /// skew - inline double skew() const { return s_;} - - /// image center in x - inline double px() const { return u0_;} - - /// image center in y - inline double py() const { return v0_;} - - /// First distortion coefficient - inline double k1() const { return k1_;} - - /// Second distortion coefficient - inline double k2() const { return k2_;} - - /// First tangential distortion coefficient - inline double p1() const { return p1_;} - - /// Second tangential distortion coefficient - inline double p2() const { return p2_;} - - /** - * convert intrinsic coordinates xy to (distorted) image coordinates uv - * @param p point in intrinsic coordinates - * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in (distorted) image coordinates - */ - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; - - /// Convert (distorted) image coordinates uv to intrinsic coordinates xy - Point2 calibrate(const Point2& p, const double tol=1e-5) const; - - /// Derivative of uncalibrate wrpt intrinsic coordinates - Matrix D2d_intrinsic(const Point2& p) const ; - - /// Derivative of uncalibrate wrpt the calibration parameters - Matrix D2d_calibration(const Point2& p) const ; - /// @} /// @name Manifold /// @{ @@ -156,18 +98,10 @@ private: { ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - ar & BOOST_SERIALIZATION_NVP(k1_); - ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(p1_); - ar & BOOST_SERIALIZATION_NVP(p2_); + ar & boost::serialization::make_nvp("Cal3DS2", + boost::serialization::base_object(*this)); } - /// @} }; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp new file mode 100644 index 000000000..b8181ab4d --- /dev/null +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -0,0 +1,187 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_Base.cpp + * @date Feb 28, 2010 + * @author ydjian + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3DS2_Base::Cal3DS2_Base(const Vector &v): + fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} + +/* ************************************************************************* */ +Vector Cal3DS2_Base::vector() const { + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); +} + +/* ************************************************************************* */ +void Cal3DS2_Base::print(const std::string& s_) const { + gtsam::print(K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); +} + +/* ************************************************************************* */ +bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || + fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +static Eigen::Matrix D2dcalibration(double x, double y, double xx, + double yy, double xy, double rr, double r4, double pnx, double pny, + const Eigen::Matrix& DK) { + Eigen::Matrix DR1; + DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; + Eigen::Matrix DR2; + DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // + y * rr, y * r4, rr + 2 * yy, 2 * xy; + Eigen::Matrix D; + D << DR1, DK * DR2; + return D; +} + +/* ************************************************************************* */ +static Eigen::Matrix D2dintrinsic(double x, double y, double rr, + double g, double k1, double k2, double p1, double p2, + const Eigen::Matrix& DK) { + const double drdx = 2. * x; + const double drdy = 2. * y; + const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; + const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; + + // Dx = 2*p1*xy + p2*(rr+2*xx); + // Dy = 2*p2*xy + p1*(rr+2*yy); + const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); + const double dDxdy = 2. * p1 * x + p2 * drdy; + const double dDydx = 2. * p2 * y + p1 * drdx; + const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); + + Eigen::Matrix DR; + DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy; + + return DK * DR; +} + +/* ************************************************************************* */ +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, + boost::optional H2) const { + + // rr = x^2 + y^2; + // g = (1 + k(1)*rr + k(2)*rr^2); + // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; + // pi(:,i) = g * pn(:,i) + dp; + const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor + + // tangential component + const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); + const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); + + // Radial and tangential distortion applied + const double pnx = g * x + dx; + const double pny = g * y + dy; + + Eigen::Matrix DK; + if (H1 || H2) DK << fx_, s_, 0.0, fy_; + + // Derivative for calibration + if (H1) + *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); + + // Derivative for points + if (H2) + *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); + + // Regular uncalibrate after distortion + return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { + // Use the following fixed point iteration to invert the radial distortion. + // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) + + const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), + (1 / fy_) * (pi.y() - v0_)); + + // initialize by ignoring the distortion at all, might be problematic for pixels around boundary + Point2 pn = invKPi; + + // iterate until the uncalibrate is close to the actual pixel coordinate + const int maxIterations = 10; + int iteration; + for (iteration = 0; iteration < maxIterations; ++iteration) { + if (uncalibrate(pn).distance(pi) <= tol) break; + const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + pn = (invKPi - Point2(dx, dy)) / g; + } + + if ( iteration >= maxIterations ) + throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + + return pn; +} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); +} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + const double pnx = g * x + dx; + const double pny = g * y + dy; + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h new file mode 100644 index 000000000..7be5a6874 --- /dev/null +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2.h + * @brief Calibration of a camera with radial distortion + * @date Feb 28, 2010 + * @author ydjian + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Calibration of a camera with radial distortion + * @addtogroup geometry + * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + * but using only k1,k2,p1, and p2 coefficients. + * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + * rr = Pn.x^2 + Pn.y^2 + * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + * pi = K*pn + */ +class GTSAM_EXPORT Cal3DS2_Base { + +protected: + + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point + double k1_, k2_ ; // radial 2nd-order and 4th-order + double p1_, p2_ ; // tangential distortion + +public: + Matrix K() const ; + Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} + + Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double p1 = 0.0, double p2 = 0.0) : + fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3DS2_Base(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length x + inline double fx() const { return fx_;} + + /// focal length x + inline double fy() const { return fy_;} + + /// skew + inline double skew() const { return s_;} + + /// image center in x + inline double px() const { return u0_;} + + /// image center in y + inline double py() const { return v0_;} + + /// First distortion coefficient + inline double k1() const { return k1_;} + + /// Second distortion coefficient + inline double k2() const { return k2_;} + + /// First tangential distortion coefficient + inline double p1() const { return p1_;} + + /// Second tangential distortion coefficient + inline double p2() const { return p2_;} + + /** + * convert intrinsic coordinates xy to (distorted) image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in (distorted) image coordinates + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// Convert (distorted) image coordinates uv to intrinsic coordinates xy + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Derivative of uncalibrate wrpt intrinsic coordinates + Matrix D2d_intrinsic(const Point2& p) const ; + + /// Derivative of uncalibrate wrpt the calibration parameters + Matrix D2d_calibration(const Point2& p) const ; + + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(k1_); + ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(p1_); + ar & BOOST_SERIALIZATION_NVP(p2_); + } + + /// @} + +}; + +} + diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 58e024c27..750d2ed73 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -22,8 +22,8 @@ #pragma once -#include -#include +#include +#include namespace gtsam { @@ -40,10 +40,10 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue { typedef Cal3Unified This; - typedef Cal3DS2 Base; + typedef Cal3DS2_Base Base; private: @@ -135,7 +135,9 @@ private: void serialize(Archive & ar, const unsigned int version) { ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Cal3Unified", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); } From 55bda50235607bdecfaea46f686a3902278449a8 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 13:30:43 -0400 Subject: [PATCH 21/44] move calculation of Cal3DS2 to base class, all unit tests passed and hopefully fix issue of DerivedValue --- gtsam/geometry/Cal3DS2.cpp | 146 +------------------------ gtsam/geometry/Cal3DS2.h | 84 ++------------ gtsam/geometry/Cal3DS2_Base.cpp | 187 ++++++++++++++++++++++++++++++++ gtsam/geometry/Cal3DS2_Base.h | 157 +++++++++++++++++++++++++++ gtsam/geometry/Cal3Unified.h | 12 +- 5 files changed, 361 insertions(+), 225 deletions(-) create mode 100644 gtsam/geometry/Cal3DS2_Base.cpp create mode 100644 gtsam/geometry/Cal3DS2_Base.h diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index c75eff022..044d47de1 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -23,24 +23,9 @@ namespace gtsam { -/* ************************************************************************* */ -Cal3DS2::Cal3DS2(const Vector &v): - fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} - -/* ************************************************************************* */ -Matrix Cal3DS2::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); -} - -/* ************************************************************************* */ -Vector Cal3DS2::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); -} - /* ************************************************************************* */ void Cal3DS2::print(const std::string& s_) const { - gtsam::print(K(), s_ + ".K"); - gtsam::print(Vector(k()), s_ + ".k"); + Base::print(s_); } /* ************************************************************************* */ @@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { return true; } -/* ************************************************************************* */ -static Eigen::Matrix D2dcalibration(double x, double y, double xx, - double yy, double xy, double rr, double r4, double pnx, double pny, - const Eigen::Matrix& DK) { - Eigen::Matrix DR1; - DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; - Eigen::Matrix DR2; - DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // - y * rr, y * r4, rr + 2 * yy, 2 * xy; - Eigen::Matrix D; - D << DR1, DK * DR2; - return D; -} - -/* ************************************************************************* */ -static Eigen::Matrix D2dintrinsic(double x, double y, double rr, - double g, double k1, double k2, double p1, double p2, - const Eigen::Matrix& DK) { - const double drdx = 2. * x; - const double drdy = 2. * y; - const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; - const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; - - // Dx = 2*p1*xy + p2*(rr+2*xx); - // Dy = 2*p2*xy + p1*(rr+2*yy); - const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); - const double dDxdy = 2. * p1 * x + p2 * drdy; - const double dDydx = 2. * p2 * y + p1 * drdx; - const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - - Eigen::Matrix DR; - DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // - y * dgdx + dDydx, g + y * dgdy + dDydy; - - return DK * DR; -} - -/* ************************************************************************* */ -Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { - - // rr = x^2 + y^2; - // g = (1 + k(1)*rr + k(2)*rr^2); - // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; - // pi(:,i) = g * pn(:,i) + dp; - const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; - const double rr = xx + yy; - const double r4 = rr * rr; - const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor - - // tangential component - const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); - const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); - - // Radial and tangential distortion applied - const double pnx = g * x + dx; - const double pny = g * y + dy; - - Eigen::Matrix DK; - if (H1 || H2) DK << fx_, s_, 0.0, fy_; - - // Derivative for calibration - if (H1) - *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); - - // Derivative for points - if (H2) - *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); - - // Regular uncalibrate after distortion - return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { - // Use the following fixed point iteration to invert the radial distortion. - // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) - - const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), - (1 / fy_) * (pi.y() - v0_)); - - // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; - - // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 10; - int iteration; - for (iteration = 0; iteration < maxIterations; ++iteration) { - if (uncalibrate(pn).distance(pi) <= tol) break; - const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); - const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); - pn = (invKPi - Point2(dx, dy)) / g; - } - - if ( iteration >= maxIterations ) - throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); - - return pn; -} - -/* ************************************************************************* */ -Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; - const double rr = xx + yy; - const double r4 = rr * rr; - const double g = (1 + k1_ * rr + k2_ * r4); - Eigen::Matrix DK; - DK << fx_, s_, 0.0, fy_; - return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); -} - -/* ************************************************************************* */ -Matrix Cal3DS2::D2d_calibration(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; - const double rr = xx + yy; - const double r4 = rr * rr; - const double g = (1 + k1_ * rr + k2_ * r4); - const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); - const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); - const double pnx = g * x + dx; - const double pny = g * y + dy; - Eigen::Matrix DK; - DK << fx_, s_, 0.0, fy_; - return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); -} - /* ************************************************************************* */ Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 51fe958d6..dbd50f450 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -11,7 +11,7 @@ /** * @file Cal3DS2.h - * @brief Calibration of a camera with radial distortion + * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base * @date Feb 28, 2010 * @author ydjian */ @@ -19,7 +19,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -37,34 +37,27 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3DS2 : public DerivedValue { +class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue { -protected: - - double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point - double k1_, k2_ ; // radial 2nd-order and 4th-order - double p1_, p2_ ; // tangential distortion + typedef Cal3DS2_Base Base; public: - Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } - Vector vector() const ; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} + Cal3DS2() : Base() {} Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1 = 0.0, double p2 = 0.0) : - fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {} /// @} /// @name Advanced Constructors /// @{ - Cal3DS2(const Vector &v) ; + Cal3DS2(const Vector &v) : Base(v) {} /// @} /// @name Testable @@ -76,57 +69,6 @@ public: /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; - /// @} - /// @name Standard Interface - /// @{ - - /// focal length x - inline double fx() const { return fx_;} - - /// focal length x - inline double fy() const { return fy_;} - - /// skew - inline double skew() const { return s_;} - - /// image center in x - inline double px() const { return u0_;} - - /// image center in y - inline double py() const { return v0_;} - - /// First distortion coefficient - inline double k1() const { return k1_;} - - /// Second distortion coefficient - inline double k2() const { return k2_;} - - /// First tangential distortion coefficient - inline double p1() const { return p1_;} - - /// Second tangential distortion coefficient - inline double p2() const { return p2_;} - - /** - * convert intrinsic coordinates xy to (distorted) image coordinates uv - * @param p point in intrinsic coordinates - * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in (distorted) image coordinates - */ - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; - - /// Convert (distorted) image coordinates uv to intrinsic coordinates xy - Point2 calibrate(const Point2& p, const double tol=1e-5) const; - - /// Derivative of uncalibrate wrpt intrinsic coordinates - Matrix D2d_intrinsic(const Point2& p) const ; - - /// Derivative of uncalibrate wrpt the calibration parameters - Matrix D2d_calibration(const Point2& p) const ; - /// @} /// @name Manifold /// @{ @@ -156,18 +98,10 @@ private: { ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - ar & BOOST_SERIALIZATION_NVP(k1_); - ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(p1_); - ar & BOOST_SERIALIZATION_NVP(p2_); + ar & boost::serialization::make_nvp("Cal3DS2", + boost::serialization::base_object(*this)); } - /// @} }; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp new file mode 100644 index 000000000..b8181ab4d --- /dev/null +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -0,0 +1,187 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_Base.cpp + * @date Feb 28, 2010 + * @author ydjian + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3DS2_Base::Cal3DS2_Base(const Vector &v): + fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} + +/* ************************************************************************* */ +Vector Cal3DS2_Base::vector() const { + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); +} + +/* ************************************************************************* */ +void Cal3DS2_Base::print(const std::string& s_) const { + gtsam::print(K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); +} + +/* ************************************************************************* */ +bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || + fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +static Eigen::Matrix D2dcalibration(double x, double y, double xx, + double yy, double xy, double rr, double r4, double pnx, double pny, + const Eigen::Matrix& DK) { + Eigen::Matrix DR1; + DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; + Eigen::Matrix DR2; + DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // + y * rr, y * r4, rr + 2 * yy, 2 * xy; + Eigen::Matrix D; + D << DR1, DK * DR2; + return D; +} + +/* ************************************************************************* */ +static Eigen::Matrix D2dintrinsic(double x, double y, double rr, + double g, double k1, double k2, double p1, double p2, + const Eigen::Matrix& DK) { + const double drdx = 2. * x; + const double drdy = 2. * y; + const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; + const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; + + // Dx = 2*p1*xy + p2*(rr+2*xx); + // Dy = 2*p2*xy + p1*(rr+2*yy); + const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); + const double dDxdy = 2. * p1 * x + p2 * drdy; + const double dDydx = 2. * p2 * y + p1 * drdx; + const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); + + Eigen::Matrix DR; + DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy; + + return DK * DR; +} + +/* ************************************************************************* */ +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, + boost::optional H2) const { + + // rr = x^2 + y^2; + // g = (1 + k(1)*rr + k(2)*rr^2); + // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; + // pi(:,i) = g * pn(:,i) + dp; + const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor + + // tangential component + const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); + const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); + + // Radial and tangential distortion applied + const double pnx = g * x + dx; + const double pny = g * y + dy; + + Eigen::Matrix DK; + if (H1 || H2) DK << fx_, s_, 0.0, fy_; + + // Derivative for calibration + if (H1) + *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); + + // Derivative for points + if (H2) + *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); + + // Regular uncalibrate after distortion + return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { + // Use the following fixed point iteration to invert the radial distortion. + // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) + + const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), + (1 / fy_) * (pi.y() - v0_)); + + // initialize by ignoring the distortion at all, might be problematic for pixels around boundary + Point2 pn = invKPi; + + // iterate until the uncalibrate is close to the actual pixel coordinate + const int maxIterations = 10; + int iteration; + for (iteration = 0; iteration < maxIterations; ++iteration) { + if (uncalibrate(pn).distance(pi) <= tol) break; + const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + pn = (invKPi - Point2(dx, dy)) / g; + } + + if ( iteration >= maxIterations ) + throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + + return pn; +} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); +} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + const double pnx = g * x + dx; + const double pny = g * y + dy; + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h new file mode 100644 index 000000000..fc5f5d7c1 --- /dev/null +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_Base.h + * @brief Base class of calibration of a camera with radial distortion, used for Cal3DS2 and Cal3Unified + * @date Feb 28, 2010 + * @author ydjian + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @brief Calibration of a camera with radial distortion + * @addtogroup geometry + * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + * but using only k1,k2,p1, and p2 coefficients. + * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + * rr = Pn.x^2 + Pn.y^2 + * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + * pi = K*pn + */ +class GTSAM_EXPORT Cal3DS2_Base { + +protected: + + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point + double k1_, k2_ ; // radial 2nd-order and 4th-order + double p1_, p2_ ; // tangential distortion + +public: + Matrix K() const ; + Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} + + Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double p1 = 0.0, double p2 = 0.0) : + fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3DS2_Base(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length x + inline double fx() const { return fx_;} + + /// focal length x + inline double fy() const { return fy_;} + + /// skew + inline double skew() const { return s_;} + + /// image center in x + inline double px() const { return u0_;} + + /// image center in y + inline double py() const { return v0_;} + + /// First distortion coefficient + inline double k1() const { return k1_;} + + /// Second distortion coefficient + inline double k2() const { return k2_;} + + /// First tangential distortion coefficient + inline double p1() const { return p1_;} + + /// Second tangential distortion coefficient + inline double p2() const { return p2_;} + + /** + * convert intrinsic coordinates xy to (distorted) image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in (distorted) image coordinates + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// Convert (distorted) image coordinates uv to intrinsic coordinates xy + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Derivative of uncalibrate wrpt intrinsic coordinates + Matrix D2d_intrinsic(const Point2& p) const ; + + /// Derivative of uncalibrate wrpt the calibration parameters + Matrix D2d_calibration(const Point2& p) const ; + + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(k1_); + ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(p1_); + ar & BOOST_SERIALIZATION_NVP(p2_); + } + + /// @} + +}; + +} + diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 58e024c27..750d2ed73 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -22,8 +22,8 @@ #pragma once -#include -#include +#include +#include namespace gtsam { @@ -40,10 +40,10 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue { typedef Cal3Unified This; - typedef Cal3DS2 Base; + typedef Cal3DS2_Base Base; private: @@ -135,7 +135,9 @@ private: void serialize(Archive & ar, const unsigned int version) { ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Cal3Unified", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); } From cdc121cf7da30408aed14a05f24924ad98c07a7e Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 14:25:42 -0400 Subject: [PATCH 22/44] add in testSerialization --- gtsam/geometry/Cal3Unified.h | 2 +- gtsam/geometry/tests/testSerializationGeometry.cpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 750d2ed73..03db6af9a 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -90,7 +90,7 @@ public: /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates - * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dcal optional 2*10 Jacobian wrpt Cal3Unified parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index 95001a033..a7e792cb8 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,7 @@ static Cal3Bundler cal3(1.0, 2.0, 3.0); static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); static CalibratedCamera cal5(Pose3(rt3, pt3)); +static Cal3Unified cal6(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0); static PinholeCamera cam1(pose3, cal1); static StereoCamera cam2(pose3, cal4ptr); @@ -66,6 +68,7 @@ TEST (Serialization, text_geometry) { EXPECT(equalsObj(cal3)); EXPECT(equalsObj(cal4)); EXPECT(equalsObj(cal5)); + EXPECT(equalsObj(cal6)); EXPECT(equalsObj(cam1)); EXPECT(equalsObj(cam2)); From f258bfe0443e192a6bfc341229dc641f249e139d Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 14:44:37 -0400 Subject: [PATCH 23/44] add DerivedValue test for Cal3Unified --- gtsam/geometry/tests/testCal3Unified.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index b260415f1..de9a8b739 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -19,6 +19,9 @@ #include #include +#include +#include + using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) @@ -97,6 +100,19 @@ TEST( Cal3Unified, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); } +/* ************************************************************************* */ +TEST( Cal3Unified, DerivedValue) +{ + Values values; + Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10); + Key key = 1; + values.insert(key, cal); + + Cal3Unified calafter = values.at(key); + + CHECK(assert_equal(cal,calafter,1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From c10993a69035f147e593d0b4a7421ca93ebdbc4f Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 16 Oct 2014 14:39:09 -0400 Subject: [PATCH 24/44] displaying nr of iterations for verbosity = TERMINATION --- gtsam/nonlinear/NonlinearOptimizer.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b4498bee4..80407a372 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -77,9 +77,11 @@ void NonlinearOptimizer::defaultOptimize() { params.errorTol, currentError, this->error(), params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::TERMINATION && - this->iterations() >= params.maxIterations) - cout << "Terminating because reached maximum iterations" << endl; + if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { + cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl; + if (this->iterations() >= params.maxIterations) + cout << "Terminating because reached maximum iterations" << endl; + } } /* ************************************************************************* */ From e96ceb2b4f6fb86e2d3df29857844feca8a9c566 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 16 Oct 2014 14:39:23 -0400 Subject: [PATCH 25/44] extended example for robust kernels --- examples/Pose2SLAMExample_g2o.cpp | 48 ++++++++++++++++++++++++------- 1 file changed, 37 insertions(+), 11 deletions(-) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 8f1a53a66..8d8f2edc1 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -26,30 +26,53 @@ using namespace std; using namespace gtsam; +// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber) int main(const int argc, const char *argv[]) { - // Read graph from file - string g2oFile; - if (argc < 2) - g2oFile = findExampleDataFile("noisyToyGraph.txt"); - else - g2oFile = argv[1]; + string kernelType = "none"; + int maxIterations = 100; // default + string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default + // Parse user's inputs + if (argc > 1){ + g2oFile = argv[1]; // input dataset filename + // outputFile = g2oFile = argv[2]; // done later + } + if (argc > 3){ + maxIterations = atoi(argv[3]); // user can specify either tukey or huber + } + if (argc > 4){ + kernelType = argv[4]; // user can specify either tukey or huber + } + + // reading file and creating factor graph NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; - boost::tie(graph, initial) = readG2o(g2oFile); + bool is3D = false; + if(kernelType.compare("none") == 0){ + boost::tie(graph, initial) = readG2o(g2oFile,is3D); + } + if(kernelType.compare("huber") == 0){ + std::cout << "Using robust kernel: huber " << std::endl; + boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER); + } + if(kernelType.compare("tukey") == 0){ + std::cout << "Using robust kernel: tukey " << std::endl; + boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY); + } // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + std::cout << "Adding prior on pose 0 " << std::endl; GaussNewtonParams params; params.setVerbosity("TERMINATION"); - if (argc == 4) { - params.maxIterations = atoi(argv[3]); - std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl; + if (argc > 3) { + params.maxIterations = maxIterations; + std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl; } std::cout << "Optimizing the factor graph" << std::endl; @@ -65,7 +88,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, result, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); std::cout << "done! " << std::endl; } return 0; From a61b49dafadca3b795c8d3c5ad3a1714cf5dd239 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Fri, 17 Oct 2014 15:25:40 -0400 Subject: [PATCH 26/44] remove augmented parts in constraint noise model hack --- gtsam/base/Matrix.cpp | 3 +-- gtsam/base/Matrix.h | 1 - gtsam/linear/NoiseModel.cpp | 26 +++++++++----------------- gtsam/linear/NoiseModel.h | 5 ++--- gtsam/nonlinear/NonlinearFactor.h | 13 ++++--------- 5 files changed, 16 insertions(+), 32 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index f51197cf2..dd17c00ef 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...) void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { const DenseIndex m = A.rows(); if (inf_mask) { - // only scale the first v.size() rows of A to support augmented Matrix - for (DenseIndex i=0; i= b.size() - Vector c = a; - for( DenseIndex i = 0; i < b.size(); i++ ) { + size_t n = a.size(); + assert (b.size()==a.size()); + Vector c(n); + for( size_t i = 0; i < n; i++ ) { const double& ai = a(i), &bi = b(i); - if (bi!=0) c(i) = ai/bi; + c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() } return c; } @@ -362,11 +362,7 @@ double Constrained::distance(const Vector& v) const { /* ************************************************************************* */ Matrix Constrained::Whiten(const Matrix& H) const { // selective scaling - // Now allow augmented Matrix with a new additional part coming - // from the Lagrange multiplier. - Matrix M(H.block(0, 0, dim(), H.cols())); - Constrained::WhitenInPlace(M); - return M; + return vector_scale(invsigmas(), H, true); } /* ************************************************************************* */ @@ -375,8 +371,6 @@ void Constrained::WhitenInPlace(Matrix& H) const { // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas) // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0, // indicating a hard constraint, we leave H's row i in place. - // Now allow augmented Matrix with a new additional part coming - // from the Lagrange multiplier. // Inlined: vector_scale_inplace(invsigmas(), H, true); // vector_scale_inplace(v, A, true); for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) { @@ -399,14 +393,12 @@ void Constrained::WhitenInPlace(Eigen::Block H) const { } /* ************************************************************************* */ -Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const { - Vector sigmas = ones(dim()+augmentedDim); +Constrained::shared_ptr Constrained::unit() const { + Vector sigmas = ones(dim()); for (size_t i=0; isigmas_(i) == 0.0) sigmas(i) = 0.0; - Vector augmentedMu = zero(dim()+augmentedDim); - subInsert(augmentedMu, mu_, 0); - return MixedSigmas(augmentedMu, sigmas); + return MixedSigmas(mu_, sigmas); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4b0e4848d..069f2c0aa 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -481,9 +481,8 @@ namespace gtsam { /** * Returns a Unit version of a constrained noisemodel in which * constrained sigmas remain constrained and the rest are unit scaled - * Now support augmented part from the Lagrange multiplier. */ - shared_ptr unit(size_t augmentedDim = 0) const; + shared_ptr unit() const; private: /** Serialization function */ @@ -732,7 +731,7 @@ namespace gtsam { }; /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: - /// Dipl.-Inform. Jan Oberl�nder (M.Sc.), FZI Research Center for + /// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for /// Information Technology, Karlsruhe, Germany. /// oberlaender@fzi.de /// Thanks Jan! diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 1092d8ac2..e43346ca8 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -319,18 +319,13 @@ public: terms[j].second.swap(A[j]); } - // TODO pass unwhitened + noise model to Gaussian factor - // For now, only linearized constrained factors have noise model at linear level!!! if(noiseModel_) { + // TODO pass unwhitened + noise model to Gaussian factor noiseModel::Constrained::shared_ptr constrained = - boost::dynamic_pointer_cast(this->noiseModel_); - if(constrained) { - size_t augmentedDim = terms[0].second.rows() - constrained->dim(); - Vector augmentedZero = zero(augmentedDim); - Vector augmentedb = concatVectors(2, &b, &augmentedZero); - return GaussianFactor::shared_ptr(new JacobianFactor(terms, augmentedb, constrained->unit(augmentedDim))); - } + boost::dynamic_pointer_cast(this->noiseModel_); + if(constrained) + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit())); else return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); } From 5995b20ebe9deb55c540ee2cbc64922b62146d95 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 17 Oct 2014 15:48:20 -0400 Subject: [PATCH 27/44] fixed test --- gtsam/geometry/tests/testPinholeCamera.cpp | 43 ++++++++++++---------- 1 file changed, 24 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 6ed49d0d9..101070940 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { -// Point3 point(point2D.x(), point2D.y(), 1.0); -// return Camera(pose,cal).projectPointAtInfinity(point); -//} -// -//TEST( PinholeCamera, Dproject_Infinity) -//{ -// Matrix Dpose, Dpoint, Dcal; -// Point2 point2D(-0.08,-0.08); -// Point3 point3D(point1.x(), point1.y(), 1.0); -// Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); -// Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); -// Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); -// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K); -// CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); -// CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); -// CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); -//} -// +static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { + return Camera(pose,cal).projectPointAtInfinity(point3D); +} + +TEST( PinholeCamera, Dproject_Infinity) +{ + Matrix Dpose, Dpoint, Dcal; + Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera + + // test Projection + Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Point2 expected(-5.0, 5.0); + CHECK(assert_equal(actual, expected, 1e-7)); + + // test Jacobians + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K); + Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K); + Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); +} + /* ************************************************************************* */ static Point2 project4(const Camera& camera, const Point3& point) { return camera.project2(point); From b0ea3b715b191de629a4f59b47d8f20a86c947bd Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 20 Oct 2014 10:48:50 -0400 Subject: [PATCH 28/44] fixed unit test for implicitSchur factor: Point covariance was not invertible, causing eigen weirdness --- .cproject | 106 ++++---- gtsam/slam/ImplicitSchurFactor.h | 43 +-- gtsam/slam/tests/testImplicitSchurFactor.cpp | 259 +++++++++++++++++++ 3 files changed, 343 insertions(+), 65 deletions(-) create mode 100644 gtsam/slam/tests/testImplicitSchurFactor.cpp diff --git a/.cproject b/.cproject index 46b623bb9..e08e9bc62 100644 --- a/.cproject +++ b/.cproject @@ -582,6 +582,7 @@ make + tests/testBayesTree.run true false @@ -589,6 +590,7 @@ make + testBinaryBayesNet.run true false @@ -636,6 +638,7 @@ make + testSymbolicBayesNet.run true false @@ -643,6 +646,7 @@ make + tests/testSymbolicFactor.run true false @@ -650,6 +654,7 @@ make + testSymbolicFactorGraph.run true false @@ -665,6 +670,7 @@ make + tests/testBayesTree true false @@ -1000,6 +1006,7 @@ make + testErrors.run true false @@ -1229,6 +1236,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1311,7 +1358,6 @@ make - testSimulated2DOriented.run true false @@ -1351,7 +1397,6 @@ make - testSimulated2D.run true false @@ -1359,7 +1404,6 @@ make - testSimulated3D.run true false @@ -1373,46 +1417,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1670,6 +1674,7 @@ cpack + -G DEB true false @@ -1677,6 +1682,7 @@ cpack + -G RPM true false @@ -1684,6 +1690,7 @@ cpack + -G TGZ true false @@ -1691,6 +1698,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2417,6 +2425,7 @@ make + testGraph.run true false @@ -2424,6 +2433,7 @@ make + testJunctionTree.run true false @@ -2431,6 +2441,7 @@ make + testSymbolicBayesNetB.run true false @@ -2636,6 +2647,14 @@ true true + + make + -j5 + testImplicitSchurFactor.run + true + true + true + make -j2 @@ -2910,7 +2929,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/slam/ImplicitSchurFactor.h b/gtsam/slam/ImplicitSchurFactor.h index c0f339b30..8c6d8f1ff 100644 --- a/gtsam/slam/ImplicitSchurFactor.h +++ b/gtsam/slam/ImplicitSchurFactor.h @@ -81,7 +81,7 @@ public: } /// Get matrix P - inline const Matrix& getPointCovariance() const { + inline const Matrix3& getPointCovariance() const { return PointCovariance_; } @@ -286,26 +286,27 @@ public: return 0.5 * (result + f); } - /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) - // This is wrong and does not match the definition in Hessian - // virtual double error(const VectorValues& x) const { - // - // // resize does not do malloc if correct size - // e1.resize(size()); - // e2.resize(size()); - // - // // e1 = F * x - b = (2m*dm)*dm - // for (size_t k = 0; k < size(); ++k) - // e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); - // projectError(e1, e2); - // - // double result = 0; - // for (size_t k = 0; k < size(); ++k) - // result += dot(e2[k], e2[k]); - // - // std::cout << "implicitFactor::error result " << result << std::endl; - // return 0.5 * result; - // } + // needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + // This is wrong and does not match the definition in Hessian, + // but it matches the definition of the Jacobian factor (JF) + double errorJF(const VectorValues& x) const { + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x - b = (2m*dm)*dm + for (size_t k = 0; k < size(); ++k) + e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + projectError(e1, e2); + + double result = 0; + for (size_t k = 0; k < size(); ++k) + result += dot(e2[k], e2[k]); + + // std::cout << "implicitFactor::error result " << result << std::endl; + return 0.5 * result; + } /** * @brief Calculate corrected error Q*e = (I - E*P*E')*e */ diff --git a/gtsam/slam/tests/testImplicitSchurFactor.cpp b/gtsam/slam/tests/testImplicitSchurFactor.cpp new file mode 100644 index 000000000..77faaacc1 --- /dev/null +++ b/gtsam/slam/tests/testImplicitSchurFactor.cpp @@ -0,0 +1,259 @@ +/** + * @file testImplicitSchurFactor.cpp + * @brief unit test implicit jacobian factors + * @author Frank Dellaert + * @date Oct 20, 2013 + */ + +//#include +#include +//#include +#include +//#include "gtsam_unstable/slam/JacobianFactorQR.h" +#include "gtsam/slam/JacobianFactorQR.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +// F +typedef Eigen::Matrix Matrix26; +const Matrix26 F0 = Matrix26::Ones(); +const Matrix26 F1 = 2 * Matrix26::Ones(); +const Matrix26 F3 = 3 * Matrix26::Ones(); +const vector > Fblocks = list_of > // + (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); +// RHS and sigmas +const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.); + +//************************************************************************************* +TEST( implicitSchurFactor, creation ) { + // Matrix E = Matrix::Ones(6,3); + Matrix E = zeros(6, 3); + E.block<2,2>(0, 0) = eye(2); + E.block<2,3>(2, 0) = 2 * ones(2, 3); + Matrix3 P = (E.transpose() * E).inverse(); + ImplicitSchurFactor<6> expected(Fblocks, E, P, b); + Matrix expectedP = expected.getPointCovariance(); + EXPECT(assert_equal(expectedP, P)); +} + +/* ************************************************************************* */ +TEST( implicitSchurFactor, addHessianMultiply ) { + + Matrix E = zeros(6, 3); + E.block<2,2>(0, 0) = eye(2); + E.block<2,3>(2, 0) = 2 * ones(2, 3); + E.block<2,2>(4, 1) = eye(2); + Matrix3 P = (E.transpose() * E).inverse(); + + double alpha = 0.5; + VectorValues xvalues = map_list_of // + (0, gtsam::repeat(6, 2))// + (1, gtsam::repeat(6, 4))// + (2, gtsam::repeat(6, 0))// distractor + (3, gtsam::repeat(6, 8)); + + VectorValues yExpected = map_list_of// + (0, gtsam::repeat(6, 27))// + (1, gtsam::repeat(6, -40))// + (2, gtsam::repeat(6, 0))// distractor + (3, gtsam::repeat(6, 279)); + + // Create full F + size_t M=4, m = 3, d = 6; + Matrix F(2 * m, d * M); + F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; + + // Calculate expected result F'*alpha*(I - E*P*E')*F*x + FastVector keys; + keys += 0,1,2,3; + Vector x = xvalues.vector(keys); + Vector expected = zero(24); + ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); + EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); + + // Create ImplicitSchurFactor + ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); + + VectorValues zero = 0 * yExpected;// quick way to get zero w right structure + { // First Version + VectorValues yActual = zero; + implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(yExpected, yActual, 1e-8)); + implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); + implicitFactor.multiplyHessianAdd(-1, xvalues, yActual); + EXPECT(assert_equal(zero, yActual, 1e-8)); + } + + typedef Eigen::Matrix DeltaX; + typedef Eigen::Map XMap; + double* y = new double[24]; + double* xdata = x.data(); + + { // Raw memory Version + std::fill(y, y + 24, 0);// zero y ! + implicitFactor.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(expected, XMap(y), 1e-8)); + implicitFactor.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); + implicitFactor.multiplyHessianAdd(-1, xdata, y); + EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); + } + + // Create JacobianFactor with same error + const SharedDiagonal model; + JacobianFactorQ<6> jf(Fblocks, E, P, b, model); + + { // error + double expectedError = jf.error(xvalues); + double actualError = implicitFactor.errorJF(xvalues); + DOUBLES_EQUAL(expectedError,actualError,1e-7) + } + + { // JacobianFactor with same error + VectorValues yActual = zero; + jf.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(yExpected, yActual, 1e-8)); + jf.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); + jf.multiplyHessianAdd(-1, xvalues, yActual); + EXPECT(assert_equal(zero, yActual, 1e-8)); + } + + { // check hessian Diagonal + VectorValues diagExpected = jf.hessianDiagonal(); + VectorValues diagActual = implicitFactor.hessianDiagonal(); + EXPECT(assert_equal(diagExpected, diagActual, 1e-8)); + } + + { // check hessian Block Diagonal + map BD = jf.hessianBlockDiagonal(); + map actualBD = implicitFactor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + EXPECT(assert_equal(BD[0],actualBD[0])); + EXPECT(assert_equal(BD[1],actualBD[1])); + EXPECT(assert_equal(BD[3],actualBD[3])); + } + + { // Raw memory Version + std::fill(y, y + 24, 0);// zero y ! + jf.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(expected, XMap(y), 1e-8)); + jf.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); + jf.multiplyHessianAdd(-1, xdata, y); + EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); + } + + { // Check gradientAtZero + VectorValues expected = jf.gradientAtZero(); + VectorValues actual = implicitFactor.gradientAtZero(); + EXPECT(assert_equal(expected, actual, 1e-8)); + } + + // Create JacobianFactorQR + JacobianFactorQR<6> jfq(Fblocks, E, P, b, model); + { + const SharedDiagonal model; + VectorValues yActual = zero; + jfq.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(yExpected, yActual, 1e-8)); + jfq.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); + jfq.multiplyHessianAdd(-1, xvalues, yActual); + EXPECT(assert_equal(zero, yActual, 1e-8)); + } + + { // Raw memory Version + std::fill(y, y + 24, 0);// zero y ! + jfq.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(expected, XMap(y), 1e-8)); + jfq.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); + jfq.multiplyHessianAdd(-1, xdata, y); + EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); + } + delete [] y; +} + +/* ************************************************************************* */ +TEST(implicitSchurFactor, hessianDiagonal) +{ + /* TESTED AGAINST MATLAB + * F = [ones(2,6) zeros(2,6) zeros(2,6) + zeros(2,6) 2*ones(2,6) zeros(2,6) + zeros(2,6) zeros(2,6) 3*ones(2,6)] + E = [[1:6] [1:6] [0.5 1:5]]; + E = reshape(E',3,6)' + P = inv(E' * E) + H = F' * (eye(6) - E * P * E') * F + diag(H) + */ + Matrix E(6,3); + E.block<2,3>(0, 0) << 1,2,3,4,5,6; + E.block<2,3>(2, 0) << 1,2,3,4,5,6; + E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; + Matrix3 P = (E.transpose() * E).inverse(); + ImplicitSchurFactor<6> factor(Fblocks, E, P, b); + + // hessianDiagonal + VectorValues expected; + expected.insert(0, 1.195652*ones(6)); + expected.insert(1, 4.782608*ones(6)); + expected.insert(3, 7.043478*ones(6)); + EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5)); + + // hessianBlockDiagonal + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + Matrix FtE0 = F0.transpose() * E.block<2,3>(0, 0); + Matrix FtE1 = F1.transpose() * E.block<2,3>(2, 0); + Matrix FtE3 = F3.transpose() * E.block<2,3>(4, 0); + + // variant one + EXPECT(assert_equal(F0.transpose()*F0-FtE0*P*FtE0.transpose(),actualBD[0])); + EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); + EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); + + // variant two + Matrix I2 = eye(2); + Matrix E0 = E.block<2,3>(0, 0); + Matrix F0t = F0.transpose(); + EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0])); + EXPECT(assert_equal(F0t*(F0-E0*P*E0.transpose()*F0),actualBD[0])); + + Matrix M1 = F0t*(F0-E0*P*E0.transpose()*F0); + Matrix M2 = F0t*F0-F0t*E0*P*E0.transpose()*F0; + + EXPECT(assert_equal( M1 , actualBD[0] )); + EXPECT(assert_equal( M1 , M2 )); + + Matrix M1b = F0t*(E0*P*E0.transpose()*F0); + Matrix M2b = F0t*E0*P*E0.transpose()*F0; + EXPECT(assert_equal( M1b , M2b )); + + EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0])); + EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); + EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); +} + +/* ************************************************************************* */ +int main(void) { + TestResult tr; + int result = TestRegistry::runAllTests(tr); + return result; +} +//************************************************************************************* From 10435794ed5b193632b27ed3cccdf56c2feb5e7f Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 21 Oct 2014 16:49:08 -0400 Subject: [PATCH 29/44] small typos --- gtsam/base/Value.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 4d7104ad7..ab23e47fa 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -36,9 +36,9 @@ namespace gtsam { * Values can operate generically on Value objects, retracting or computing * local coordinates for many Value objects of different types. * - * Inhereting from the DerivedValue class templated provides a generic implementation of + * Inheriting from the DerivedValue class templated provides a generic implementation of * the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating - * the need to implement these functions in your class. Note that you must inheret from + * the need to implement these functions in your class. Note that you must inherit from * DerivedValue templated on the class you are defining. For example you cannot define * the following * \code From 59db3b72aa9c12ad0f3ea30776552fa386afbdcd Mon Sep 17 00:00:00 2001 From: Siddharth Choudhary Date: Wed, 22 Oct 2014 17:49:18 -0400 Subject: [PATCH 30/44] Changed default NoiseFormat to NoiseFormatAUTO which tries to guess the noise format. --- gtsam/slam/dataset.cpp | 18 ++++++++++++++++++ gtsam/slam/dataset.h | 7 ++++--- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 607819cc7..0f4f24bd0 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -105,6 +105,23 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // Read matrix and check that diagonal entries are non-zero Matrix M(3, 3); switch (noiseFormat) { + case NoiseFormatAUTO: + // Try to guess covariance matrix layout + if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) + { + // NoiseFormatGRAPH + M << v1, v2, v5, v2, v3, v6, v5, v6, v4; + } + else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) + { + // NoiseFormatCOV + M << v1, v2, v3, v2, v4, v5, v3, v5, v6; + } + else + { + throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); + } + break; case NoiseFormatG2O: case NoiseFormatCOV: // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] @@ -136,6 +153,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // In both cases, what is stored in file is the information matrix model = noiseModel::Gaussian::Information(M, smart); break; + case NoiseFormatAUTO: case NoiseFormatGRAPH: case NoiseFormatCOV: // These cases expect covariance matrix diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 09a5baac7..3a0f8edb7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -57,7 +57,8 @@ enum NoiseFormat { NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix ! - NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 + NoiseFormatCOV, ///< Covariance matrix C11, C12, C13, C22, C23, C33 + NoiseFormatAUTO ///< Try to guess covariance matrix layout }; /// Robust kernel type to wrap around quadratic noise model @@ -79,7 +80,7 @@ GTSAM_EXPORT GraphAndValues load2D( std::pair dataset, int maxID = 0, bool addNoise = false, bool smart = true, // - NoiseFormat noiseFormat = NoiseFormatGRAPH, + NoiseFormat noiseFormat = NoiseFormatAUTO, KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** @@ -95,7 +96,7 @@ GTSAM_EXPORT GraphAndValues load2D( */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise = - false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, // + false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel From de175ea12857a16bf665b4a38c445067b9225a39 Mon Sep 17 00:00:00 2001 From: Siddharth Choudhary Date: Wed, 22 Oct 2014 19:00:39 -0400 Subject: [PATCH 31/44] Moved NoiseFormatAUTO check out of switch case --- gtsam/slam/dataset.cpp | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0f4f24bd0..0bf5f4f5b 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -102,26 +102,28 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, double v1, v2, v3, v4, v5, v6; is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; + if (noiseFormat == NoiseFormatAUTO) + { + // Try to guess covariance matrix layout + if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) + { + // NoiseFormatGRAPH + noiseFormat = NoiseFormatGRAPH; + } + else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) + { + // NoiseFormatCOV + noiseFormat = NoiseFormatCOV; + } + else + { + throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); + } + } + // Read matrix and check that diagonal entries are non-zero Matrix M(3, 3); switch (noiseFormat) { - case NoiseFormatAUTO: - // Try to guess covariance matrix layout - if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) - { - // NoiseFormatGRAPH - M << v1, v2, v5, v2, v3, v6, v5, v6, v4; - } - else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) - { - // NoiseFormatCOV - M << v1, v2, v3, v2, v4, v5, v3, v5, v6; - } - else - { - throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); - } - break; case NoiseFormatG2O: case NoiseFormatCOV: // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] @@ -153,7 +155,6 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // In both cases, what is stored in file is the information matrix model = noiseModel::Gaussian::Information(M, smart); break; - case NoiseFormatAUTO: case NoiseFormatGRAPH: case NoiseFormatCOV: // These cases expect covariance matrix From fc21cb49cb3d9402a2ad18020b8c28ff273a9f48 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 22 Oct 2014 23:27:01 -0400 Subject: [PATCH 32/44] issue#124. Turn off MacOS relative flag in building cmake. --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fcce54b6..db3e5f2af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,10 @@ project(GTSAM CXX C) cmake_minimum_required(VERSION 2.6) +# new feature to Cmake Version > 2.8.12 +# turn off the MACOS Relative Path flag. +set(CMAKE_MACOSX_RPATH 0) + # Set the version number for the library set (GTSAM_VERSION_MAJOR 3) set (GTSAM_VERSION_MINOR 1) From c3ac45b05b86bf5a494bbf614a149bb40351604c Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 22 Oct 2014 23:42:35 -0400 Subject: [PATCH 33/44] issue #124. Turn off Mac RPATH on non Mac machines work. --- CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index db3e5f2af..bc7d8aecd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,8 +3,10 @@ project(GTSAM CXX C) cmake_minimum_required(VERSION 2.6) # new feature to Cmake Version > 2.8.12 -# turn off the MACOS Relative Path flag. -set(CMAKE_MACOSX_RPATH 0) +# Mac ONLY. Define Relative Path on Mac OS +if(NOT DEFINED CMAKE_MACOSX_RPATH) + set(CMAKE_MACOSX_RPATH 0) +endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 3) From f81fa0a2030a8b9f9fe14568b0f6116b9f920081 Mon Sep 17 00:00:00 2001 From: Siddharth Choudhary Date: Thu, 23 Oct 2014 15:51:47 -0400 Subject: [PATCH 34/44] Unit test for load2D for victoria park dataset --- gtsam/slam/tests/testDataset.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index a743200c9..a40ddce3d 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -56,6 +56,17 @@ TEST( dataSet, load2D) EXPECT(assert_equal(expected, *actual)); } +/* ************************************************************************* */ +TEST( dataSet, load2DVictoriaPark) +{ + const string filename = findExampleDataFile("victoria_park.txt"); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = load2D(filename); + EXPECT_LONGS_EQUAL(10608,graph->size()); + EXPECT_LONGS_EQUAL(7120,initial->size()); +} + /* ************************************************************************* */ TEST( dataSet, Balbianello) { From 265bd1972dffadf02b54ebe9be755dbd789c5a36 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 23 Oct 2014 17:08:30 -0400 Subject: [PATCH 35/44] Fix for flipped sign in quaternion mode on Linux and Windows --- gtsam/geometry/tests/testRot3.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 2bc4c58f0..ca1e2220c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -179,7 +179,15 @@ TEST(Rot3, log) CHECK_OMEGA( PI, 0, 0) CHECK_OMEGA( 0, PI, 0) CHECK_OMEGA( 0, 0, PI) + + // Windows and Linux have flipped sign in quaternion mode +#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) + w = (Vector(3) << x*PI, y*PI, z*PI); + R = Rot3::rodriguez(w); + EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); +#else CHECK_OMEGA(x*PI,y*PI,z*PI) +#endif // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X,Y,Z) \ From 1dec2c230c6e251c86b7493e7f2871abb89710df Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 24 Oct 2014 13:01:30 -0400 Subject: [PATCH 36/44] Kill unused variables --- gtsam/geometry/tests/testRot3M.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 12fb9bdad..c3db476b5 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -34,7 +34,6 @@ using namespace gtsam; static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); -static double error = 1e-9, epsilon = 0.001; static const Matrix I3 = eye(3); /* ************************************************************************* */ From f916531492de2b406d3a00a592797bade4bdb7d0 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 24 Oct 2014 13:06:45 -0400 Subject: [PATCH 37/44] remove typename from non-templated class --- gtsam_unstable/slam/BiasedGPSFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index fc525f52a..c3f3f1d10 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -39,7 +39,7 @@ namespace gtsam { public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** default constructor - only use for serialization */ BiasedGPSFactor() {} From f19cb3e677d60468d488aee79d4b8eef7757c9a4 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sat, 25 Oct 2014 01:08:59 -0400 Subject: [PATCH 38/44] Change expected error value back to negative, as it was (accidentally?) flipped after recent pull request , which didn't pass. The negative value, which I believe to be correct, passes in Quaternion mode, Full Expmap mode, as well as regular rotation matrix mode (with a lesser tolerance.) --- gtsam/slam/tests/testPoseRotationPrior.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 987f8619b..c9e4acaf7 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -62,17 +62,19 @@ TEST( testPoseRotationFactor, level3_zero_error ) { TEST( testPoseRotationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3C, model3); -#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) Matrix actH1; - EXPECT(assert_equal((Vector(3) << 0.1,0.2,0.3), factor.evaluateError(pose1, actH1))); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1))); +#else + EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); +#endif // the derivative is more complex, but is close to the identity for Rot3 around the origin /* Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-2); EXPECT(assert_equal(expH1, actH1, tol));*/ -#else // If not using true expmap will be close, but not exact around the origin -#endif + } /* ************************************************************************* */ From 60ebd9cb4ea13da62c110441867d93219f129e77 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sat, 25 Oct 2014 06:01:20 -0400 Subject: [PATCH 39/44] rename simple namespace to avoid clash on Windows --- gtsam/slam/tests/testLago.cpp | 56 +++++++++++++++++------------------ 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 0d586a051..95455f078 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -36,7 +36,7 @@ using namespace boost::assign; static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); -namespace simple { +namespace simpleLago { // We consider a small graph: // symbolic FG // x2 0 1 @@ -67,7 +67,7 @@ NonlinearFactorGraph graph() { /* *************************************************************************** */ TEST( Lago, checkSTandChords ) { - NonlinearFactorGraph g = simple::graph(); + NonlinearFactorGraph g = simpleLago::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); @@ -84,7 +84,7 @@ TEST( Lago, checkSTandChords ) { /* *************************************************************************** */ TEST( Lago, orientationsOverSpanningTree ) { - NonlinearFactorGraph g = simple::graph(); + NonlinearFactorGraph g = simpleLago::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); @@ -115,7 +115,7 @@ TEST( Lago, orientationsOverSpanningTree ) { /* *************************************************************************** */ TEST( Lago, regularizedMeasurements ) { - NonlinearFactorGraph g = simple::graph(); + NonlinearFactorGraph g = simpleLago::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); @@ -141,7 +141,7 @@ TEST( Lago, regularizedMeasurements ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { bool useOdometricPath = false; - VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath); + VectorValues initial = lago::initializeOrientations(simpleLago::graph(), useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -153,7 +153,7 @@ TEST( Lago, smallGraphVectorValues ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValuesSP ) { - VectorValues initial = lago::initializeOrientations(simple::graph()); + VectorValues initial = lago::initializeOrientations(simpleLago::graph()); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -165,8 +165,8 @@ TEST( Lago, smallGraphVectorValuesSP ) { /* *************************************************************************** */ TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; - NonlinearFactorGraph g = simple::graph(); - g.add(PriorFactor(x1, simple::pose1, model)); + NonlinearFactorGraph g = simpleLago::graph(); + g.add(PriorFactor(x1, simpleLago::pose1, model)); VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -178,8 +178,8 @@ TEST( Lago, multiplePosePriors ) { /* *************************************************************************** */ TEST( Lago, multiplePosePriorsSP ) { - NonlinearFactorGraph g = simple::graph(); - g.add(PriorFactor(x1, simple::pose1, model)); + NonlinearFactorGraph g = simpleLago::graph(); + g.add(PriorFactor(x1, simpleLago::pose1, model)); VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -192,8 +192,8 @@ TEST( Lago, multiplePosePriorsSP ) { /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; - NonlinearFactorGraph g = simple::graph(); - g.add(PriorFactor(x1, simple::pose1.theta(), model)); + NonlinearFactorGraph g = simpleLago::graph(); + g.add(PriorFactor(x1, simpleLago::pose1.theta(), model)); VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -205,8 +205,8 @@ TEST( Lago, multiplePoseAndRotPriors ) { /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriorsSP ) { - NonlinearFactorGraph g = simple::graph(); - g.add(PriorFactor(x1, simple::pose1.theta(), model)); + NonlinearFactorGraph g = simpleLago::graph(); + g.add(PriorFactor(x1, simpleLago::pose1.theta(), model)); VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -221,20 +221,20 @@ TEST( Lago, smallGraphValues ) { // we set the orientations in the initial guess to zero Values initialGuess; - initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0)); - initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0)); - initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0)); - initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); + initialGuess.insert(x0,Pose2(simpleLago::pose0.x(),simpleLago::pose0.y(),0.0)); + initialGuess.insert(x1,Pose2(simpleLago::pose1.x(),simpleLago::pose1.y(),0.0)); + initialGuess.insert(x2,Pose2(simpleLago::pose2.x(),simpleLago::pose2.y(),0.0)); + initialGuess.insert(x3,Pose2(simpleLago::pose3.x(),simpleLago::pose3.y(),0.0)); // lago does not touch the Cartesian part and only fixed the orientations - Values actual = lago::initialize(simple::graph(), initialGuess); + Values actual = lago::initialize(simpleLago::graph(), initialGuess); // we are in a noiseless case Values expected; - expected.insert(x0,simple::pose0); - expected.insert(x1,simple::pose1); - expected.insert(x2,simple::pose2); - expected.insert(x3,simple::pose3); + expected.insert(x0,simpleLago::pose0); + expected.insert(x1,simpleLago::pose1); + expected.insert(x2,simpleLago::pose2); + expected.insert(x3,simpleLago::pose3); EXPECT(assert_equal(expected, actual, 1e-6)); } @@ -243,14 +243,14 @@ TEST( Lago, smallGraphValues ) { TEST( Lago, smallGraph2 ) { // lago does not touch the Cartesian part and only fixed the orientations - Values actual = lago::initialize(simple::graph()); + Values actual = lago::initialize(simpleLago::graph()); // we are in a noiseless case Values expected; - expected.insert(x0,simple::pose0); - expected.insert(x1,simple::pose1); - expected.insert(x2,simple::pose2); - expected.insert(x3,simple::pose3); + expected.insert(x0,simpleLago::pose0); + expected.insert(x1,simpleLago::pose1); + expected.insert(x2,simpleLago::pose2); + expected.insert(x3,simpleLago::pose3); EXPECT(assert_equal(expected, actual, 1e-6)); } From f833472898f4af14cd206565a2c0dcbb7ff6263e Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sat, 25 Oct 2014 20:18:55 -0400 Subject: [PATCH 40/44] Remove intermediate file for dataset I/O test --- examples/Data/pose3example-rewritten.txt | 11 ----------- 1 file changed, 11 deletions(-) delete mode 100644 examples/Data/pose3example-rewritten.txt diff --git a/examples/Data/pose3example-rewritten.txt b/examples/Data/pose3example-rewritten.txt deleted file mode 100644 index 6d342fcb0..000000000 --- a/examples/Data/pose3example-rewritten.txt +++ /dev/null @@ -1,11 +0,0 @@ -VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 -VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 -VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446 -VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024 -VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488 -EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592076 0.30338 -0.513225 0.542222 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 From fdaaf4c558c4c6638ead404e0096c3134caccd98 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sat, 25 Oct 2014 20:20:35 -0400 Subject: [PATCH 41/44] Add intermediate file for round-trip dataset test to ignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index cf23a5147..60633adaf 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,5 @@ *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt -/examples/Data/pose2example-rewritten.txt \ No newline at end of file +/examples/Data/pose2example-rewritten.txt +/examples/Data/pose3example-rewritten.txt From 57b4c79cad4d4b00dd7e2f2ace9ec7b0e8d9bf84 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 29 Oct 2014 13:58:46 -0400 Subject: [PATCH 42/44] minor doxygen fixes --- gtsam/geometry/triangulation.h | 4 ++-- gtsam/linear/GaussianConditional.h | 2 +- gtsam/slam/EssentialMatrixFactor.h | 10 ++++++++++ 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 6899c616a..fabcc4c02 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -174,7 +174,7 @@ Point3 triangulateNonlinear( * @param poses A vector of camera poses * @param sharedCal shared pointer to single calibration object * @param measurements A vector of camera measurements - * @param rank tolerance, default 1e-9 + * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ @@ -222,7 +222,7 @@ Point3 triangulatePoint3(const std::vector& poses, * no other checks to verify the quality of the triangulation. * @param cameras pinhole cameras * @param measurements A vector of camera measurements - * @param rank tolerance, default 1e-9 + * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index a36de0dc2..9cce29d60 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -111,7 +111,7 @@ namespace gtsam { * assumed to have already been solved in and their values are read from \c x. * This function works for multiple frontal variables. * - * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, + * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2 \f$, * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator * variables of this conditional, this solve function computes * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 9e6f3f8ba..a910b3e35 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -29,6 +29,7 @@ public: /** * Constructor + * @param key Essential Matrix variable key * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates * @param model noise model is about dot product in ideal, homogeneous coordinates @@ -42,6 +43,7 @@ public: /** * Constructor + * @param key Essential Matrix variable key * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates * @param model noise model is about dot product in ideal, homogeneous coordinates @@ -99,6 +101,8 @@ public: /** * Constructor + * @param key1 Essential Matrix variable key + * @param key2 Inverse depth variable key * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates * @param model noise model should be in pixels, as well @@ -113,6 +117,8 @@ public: /** * Constructor + * @param key1 Essential Matrix variable key + * @param key2 Inverse depth variable key * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates * @param K calibration object, will be used only in constructor @@ -216,6 +222,8 @@ public: /** * Constructor + * @param key1 Essential Matrix variable key + * @param key2 Inverse depth variable key * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates * @param bRc extra rotation between "body" and "camera" frame @@ -228,6 +236,8 @@ public: /** * Constructor + * @param key1 Essential Matrix variable key + * @param key2 Inverse depth variable key * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates * @param K calibration object, will be used only in constructor From 699153ece9fd4c8dcbae5ed9b17e3c32da97f1e9 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 30 Oct 2014 12:44:46 -0400 Subject: [PATCH 43/44] Coding convention: convert tabs to two spaces --- gtsam/base/LieMatrix.h | 2 +- gtsam/linear/GaussianFactorGraph.cpp | 6 +- gtsam/linear/HessianFactor.cpp | 2 +- gtsam/navigation/CombinedImuFactor.h | 96 +- gtsam/navigation/ImuFactor.h | 74 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 64 +- gtsam_unstable/base/DSFMap.h | 138 +-- .../discrete/tests/testLoopyBelief.cpp | 4 +- gtsam_unstable/partition/FindSeparator-inl.h | 910 +++++++++--------- gtsam_unstable/partition/FindSeparator.h | 42 +- gtsam_unstable/partition/GenericGraph.cpp | 792 +++++++-------- gtsam_unstable/partition/GenericGraph.h | 216 ++--- .../partition/NestedDissection-inl.h | 404 ++++---- gtsam_unstable/partition/NestedDissection.h | 80 +- gtsam_unstable/partition/PartitionWorkSpace.h | 44 +- .../partition/tests/testFindSeparator.cpp | 70 +- .../partition/tests/testGenericGraph.cpp | 270 +++--- .../partition/tests/testNestedDissection.cpp | 434 ++++----- gtsam_unstable/slam/BetweenFactorEM.h | 100 +- .../slam/GaussMarkov1stOrderFactor.h | 8 +- .../slam/tests/testBetweenFactorEM.cpp | 54 +- 21 files changed, 1905 insertions(+), 1905 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 23e5fd023..ec2dec815 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -157,7 +157,7 @@ struct LieMatrix : public Matrix, public DerivedValue { result.data(), p.rows(), p.cols()) = p; return result; } - + /// @} private: diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 438a06783..54e721cd7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -85,7 +85,7 @@ namespace gtsam { dims_accumulated.resize(dims.size()+1,0); dims_accumulated[0]=0; for (size_t i=1; i FactorKeys = getkeydim(); - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + vector FactorKeys = getkeydim(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) f->multiplyHessianAdd(alpha, x, y, FactorKeys); } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 58aee8073..f282682b3 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -538,7 +538,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // copy to yvalues for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { - bool didNotExist; + bool didNotExist; VectorValues::iterator it; boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); if (didNotExist) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 725274acc..8c53dbf93 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -345,12 +345,12 @@ namespace gtsam { /** Constructor */ CombinedImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias_i, ///< previous bias key - Key bias_j, ///< current bias key + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias_i, ///< previous bias key + Key bias_j, ///< current bias key const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements const Vector3& gravity, ///< gravity vector const Vector3& omegaCoriolis, ///< rotation rate of inertial frame @@ -480,33 +480,33 @@ namespace gtsam { Matrix3 dfPdPi; Matrix3 dfVdPi; if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; } else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); } - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); } if(H2) { @@ -517,13 +517,13 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); } if(H3) { @@ -643,21 +643,21 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d68af4f8b..b95f9c346 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -308,11 +308,11 @@ namespace gtsam { /** Constructor */ ImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias, ///< previous bias key + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias, ///< previous bias key const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements const Vector3& gravity, ///< gravity vector const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame @@ -419,29 +419,29 @@ namespace gtsam { Matrix3 dfPdPi; Matrix3 dfVdPi; if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; } else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); } - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); } if(H2) { @@ -540,22 +540,22 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 63a1a2218..3bbb63b88 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -176,11 +176,11 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, stm << "];\n"; if (firstTimePoses) { - lastKey = key; - firstTimePoses = false; + lastKey = key; + firstTimePoses = false; } else { - stm << " var" << key << "--" << "var" << lastKey << ";\n"; - lastKey = key; + stm << " var" << key << "--" << "var" << lastKey << ";\n"; + lastKey = key; } } stm << "\n"; @@ -219,37 +219,37 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections for(size_t i = 0; i < this->size(); ++i) { if(graphvizFormatting.plotFactorPoints){ - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; - } - stm << "];\n"; + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = graphvizFormatting.factorPositions.find(i); + if(pos != graphvizFormatting.factorPositions.end()) + stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; - // Make factor-variable connections - if(graphvizFormatting.connectKeysToFactor && this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; - } - } + // Make factor-variable connections + if(graphvizFormatting.connectKeysToFactor && this->at(i)) { + BOOST_FOREACH(Key key, *this->at(i)) { + stm << " var" << key << "--" << "factor" << i << ";\n"; + } + } - } + } else { - if(this->at(i)) { - Key k; - bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { - if(firstTime){ - k = key; - firstTime = false; - continue; - } - stm << " var" << key << "--" << "var" << k << ";\n"; - k = key; - } - } + if(this->at(i)) { + Key k; + bool firstTime = true; + BOOST_FOREACH(Key key, *this->at(i)) { + if(firstTime){ + k = key; + firstTime = false; + continue; + } + stm << " var" << key << "--" << "var" << k << ";\n"; + k = key; + } + } } } } diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam_unstable/base/DSFMap.h index 6832c7f83..6ddb74cfd 100644 --- a/gtsam_unstable/base/DSFMap.h +++ b/gtsam_unstable/base/DSFMap.h @@ -34,89 +34,89 @@ class DSFMap { protected: - /// We store the forest in an STL map, but parents are done with pointers - struct Entry { - typename std::map::iterator parent_; - size_t rank_; - Entry() {} - }; + /// We store the forest in an STL map, but parents are done with pointers + struct Entry { + typename std::map::iterator parent_; + size_t rank_; + Entry() {} + }; typedef typename std::map Map; - typedef typename Map::iterator iterator; - mutable Map entries_; + typedef typename Map::iterator iterator; + mutable Map entries_; - /// Given key, find iterator to initial entry - iterator find__(const KEY& key) const { - static const Entry empty; - iterator it = entries_.find(key); - // if key does not exist, create and return itself - if (it == entries_.end()) { - it = entries_.insert(std::make_pair(key, empty)).first; - it->second.parent_ = it; - it->second.rank_ = 0; - } - return it; - } + /// Given key, find iterator to initial entry + iterator find__(const KEY& key) const { + static const Entry empty; + iterator it = entries_.find(key); + // if key does not exist, create and return itself + if (it == entries_.end()) { + it = entries_.insert(std::make_pair(key, empty)).first; + it->second.parent_ = it; + it->second.rank_ = 0; + } + return it; + } - /// Given iterator to initial entry, find the root Entry - iterator find_(const iterator& it) const { - // follow parent pointers until we reach set representative - iterator& parent = it->second.parent_; - if (parent != it) - parent = find_(parent); // not yet, recurse! - return parent; - } + /// Given iterator to initial entry, find the root Entry + iterator find_(const iterator& it) const { + // follow parent pointers until we reach set representative + iterator& parent = it->second.parent_; + if (parent != it) + parent = find_(parent); // not yet, recurse! + return parent; + } - /// Given key, find the root Entry - inline iterator find_(const KEY& key) const { - iterator initial = find__(key); - return find_(initial); - } + /// Given key, find the root Entry + inline iterator find_(const KEY& key) const { + iterator initial = find__(key); + return find_(initial); + } public: - typedef std::set Set; + typedef std::set Set; - /// constructor - DSFMap() { - } + /// constructor + DSFMap() { + } - /// Given key, find the representative key for the set in which it lives - inline KEY find(const KEY& key) const { - iterator root = find_(key); - return root->first; - } + /// Given key, find the representative key for the set in which it lives + inline KEY find(const KEY& key) const { + iterator root = find_(key); + return root->first; + } - /// Merge two sets - void merge(const KEY& x, const KEY& y) { + /// Merge two sets + void merge(const KEY& x, const KEY& y) { - // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure - iterator xRoot = find_(x); - iterator yRoot = find_(y); - if (xRoot == yRoot) - return; + // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure + iterator xRoot = find_(x); + iterator yRoot = find_(y); + if (xRoot == yRoot) + return; - // Merge sets - if (xRoot->second.rank_ < yRoot->second.rank_) - xRoot->second.parent_ = yRoot; - else if (xRoot->second.rank_ > yRoot->second.rank_) - yRoot->second.parent_ = xRoot; - else { - yRoot->second.parent_ = xRoot; - xRoot->second.rank_ = xRoot->second.rank_ + 1; - } - } + // Merge sets + if (xRoot->second.rank_ < yRoot->second.rank_) + xRoot->second.parent_ = yRoot; + else if (xRoot->second.rank_ > yRoot->second.rank_) + yRoot->second.parent_ = xRoot; + else { + yRoot->second.parent_ = xRoot; + xRoot->second.rank_ = xRoot->second.rank_ + 1; + } + } - /// return all sets, i.e. a partition of all elements - std::map sets() const { - std::map sets; - iterator it = entries_.begin(); - for (; it != entries_.end(); it++) { - iterator root = find_(it); - sets[root->first].insert(it->first); - } - return sets; - } + /// return all sets, i.e. a partition of all elements + std::map sets() const { + std::map sets; + iterator it = entries_.begin(); + for (; it != entries_.end(); it++) { + iterator root = find_(it); + sets[root->first].insert(it->first); + } + return sets; + } }; diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 6dd9dd1b5..b579364b6 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -1,8 +1,8 @@ /** - * @file testLoopyBelief.cpp + * @file testLoopyBelief.cpp * @brief * @author Duy-Nguyen Ta - * @date Oct 11, 2013 + * @date Oct 11, 2013 */ #include diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index dacbebc76..849cf7a05 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -26,539 +26,539 @@ extern "C" { namespace gtsam { namespace partition { - typedef boost::shared_array sharedInts; + typedef boost::shared_array sharedInts; - /* ************************************************************************* */ - /** - * Return the size of the separator and the partiion indices {part} - * Part [j] is 0, 1, or 2, depending on - * whether node j is in the left part of the graph, the right part, or the - * separator, respectively - */ - std::pair separatorMetis(idx_t n, const sharedInts& xadj, - const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { + /* ************************************************************************* */ + /** + * Return the size of the separator and the partiion indices {part} + * Part [j] is 0, 1, or 2, depending on + * whether node j is in the left part of the graph, the right part, or the + * separator, respectively + */ + std::pair separatorMetis(idx_t n, const sharedInts& xadj, + const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { - // control parameters - idx_t vwgt[n]; // the weights of the vertices - idx_t options[METIS_NOPTIONS]; - METIS_SetDefaultOptions(options); // use defaults - idx_t sepsize; // the size of the separator, output - sharedInts part_(new idx_t[n]); // the partition of each vertex, output + // control parameters + idx_t vwgt[n]; // the weights of the vertices + idx_t options[METIS_NOPTIONS]; + METIS_SetDefaultOptions(options); // use defaults + idx_t sepsize; // the size of the separator, output + sharedInts part_(new idx_t[n]); // the partition of each vertex, output - // set uniform weights on the vertices - std::fill(vwgt, vwgt+n, 1); + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); - // TODO: Fix at later time - //boost::timer::cpu_timer TOTALTmr; - if (verbose) { - printf("**********************************************************************\n"); - printf("Graph Information ---------------------------------------------------\n"); - printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); - printf("\nND Partitioning... -------------------------------------------\n"); - //TOTALTmr.start() - } + // TODO: Fix at later time + //boost::timer::cpu_timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + //TOTALTmr.start() + } - // call metis parition routine - METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), + // call metis parition routine + METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), vwgt, options, &sepsize, part_.get()); - if (verbose) { - //boost::cpu_times const elapsed_times(timer.elapsed()); - //printf("\nTiming Information --------------------------------------------------\n"); - //printf(" Total: \t\t %7.3f\n", elapsed_times); - printf(" Sep size: \t\t %d\n", sepsize); - printf("**********************************************************************\n"); - } + if (verbose) { + //boost::cpu_times const elapsed_times(timer.elapsed()); + //printf("\nTiming Information --------------------------------------------------\n"); + //printf(" Total: \t\t %7.3f\n", elapsed_times); + printf(" Sep size: \t\t %d\n", sepsize); + printf("**********************************************************************\n"); + } - return std::make_pair(sepsize, part_); - } + return std::make_pair(sepsize, part_); + } - /* ************************************************************************* */ - void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt, - idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part) - { + /* ************************************************************************* */ + void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt, + idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part) + { idx_t i, ncon; - graph_t *graph; - real_t *tpwgts2; - ctrl_t *ctrl; - ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); - ctrl->iptype = METIS_IPTYPE_GROW; - //if () == NULL) - // return METIS_ERROR_INPUT; + graph_t *graph; + real_t *tpwgts2; + ctrl_t *ctrl; + ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); + ctrl->iptype = METIS_IPTYPE_GROW; + //if () == NULL) + // return METIS_ERROR_INPUT; - InitRandom(ctrl->seed); + InitRandom(ctrl->seed); - graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL); + graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL); - AllocateWorkSpace(ctrl, graph); + AllocateWorkSpace(ctrl, graph); - ncon = graph->ncon; - ctrl->ncuts = 1; - - /* determine the weights of the two partitions as a function of the weight of the - target partition weights */ + ncon = graph->ncon; + ctrl->ncuts = 1; + + /* determine the weights of the two partitions as a function of the weight of the + target partition weights */ - tpwgts2 = rwspacemalloc(ctrl, 2*ncon); - for (i=0; i>1), ctrl->tpwgts+i, ncon); - tpwgts2[ncon+i] = 1.0 - tpwgts2[i]; - } - /* perform the bisection */ - *edgecut = MultilevelBisect(ctrl, graph, tpwgts2); + tpwgts2 = rwspacemalloc(ctrl, 2*ncon); + for (i=0; i>1), ctrl->tpwgts+i, ncon); + tpwgts2[ncon+i] = 1.0 - tpwgts2[i]; + } + /* perform the bisection */ + *edgecut = MultilevelBisect(ctrl, graph, tpwgts2); - // ConstructMinCoverSeparator(&ctrl, &graph, 1.05); - // *edgecut = graph->mincut; - // *sepsize = graph.pwgts[2]; - icopy(*nvtxs, graph->where, part); - std::cout << "Finished bisection:" << *edgecut << std::endl; - FreeGraph(&graph); + // ConstructMinCoverSeparator(&ctrl, &graph, 1.05); + // *edgecut = graph->mincut; + // *sepsize = graph.pwgts[2]; + icopy(*nvtxs, graph->where, part); + std::cout << "Finished bisection:" << *edgecut << std::endl; + FreeGraph(&graph); - FreeCtrl(&ctrl); - } + FreeCtrl(&ctrl); + } - /* ************************************************************************* */ - /** - * Return the number of edge cuts and the partition indices {part} - * Part [j] is 0 or 1, depending on - * whether node j is in the left part of the graph or the right part respectively - */ - std::pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, - const sharedInts& adjwgt, bool verbose) { + /* ************************************************************************* */ + /** + * Return the number of edge cuts and the partition indices {part} + * Part [j] is 0 or 1, depending on + * whether node j is in the left part of the graph or the right part respectively + */ + std::pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, + const sharedInts& adjwgt, bool verbose) { - // control parameters - idx_t vwgt[n]; // the weights of the vertices - idx_t options[METIS_NOPTIONS]; - METIS_SetDefaultOptions(options); // use defaults - idx_t edgecut; // the number of edge cuts, output - sharedInts part_(new idx_t[n]); // the partition of each vertex, output + // control parameters + idx_t vwgt[n]; // the weights of the vertices + idx_t options[METIS_NOPTIONS]; + METIS_SetDefaultOptions(options); // use defaults + idx_t edgecut; // the number of edge cuts, output + sharedInts part_(new idx_t[n]); // the partition of each vertex, output - // set uniform weights on the vertices - std::fill(vwgt, vwgt+n, 1); + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); - //TODO: Fix later - //boost::timer TOTALTmr; - if (verbose) { - printf("**********************************************************************\n"); - printf("Graph Information ---------------------------------------------------\n"); - printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); - printf("\nND Partitioning... -------------------------------------------\n"); - //cleartimer(TOTALTmr); - //starttimer(TOTALTmr); - } + //TODO: Fix later + //boost::timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + //cleartimer(TOTALTmr); + //starttimer(TOTALTmr); + } - //int wgtflag = 1; // only edge weights - //int numflag = 0; // c style numbering starting from 0 - //int nparts = 2; // partition the graph to 2 submaps - modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), - options, &edgecut, part_.get()); + //int wgtflag = 1; // only edge weights + //int numflag = 0; // c style numbering starting from 0 + //int nparts = 2; // partition the graph to 2 submaps + modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), + options, &edgecut, part_.get()); - - if (verbose) { - //stoptimer(TOTALTmr); - printf("\nTiming Information --------------------------------------------------\n"); - //printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); - printf(" Edge cuts: \t\t %d\n", edgecut); - printf("**********************************************************************\n"); - } + + if (verbose) { + //stoptimer(TOTALTmr); + printf("\nTiming Information --------------------------------------------------\n"); + //printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); + printf(" Edge cuts: \t\t %d\n", edgecut); + printf("**********************************************************************\n"); + } - return std::make_pair(edgecut, part_); - } + return std::make_pair(edgecut, part_); + } - /* ************************************************************************* */ - /** - * Prepare the data structure {xadj} and {adjncy} required by metis - * xadj always has the size equal to the no. of the nodes plus 1 - * adjncy always has the size equal to two times of the no. of the edges in the Metis graph - */ - template - void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, - sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { + /* ************************************************************************* */ + /** + * Prepare the data structure {xadj} and {adjncy} required by metis + * xadj always has the size equal to the no. of the nodes plus 1 + * adjncy always has the size equal to two times of the no. of the edges in the Metis graph + */ + template + void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, + sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { - typedef int Weight; - typedef std::vector Weights; - typedef std::vector Neighbors; - typedef std::pair NeighborsInfo; + typedef int Weight; + typedef std::vector Weights; + typedef std::vector Neighbors; + typedef std::pair NeighborsInfo; - // set up dictionary - std::vector& dictionary = workspace.dictionary; - workspace.prepareDictionary(keys); + // set up dictionary + std::vector& dictionary = workspace.dictionary; + workspace.prepareDictionary(keys); - // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights - int numNodes = keys.size(); - int numEdges = 0; - std::vector adjacencyMap; - adjacencyMap.resize(numNodes); - std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; - int index1, index2; + // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights + int numNodes = keys.size(); + int numEdges = 0; + std::vector adjacencyMap; + adjacencyMap.resize(numNodes); + std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; + int index1, index2; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ - index1 = dictionary[factor->key1.index]; - index2 = dictionary[factor->key2.index]; - std::cout << "index1: " << index1 << std::endl; - std::cout << "index2: " << index2 << std::endl; - // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator - if (index1 >= 0 && index2 >= 0) { - std::pair& adjacencyMap1 = adjacencyMap[index1]; - std::pair& adjacencyMap2 = adjacencyMap[index2]; - try{ - adjacencyMap1.first.push_back(index2); - adjacencyMap1.second.push_back(factor->weight); - adjacencyMap2.first.push_back(index1); - adjacencyMap2.second.push_back(factor->weight); - }catch(std::exception& e){ - std::cout << e.what() << std::endl; - } - numEdges++; - } - } + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + index1 = dictionary[factor->key1.index]; + index2 = dictionary[factor->key2.index]; + std::cout << "index1: " << index1 << std::endl; + std::cout << "index2: " << index2 << std::endl; + // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator + if (index1 >= 0 && index2 >= 0) { + std::pair& adjacencyMap1 = adjacencyMap[index1]; + std::pair& adjacencyMap2 = adjacencyMap[index2]; + try{ + adjacencyMap1.first.push_back(index2); + adjacencyMap1.second.push_back(factor->weight); + adjacencyMap2.first.push_back(index1); + adjacencyMap2.second.push_back(factor->weight); + }catch(std::exception& e){ + std::cout << e.what() << std::endl; + } + numEdges++; + } + } - // prepare for {xadj}, {adjncy}, and {adjwgt} - *ptr_xadj = sharedInts(new idx_t[numNodes+1]); - *ptr_adjncy = sharedInts(new idx_t[numEdges*2]); - *ptr_adjwgt = sharedInts(new idx_t[numEdges*2]); - sharedInts& xadj = *ptr_xadj; - sharedInts& adjncy = *ptr_adjncy; - sharedInts& adjwgt = *ptr_adjwgt; - int ind_xadj = 0, ind_adjncy = 0; - BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { - *(xadj.get() + ind_xadj) = ind_adjncy; - std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy); - std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy); - assert(info.first.size() == info.second.size()); - ind_adjncy += info.first.size(); - ind_xadj ++; - } - if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes"); - *(xadj.get() + ind_xadj) = ind_adjncy; - } + // prepare for {xadj}, {adjncy}, and {adjwgt} + *ptr_xadj = sharedInts(new idx_t[numNodes+1]); + *ptr_adjncy = sharedInts(new idx_t[numEdges*2]); + *ptr_adjwgt = sharedInts(new idx_t[numEdges*2]); + sharedInts& xadj = *ptr_xadj; + sharedInts& adjncy = *ptr_adjncy; + sharedInts& adjwgt = *ptr_adjwgt; + int ind_xadj = 0, ind_adjncy = 0; + BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { + *(xadj.get() + ind_xadj) = ind_adjncy; + std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy); + std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy); + assert(info.first.size() == info.second.size()); + ind_adjncy += info.first.size(); + ind_xadj ++; + } + if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes"); + *(xadj.get() + ind_xadj) = ind_adjncy; + } - /* ************************************************************************* */ - template - boost::optional separatorPartitionByMetis(const GenericGraph& graph, - const std::vector& keys, WorkSpace& workspace, bool verbose) { - // create a metis graph - size_t numKeys = keys.size(); - if (verbose) - std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; + /* ************************************************************************* */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, + const std::vector& keys, WorkSpace& workspace, bool verbose) { + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) + std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; - sharedInts xadj, adjncy, adjwgt; + sharedInts xadj, adjncy, adjwgt; - prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); - // run ND on the graph - size_t sepsize; - sharedInts part; - boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); - if (!sepsize) return boost::optional(); + // run ND on the graph + size_t sepsize; + sharedInts part; + boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); + if (!sepsize) return boost::optional(); - // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later - // we will have more submaps - MetisResult result; - result.C.reserve(sepsize); - result.A.reserve(numKeys - sepsize); - result.B.reserve(numKeys - sepsize); - int* ptr_part = part.get(); - std::vector::const_iterator itKey = keys.begin(); - std::vector::const_iterator itKeyLast = keys.end(); - while(itKey != itKeyLast) { - switch(*(ptr_part++)) { - case 0: result.A.push_back(*(itKey++)); break; - case 1: result.B.push_back(*(itKey++)); break; - case 2: result.C.push_back(*(itKey++)); break; - default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); - } - } + // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later + // we will have more submaps + MetisResult result; + result.C.reserve(sepsize); + result.A.reserve(numKeys - sepsize); + result.B.reserve(numKeys - sepsize); + int* ptr_part = part.get(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + case 2: result.C.push_back(*(itKey++)); break; + default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); + } + } - if (verbose) { - std::cout << "total key: " << keys.size() - << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " - << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; - //throw runtime_error("separatorPartitionByMetis:stop for debug"); - } + if (verbose) { + std::cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " + << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; + //throw runtime_error("separatorPartitionByMetis:stop for debug"); + } - if(result.C.size() != sepsize) { - std::cout << "total key: " << keys.size() - << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() - << "; sepsize from Metis = " << sepsize << std::endl; - throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); - } + if(result.C.size() != sepsize) { + std::cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() + << "; sepsize from Metis = " << sepsize << std::endl; + throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); + } - return boost::make_optional(result); - } + return boost::make_optional(result); + } - /* *************************************************************************/ - template - boost::optional edgePartitionByMetis(const GenericGraph& graph, - const std::vector& keys, WorkSpace& workspace, bool verbose) { + /* *************************************************************************/ + template + boost::optional edgePartitionByMetis(const GenericGraph& graph, + const std::vector& keys, WorkSpace& workspace, bool verbose) { - // a small hack for handling the camera1-camera2 case used in the unit tests - if (graph.size() == 1 && keys.size() == 2) { - MetisResult result; - result.A.push_back(keys.front()); - result.B.push_back(keys.back()); - return result; - } + // a small hack for handling the camera1-camera2 case used in the unit tests + if (graph.size() == 1 && keys.size() == 2) { + MetisResult result; + result.A.push_back(keys.front()); + result.B.push_back(keys.back()); + return result; + } - // create a metis graph - size_t numKeys = keys.size(); - if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; - sharedInts xadj, adjncy, adjwgt; - prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; + sharedInts xadj, adjncy, adjwgt; + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); - // run metis on the graph - int edgecut; - sharedInts part; - boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose); + // run metis on the graph + int edgecut; + sharedInts part; + boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose); - // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps - MetisResult result; - result.A.reserve(numKeys); - result.B.reserve(numKeys); - int* ptr_part = part.get(); - std::vector::const_iterator itKey = keys.begin(); - std::vector::const_iterator itKeyLast = keys.end(); - while(itKey != itKeyLast) { - if (*ptr_part != 0 && *ptr_part != 1) - std::cout << *ptr_part << "!!!" << std::endl; - switch(*(ptr_part++)) { - case 0: result.A.push_back(*(itKey++)); break; - case 1: result.B.push_back(*(itKey++)); break; - default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); - } - } + // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps + MetisResult result; + result.A.reserve(numKeys); + result.B.reserve(numKeys); + int* ptr_part = part.get(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + if (*ptr_part != 0 && *ptr_part != 1) + std::cout << *ptr_part << "!!!" << std::endl; + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); + } + } - if (verbose) { - std::cout << "the size of two submaps in the reduced graph: " << result.A.size() - << " " << result.B.size() << std::endl; - int edgeCut = 0; + if (verbose) { + std::cout << "the size of two submaps in the reduced graph: " << result.A.size() + << " " << result.B.size() << std::endl; + int edgeCut = 0; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ - int key1 = factor->key1.index; - int key2 = factor->key2.index; - // print keys and their subgraph assignment - std::cout << key1; - if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A "; - if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B "; + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + int key1 = factor->key1.index; + int key2 = factor->key2.index; + // print keys and their subgraph assignment + std::cout << key1; + if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B "; - std::cout << key2; - if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A "; - if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B "; + std::cout << key2; + if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B "; std::cout << "weight " << factor->weight;; - // find vertices that were assigned to sets A & B. Their edge will be cut - if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() && - std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) || - (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() && - std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){ - edgeCut ++; - std::cout << " CUT "; - } - std::cout << std::endl; - } - std::cout << "edgeCut: " << edgeCut << std::endl; - } + // find vertices that were assigned to sets A & B. Their edge will be cut + if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() && + std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) || + (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() && + std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){ + edgeCut ++; + std::cout << " CUT "; + } + std::cout << std::endl; + } + std::cout << "edgeCut: " << edgeCut << std::endl; + } - return boost::make_optional(result); - } + return boost::make_optional(result); + } - /* ************************************************************************* */ - bool isLargerIsland(const std::vector& island1, const std::vector& island2) { - return island1.size() > island2.size(); - } + /* ************************************************************************* */ + bool isLargerIsland(const std::vector& island1, const std::vector& island2) { + return island1.size() > island2.size(); + } - /* ************************************************************************* */ - // debug functions - void printIsland(const std::vector& island) { - std::cout << "island: "; - BOOST_FOREACH(const size_t key, island) - std::cout << key << " "; - std::cout << std::endl; - } + /* ************************************************************************* */ + // debug functions + void printIsland(const std::vector& island) { + std::cout << "island: "; + BOOST_FOREACH(const size_t key, island) + std::cout << key << " "; + std::cout << std::endl; + } - void printIslands(const std::list >& islands) { - BOOST_FOREACH(const std::vector& island, islands) - printIsland(island); - } + void printIslands(const std::list >& islands) { + BOOST_FOREACH(const std::vector& island, islands) + printIsland(island); + } - void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { - int numCamera = 0, numLandmark = 0; - BOOST_FOREACH(const size_t key, keys) - if (int2symbol[key].chr() == 'x') - numCamera++; - else - numLandmark++; - std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; - } + void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { + int numCamera = 0, numLandmark = 0; + BOOST_FOREACH(const size_t key, keys) + if (int2symbol[key].chr() == 'x') + numCamera++; + else + numLandmark++; + std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; + } - /* ************************************************************************* */ - template - void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector& landmarkKeys, - MetisResult& partitionResult, WorkSpace& workspace) { + /* ************************************************************************* */ + template + void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector& landmarkKeys, + MetisResult& partitionResult, WorkSpace& workspace) { - // set up cameras in the dictionary - std::vector& A = partitionResult.A; - std::vector& B = partitionResult.B; - std::vector& C = partitionResult.C; - std::vector& dictionary = workspace.dictionary; - std::fill(dictionary.begin(), dictionary.end(), -1); - BOOST_FOREACH(const size_t a, A) - dictionary[a] = 1; - BOOST_FOREACH(const size_t b, B) - dictionary[b] = 2; - if (!C.empty()) - throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); + // set up cameras in the dictionary + std::vector& A = partitionResult.A; + std::vector& B = partitionResult.B; + std::vector& C = partitionResult.C; + std::vector& dictionary = workspace.dictionary; + std::fill(dictionary.begin(), dictionary.end(), -1); + BOOST_FOREACH(const size_t a, A) + dictionary[a] = 1; + BOOST_FOREACH(const size_t b, B) + dictionary[b] = 2; + if (!C.empty()) + throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); - // set up landmarks - size_t i,j; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { - i = factor->key1.index; - j = factor->key2.index; - if (dictionary[j] == 0) // if the landmark is already in the separator, continue - continue; - else if (dictionary[j] == -1) - dictionary[j] = dictionary[i]; - else { - if (dictionary[j] != dictionary[i]) - dictionary[j] = 0; - } -// if (j == 67980) -// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; - } + // set up landmarks + size_t i,j; + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { + i = factor->key1.index; + j = factor->key2.index; + if (dictionary[j] == 0) // if the landmark is already in the separator, continue + continue; + else if (dictionary[j] == -1) + dictionary[j] = dictionary[i]; + else { + if (dictionary[j] != dictionary[i]) + dictionary[j] = 0; + } +// if (j == 67980) +// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; + } - BOOST_FOREACH(const size_t j, landmarkKeys) { - switch(dictionary[j]) { - case 0: C.push_back(j); break; - case 1: A.push_back(j); break; - case 2: B.push_back(j); break; - default: std::cout << j << ": " << dictionary[j] << std::endl; - throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); - } - } - } + BOOST_FOREACH(const size_t j, landmarkKeys) { + switch(dictionary[j]) { + case 0: C.push_back(j); break; + case 1: A.push_back(j); break; + case 2: B.push_back(j); break; + default: std::cout << j << ": " << dictionary[j] << std::endl; + throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); + } + } + } #define REDUCE_CAMERA_GRAPH - /* ************************************************************************* */ - template - boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, - WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph) { - boost::optional result; - GenericGraph reducedGraph; - std::vector keyToPartition; - std::vector cameraKeys, landmarkKeys; - if (reduceGraph) { - if (!int2symbol.is_initialized()) - throw std::invalid_argument("findSeparator: int2symbol must be valid!"); + /* ************************************************************************* */ + template + boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, + WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph) { + boost::optional result; + GenericGraph reducedGraph; + std::vector keyToPartition; + std::vector cameraKeys, landmarkKeys; + if (reduceGraph) { + if (!int2symbol.is_initialized()) + throw std::invalid_argument("findSeparator: int2symbol must be valid!"); - // find out all the landmark keys, which are to be eliminated - cameraKeys.reserve(keys.size()); - landmarkKeys.reserve(keys.size()); - BOOST_FOREACH(const size_t key, keys) { - if((*int2symbol)[key].chr() == 'x') - cameraKeys.push_back(key); - else - landmarkKeys.push_back(key); - } + // find out all the landmark keys, which are to be eliminated + cameraKeys.reserve(keys.size()); + landmarkKeys.reserve(keys.size()); + BOOST_FOREACH(const size_t key, keys) { + if((*int2symbol)[key].chr() == 'x') + cameraKeys.push_back(key); + else + landmarkKeys.push_back(key); + } - keyToPartition = cameraKeys; - workspace.prepareDictionary(keyToPartition); - const std::vector& dictionary = workspace.dictionary; - reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); - std::cout << "original graph: V" << keys.size() << ", E" << graph.size() - << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; - result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); - } else // call Metis to partition the graph to A, B, C - result = separatorPartitionByMetis(graph, keys, workspace, verbose); + keyToPartition = cameraKeys; + workspace.prepareDictionary(keyToPartition); + const std::vector& dictionary = workspace.dictionary; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); + std::cout << "original graph: V" << keys.size() << ", E" << graph.size() + << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; + result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); + } else // call Metis to partition the graph to A, B, C + result = separatorPartitionByMetis(graph, keys, workspace, verbose); - if (!result.is_initialized()) { - std::cout << "metis failed!" << std::endl; - return 0; - } + if (!result.is_initialized()) { + std::cout << "metis failed!" << std::endl; + return 0; + } - if (reduceGraph) { - addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); - std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; - } + if (reduceGraph) { + addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); + std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; + } - return result; - } + return result; + } - /* ************************************************************************* */ - template - int findSeparator(const GenericGraph& graph, const std::vector& keys, - const int minNodesPerMap, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + /* ************************************************************************* */ + template + int findSeparator(const GenericGraph& graph, const std::vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { - boost::optional result = findPartitoning(graph, keys, workspace, - verbose, int2symbol, reduceGraph); + boost::optional result = findPartitoning(graph, keys, workspace, + verbose, int2symbol, reduceGraph); - // find the island in A and B, and make them separated submaps - typedef std::vector Island; - std::list islands; + // find the island in A and B, and make them separated submaps + typedef std::vector Island; + std::list islands; - std::list islands_in_A = findIslands(graph, result->A, workspace, - minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + std::list islands_in_A = findIslands(graph, result->A, workspace, + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); - std::list islands_in_B = findIslands(graph, result->B, workspace, - minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + std::list islands_in_B = findIslands(graph, result->B, workspace, + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); - islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end()); - islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end()); - islands.sort(isLargerIsland); - size_t numIsland0 = islands.size(); + islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end()); + islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end()); + islands.sort(isLargerIsland); + size_t numIsland0 = islands.size(); #ifdef NDEBUG -// verbose = true; -// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); -// std::cout << "sep size: " << result->C.size() << "; "; -// printNumCamerasLandmarks(result->C, *int2symbol); -// std::cout << "no. of island: " << islands.size() << "; "; -// std::cout << "island size: "; -// BOOST_FOREACH(const Island& island, islands) -// std::cout << island.size() << " "; -// std::cout << std::endl; +// verbose = true; +// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); +// std::cout << "sep size: " << result->C.size() << "; "; +// printNumCamerasLandmarks(result->C, *int2symbol); +// std::cout << "no. of island: " << islands.size() << "; "; +// std::cout << "island size: "; +// BOOST_FOREACH(const Island& island, islands) +// std::cout << island.size() << " "; +// std::cout << std::endl; -// BOOST_FOREACH(const Island& island, islands) { -// printNumCamerasLandmarks(island, int2symbol); -// } +// BOOST_FOREACH(const Island& island, islands) { +// printNumCamerasLandmarks(island, int2symbol); +// } #endif - // absorb small components into the separator - size_t oldSize = islands.size(); - while(true) { - if (islands.size() < 2) { - std::cout << "numIsland: " << numIsland0 << std::endl; - throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); - } + // absorb small components into the separator + size_t oldSize = islands.size(); + while(true) { + if (islands.size() < 2) { + std::cout << "numIsland: " << numIsland0 << std::endl; + throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); + } - std::list::reference island = islands.back(); - if ((int)island.size() >= minNodesPerMap) break; - result->C.insert(result->C.end(), island.begin(), island.end()); - islands.pop_back(); - } - if (islands.size() != oldSize){ - if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; - } - else{ - if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; - } + std::list::reference island = islands.back(); + if ((int)island.size() >= minNodesPerMap) break; + result->C.insert(result->C.end(), island.begin(), island.end()); + islands.pop_back(); + } + if (islands.size() != oldSize){ + if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; + } + else{ + if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; + } - // generate the node map - std::vector& partitionTable = workspace.partitionTable; - std::fill(partitionTable.begin(), partitionTable.end(), -1); - BOOST_FOREACH(const size_t key, result->C) - partitionTable[key] = 0; - int idx = 0; - BOOST_FOREACH(const Island& island, islands) { - idx++; - BOOST_FOREACH(const size_t key, island) { - partitionTable[key] = idx; - } - } + // generate the node map + std::vector& partitionTable = workspace.partitionTable; + std::fill(partitionTable.begin(), partitionTable.end(), -1); + BOOST_FOREACH(const size_t key, result->C) + partitionTable[key] = 0; + int idx = 0; + BOOST_FOREACH(const Island& island, islands) { + idx++; + BOOST_FOREACH(const size_t key, island) { + partitionTable[key] = idx; + } + } - return islands.size(); - } + return islands.size(); + } }} //namespace diff --git a/gtsam_unstable/partition/FindSeparator.h b/gtsam_unstable/partition/FindSeparator.h index e52d40a12..42d971a82 100644 --- a/gtsam_unstable/partition/FindSeparator.h +++ b/gtsam_unstable/partition/FindSeparator.h @@ -16,29 +16,29 @@ namespace gtsam { namespace partition { -// typedef std::map PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id +// typedef std::map PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id - /** the metis Nest dissection result */ - struct MetisResult { - std::vector A, B; // frontals - std::vector C; // separator - }; + /** the metis Nest dissection result */ + struct MetisResult { + std::vector A, B; // frontals + std::vector C; // separator + }; - /** - * use Metis library to partition, return the size of separator and the optional partition table - * the size of dictionary mush be equal to the number of variables in the original graph (the largest one) - */ - template - boost::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, - WorkSpace& workspace, bool verbose); + /** + * use Metis library to partition, return the size of separator and the optional partition table + * the size of dictionary mush be equal to the number of variables in the original graph (the largest one) + */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, + WorkSpace& workspace, bool verbose); - /** - * return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**). - * return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator. - */ - template - int findSeparator(const GenericGraph& graph, const std::vector& keys, - const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional >& int2symbol, - const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + /** + * return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**). + * return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator. + */ + template + int findSeparator(const GenericGraph& graph, const std::vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional >& int2symbol, + const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); }} //namespace diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index a3db6a9c1..b78c1d3dc 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -19,459 +19,459 @@ using namespace std; namespace gtsam { namespace partition { - /** - * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys} - */ - list > findIslands(const GenericGraph2D& graph, const vector& keys, WorkSpace& workspace, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) - { - typedef pair IntPair; - typedef list FactorList; - typedef map Connections; + /** + * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys} + */ + list > findIslands(const GenericGraph2D& graph, const vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) + { + typedef pair IntPair; + typedef list FactorList; + typedef map Connections; - // create disjoin set forest - DSFVector dsf(workspace.dsf, keys); + // create disjoin set forest + DSFVector dsf(workspace.dsf, keys); - FactorList factors(graph.begin(), graph.end()); - size_t nrFactors = factors.size(); - FactorList::iterator itEnd; - workspace.prepareDictionary(keys); - while (nrFactors) { - Connections connections; - bool succeed = false; - itEnd = factors.end(); - list toErase; - for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + FactorList factors(graph.begin(), graph.end()); + size_t nrFactors = factors.size(); + FactorList::iterator itEnd; + workspace.prepareDictionary(keys); + while (nrFactors) { + Connections connections; + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { - // remove invalid factors - GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; - if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { - toErase.push_back(itFactor); nrFactors--; continue; - } + // remove invalid factors + GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } - size_t label1 = dsf.findSet(key1.index); - size_t label2 = dsf.findSet(key2.index); - if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } - // merge two trees if the connection is strong enough, otherwise cache it - // an odometry factor always merges two islands - if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { - toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - succeed = true; - break; - } + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } - // single landmark island only need one measurement - if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || - (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { - toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - succeed = true; - break; - } + // single landmark island only need one measurement + if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || + (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } - // stack the current factor with the cached constraint - IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1); - Connections::iterator itCached = connections.find(labels); - if (itCached == connections.end()) { - connections.insert(make_pair(labels, itFactor)); - continue; - } else { - GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2; - // if observe the same landmark, we can not merge, abandon the current factor - if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) || - (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) || - (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) || - (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) { - toErase.push_back(itFactor); nrFactors--; - continue; - } else { - toErase.push_back(itFactor); nrFactors--; - toErase.push_back(itCached->second); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - connections.erase(itCached); - succeed = true; - break; - } - } - } + // stack the current factor with the cached constraint + IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1); + Connections::iterator itCached = connections.find(labels); + if (itCached == connections.end()) { + connections.insert(make_pair(labels, itFactor)); + continue; + } else { + GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2; + // if observe the same landmark, we can not merge, abandon the current factor + if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) || + (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) || + (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) || + (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + continue; + } else { + toErase.push_back(itFactor); nrFactors--; + toErase.push_back(itCached->second); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + connections.erase(itCached); + succeed = true; + break; + } + } + } - // erase unused factors - BOOST_FOREACH(const FactorList::iterator& it, toErase) - factors.erase(it); + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); - if (!succeed) break; - } + if (!succeed) break; + } - list > islands; - map > arrays = dsf.arrays(); - size_t key; vector array; - BOOST_FOREACH(boost::tie(key, array), arrays) - islands.push_back(array); - return islands; - } + list > islands; + map > arrays = dsf.arrays(); + size_t key; vector array; + BOOST_FOREACH(boost::tie(key, array), arrays) + islands.push_back(array); + return islands; + } - /* ************************************************************************* */ - void print(const GenericGraph2D& graph, const std::string name) { - cout << name << endl; - BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) - cout << factor_->key1.index << " " << factor_->key2.index << endl; - } + /* ************************************************************************* */ + void print(const GenericGraph2D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << endl; + } - /* ************************************************************************* */ - void print(const GenericGraph3D& graph, const std::string name) { - cout << name << endl; - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) - cout << factor_->key1.index << " " << factor_->key2.index << " (" << - factor_->key1.type << ", " << factor_->key2.type <<")" << endl; - } + /* ************************************************************************* */ + void print(const GenericGraph3D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << " (" << + factor_->key1.type << ", " << factor_->key2.type <<")" << endl; + } - /* ************************************************************************* */ - // create disjoin set forest - DSFVector createDSF(const GenericGraph3D& graph, const vector& keys, const WorkSpace& workspace) { - DSFVector dsf(workspace.dsf, keys); - typedef list FactorList; + /* ************************************************************************* */ + // create disjoin set forest + DSFVector createDSF(const GenericGraph3D& graph, const vector& keys, const WorkSpace& workspace) { + DSFVector dsf(workspace.dsf, keys); + typedef list FactorList; - FactorList factors(graph.begin(), graph.end()); - size_t nrFactors = factors.size(); - FactorList::iterator itEnd; - while (nrFactors) { + FactorList factors(graph.begin(), graph.end()); + size_t nrFactors = factors.size(); + FactorList::iterator itEnd; + while (nrFactors) { - bool succeed = false; - itEnd = factors.end(); - list toErase; - for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { - // remove invalid factors - if (graph.size() == 178765) cout << "kai21" << endl; - GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; - if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl; - if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { - toErase.push_back(itFactor); nrFactors--; continue; - } + // remove invalid factors + if (graph.size() == 178765) cout << "kai21" << endl; + GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } - if (graph.size() == 178765) cout << "kai22" << endl; - size_t label1 = dsf.findSet(key1.index); - size_t label2 = dsf.findSet(key2.index); - if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + if (graph.size() == 178765) cout << "kai22" << endl; + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } - if (graph.size() == 178765) cout << "kai23" << endl; - // merge two trees if the connection is strong enough, otherwise cache it - // an odometry factor always merges two islands - if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || - (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { - toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - succeed = true; - break; - } + if (graph.size() == 178765) cout << "kai23" << endl; + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || + (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } - if (graph.size() == 178765) cout << "kai24" << endl; + if (graph.size() == 178765) cout << "kai24" << endl; - } + } - // erase unused factors - BOOST_FOREACH(const FactorList::iterator& it, toErase) - factors.erase(it); + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); - if (!succeed) break; - } - return dsf; - } + if (!succeed) break; + } + return dsf; + } - /* ************************************************************************* */ - // first check the type of the key (pose or landmark), and then check whether it is singular - inline bool isSingular(const set& singularCameras, const set& singularLandmarks, const GenericNode3D& node) { - switch(node.type) { - case NODE_POSE_3D: - return singularCameras.find(node.index) != singularCameras.end(); break; - case NODE_LANDMARK_3D: - return singularLandmarks.find(node.index) != singularLandmarks.end(); break; - default: - throw runtime_error("unrecognized key type!"); - } - } + /* ************************************************************************* */ + // first check the type of the key (pose or landmark), and then check whether it is singular + inline bool isSingular(const set& singularCameras, const set& singularLandmarks, const GenericNode3D& node) { + switch(node.type) { + case NODE_POSE_3D: + return singularCameras.find(node.index) != singularCameras.end(); break; + case NODE_LANDMARK_3D: + return singularLandmarks.find(node.index) != singularLandmarks.end(); break; + default: + throw runtime_error("unrecognized key type!"); + } + } - /* ************************************************************************* */ - void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace, - const vector& isCamera, const vector& isLandmark, - set& singularCameras, set& singularLandmarks, vector& nrConstraints, - bool& foundSingularCamera, bool& foundSingularLandmark, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + /* ************************************************************************* */ + void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace, + const vector& isCamera, const vector& isLandmark, + set& singularCameras, set& singularLandmarks, vector& nrConstraints, + bool& foundSingularCamera, bool& foundSingularLandmark, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { - // compute the constraint number per camera - std::fill(nrConstraints.begin(), nrConstraints.end(), 0); - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - const int& key1 = factor_->key1.index; - const int& key2 = factor_->key2.index; - if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && - !isSingular(singularCameras, singularLandmarks, factor_->key1) && - !isSingular(singularCameras, singularLandmarks, factor_->key2)) { - nrConstraints[key1]++; - nrConstraints[key2]++; + // compute the constraint number per camera + std::fill(nrConstraints.begin(), nrConstraints.end(), 0); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + const int& key1 = factor_->key1.index; + const int& key2 = factor_->key2.index; + if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && + !isSingular(singularCameras, singularLandmarks, factor_->key1) && + !isSingular(singularCameras, singularLandmarks, factor_->key2)) { + nrConstraints[key1]++; + nrConstraints[key2]++; - // a single pose constraint is sufficient for stereo, so we add 2 to the counter - // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera - if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ - nrConstraints[key1]+=2; - nrConstraints[key2]+=2; - } - } - } + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } + } - // find singular cameras and landmarks - foundSingularCamera = false; - foundSingularLandmark = false; - for (size_t i=0; i > findIslands(const GenericGraph3D& graph, const vector& keys, WorkSpace& workspace, - const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { + /* ************************************************************************* */ + list > findIslands(const GenericGraph3D& graph, const vector& keys, WorkSpace& workspace, + const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { - // create disjoint set forest - workspace.prepareDictionary(keys); - DSFVector dsf = createDSF(graph, keys, workspace); + // create disjoint set forest + workspace.prepareDictionary(keys); + DSFVector dsf = createDSF(graph, keys, workspace); - const bool verbose = false; - bool foundSingularCamera = true; - bool foundSingularLandmark = true; + const bool verbose = false; + bool foundSingularCamera = true; + bool foundSingularLandmark = true; - list > islands; - set singularCameras, singularLandmarks; - vector isCamera(workspace.dictionary.size(), false); - vector isLandmark(workspace.dictionary.size(), false); + list > islands; + set singularCameras, singularLandmarks; + vector isCamera(workspace.dictionary.size(), false); + vector isLandmark(workspace.dictionary.size(), false); - // check the constraint number of every variable - // find the camera and landmark keys - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM - if (workspace.dictionary[factor_->key1.index] != -1) { - if (factor_->key1.type == NODE_POSE_3D) - isCamera[factor_->key1.index] = true; - else - isLandmark[factor_->key1.index] = true; - } + // check the constraint number of every variable + // find the camera and landmark keys + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM + if (workspace.dictionary[factor_->key1.index] != -1) { + if (factor_->key1.type == NODE_POSE_3D) + isCamera[factor_->key1.index] = true; + else + isLandmark[factor_->key1.index] = true; + } if (workspace.dictionary[factor_->key2.index] != -1) { - if (factor_->key2.type == NODE_POSE_3D) - isCamera[factor_->key2.index] = true; - else - isLandmark[factor_->key2.index] = true; + if (factor_->key2.type == NODE_POSE_3D) + isCamera[factor_->key2.index] = true; + else + isLandmark[factor_->key2.index] = true; } - } + } - vector nrConstraints(workspace.dictionary.size(), 0); - // iterate until all singular variables have been removed. Removing a singular variable - // can cause another to become singular, so this will probably run several times - while (foundSingularCamera || foundSingularLandmark) { - findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input - singularCameras, singularLandmarks, nrConstraints, // output - foundSingularCamera, foundSingularLandmark, // output - minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input - } + vector nrConstraints(workspace.dictionary.size(), 0); + // iterate until all singular variables have been removed. Removing a singular variable + // can cause another to become singular, so this will probably run several times + while (foundSingularCamera || foundSingularLandmark) { + findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input + singularCameras, singularLandmarks, nrConstraints, // output + foundSingularCamera, foundSingularLandmark, // output + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input + } - // add singular variables directly as islands - if (!singularCameras.empty()) { - if (verbose) cout << "singular cameras:"; - BOOST_FOREACH(const size_t i, singularCameras) { - islands.push_back(vector(1, i)); // <--------------------------- - if (verbose) cout << i << " "; - } - if (verbose) cout << endl; - } - if (!singularLandmarks.empty()) { - if (verbose) cout << "singular landmarks:"; - BOOST_FOREACH(const size_t i, singularLandmarks) { - islands.push_back(vector(1, i)); // <--------------------------- - if (verbose) cout << i << " "; - } - if (verbose) cout << endl; - } + // add singular variables directly as islands + if (!singularCameras.empty()) { + if (verbose) cout << "singular cameras:"; + BOOST_FOREACH(const size_t i, singularCameras) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } + if (!singularLandmarks.empty()) { + if (verbose) cout << "singular landmarks:"; + BOOST_FOREACH(const size_t i, singularLandmarks) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } - // regenerating islands - map > labelIslands = dsf.arrays(); - size_t label; vector island; - BOOST_FOREACH(boost::tie(label, island), labelIslands) { - vector filteredIsland; // remove singular cameras from array - filteredIsland.reserve(island.size()); - BOOST_FOREACH(const size_t key, island) { - if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular - (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular - (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined - filteredIsland.push_back(key); - } - } - islands.push_back(filteredIsland); - } + // regenerating islands + map > labelIslands = dsf.arrays(); + size_t label; vector island; + BOOST_FOREACH(boost::tie(label, island), labelIslands) { + vector filteredIsland; // remove singular cameras from array + filteredIsland.reserve(island.size()); + BOOST_FOREACH(const size_t key, island) { + if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular + (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular + (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined + filteredIsland.push_back(key); + } + } + islands.push_back(filteredIsland); + } - // sanity check - size_t nrKeys = 0; - BOOST_FOREACH(const vector& island, islands) - nrKeys += island.size(); - if (nrKeys != keys.size()) { - cout << nrKeys << " vs " << keys.size() << endl; - throw runtime_error("findIslands: the number of keys is inconsistent!"); - } + // sanity check + size_t nrKeys = 0; + BOOST_FOREACH(const vector& island, islands) + nrKeys += island.size(); + if (nrKeys != keys.size()) { + cout << nrKeys << " vs " << keys.size() << endl; + throw runtime_error("findIslands: the number of keys is inconsistent!"); + } - if (verbose) cout << "found " << islands.size() << " islands!" << endl; - return islands; - } + if (verbose) cout << "found " << islands.size() << " islands!" << endl; + return islands; + } - /* ************************************************************************* */ - // return the number of intersection between two **sorted** landmark vectors - inline int getNrCommonLandmarks(const vector& landmarks1, const vector& landmarks2){ - size_t i1 = 0, i2 = 0; - int nrCommonLandmarks = 0; - while (i1 < landmarks1.size() && i2 < landmarks2.size()) { - if (landmarks1[i1] < landmarks2[i2]) - i1 ++; - else if (landmarks1[i1] > landmarks2[i2]) - i2 ++; - else { - i1++; i2++; - nrCommonLandmarks ++; - } - } - return nrCommonLandmarks; - } + /* ************************************************************************* */ + // return the number of intersection between two **sorted** landmark vectors + inline int getNrCommonLandmarks(const vector& landmarks1, const vector& landmarks2){ + size_t i1 = 0, i2 = 0; + int nrCommonLandmarks = 0; + while (i1 < landmarks1.size() && i2 < landmarks2.size()) { + if (landmarks1[i1] < landmarks2[i2]) + i1 ++; + else if (landmarks1[i1] > landmarks2[i2]) + i2 ++; + else { + i1++; i2++; + nrCommonLandmarks ++; + } + } + return nrCommonLandmarks; + } - /* ************************************************************************* */ - void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, - const std::vector& dictionary, GenericGraph3D& reducedGraph) { + /* ************************************************************************* */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph) { - typedef size_t CameraKey; - typedef pair CameraPair; - typedef size_t LandmarkKey; - // get a mapping from each landmark to its connected cameras - vector > cameraToLandmarks(dictionary.size()); - // for odometry xi-xj where i cameraToCamera(dictionary.size(), -1); - size_t key_i, key_j; - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - if (factor_->key1.type == NODE_POSE_3D) { - if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor - cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); - } - else { // odometry factor - if (factor_->key1.index < factor_->key2.index) { - key_i = factor_->key1.index; - key_j = factor_->key2.index; - } else { - key_i = factor_->key2.index; - key_j = factor_->key1.index; - } - cameraToCamera[key_i] = key_j; - } - } - } + typedef size_t CameraKey; + typedef pair CameraPair; + typedef size_t LandmarkKey; + // get a mapping from each landmark to its connected cameras + vector > cameraToLandmarks(dictionary.size()); + // for odometry xi-xj where i cameraToCamera(dictionary.size(), -1); + size_t key_i, key_j; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + if (factor_->key1.type == NODE_POSE_3D) { + if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor + cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); + } + else { // odometry factor + if (factor_->key1.index < factor_->key2.index) { + key_i = factor_->key1.index; + key_j = factor_->key2.index; + } else { + key_i = factor_->key2.index; + key_j = factor_->key1.index; + } + cameraToCamera[key_i] = key_j; + } + } + } - // sort the landmark keys for the late getNrCommonLandmarks call - BOOST_FOREACH(vector &landmarks, cameraToLandmarks){ - if (!landmarks.empty()) - std::sort(landmarks.begin(), landmarks.end()); - } + // sort the landmark keys for the late getNrCommonLandmarks call + BOOST_FOREACH(vector &landmarks, cameraToLandmarks){ + if (!landmarks.empty()) + std::sort(landmarks.begin(), landmarks.end()); + } - // generate the reduced graph - reducedGraph.clear(); - int factorIndex = 0; - int camera1, camera2, nrTotalConstraints; - bool hasOdometry; - for (size_t i1=0; i1 0 || hasOdometry) { - nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); - reducedGraph.push_back(boost::make_shared(camera1, camera2, - factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); - } - } - } - } + // generate the reduced graph + reducedGraph.clear(); + int factorIndex = 0; + int camera1, camera2, nrTotalConstraints; + bool hasOdometry; + for (size_t i1=0; i1 0 || hasOdometry) { + nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); + reducedGraph.push_back(boost::make_shared(camera1, camera2, + factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); + } + } + } + } - /* ************************************************************************* */ - void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, - WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { - workspace.prepareDictionary(frontals); - vector nrConstraints(workspace.dictionary.size(), 0); + /* ************************************************************************* */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { + workspace.prepareDictionary(frontals); + vector nrConstraints(workspace.dictionary.size(), 0); - // summarize the constraint number - const vector& dictionary = workspace.dictionary; - vector isValidCamera(workspace.dictionary.size(), false); - vector isValidLandmark(workspace.dictionary.size(), false); - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - assert(factor_->key1.type == NODE_POSE_3D); - //assert(factor_->key2.type == NODE_LANDMARK_3D); - const size_t& key1 = factor_->key1.index; - const size_t& key2 = factor_->key2.index; - if (dictionary[key1] == -1 || dictionary[key2] == -1) - continue; + // summarize the constraint number + const vector& dictionary = workspace.dictionary; + vector isValidCamera(workspace.dictionary.size(), false); + vector isValidLandmark(workspace.dictionary.size(), false); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + assert(factor_->key1.type == NODE_POSE_3D); + //assert(factor_->key2.type == NODE_LANDMARK_3D); + const size_t& key1 = factor_->key1.index; + const size_t& key2 = factor_->key2.index; + if (dictionary[key1] == -1 || dictionary[key2] == -1) + continue; - isValidCamera[key1] = true; - if(factor_->key2.type == NODE_LANDMARK_3D) - isValidLandmark[key2] = true; - else - isValidCamera[key2] = true; + isValidCamera[key1] = true; + if(factor_->key2.type == NODE_LANDMARK_3D) + isValidLandmark[key2] = true; + else + isValidCamera[key2] = true; - nrConstraints[key1]++; - nrConstraints[key2]++; + nrConstraints[key1]++; + nrConstraints[key2]++; - // a single pose constraint is sufficient for stereo, so we add 2 to the counter - // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera - if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ - nrConstraints[key1]+=2; - nrConstraints[key2]+=2; - } - } + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } - // find the minimum constraint for cameras and landmarks - size_t minFoundConstraintsPerCamera = 10000; - size_t minFoundConstraintsPerLandmark = 10000; + // find the minimum constraint for cameras and landmarks + size_t minFoundConstraintsPerCamera = 10000; + size_t minFoundConstraintsPerLandmark = 10000; - for (size_t i=0; i(minFoundConstraintsPerCamera)); - if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark) - throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast(minFoundConstraintsPerLandmark)); - } + if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera) + throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast(minFoundConstraintsPerCamera)); + if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark) + throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast(minFoundConstraintsPerLandmark)); + } }} // namespace diff --git a/gtsam_unstable/partition/GenericGraph.h b/gtsam_unstable/partition/GenericGraph.h index c722721b7..803a79719 100644 --- a/gtsam_unstable/partition/GenericGraph.h +++ b/gtsam_unstable/partition/GenericGraph.h @@ -17,133 +17,133 @@ namespace gtsam { namespace partition { - /*************************************************** - * 2D generic factors and their factor graph - ***************************************************/ - enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D }; + /*************************************************** + * 2D generic factors and their factor graph + ***************************************************/ + enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D }; - /** the index of the node and the type of the node */ - struct GenericNode2D { - std::size_t index; - GenericNode2DType type; - GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {} - }; + /** the index of the node and the type of the node */ + struct GenericNode2D { + std::size_t index; + GenericNode2DType type; + GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {} + }; - /** a factor always involves two nodes/variables for now */ - struct GenericFactor2D { - GenericNode2D key1; - GenericNode2D key2; - int index; // the factor index in the original nonlinear factor graph - int weight; // the weight of the edge - GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1) - : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} - GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1) - : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), - key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {} - }; + /** a factor always involves two nodes/variables for now */ + struct GenericFactor2D { + GenericNode2D key1; + GenericNode2D key2; + int index; // the factor index in the original nonlinear factor graph + int weight; // the weight of the edge + GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), + key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {} + }; - /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericFactor2D; - typedef std::vector GenericGraph2D; + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor2D; + typedef std::vector GenericGraph2D; - /** merge nodes in DSF using constraints captured by the given graph */ - std::list > findIslands(const GenericGraph2D& graph, const std::vector& keys, WorkSpace& workspace, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph2D& graph, const std::vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); - /** eliminate the sensors from generic graph */ - inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, - const std::vector& dictionary, GenericGraph2D& reducedGraph) { - throw std::runtime_error("reduceGenericGraph 2d not implemented"); - } + /** eliminate the sensors from generic graph */ + inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph2D& reducedGraph) { + throw std::runtime_error("reduceGenericGraph 2d not implemented"); + } - /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */ - inline void checkSingularity(const GenericGraph2D& graph, const std::vector& frontals, - WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; } + /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */ + inline void checkSingularity(const GenericGraph2D& graph, const std::vector& frontals, + WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; } - /** print the graph **/ - void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D"); + /** print the graph **/ + void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D"); - /*************************************************** - * 3D generic factors and their factor graph - ***************************************************/ - enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D }; + /*************************************************** + * 3D generic factors and their factor graph + ***************************************************/ + enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D }; -// const int minNrConstraintsPerCamera = 7; -// const int minNrConstraintsPerLandmark = 2; +// const int minNrConstraintsPerCamera = 7; +// const int minNrConstraintsPerLandmark = 2; - /** the index of the node and the type of the node */ - struct GenericNode3D { - std::size_t index; - GenericNode3DType type; - GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {} - }; + /** the index of the node and the type of the node */ + struct GenericNode3D { + std::size_t index; + GenericNode3DType type; + GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {} + }; - /** a factor always involves two nodes/variables for now */ - struct GenericFactor3D { - GenericNode3D key1; - GenericNode3D key2; - int index; // the index in the entire graph, 0-based - int weight; // the weight of the edge - GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {} - GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1, - const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1) - : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} - }; + /** a factor always involves two nodes/variables for now */ + struct GenericFactor3D { + GenericNode3D key1; + GenericNode3D key2; + int index; // the index in the entire graph, 0-based + int weight; // the weight of the edge + GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {} + GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1, + const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + }; - /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericFactor3D; - typedef std::vector GenericGraph3D; + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor3D; + typedef std::vector GenericGraph3D; - /** merge nodes in DSF using constraints captured by the given graph */ - std::list > findIslands(const GenericGraph3D& graph, const std::vector& keys, WorkSpace& workspace, - const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph3D& graph, const std::vector& keys, WorkSpace& workspace, + const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); - /** eliminate the sensors from generic graph */ - void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, - const std::vector& dictionary, GenericGraph3D& reducedGraph); + /** eliminate the sensors from generic graph */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph); - /** check whether the 3D graph is singular (under constrained) */ - void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, - WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); + /** check whether the 3D graph is singular (under constrained) */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); - /** print the graph **/ - void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D"); + /** print the graph **/ + void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D"); - /*************************************************** - * unary generic factors and their factor graph - ***************************************************/ - /** a factor involves a single variable */ - struct GenericUnaryFactor { - GenericNode2D key; - int index; // the factor index in the original nonlinear factor graph - GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1) - : key(key_, type_), index(index_) {} - GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1) - : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {} - }; + /*************************************************** + * unary generic factors and their factor graph + ***************************************************/ + /** a factor involves a single variable */ + struct GenericUnaryFactor { + GenericNode2D key; + int index; // the factor index in the original nonlinear factor graph + GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1) + : key(key_, type_), index(index_) {} + GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1) + : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {} + }; - /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericUnaryFactor; - typedef std::vector GenericUnaryGraph; + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericUnaryFactor; + typedef std::vector GenericUnaryGraph; - /*************************************************** - * utility functions - ***************************************************/ - inline bool hasCommonCamera(const std::set& cameras1, const std::set& cameras2) { - if (cameras1.empty() || cameras2.empty()) - throw std::invalid_argument("hasCommonCamera: the input camera set is empty!"); - std::set::const_iterator it1 = cameras1.begin(); - std::set::const_iterator it2 = cameras2.begin(); - while (it1 != cameras1.end() && it2 != cameras2.end()) { - if (*it1 == *it2) - return true; - else if (*it1 < *it2) - it1++; - else - it2++; - } - return false; - } + /*************************************************** + * utility functions + ***************************************************/ + inline bool hasCommonCamera(const std::set& cameras1, const std::set& cameras2) { + if (cameras1.empty() || cameras2.empty()) + throw std::invalid_argument("hasCommonCamera: the input camera set is empty!"); + std::set::const_iterator it1 = cameras1.begin(); + std::set::const_iterator it2 = cameras2.begin(); + while (it1 != cameras1.end() && it2 != cameras2.end()) { + if (*it1 == *it2) + return true; + else if (*it1 < *it2) + it1++; + else + it2++; + } + return false; + } }} // namespace diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h index d6dafce49..6f1818a3e 100644 --- a/gtsam_unstable/partition/NestedDissection-inl.h +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -16,236 +16,236 @@ namespace gtsam { namespace partition { - /* ************************************************************************* */ - template - NestedDissection::NestedDissection( - const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) : - fg_(fg), ordering_(ordering){ - GenericUnaryGraph unaryFactors; - GenericGraph gfg; - boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) : + fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); - // build reverse mapping from integer to symbol - int numNodes = ordering.size(); - int2symbol_.resize(numNodes); - Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); - while(it != itLast) - int2symbol_[it->second] = (it++)->first; + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; - vector keys; - keys.reserve(numNodes); - for(int i=0; i keys; + keys.reserve(numNodes); + for(int i=0; i(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr(), workspace, verbose); - } + WorkSpace workspace(numNodes); + root_ = recursivePartition(gfg, unaryFactors, keys, vector(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr(), workspace, verbose); + } - /* ************************************************************************* */ - template - NestedDissection::NestedDissection( - const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ - GenericUnaryGraph unaryFactors; - GenericGraph gfg; - boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); - // build reverse mapping from integer to symbol - int numNodes = ordering.size(); - int2symbol_.resize(numNodes); - Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); - while(it != itLast) - int2symbol_[it->second] = (it++)->first; + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; - vector keys; - keys.reserve(numNodes); - for(int i=0; i keys; + keys.reserve(numNodes); + for(int i=0; i(), cuts, boost::shared_ptr(), workspace, verbose); - } + WorkSpace workspace(numNodes); + root_ = recursivePartition(gfg, unaryFactors, keys, vector(), cuts, boost::shared_ptr(), workspace, verbose); + } - /* ************************************************************************* */ - template - boost::shared_ptr NestedDissection::makeSubNLG( - const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { - OrderedSymbols frontalKeys; - BOOST_FOREACH(const size_t index, frontals) - frontalKeys.push_back(int2symbol_[index]); + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::makeSubNLG( + const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { + OrderedSymbols frontalKeys; + BOOST_FOREACH(const size_t index, frontals) + frontalKeys.push_back(int2symbol_[index]); - UnorderedSymbols sepKeys; - BOOST_FOREACH(const size_t index, sep) - sepKeys.insert(int2symbol_[index]); + UnorderedSymbols sepKeys; + BOOST_FOREACH(const size_t index, sep) + sepKeys.insert(int2symbol_[index]); - return boost::make_shared(fg, frontalKeys, sepKeys, parent); - } + return boost::make_shared(fg, frontalKeys, sepKeys, parent); + } - /* ************************************************************************* */ - template - void NestedDissection::processFactor( - const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input - vector& frontalFactors, NLG& sepFactors, vector >& childSeps, // output factor graphs - typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques - list sep_; // the separator variables involved in the current factor - int partition1 = partitionTable[factor->key1.index]; - int partition2 = partitionTable[factor->key2.index]; - if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique - sepFactors.push_back(fg_[factor->index]); - } - else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques) - weeklinks.push_back(fg_[factor->index]); - } - else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques - frontalFactors[partition1 - 1].push_back(factor); - } - else { // is a joint factor in the child clique (involving varaibles in the current clique) - if (partition1 > 0 && partition2 <= 0) { - frontalFactors[partition1 - 1].push_back(factor); - childSeps[partition1 - 1].insert(factor->key2.index); - } else if (partition1 <= 0 && partition2 > 0) { - frontalFactors[partition2 - 1].push_back(factor); - childSeps[partition2 - 1].insert(factor->key1.index); - } else - throw runtime_error("processFactor: unexpected entries in the partition table!"); - } - } + /* ************************************************************************* */ + template + void NestedDissection::processFactor( + const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + vector& frontalFactors, NLG& sepFactors, vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + list sep_; // the separator variables involved in the current factor + int partition1 = partitionTable[factor->key1.index]; + int partition2 = partitionTable[factor->key2.index]; + if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique + sepFactors.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques) + weeklinks.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques + frontalFactors[partition1 - 1].push_back(factor); + } + else { // is a joint factor in the child clique (involving varaibles in the current clique) + if (partition1 > 0 && partition2 <= 0) { + frontalFactors[partition1 - 1].push_back(factor); + childSeps[partition1 - 1].insert(factor->key2.index); + } else if (partition1 <= 0 && partition2 > 0) { + frontalFactors[partition2 - 1].push_back(factor); + childSeps[partition2 - 1].insert(factor->key1.index); + } else + throw runtime_error("processFactor: unexpected entries in the partition table!"); + } + } - /* ************************************************************************* */ - /** - * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors}) - * and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals}) - * and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into - * the correspoding ordering in {childSeps}. - */ - // TODO: frontalFactors and localFrontals should be generated in findSeparator - template - void NestedDissection::partitionFactorsAndVariables( - const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector& keys, //input - const std::vector& partitionTable, const int numSubmaps, // input - vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs - vector >& childFrontals, vector >& childSeps, vector& localFrontals, // output sub-orderings - typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + /* ************************************************************************* */ + /** + * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors}) + * and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals}) + * and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into + * the correspoding ordering in {childSeps}. + */ + // TODO: frontalFactors and localFrontals should be generated in findSeparator + template + void NestedDissection::partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector& keys, //input + const std::vector& partitionTable, const int numSubmaps, // input + vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + vector >& childFrontals, vector >& childSeps, vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques - // make three lists of variables A, B, and C - int partition; - childFrontals.resize(numSubmaps); - BOOST_FOREACH(const size_t key, keys){ - partition = partitionTable[key]; - switch (partition) { - case -1: break; // the separator of the separator variables - case 0: localFrontals.push_back(key); break; // the separator variables - default: childFrontals[partition-1].push_back(key); // the frontal variables - } - } + // make three lists of variables A, B, and C + int partition; + childFrontals.resize(numSubmaps); + BOOST_FOREACH(const size_t key, keys){ + partition = partitionTable[key]; + switch (partition) { + case -1: break; // the separator of the separator variables + case 0: localFrontals.push_back(key); break; // the separator variables + default: childFrontals[partition-1].push_back(key); // the frontal variables + } + } - // group the factors to {frontalFactors} and {sepFactors},and find the joint variables - vector > childSeps_; - childSeps_.resize(numSubmaps); - childSeps.reserve(numSubmaps); - frontalFactors.resize(numSubmaps); - frontalUnaryFactors.resize(numSubmaps); - BOOST_FOREACH(typename GenericGraph::value_type factor, fg) - processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); - BOOST_FOREACH(const set& childSep, childSeps_) - childSeps.push_back(vector(childSep.begin(), childSep.end())); + // group the factors to {frontalFactors} and {sepFactors},and find the joint variables + vector > childSeps_; + childSeps_.resize(numSubmaps); + childSeps.reserve(numSubmaps); + frontalFactors.resize(numSubmaps); + frontalUnaryFactors.resize(numSubmaps); + BOOST_FOREACH(typename GenericGraph::value_type factor, fg) + processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); + BOOST_FOREACH(const set& childSep, childSeps_) + childSeps.push_back(vector(childSep.begin(), childSep.end())); - // add unary factor to the current cluster or pass it to one of the child clusters - BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { - partition = partitionTable[unaryFactor_->key.index]; - if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); - else frontalUnaryFactors[partition-1].push_back(unaryFactor_); - } - } + // add unary factor to the current cluster or pass it to one of the child clusters + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { + partition = partitionTable[unaryFactor_->key.index]; + if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); + else frontalUnaryFactors[partition-1].push_back(unaryFactor_); + } + } - /* ************************************************************************* */ - template - NLG NestedDissection::collectOriginalFactors( - const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const { - NLG sepFactors; - typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); - while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); - BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) - sepFactors.push_back(fg_[unaryFactor_->index]); - return sepFactors; - } + /* ************************************************************************* */ + template + NLG NestedDissection::collectOriginalFactors( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const { + NLG sepFactors; + typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); + while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) + sepFactors.push_back(fg_[unaryFactor_->index]); + return sepFactors; + } - /* ************************************************************************* */ - template - boost::shared_ptr NestedDissection::recursivePartition( - const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, - const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { - // if no split needed - NLG sepFactors; // factors that should remain in the current cluster - if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) { - sepFactors = collectOriginalFactors(gfg, unaryFactors); - return makeSubNLG(sepFactors, frontals, sep, parent); - } + // if no split needed + NLG sepFactors; // factors that should remain in the current cluster + if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } - // find the nested dissection separator - int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(), - NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); - partition::PartitionTable& partitionTable = workspace.partitionTable; - if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!"); + // find the nested dissection separator + int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(), + NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); + partition::PartitionTable& partitionTable = workspace.partitionTable; + if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!"); - // split the factors between child cliques and the current clique - vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; - vector localFrontals; vector > childFrontals, childSeps; - partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, - frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); - // make a new cluster - boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); - current->setWeeklinks(weeklinks); + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); - // check whether all the submaps are fully constrained - for (int i=0; i child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], - numNodeStopPartition, minNodesPerMap, current, workspace, verbose); - current->addChild(child); - } + // create child clusters + for (int i=0; i child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + numNodeStopPartition, minNodesPerMap, current, workspace, verbose); + current->addChild(child); + } - return current; - } + return current; + } - /* ************************************************************************* */ - template - boost::shared_ptr NestedDissection::recursivePartition( - const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, - const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { - // if there is no need to cut any more - NLG sepFactors; // factors that should remain in the current cluster - if (!cuts.get()) { - sepFactors = collectOriginalFactors(gfg, unaryFactors); - return makeSubNLG(sepFactors, frontals, sep, parent); - } + // if there is no need to cut any more + NLG sepFactors; // factors that should remain in the current cluster + if (!cuts.get()) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } - // retrieve the current partitioning info - int numSubmaps = 2; - partition::PartitionTable& partitionTable = cuts->partitionTable; + // retrieve the current partitioning info + int numSubmaps = 2; + partition::PartitionTable& partitionTable = cuts->partitionTable; - // split the factors between child cliques and the current clique - vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; - vector localFrontals; vector > childFrontals, childSeps; - partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, - frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); - // make a new cluster - boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); - current->setWeeklinks(weeklinks); + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); - // create child clusters - for (int i=0; i<2; i++) { - boost::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], - cuts->children.empty() ? boost::shared_ptr() : cuts->children[i], current, workspace, verbose); - current->addChild(child); - } - return current; - } + // create child clusters + for (int i=0; i<2; i++) { + boost::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + cuts->children.empty() ? boost::shared_ptr() : cuts->children[i], current, workspace, verbose); + current->addChild(child); + } + return current; + } }} //namespace diff --git a/gtsam_unstable/partition/NestedDissection.h b/gtsam_unstable/partition/NestedDissection.h index 3c294be64..82304a79d 100644 --- a/gtsam_unstable/partition/NestedDissection.h +++ b/gtsam_unstable/partition/NestedDissection.h @@ -14,56 +14,56 @@ namespace gtsam { namespace partition { - /** - * Apply nested dissection algorithm to nonlinear factor graphs - */ - template - class NestedDissection { - public: - typedef boost::shared_ptr sharedSubNLG; + /** + * Apply nested dissection algorithm to nonlinear factor graphs + */ + template + class NestedDissection { + public: + typedef boost::shared_ptr sharedSubNLG; - private: - NLG fg_; // the original nonlinear factor graph - Ordering ordering_; // the variable ordering in the nonlinear factor graph - std::vector int2symbol_; // the mapping from integer key to symbol - sharedSubNLG root_; // the root of generated cluster tree + private: + NLG fg_; // the original nonlinear factor graph + Ordering ordering_; // the variable ordering in the nonlinear factor graph + std::vector int2symbol_; // the mapping from integer key to symbol + sharedSubNLG root_; // the root of generated cluster tree - public: - sharedSubNLG root() const { return root_; } + public: + sharedSubNLG root() const { return root_; } - public: - /* constructor with post-determined partitoning*/ - NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); + public: + /* constructor with post-determined partitoning*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); - /* constructor with pre-determined cuts*/ - NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose = false); + /* constructor with pre-determined cuts*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose = false); - private: - /* convert generic subgraph to nonlinear subgraph */ - sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const boost::shared_ptr& parent) const; + private: + /* convert generic subgraph to nonlinear subgraph */ + sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const boost::shared_ptr& parent) const; - void processFactor(const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input - std::vector& frontalFactors, NLG& sepFactors, std::vector >& childSeps, // output factor graphs - typename SubNLG::Weeklinks& weeklinks) const; + void processFactor(const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + std::vector& frontalFactors, NLG& sepFactors, std::vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const; - /* recursively partition the generic graph */ - void partitionFactorsAndVariables( - const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, - const std::vector& keys, const std::vector& partitionTable, const int numSubmaps, // input - std::vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs - std::vector >& childFrontals, std::vector >& childSeps, std::vector& localFrontals, // output sub-orderings - typename SubNLG::Weeklinks& weeklinks) const; + /* recursively partition the generic graph */ + void partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, + const std::vector& keys, const std::vector& partitionTable, const int numSubmaps, // input + std::vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + std::vector >& childFrontals, std::vector >& childSeps, std::vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const; - NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const; + NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const; - /* recursively partition the generic graph */ - sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, - const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; - /* recursively partition the generic graph */ - sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, - const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; - }; + }; }} //namespace diff --git a/gtsam_unstable/partition/PartitionWorkSpace.h b/gtsam_unstable/partition/PartitionWorkSpace.h index b268c0fb2..b0bd8113e 100644 --- a/gtsam_unstable/partition/PartitionWorkSpace.h +++ b/gtsam_unstable/partition/PartitionWorkSpace.h @@ -13,32 +13,32 @@ namespace gtsam { namespace partition { - typedef std::vector PartitionTable; + typedef std::vector PartitionTable; - // the work space, preallocated memory - struct WorkSpace { - std::vector dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs - boost::shared_ptr > dsf; // a block memory pre-allocated for DSFVector - PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap + // the work space, preallocated memory + struct WorkSpace { + std::vector dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs + boost::shared_ptr > dsf; // a block memory pre-allocated for DSFVector + PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap - // constructor - WorkSpace(const size_t numNodes) : dictionary(numNodes,0), - dsf(new std::vector(numNodes, 0)), partitionTable(numNodes, -1) { } + // constructor + WorkSpace(const size_t numNodes) : dictionary(numNodes,0), + dsf(new std::vector(numNodes, 0)), partitionTable(numNodes, -1) { } - // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index - inline void prepareDictionary(const std::vector& keys) { - int index = 0; - std::fill(dictionary.begin(), dictionary.end(), -1); - std::vector::const_iterator it=keys.begin(), itLast=keys.end(); - while(it!=itLast) dictionary[*(it++)] = index++; - } - }; + // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index + inline void prepareDictionary(const std::vector& keys) { + int index = 0; + std::fill(dictionary.begin(), dictionary.end(), -1); + std::vector::const_iterator it=keys.begin(), itLast=keys.end(); + while(it!=itLast) dictionary[*(it++)] = index++; + } + }; - // manually defined cuts - struct Cuts { - PartitionTable partitionTable; - std::vector > children; - }; + // manually defined cuts + struct Cuts { + PartitionTable partitionTable; + std::vector > children; + }; }} // namespace diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index 4943f5456..9bdf40026 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -185,45 +185,45 @@ TEST ( Partition, findSeparator2 ) // x25 x26 x27 x28 TEST ( Partition, findSeparator3_with_reduced_camera ) { - GenericGraph3D graph; - for (int j=1; j<=8; j++) - graph.push_back(boost::make_shared(25, j)); - for (int j=1; j<=16; j++) - graph.push_back(boost::make_shared(26, j)); - for (int j=9; j<=24; j++) - graph.push_back(boost::make_shared(27, j)); - for (int j=17; j<=24; j++) - graph.push_back(boost::make_shared(28, j)); + GenericGraph3D graph; + for (int j=1; j<=8; j++) + graph.push_back(boost::make_shared(25, j)); + for (int j=1; j<=16; j++) + graph.push_back(boost::make_shared(26, j)); + for (int j=9; j<=24; j++) + graph.push_back(boost::make_shared(27, j)); + for (int j=17; j<=24; j++) + graph.push_back(boost::make_shared(28, j)); - std::vector keys; - for(int i=1; i<=28; i++) - keys.push_back(i); + std::vector keys; + for(int i=1; i<=28; i++) + keys.push_back(i); - vector int2symbol; - int2symbol.push_back(Symbol('x',0)); // dummy - for(int i=1; i<=24; i++) - int2symbol.push_back(Symbol('l',i)); - int2symbol.push_back(Symbol('x',25)); - int2symbol.push_back(Symbol('x',26)); - int2symbol.push_back(Symbol('x',27)); - int2symbol.push_back(Symbol('x',28)); + vector int2symbol; + int2symbol.push_back(Symbol('x',0)); // dummy + for(int i=1; i<=24; i++) + int2symbol.push_back(Symbol('l',i)); + int2symbol.push_back(Symbol('x',25)); + int2symbol.push_back(Symbol('x',26)); + int2symbol.push_back(Symbol('x',27)); + int2symbol.push_back(Symbol('x',28)); - WorkSpace workspace(29); - bool reduceGraph = true; - int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0); - LONGS_EQUAL(2, numIsland); + WorkSpace workspace(29); + bool reduceGraph = true; + int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0); + LONGS_EQUAL(2, numIsland); - partition::PartitionTable& partitionTable = workspace.partitionTable; - for (int j=1; j<=8; j++) - LONGS_EQUAL(1, partitionTable[j]); - for (int j=9; j<=16; j++) - LONGS_EQUAL(0, partitionTable[j]); - for (int j=17; j<=24; j++) - LONGS_EQUAL(2, partitionTable[j]); - LONGS_EQUAL(1, partitionTable[25]); - LONGS_EQUAL(1, partitionTable[26]); - LONGS_EQUAL(2, partitionTable[27]); - LONGS_EQUAL(2, partitionTable[28]); + partition::PartitionTable& partitionTable = workspace.partitionTable; + for (int j=1; j<=8; j++) + LONGS_EQUAL(1, partitionTable[j]); + for (int j=9; j<=16; j++) + LONGS_EQUAL(0, partitionTable[j]); + for (int j=17; j<=24; j++) + LONGS_EQUAL(2, partitionTable[j]); + LONGS_EQUAL(1, partitionTable[25]); + LONGS_EQUAL(1, partitionTable[26]); + LONGS_EQUAL(2, partitionTable[27]); + LONGS_EQUAL(2, partitionTable[28]); } /* ************************************************************************* */ diff --git a/gtsam_unstable/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp index 7d9bf928f..c6e432902 100644 --- a/gtsam_unstable/partition/tests/testGenericGraph.cpp +++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp @@ -29,29 +29,29 @@ using namespace gtsam::partition; */ TEST ( GenerciGraph, findIslands ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; - WorkSpace workspace(10); // from 0 to 9 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 2, 3, 7, 8; - vector island2; island2 += 4, 5, 6, 9; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(10); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 2, 3, 7, 8; + vector island2; island2 += 4, 5, 6, 9; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -62,27 +62,27 @@ TEST ( GenerciGraph, findIslands ) */ TEST( GenerciGraph, findIslands2 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; - WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(1, islands.size()); - vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; - CHECK(island1 == islands.front()); + WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(1, islands.size()); + vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; + CHECK(island1 == islands.front()); } /* ************************************************************************* */ @@ -92,21 +92,21 @@ TEST( GenerciGraph, findIslands2 ) */ TEST ( GenerciGraph, findIslands3 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6; + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6; - WorkSpace workspace(7); // from 0 to 9 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 5; - vector island2; island2 += 2, 3, 4, 6; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(7); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 5; + vector island2; island2 += 2, 3, 4, 6; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -115,18 +115,18 @@ TEST ( GenerciGraph, findIslands3 ) */ TEST ( GenerciGraph, findIslands4 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - std::vector keys; keys += 3, 4, 7; + GenericGraph2D graph; + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + std::vector keys; keys += 3, 4, 7; - WorkSpace workspace(8); // from 0 to 7 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 3, 4; - vector island2; island2 += 7; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(8); // from 0 to 7 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 3, 4; + vector island2; island2 += 7; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -137,24 +137,24 @@ TEST ( GenerciGraph, findIslands4 ) */ TEST ( GenerciGraph, findIslands5 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5; + std::vector keys; keys += 1, 2, 3, 4, 5; - WorkSpace workspace(6); // from 0 to 5 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 3, 5; - vector island2; island2 += 2, 4; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(6); // from 0 to 5 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 3, 5; + vector island2; island2 += 2, 4; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -165,31 +165,31 @@ TEST ( GenerciGraph, findIslands5 ) */ TEST ( GenerciGraph, reduceGenericGraph ) { - GenericGraph3D graph; - graph.push_back(boost::make_shared(1, 3)); - graph.push_back(boost::make_shared(1, 4)); - graph.push_back(boost::make_shared(1, 5)); - graph.push_back(boost::make_shared(2, 5)); - graph.push_back(boost::make_shared(2, 6)); + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3)); + graph.push_back(boost::make_shared(1, 4)); + graph.push_back(boost::make_shared(1, 5)); + graph.push_back(boost::make_shared(2, 5)); + graph.push_back(boost::make_shared(2, 6)); - std::vector cameraKeys, landmarkKeys; - cameraKeys.push_back(1); - cameraKeys.push_back(2); - landmarkKeys.push_back(3); - landmarkKeys.push_back(4); - landmarkKeys.push_back(5); - landmarkKeys.push_back(6); + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); - std::vector dictionary; - dictionary.resize(7, -1); // from 0 to 6 - dictionary[1] = 0; - dictionary[2] = 1; + std::vector dictionary; + dictionary.resize(7, -1); // from 0 to 6 + dictionary[1] = 0; + dictionary[2] = 1; - GenericGraph3D reduced; - std::map > cameraToLandmarks; - reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); - LONGS_EQUAL(1, reduced.size()); - LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(1, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); } /* ************************************************************************* */ @@ -200,53 +200,53 @@ TEST ( GenerciGraph, reduceGenericGraph ) */ TEST ( GenericGraph, reduceGenericGraph2 ) { - GenericGraph3D graph; - graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); - std::vector cameraKeys, landmarkKeys; - cameraKeys.push_back(1); - cameraKeys.push_back(2); - cameraKeys.push_back(7); - landmarkKeys.push_back(3); - landmarkKeys.push_back(4); - landmarkKeys.push_back(5); - landmarkKeys.push_back(6); + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + cameraKeys.push_back(7); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); - std::vector dictionary; - dictionary.resize(8, -1); // from 0 to 7 - dictionary[1] = 0; - dictionary[2] = 1; - dictionary[7] = 6; + std::vector dictionary; + dictionary.resize(8, -1); // from 0 to 7 + dictionary[1] = 0; + dictionary[2] = 1; + dictionary[7] = 6; - GenericGraph3D reduced; - std::map > cameraToLandmarks; - reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); - LONGS_EQUAL(2, reduced.size()); - LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); - LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(2, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); + LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); } /* ************************************************************************* */ TEST ( GenerciGraph, hasCommonCamera ) { - std::set cameras1; cameras1 += 1, 2, 3, 4, 5; - std::set cameras2; cameras2 += 8, 7, 6, 5; - bool actual = hasCommonCamera(cameras1, cameras2); - CHECK(actual); + std::set cameras1; cameras1 += 1, 2, 3, 4, 5; + std::set cameras2; cameras2 += 8, 7, 6, 5; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(actual); } /* ************************************************************************* */ TEST ( GenerciGraph, hasCommonCamera2 ) { - std::set cameras1; cameras1 += 1, 3, 5, 7; - std::set cameras2; cameras2 += 2, 4, 6, 8, 10; - bool actual = hasCommonCamera(cameras1, cameras2); - CHECK(!actual); + std::set cameras1; cameras1 += 1, 3, 5, 7; + std::set cameras2; cameras2 += 2, 4, 6, 8, 10; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(!actual); } /* ************************************************************************* */ diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp index 906178914..d7035c2d8 100644 --- a/gtsam_unstable/partition/tests/testNestedDissection.cpp +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -32,22 +32,22 @@ using namespace gtsam::partition; // l1 TEST ( NestedDissection, oneIsland ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 2, Pose2(), odoNoise); - fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); - fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); - fg.addPoseConstraint(1, Pose2()); + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); - Ordering ordering; ordering += x1, x2, l1; + Ordering ordering; ordering += x1, x2, l1; - int numNodeStopPartition = 1e3; - int minNodesPerMap = 1e3; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(4, nd.root()->size()); - LONGS_EQUAL(3, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->children().size()); + int numNodeStopPartition = 1e3; + int minNodesPerMap = 1e3; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(4, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->children().size()); } /* ************************************************************************* */ @@ -56,35 +56,35 @@ TEST ( NestedDissection, oneIsland ) // x2/ \x5 TEST ( NestedDissection, TwoIslands ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 2, Pose2(), odoNoise); - fg.addOdometry(1, 3, Pose2(), odoNoise); - fg.addOdometry(2, 3, Pose2(), odoNoise); - fg.addOdometry(3, 4, Pose2(), odoNoise); - fg.addOdometry(4, 5, Pose2(), odoNoise); - fg.addOdometry(3, 5, Pose2(), odoNoise); - fg.addPoseConstraint(1, Pose2()); - fg.addPoseConstraint(4, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5; + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(4, 5, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; - int numNodeStopPartition = 2; - int minNodesPerMap = 1; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - // root submap - LONGS_EQUAL(0, nd.root()->size()); - LONGS_EQUAL(1, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + // root submap + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps - // the 1st submap - LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); - LONGS_EQUAL(4, nd.root()->children()[0]->size()); + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[0]->size()); - // the 2nd submap - LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); - LONGS_EQUAL(4, nd.root()->children()[1]->size()); + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[1]->size()); } /* ************************************************************************* */ @@ -93,40 +93,40 @@ TEST ( NestedDissection, TwoIslands ) // x2/ \x5 TEST ( NestedDissection, FourIslands ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 3, Pose2(), odoNoise); - fg.addOdometry(2, 3, Pose2(), odoNoise); - fg.addOdometry(3, 4, Pose2(), odoNoise); - fg.addOdometry(3, 5, Pose2(), odoNoise); - fg.addPoseConstraint(1, Pose2()); - fg.addPoseConstraint(4, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5; + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; - int numNodeStopPartition = 2; - int minNodesPerMap = 1; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(0, nd.root()->size()); - LONGS_EQUAL(1, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps - // the 1st submap - LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); - LONGS_EQUAL(2, nd.root()->children()[0]->size()); + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[0]->size()); - // the 2nd submap - LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); - LONGS_EQUAL(2, nd.root()->children()[1]->size()); + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[1]->size()); - // the 3rd submap - LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); - LONGS_EQUAL(1, nd.root()->children()[2]->size()); + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[2]->size()); - // the 4th submap - LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); - LONGS_EQUAL(1, nd.root()->children()[3]->size()); + // the 4th submap + LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[3]->size()); } /* ************************************************************************* */ @@ -137,41 +137,41 @@ TEST ( NestedDissection, FourIslands ) // x5 TEST ( NestedDissection, weekLinks ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 2, Pose2(), odoNoise); - fg.addOdometry(2, 3, Pose2(), odoNoise); - fg.addOdometry(2, 4, Pose2(), odoNoise); - fg.addOdometry(3, 4, Pose2(), odoNoise); - fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); - fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); - fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); - fg.addPoseConstraint(1, Pose2()); - fg.addPoseConstraint(4, Pose2()); - fg.addPoseConstraint(5, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(2, 4, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + fg.addPoseConstraint(5, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; - int numNodeStopPartition = 2; - int minNodesPerMap = 1; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(0, nd.root()->size()); // one weeklink - LONGS_EQUAL(1, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps - LONGS_EQUAL(1, nd.root()->weeklinks().size()); + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); // one weeklink + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps + LONGS_EQUAL(1, nd.root()->weeklinks().size()); - // the 1st submap - LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 - LONGS_EQUAL(4, nd.root()->children()[0]->size()); + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); - // the 2nd submap - LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 - LONGS_EQUAL(4, nd.root()->children()[1]->size()); - // - // the 3rd submap - LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 - LONGS_EQUAL(1, nd.root()->children()[2]->size()); + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 + LONGS_EQUAL(4, nd.root()->children()[1]->size()); + // + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 + LONGS_EQUAL(1, nd.root()->children()[2]->size()); } /* ************************************************************************* */ @@ -184,86 +184,86 @@ TEST ( NestedDissection, weekLinks ) */ TEST ( NestedDissection, manual_cuts ) { - using namespace submapPlanarSLAM; - typedef partition::Cuts Cuts; - typedef TSAM2D::SubNLG SubNLG; - typedef partition::PartitionTable PartitionTable; - Graph fg; - fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); - fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); + using namespace submapPlanarSLAM; + typedef partition::Cuts Cuts; + typedef TSAM2D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + Graph fg; + fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); + fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); - fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); - fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); + fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); - // generate ordering - Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; + // generate ordering + Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; - // define cuts - boost::shared_ptr cuts(new Cuts()); - cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; - //x0 x1 x2 l1 l2 l3 l4 l5 l6 - (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; + // define cuts + boost::shared_ptr cuts(new Cuts()); + cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; - cuts->children.push_back(boost::shared_ptr(new Cuts())); - cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; - //x0 x1 x2 l1 l2 l3 l4 l5 l6 - (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; - cuts->children.push_back(boost::shared_ptr(new Cuts())); - cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; - //x0 x1 x2 l1 l2 l3 l4 l5 l6 - (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; - // nested dissection - NestedDissection nd(fg, ordering, cuts); - LONGS_EQUAL(2, nd.root()->size()); - LONGS_EQUAL(3, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps - LONGS_EQUAL(0, nd.root()->weeklinks().size()); + // nested dissection + NestedDissection nd(fg, ordering, cuts); + LONGS_EQUAL(2, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); - // the 1st submap - LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 - LONGS_EQUAL(4, nd.root()->children()[0]->size()); - LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); - // the 1-1st submap - LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 - LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); + // the 1-1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); - // the 1-2nd submap - LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 - LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); + // the 1-2nd submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); - // the 2nd submap - LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 - LONGS_EQUAL(3, nd.root()->children()[1]->size()); - LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 + LONGS_EQUAL(3, nd.root()->children()[1]->size()); + LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); - // the 2-1st submap - LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 - LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); + // the 2-1st submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); - // the 2-2nd submap - LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 - LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); + // the 2-2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); } @@ -272,65 +272,65 @@ TEST ( NestedDissection, manual_cuts ) // / | / \ | \ // x0 x1 x2 x3 TEST( NestedDissection, Graph3D) { - using namespace gtsam::submapVisualSLAM; - typedef TSAM3D::SubNLG SubNLG; - typedef partition::PartitionTable PartitionTable; - vector cameras; - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); + using namespace gtsam::submapVisualSLAM; + typedef TSAM3D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + vector cameras; + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); - vector points; - for (int cube_index = 0; cube_index <= 3; cube_index++) { - Point3 center((cube_index-1) * 3, 0.5, 10.); - points.push_back(center + Point3(-0.5, -0.5, -0.5)); - points.push_back(center + Point3(-0.5, 0.5, -0.5)); - points.push_back(center + Point3( 0.5, 0.5, -0.5)); - points.push_back(center + Point3( 0.5, -0.5, -0.5)); - points.push_back(center + Point3(-0.5, -0.5, 0.5)); - points.push_back(center + Point3(-0.5, 0.5, 0.5)); - points.push_back(center + Point3( 0.5, 0.5, 0.5)); - points.push_back(center + Point3( 0.5, 0.5, 0.5)); - } + vector points; + for (int cube_index = 0; cube_index <= 3; cube_index++) { + Point3 center((cube_index-1) * 3, 0.5, 10.); + points.push_back(center + Point3(-0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, -0.5, 0.5)); + points.push_back(center + Point3(-0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + } - Graph graph; - SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); - SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); - for (int j=1; j<=8; j++) - graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - for (int j=1; j<=16; j++) - graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - for (int j=9; j<=24; j++) - graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - for (int j=17; j<=24; j++) - graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + Graph graph; + SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); + SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); + for (int j=1; j<=8; j++) + graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=1; j<=16; j++) + graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=9; j<=24; j++) + graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=17; j<=24; j++) + graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - // make an easy ordering - Ordering ordering; ordering += x0, x1, x2, x3; - for (int j=1; j<=24; j++) - ordering += Symbol('l', j); + // make an easy ordering + Ordering ordering; ordering += x0, x1, x2, x3; + for (int j=1; j<=24; j++) + ordering += Symbol('l', j); - // nested dissection - const int numNodeStopPartition = 10; - const int minNodesPerMap = 5; - NestedDissection nd(graph, ordering, numNodeStopPartition, minNodesPerMap); + // nested dissection + const int numNodeStopPartition = 10; + const int minNodesPerMap = 5; + NestedDissection nd(graph, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(0, nd.root()->size()); - LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps - LONGS_EQUAL(0, nd.root()->weeklinks().size()); + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); - // the 1st submap - LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 - LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 - LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); + // the 1st submap + LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 + LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); - // the 2nd submap - LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 - LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 - LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); + // the 2nd submap + LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 + LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); } diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 6c89f4c03..c147552b3 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -295,87 +295,87 @@ namespace gtsam { /* ************************************************************************* */ SharedGaussian get_model_inlier() const { - return model_inlier_; + return model_inlier_; } /* ************************************************************************* */ SharedGaussian get_model_outlier() const { - return model_outlier_; + return model_outlier_; } /* ************************************************************************* */ Matrix get_model_inlier_cov() const { - return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); } /* ************************************************************************* */ Matrix get_model_outlier_cov() const { - return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); } /* ************************************************************************* */ void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ - // get joint covariance of the involved states - std::vector Keys; - Keys.push_back(key1_); - Keys.push_back(key2_); - Marginals marginals( graph, values, Marginals::QR ); - JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); - Matrix cov1 = joint_marginal12(key1_, key1_); - Matrix cov2 = joint_marginal12(key2_, key2_); - Matrix cov12 = joint_marginal12(key1_, key2_); + // get joint covariance of the involved states + std::vector Keys; + Keys.push_back(key1_); + Keys.push_back(key2_); + Marginals marginals( graph, values, Marginals::QR ); + JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); + Matrix cov1 = joint_marginal12(key1_, key1_); + Matrix cov2 = joint_marginal12(key2_, key2_); + Matrix cov12 = joint_marginal12(key1_, key2_); - updateNoiseModels_givenCovs(values, cov1, cov2, cov12); + updateNoiseModels_givenCovs(values, cov1, cov2, cov12); } /* ************************************************************************* */ void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ - const T& p1 = values.at(key1_); - const T& p2 = values.at(key2_); + const T& p1 = values.at(key1_); + const T& p2 = values.at(key2_); - Matrix H1, H2; - T hx = p1.between(p2, H1, H2); // h(x) + Matrix H1, H2; + T hx = p1.between(p2, H1, H2); // h(x) - Matrix H; - H.resize(H1.rows(), H1.rows()+H2.rows()); - H << H1, H2; // H = [H1 H2] + Matrix H; + H.resize(H1.rows(), H1.rows()+H2.rows()); + H << H1, H2; // H = [H1 H2] - Matrix joint_cov; - joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); - joint_cov << cov1, cov12, - cov12.transpose(), cov2; + Matrix joint_cov; + joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); + joint_cov << cov1, cov12, + cov12.transpose(), cov2; - Matrix cov_state = H*joint_cov*H.transpose(); + Matrix cov_state = H*joint_cov*H.transpose(); - // model_inlier_->print("before:"); + // model_inlier_->print("before:"); - // update inlier and outlier noise models - Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); - model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); + // update inlier and outlier noise models + Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); - Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); - model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); + Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); - // model_inlier_->print("after:"); - // std::cout<<"covRinlier + cov_state: "<print("after:"); + // std::cout<<"covRinlier + cov_state: "< (&expected); + const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol); } diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 9fb7942e7..b3df86886 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -256,43 +256,43 @@ TEST( BetweenFactorEM, CaseStudy) ///* ************************************************************************** */ TEST (BetweenFactorEM, updateNoiseModel ) { - gtsam::Key key1(1); - gtsam::Key key2(2); + gtsam::Key key1(1); + gtsam::Key key2(2); - gtsam::Pose2 p1(10.0, 15.0, 0.1); - gtsam::Pose2 p2(15.0, 15.0, 0.3); - gtsam::Pose2 noise(0.5, 0.4, 0.01); - gtsam::Pose2 rel_pose_ideal = p1.between(p2); - gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0))); - gtsam::Values values; - values.insert(key1, p1); - values.insert(key2, p2); + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); - double prior_outlier = 0.0; - double prior_inlier = 1.0; + double prior_outlier = 0.0; + double prior_inlier = 1.0; - BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, - prior_inlier, prior_outlier); + BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); - SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); + SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); - NonlinearFactorGraph graph; - graph.push_back(gtsam::PriorFactor(key1, p1, model)); - graph.push_back(gtsam::PriorFactor(key2, p2, model)); + NonlinearFactorGraph graph; + graph.push_back(gtsam::PriorFactor(key1, p1, model)); + graph.push_back(gtsam::PriorFactor(key2, p2, model)); - f.updateNoiseModels(values, graph); + f.updateNoiseModels(values, graph); - SharedGaussian model_inlier_new = f.get_model_inlier(); - SharedGaussian model_outlier_new = f.get_model_outlier(); + SharedGaussian model_inlier_new = f.get_model_inlier(); + SharedGaussian model_outlier_new = f.get_model_outlier(); - model_inlier->print("model_inlier:"); - model_outlier->print("model_outlier:"); - model_inlier_new->print("model_inlier_new:"); - model_outlier_new->print("model_outlier_new:"); + model_inlier->print("model_inlier:"); + model_outlier->print("model_outlier:"); + model_inlier_new->print("model_inlier_new:"); + model_outlier_new->print("model_outlier_new:"); } From c212ba0984b6660577eb2285414d8c6ff8206429 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 3 Nov 2014 11:50:21 +0100 Subject: [PATCH 44/44] Avoid warnings --- gtsam/geometry/Cal3Bundler.h | 2 ++ gtsam/geometry/Cal3DS2.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index fff9a6e6d..53529ba46 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -53,6 +53,8 @@ public: */ Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); + virtual ~Cal3Bundler() {} + /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index dbd50f450..617fcf5ff 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -53,6 +53,8 @@ public: double k1, double k2, double p1 = 0.0, double p2 = 0.0) : Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {} + virtual ~Cal3DS2() {} + /// @} /// @name Advanced Constructors /// @{