65 lines
1.9 KiB
C++
65 lines
1.9 KiB
C++
/*
|
|
* LocalOrientedPlane3Factor.cpp
|
|
*
|
|
* Author: David Wisth
|
|
* Created on: February 12, 2021
|
|
*/
|
|
|
|
#include "LocalOrientedPlane3Factor.h"
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
//***************************************************************************
|
|
void LocalOrientedPlane3Factor::print(const string& s,
|
|
const KeyFormatter& keyFormatter) const {
|
|
cout << s << (s == "" ? "" : "\n");
|
|
cout << "LocalOrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", "
|
|
<< keyFormatter(key2()) << ", " << keyFormatter(key3()) << ")\n";
|
|
measured_p_.print("Measured Plane");
|
|
this->noiseModel_->print(" noise model: ");
|
|
}
|
|
|
|
//***************************************************************************
|
|
Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
|
|
const Pose3& wTwa, const OrientedPlane3& a_plane,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
|
boost::optional<Matrix&> H3) const {
|
|
|
|
Matrix66 aTai_H_wTwa, aTai_H_wTwi;
|
|
Matrix36 predicted_H_aTai;
|
|
Matrix33 predicted_H_plane, error_H_predicted;
|
|
|
|
// Find the relative transform from anchor to sensor frame.
|
|
const Pose3 aTai = wTwa.transformPoseTo(wTwi,
|
|
H2 ? &aTai_H_wTwa : nullptr,
|
|
H1 ? &aTai_H_wTwi : nullptr);
|
|
|
|
// Transform the plane measurement into sensor frame.
|
|
const OrientedPlane3 i_plane = a_plane.transform(aTai,
|
|
H2 ? &predicted_H_plane : nullptr,
|
|
(H1 || H3) ? &predicted_H_aTai : nullptr);
|
|
|
|
// Calculate the error between measured and estimated planes in sensor frame.
|
|
const Vector3 err = measured_p_.errorVector(i_plane,
|
|
boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr);
|
|
|
|
// Apply the chain rule to calculate the derivatives.
|
|
if (H1) {
|
|
*H1 = error_H_predicted * predicted_H_aTai * aTai_H_wTwi;
|
|
}
|
|
|
|
if (H2) {
|
|
*H2 = error_H_predicted * predicted_H_aTai * aTai_H_wTwa;
|
|
}
|
|
|
|
if (H3) {
|
|
*H3 = error_H_predicted * predicted_H_plane;
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
} // namespace gtsam
|