From 5dc149fe232f81ab82952d4c97c677c76451b8e3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Jun 2015 18:37:09 -0400 Subject: [PATCH] Faster linearize now in class --- gtsam/slam/GeneralSFMFactor.h | 43 +++++++++++++++-- gtsam/slam/tests/testGeneralSFMFactor.cpp | 57 +++-------------------- 2 files changed, 47 insertions(+), 53 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2516b2bcc..ba3d27202 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -116,7 +116,6 @@ namespace gtsam { /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - try { Point2 reprojError(camera.project2(point,H1,H2) - measured_); return reprojError.vector(); @@ -124,12 +123,50 @@ namespace gtsam { catch( CheiralityException& e) { if (H1) *H1 = zeros(2, DimC); if (H2) *H2 = zeros(2, DimL); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + // TODO warn if verbose output asked for return zero(2); } } + /// Linearize using fixed-size matrices + boost::shared_ptr linearize(const Values& values) { + // Only linearize if the factor is active + if (!this->active(values)) return boost::shared_ptr(); + + const Key key1 = this->key1(), key2 = this->key2(); + Eigen::Matrix H1; + Eigen::Matrix H2; + Vector2 b; + try { + const CAMERA& camera = values.at(key1); + const LANDMARK& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + // TODO warn if verbose output asked for + return boost::make_shared(); + } + + // Whiten the system if needed + const SharedNoiseModel& noiseModel = this->get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include + // above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); + } + + if (noiseModel && noiseModel->isConstrained()) { + using noiseModel::Constrained; + return boost::make_shared( + key1, H1, key2, H2, b, + boost::static_pointer_cast(noiseModel)->unit()); + } else { + return boost::make_shared(key1, H1, key2, H2, b); + } + } + /** return the measured */ inline const Point2 measured() const { return measured_; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index ad25b611c..a83db3f1d 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -430,49 +430,6 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { EXPECT(assert_equal(expected, actual, 1e-4)); } -/* ************************************************************************* */ - -static const int DimC = 11, DimL = 3; - -/// Linearize using fixed-size matrices -boost::shared_ptr linearize(const Projection& factor, - const Values& values) { - // Only linearize if the factor is active - if (!factor.active(values)) return boost::shared_ptr(); - - const Key key1 = factor.key1(), key2 = factor.key2(); - Eigen::Matrix H1; - Eigen::Matrix H2; - Vector2 b; - try { - const GeneralCamera& camera = values.at(key1); - const Point3& point = values.at(key2); - Point2 reprojError(camera.project2(point, H1, H2) - factor.measured()); - b = -reprojError.vector(); - } catch (CheiralityException& e) { - // TODO warn if verbose output asked for - return boost::make_shared(); - } - - // Whiten the system if needed - const SharedNoiseModel& noiseModel = factor.get_noiseModel(); - if (noiseModel && !noiseModel->isUnit()) { - // TODO: implement WhitenSystem for fixed size matrices and include above - H1 = noiseModel->Whiten(H1); - H2 = noiseModel->Whiten(H2); - b = noiseModel->Whiten(b); - } - - if (noiseModel && noiseModel->isConstrained()) { - using noiseModel::Constrained; - return boost::make_shared( - key1, H1, key2, H2, b, - boost::static_pointer_cast(noiseModel)->unit()); - } else { - return boost::make_shared(key1, H1, key2, H2, b); - } -} - /* ************************************************************************* */ TEST(GeneralSFMFactor, Linearize) { Point2 z(3.,0.); @@ -490,31 +447,31 @@ TEST(GeneralSFMFactor, Linearize) { const SharedNoiseModel model; Projection factor(z, model, X(1), L(1)); GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } // Test with Unit Model { const SharedNoiseModel model(noiseModel::Unit::Create(2)); Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } // Test with Isotropic noise { const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5)); Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } // Test with Constrained Model { const SharedNoiseModel model(noiseModel::Constrained::All(2)); Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } }