simulated3D.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
 
 * -------------------------------------------------------------------------- */
 
// \callgraph
 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include "gtsam/base/OptionalJacobian.h"
 
// \namespace
 
namespace gtsam {
namespace simulated3D {
 
Point3 prior(const Point3& x, OptionalJacobian<3,3> H = OptionalNone) {
  if (H) *H = I_3x3;
  return x;
}
 
Point3 odo(const Point3& x1, const Point3& x2,
    OptionalJacobian<3,3> H1 = OptionalNone,
    OptionalJacobian<3,3> H2 = OptionalNone) {
  if (H1) *H1 = -1 * I_3x3;
  if (H2) *H2 = I_3x3;
  return x2 - x1;
}
 
Point3 mea(const Point3& x, const Point3& l,
    OptionalJacobian<3,3> H1 = OptionalNone,
    OptionalJacobian<3,3> H2 = OptionalNone) {
  if (H1) *H1 = -1 * I_3x3;
  if (H2) *H2 = I_3x3;
  return l - x;
}
 
struct PointPrior3D: public NoiseModelFactor1<Point3> {
  using NoiseModelFactor1<Point3>::evaluateError;
 
  Point3 measured_; 
 
  PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) :
    NoiseModelFactorN<Point3> (model, key), measured_(measured) {
  }
 
  Vector evaluateError(const Point3& x, OptionalMatrixType H) const override {
    return simulated3D::prior(x, H) - measured_;
  }
};
 
struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
  using NoiseModelFactor2<Point3, Point3>::evaluateError;
 
  Point3 measured_; 
 
  Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) :
        NoiseModelFactorN<Point3, Point3>(model, i, j), measured_(measured) {}
 
  Vector evaluateError(const Point3& x1, const Point3& x2,
      OptionalMatrixType H1, OptionalMatrixType H2) const override {
    return mea(x1, x2, H1, H2) - measured_;
  }
};
 
}} // namespace simulated3D