LocalizationFactor.cpp

Go to the documentation of this file.

class UnaryFactor: public NoiseModelFactor1<Pose2> {
  double mx_, my_; 
  
public:

  // Provide access to the Matrix& version of evaluateError:
  using gtsam::NoiseModelFactor1<Pose2>::evaluateError;

  UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
    NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}

  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
    const Rot2& R = q.rotation();
    if (H) (*H) = (gtsam::Matrix(2, 3) <<
            R.c(), -R.s(), 0.0,
            R.s(), R.c(), 0.0).finished();
    return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
  }
};