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

rebase and remove boost

release/4.3a0
Akshay Krishnan 2024-09-08 19:44:28 -04:00
parent 9021c888ef
commit a6861392c9
1 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/sfm/TranslationFactor.h
View File

@ -44,7 +44,6 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
Point3 measured_w_aZb_; Point3 measured_w_aZb_;
public: public:
// Provide access to the Matrix& version of evaluateError: // Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<Point3, Point3>::evaluateError; using NoiseModelFactor2<Point3, Point3>::evaluateError;
@ -66,10 +65,9 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
* @param H2 optional jacobian in Tb * @param H2 optional jacobian in Tb
* @return * Vector * @return * Vector
*/ */
Vector evaluateError( Vector evaluateError(const Point3& Ta, const Point3& Tb,
const Point3& Ta, const Point3& Tb, OptionalMatrixType H1,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
OptionalMatrixType H2) const override {
const Point3 dir = Tb - Ta; const Point3 dir = Tb - Ta;
Matrix33 H_predicted_dir; Matrix33 H_predicted_dir;
const Point3 predicted = const Point3 predicted =
@ -95,11 +93,11 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
* w_aZb. The measurement equation is * w_aZb. The measurement equation is
* w_aZb = scale * (Tb - Ta) * w_aZb = scale * (Tb - Ta)
* i.e., w_aZb is the translation direction from frame A to B, in world * i.e., w_aZb is the translation direction from frame A to B, in world
* coordinates. * coordinates.
* *
* Instead of normalizing the Tb - Ta vector, we multiply it by a scalar * Instead of normalizing the Tb - Ta vector, we multiply it by a scalar
* which is also jointly optimized. This is inspired by the work on * which is also jointly optimized. This is inspired by the work on
* BATA (Zhuang et al, CVPR 2018). * BATA (Zhuang et al, CVPR 2018).
* *
* @ingroup sfm * @ingroup sfm
*/ */
@ -113,10 +111,10 @@ class BilinearAngleTranslationFactor
/// default constructor /// default constructor
BilinearAngleTranslationFactor() {} BilinearAngleTranslationFactor() {}
BilinearAngleTranslationFactor(Key a, Key b, Key scale_key, const Unit3& w_aZb, BilinearAngleTranslationFactor(Key a, Key b, Key scale_key,
const Unit3& w_aZb,
const SharedNoiseModel& noiseModel) const SharedNoiseModel& noiseModel)
: Base(noiseModel, a, b, scale_key), : Base(noiseModel, a, b, scale_key), measured_w_aZb_(w_aZb.point3()) {}
measured_w_aZb_(w_aZb.point3()) {}
/** /**
* @brief Caclulate error: (scale * (Tb - Ta) - measurement) * @brief Caclulate error: (scale * (Tb - Ta) - measurement)
@ -129,11 +127,9 @@ class BilinearAngleTranslationFactor
* @param H2 optional jacobian in Tb * @param H2 optional jacobian in Tb
* @return * Vector * @return * Vector
*/ */
Vector evaluateError( Vector evaluateError(const Point3& Ta, const Point3& Tb, const Vector1& scale,
const Point3& Ta, const Point3& Tb, const Vector1& scale, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
// Ideally we should use a positive real valued scalar datatype for scale. // Ideally we should use a positive real valued scalar datatype for scale.
const double abs_scale = abs(scale[0]); const double abs_scale = abs(scale[0]);
const Point3 predicted = (Tb - Ta) * abs_scale; const Point3 predicted = (Tb - Ta) * abs_scale;