From 83a0f515875b666a9cdae1e4af1668d6ec737fc8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 03:15:38 -0400 Subject: [PATCH] Moved TranslationFactor prototype into sfm directory --- gtsam/sfm/TranslationFactor.h | 83 ++++++++++++++++++++++++++++++ tests/testTranslationRecovery.cpp | 84 +------------------------------ 2 files changed, 84 insertions(+), 83 deletions(-) create mode 100644 gtsam/sfm/TranslationFactor.h diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h new file mode 100644 index 000000000..626214dcc --- /dev/null +++ b/gtsam/sfm/TranslationFactor.h @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file TranslationFactor.h + * @author Frank Dellaert + * @date March 2020 + * @brief Binary factor for a relative translation direction measurement. + */ + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor for a relative translation direction measurement + * w_aZb. The measurement equation is + * w_aZb = Unit3(Tb - Ta) + * i.e., w_aZb is the translation direction from frame A to B, in world + * coordinates. Although Unit3 instances live on a manifold, following + * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the + * ambient world coordinate frame: + * normalized(Tb - Ta) - w_aZb.point3() + * + * @addtogroup SAM + */ +class TranslationFactor : public NoiseModelFactor2 { + private: + typedef NoiseModelFactor2 Base; + Point3 measured_w_aZb_; + + public: + /// default constructor + TranslationFactor() {} + + TranslationFactor(Key a, Key b, const Unit3& w_aZb, + const SharedNoiseModel& noiseModel) + : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} + + /** + * @brief Caclulate error norm(p1-p2) - measured + * + * @param Ta translation for key a + * @param Tb translation for key b + * @param H1 optional jacobian in Ta + * @param H2 optional jacobian in Tb + * @return * Vector + */ + Vector evaluateError( + const Point3& Ta, const Point3& Tb, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const Point3 dir = Tb - Ta; + Matrix33 H_predicted_dir; + const Point3 predicted = + dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); + if (H1) *H1 = H_predicted_dir; + if (H2) *H2 = -H_predicted_dir; + return predicted - measured_w_aZb_; + } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +}; // \ TranslationFactor +} // namespace gtsam diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index bd8a81f20..8742820f4 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -1,85 +1,3 @@ -/* ---------------------------------------------------------------------------- - - * 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 TranslationFactor.h - * @author Frank Dellaert - * @date March 2020 - * @brief Binary factor for a relative translation direction measurement. - */ - -#include -#include -#include -#include - -namespace gtsam { - -/** - * Binary factor for a relative translation direction measurement - * w_aZb. The measurement equation is - * w_aZb = Unit3(Tb - Ta) - * i.e., w_aZb is the translation direction from frame A to B, in world - * coordinates. Although Unit3 instances live on a manifold, following - * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the - * ambient world coordinate frame: - * normalized(Tb - Ta) - w_aZb.point3() - * - * @addtogroup SAM - */ -class TranslationFactor : public NoiseModelFactor2 { - private: - typedef NoiseModelFactor2 Base; - Point3 measured_w_aZb_; - - public: - /// default constructor - TranslationFactor() {} - - TranslationFactor(Key a, Key b, const Unit3& w_aZb, - const SharedNoiseModel& noiseModel) - : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} - - /** - * @brief Caclulate error norm(p1-p2) - measured - * - * @param Ta translation for key a - * @param Tb translation for key b - * @param H1 optional jacobian in Ta - * @param H2 optional jacobian in Tb - * @return * Vector - */ - Vector evaluateError( - const Point3& Ta, const Point3& Tb, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { - const Point3 dir = Tb - Ta; - Matrix33 H_predicted_dir; - const Point3 predicted = - dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); - if (H1) *H1 = H_predicted_dir; - if (H2) *H2 = -H_predicted_dir; - return predicted - measured_w_aZb_; - } - - private: - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "Base", boost::serialization::base_object(*this)); - } -}; // \ TranslationFactor -} // namespace gtsam - /* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, @@ -98,7 +16,7 @@ class TranslationFactor : public NoiseModelFactor2 { * @brief test recovering translations when rotations are given. */ -// #include +#include #include #include #include