12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved TranslationFactor prototype into sfm directory

release/4.3a0
Frank Dellaert 2020-03-08 03:15:38 -04:00
parent 652302f5ad
commit 83a0f51587
2 changed files with 84 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/sfm/TranslationFactor.h Normal file
View File

@ -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

84
tests/testTranslationRecovery.cpp
View File

@ -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>