TSAMFactors.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
namespace gtsam {

class DeltaFactor : public NoiseModelFactorT<Vector2, Pose2, Point2> {
 
public:
  typedef DeltaFactor This;
  typedef NoiseModelFactorT<Vector2, Pose2, Point2> Base;
  typedef std::shared_ptr<This> shared_ptr;
 
private:
  Point2 measured_; 
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  DeltaFactor(Key i, Key j, const Point2& measured,
      const SharedNoiseModel& model) :
      Base(model, i, j), measured_(measured) {
  }

  Vector2 evaluateError(const Pose2& pose, const Point2& point,
                        OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    return pose.transformTo(point, H1, H2) - measured_;
  }
};

class DeltaFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> {
 
public:
  typedef DeltaFactorBase This;
  typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> Base;
  typedef std::shared_ptr<This> shared_ptr;
 
private:
  Point2 measured_; 
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured,
      const SharedNoiseModel& model) :
      Base(model, b1, i, b2, j), measured_(measured) {
  }

  Vector evaluateError(const Pose2& base1, const Pose2& pose,
      const Pose2& base2, const Point2& point, //
      OptionalMatrixType H1, OptionalMatrixType H2, //
      OptionalMatrixType H3, OptionalMatrixType H4) const override {
    if (H1 || H2 || H3 || H4) {
      // TODO use fixed-size matrices
      Matrix D_pose_g_base1, D_pose_g_pose;
      Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose);
      Matrix D_point_g_base2, D_point_g_point;
      Point2 point_g = base2.transformFrom(point, D_point_g_base2,
          D_point_g_point);
      Matrix D_e_pose_g, D_e_point_g;
      Point2 d = pose_g.transformTo(point_g, D_e_pose_g, D_e_point_g);
      if (H1)
        *H1 = D_e_pose_g * D_pose_g_base1;
      if (H2)
        *H2 = D_e_pose_g * D_pose_g_pose;
      if (H3)
        *H3 = D_e_point_g * D_point_g_base2;
      if (H4)
        *H4 = D_e_point_g * D_point_g_point;
      return d - measured_;
    } else {
      Pose2 pose_g = base1.compose(pose);
      Point2 point_g = base2.transformFrom(point);
      Point2 d = pose_g.transformTo(point_g);
      return d - measured_;
    }
  }
};

class OdometryFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> {
 
public:
  typedef OdometryFactorBase This;
  typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> Base;
  typedef std::shared_ptr<This> shared_ptr;
 
private:
  Pose2 measured_; 
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured,
      const SharedNoiseModel& model) :
      Base(model, b1, i, b2, j), measured_(measured) {
  }

  Vector evaluateError(const Pose2& base1, const Pose2& pose1,
      const Pose2& base2, const Pose2& pose2,
      OptionalMatrixType H1, OptionalMatrixType H2,
      OptionalMatrixType H3, OptionalMatrixType H4) const override {
    if (H1 || H2 || H3 || H4) {
      // TODO use fixed-size matrices
      Matrix D_pose1_g_base1, D_pose1_g_pose1;
      Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1);
      Matrix D_pose2_g_base2, D_pose2_g_pose2;
      Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2);
      Matrix D_e_pose1_g, D_e_pose2_g;
      Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g);
      Matrix3 localJacobian;
      const Vector error =
          measured_.localCoordinates(d, nullptr, localJacobian);
      if (H1) *H1 = localJacobian * D_e_pose1_g * D_pose1_g_base1;
      if (H2) *H2 = localJacobian * D_e_pose1_g * D_pose1_g_pose1;
      if (H3) *H3 = localJacobian * D_e_pose2_g * D_pose2_g_base2;
      if (H4) *H4 = localJacobian * D_e_pose2_g * D_pose2_g_pose2;
      return error;
    } else {
      Pose2 pose1_g = base1.compose(pose1);
      Pose2 pose2_g = base2.compose(pose2);
      Pose2 d = pose1_g.between(pose2_g);
      return measured_.localCoordinates(d);
    }
  }
};
 
}