1/*
2 * LocalOrientedPlane3Factor.cpp
3 *
4 * Author: David Wisth
5 * Created on: February 12, 2021
6 */
7
8#include "LocalOrientedPlane3Factor.h"
9
10using namespace std;
11
12namespace gtsam {
13
14//***************************************************************************
15void LocalOrientedPlane3Factor::print(const string& s,
16 const KeyFormatter& keyFormatter) const {
17 cout << s << (s == "" ? "" : "\n");
18 cout << "LocalOrientedPlane3Factor Factor (" << keyFormatter(key<1>()) << ", "
19 << keyFormatter(key<2>()) << ", " << keyFormatter(key<3>()) << ")\n";
20 measured_p_.print(s: "Measured Plane");
21 this->noiseModel_->print(name: " noise model: ");
22}
23
24//***************************************************************************
25Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
26 const Pose3& wTwa, const OrientedPlane3& a_plane,
27 OptionalMatrixType H1, OptionalMatrixType H2,
28 OptionalMatrixType H3) const {
29
30 Matrix66 aTai_H_wTwa, aTai_H_wTwi;
31 Matrix36 predicted_H_aTai;
32 Matrix33 predicted_H_plane, error_H_predicted;
33
34 // Find the relative transform from anchor to sensor frame.
35 const Pose3 aTai = wTwa.transformPoseTo(wTb: wTwi,
36 Hself: H2 ? &aTai_H_wTwa : nullptr,
37 HwTb: H1 ? &aTai_H_wTwi : nullptr);
38
39 // Transform the plane measurement into sensor frame.
40 const OrientedPlane3 i_plane = a_plane.transform(xr: aTai,
41 Hp: H2 ? &predicted_H_plane : nullptr,
42 Hr: (H1 || H3) ? &predicted_H_aTai : nullptr);
43
44 // Calculate the error between measured and estimated planes in sensor frame.
45 const Vector3 err = measured_p_.errorVector(other: i_plane,
46 H1: {}, H2: (H1 || H2 || H3) ? &error_H_predicted : nullptr);
47
48 // Apply the chain rule to calculate the derivatives.
49 if (H1) {
50 *H1 = error_H_predicted * predicted_H_aTai * aTai_H_wTwi;
51 }
52
53 if (H2) {
54 *H2 = error_H_predicted * predicted_H_aTai * aTai_H_wTwa;
55 }
56
57 if (H3) {
58 *H3 = error_H_predicted * predicted_H_plane;
59 }
60
61 return err;
62}
63
64} // namespace gtsam
65