LocalOrientedPlane3Factor.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
 
 * -------------------------------------------------------------------------- */

 
/*
 * @file LocalOrientedPlane3Factor.h
 * @brief LocalOrientedPlane3 Factor class
 * @author David Wisth
 * @date February 12, 2021
 */
 
#pragma once
 
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam_unstable/dllexport.h>
 
#include <string>
 
namespace gtsam {

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

  LocalOrientedPlane3Factor() {}
 
  ~LocalOrientedPlane3Factor() override {}

  LocalOrientedPlane3Factor(const Vector4& z, const SharedNoiseModel& noiseModel,
                            Key poseKey, Key anchorPoseKey, Key landmarkKey)
      : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
 
  LocalOrientedPlane3Factor(const OrientedPlane3& z,
                            const SharedNoiseModel& noiseModel,
                            Key poseKey, Key anchorPoseKey, Key landmarkKey)
    : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}

  void print(const std::string& s = "LocalOrientedPlane3Factor",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
  /***
    * Vector of errors
    * @brief Error = measured_plane_.error(a_plane.transform(inv(wTwa) * wTwi))
    * 
    * This is the error of the measured and predicted plane in the current
    * sensor frame, i. The plane is represented in the anchor pose, a.
    *
    * @param wTwi The pose of the sensor in world coordinates
    * @param wTwa The pose of the anchor frame in world coordinates
    * @param a_plane The estimated plane in anchor frame.
    *
    * Note: The optimized plane is represented in anchor frame, a, not the
    * world frame.
    */
  Vector3 evaluateError(const Pose3& wTwi, const Pose3& wTwa,
                        const OrientedPlane3& a_plane, OptionalMatrixType H1,
                        OptionalMatrixType H2,
                        OptionalMatrixType H3) const override;
};
 
}  // namespace gtsam