Moved TranslationFactor prototype into sfm directory
							parent
							
								
									652302f5ad
								
							
						
					
					
						commit
						83a0f51587
					
				|  | @ -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 <gtsam/geometry/Point3.h> | ||||
| #include <gtsam/geometry/Unit3.h> | ||||
| #include <gtsam/linear/NoiseModel.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| 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<Point3, Point3> { | ||||
|  private: | ||||
|   typedef NoiseModelFactor2<Point3, Point3> 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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> 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 <class ARCHIVE> | ||||
|   void serialize(ARCHIVE& ar, const unsigned int /*version*/) { | ||||
|     ar& boost::serialization::make_nvp( | ||||
|         "Base", boost::serialization::base_object<Base>(*this)); | ||||
|   } | ||||
| };  // \ TranslationFactor
 | ||||
| }  // namespace gtsam
 | ||||
|  | @ -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 <gtsam/geometry/Point3.h> | ||||
| #include <gtsam/geometry/Unit3.h> | ||||
| #include <gtsam/linear/NoiseModel.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| 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<Point3, Point3> { | ||||
|  private: | ||||
|   typedef NoiseModelFactor2<Point3, Point3> 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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> 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 <class ARCHIVE> | ||||
|   void serialize(ARCHIVE& ar, const unsigned int /*version*/) { | ||||
|     ar& boost::serialization::make_nvp( | ||||
|         "Base", boost::serialization::base_object<Base>(*this)); | ||||
|   } | ||||
| };  // \ TranslationFactor
 | ||||
| }  // namespace gtsam
 | ||||
| 
 | ||||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  | @ -98,7 +16,7 @@ class TranslationFactor : public NoiseModelFactor2<Point3, Point3> { | |||
|  * @brief test recovering translations when rotations are given. | ||||
|  */ | ||||
| 
 | ||||
| // #include <gtsam/sam/TranslationFactor.h>
 | ||||
| #include <gtsam/sfm/TranslationFactor.h> | ||||
| #include <gtsam/geometry/Point3.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/geometry/Unit3.h> | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue