1/*
2 * @file LocalOrientedPlane3Factor.h
3 * @brief LocalOrientedPlane3 Factor class
4 * @author David Wisth
5 * @date February 12, 2021
6 */
7
8#pragma once
9
10#include <gtsam/geometry/OrientedPlane3.h>
11#include <gtsam/nonlinear/NonlinearFactor.h>
12#include <gtsam_unstable/dllexport.h>
13
14#include <string>
15
16namespace gtsam {
17
18/**
19 * Factor to measure a planar landmark from a given pose, with a given local
20 * linearization point.
21 *
22 * This factor is based on the relative plane factor formulation proposed in:
23 * Equation 25,
24 * M. Kaess, "Simultaneous Localization and Mapping with Infinite Planes",
25 * IEEE International Conference on Robotics and Automation, 2015.
26 *
27 *
28 * The main purpose of this factor is to improve the numerical stability of the
29 * optimization, especially compared to gtsam::OrientedPlane3Factor. This
30 * is especially relevant when the sensor is far from the origin (and thus
31 * the derivatives associated to transforming the plane are large).
32 *
33 * x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e.
34 * a local linearisation point for the plane. The plane is representated and
35 * optimized in x1 frame in the optimization.
36 */
37class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
38 : public NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> {
39 protected:
40 OrientedPlane3 measured_p_;
41 typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base;
42public:
43
44 // Provide access to the Matrix& version of evaluateError:
45 using Base::evaluateError;
46
47 /// Constructor
48 LocalOrientedPlane3Factor() {}
49
50 ~LocalOrientedPlane3Factor() override {}
51
52 /** Constructor with measured plane (a,b,c,d) coefficients
53 * @param z measured plane (a,b,c,d) coefficients as 4D vector
54 * @param noiseModel noiseModel Gaussian noise model
55 * @param poseKey Key or symbol for unknown pose
56 * @param anchorPoseKey Key or symbol for the plane's linearization point,
57 (called the "anchor pose").
58 * @param landmarkKey Key or symbol for unknown planar landmark
59 *
60 * Note: The anchorPoseKey can simply be chosen as the first pose a plane
61 * is observed.
62 */
63 LocalOrientedPlane3Factor(const Vector4& z, const SharedNoiseModel& noiseModel,
64 Key poseKey, Key anchorPoseKey, Key landmarkKey)
65 : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
66
67 LocalOrientedPlane3Factor(const OrientedPlane3& z,
68 const SharedNoiseModel& noiseModel,
69 Key poseKey, Key anchorPoseKey, Key landmarkKey)
70 : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
71
72 /// print
73 void print(const std::string& s = "LocalOrientedPlane3Factor",
74 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
75
76 /***
77 * Vector of errors
78 * @brief Error = measured_plane_.error(a_plane.transform(inv(wTwa) * wTwi))
79 *
80 * This is the error of the measured and predicted plane in the current
81 * sensor frame, i. The plane is represented in the anchor pose, a.
82 *
83 * @param wTwi The pose of the sensor in world coordinates
84 * @param wTwa The pose of the anchor frame in world coordinates
85 * @param a_plane The estimated plane in anchor frame.
86 *
87 * Note: The optimized plane is represented in anchor frame, a, not the
88 * world frame.
89 */
90 Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa,
91 const OrientedPlane3& a_plane, OptionalMatrixType H1,
92 OptionalMatrixType H2, OptionalMatrixType H3) const override;
93};
94
95} // namespace gtsam
96
97