OrientedPlane3Factor.h

/* ----------------------------------------------------------------------------
 
 * 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 OrientedPlane3Factor.cpp
 * @brief OrientedPlane3 Factor class
 * @author Alex Trevor
 * @date December 22, 2013
 */
 
#pragma once
 
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
namespace gtsam {

class GTSAM_EXPORT OrientedPlane3Factor
    : public NoiseModelFactorT<Vector3, Pose3, OrientedPlane3> {
 protected:
  OrientedPlane3 measured_p_;
  typedef NoiseModelFactorT<Vector3, Pose3, OrientedPlane3> Base;
 
 public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  OrientedPlane3Factor() {
  }
  ~OrientedPlane3Factor() override {}

  OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
                       Key poseKey, Key landmarkKey)
      : Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}

  void print(const std::string& s = "OrientedPlane3Factor",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

  Vector3 evaluateError(
      const Pose3& pose, const OrientedPlane3& plane,
      OptionalMatrixType H1, OptionalMatrixType H2) const override;
};
 
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
 protected:
  OrientedPlane3 measured_p_;  
  typedef NoiseModelFactorN<OrientedPlane3> Base;
 
 public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;
 
  typedef OrientedPlane3DirectionPrior This;
  OrientedPlane3DirectionPrior() {
  }

  OrientedPlane3DirectionPrior(Key key, const Vector4& z,
                               const SharedGaussian& noiseModel)
      : Base(noiseModel, key), measured_p_(z) {}

  void print(const std::string& s = "OrientedPlane3DirectionPrior",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
  Vector evaluateError(const OrientedPlane3& plane, OptionalMatrixType H) const override;
};
 
} // gtsam