LocalOrientedPlane3Factor.cpp
Go to the documentation of this file.
1 /*
2  * LocalOrientedPlane3Factor.cpp
3  *
4  * Author: David Wisth
5  * Created on: February 12, 2021
6  */
7 
9 
10 using namespace std;
11 
12 namespace gtsam {
13 
14 //***************************************************************************
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("Measured Plane");
21  this->noiseModel_->print(" noise model: ");
22 }
23 
24 //***************************************************************************
25 Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
26  const Pose3& wTwa, const OrientedPlane3& a_plane,
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(wTwi,
36  H2 ? &aTai_H_wTwa : nullptr,
37  H1 ? &aTai_H_wTwi : nullptr);
38 
39  // Transform the plane measurement into sensor frame.
40  const OrientedPlane3 i_plane = a_plane.transform(aTai,
41  H2 ? &predicted_H_plane : nullptr,
42  (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(i_plane,
46  {}, (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
Eigen::Vector3d Vector3
Definition: Vector.h:43
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
RealScalar s
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
traits
Definition: chartTesting.h:28
OrientedPlane3 transform(const Pose3 &xr, OptionalJacobian< 3, 3 > Hp={}, OptionalJacobian< 3, 6 > Hr={}) const
Pose3 transformPoseTo(const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HwTb={}) const
Definition: Pose3.cpp:338
Vector3 errorVector(const OrientedPlane3 &other, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:34