83 lines
2.7 KiB
C++
83 lines
2.7 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& basePose,
|
|
const Pose3& anchorPose, const OrientedPlane3& plane,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
|
boost::optional<Matrix&> H3) const {
|
|
|
|
Matrix66 pose_H_anchorPose, pose_H_basePose;
|
|
Matrix36 predicted_H_pose;
|
|
Matrix33 predicted_H_plane, error_H_predicted;
|
|
|
|
// T_LB = inv(T_WL) * T_WB
|
|
const Pose3 relativePose = anchorPose.transformPoseTo(basePose,
|
|
H2 ? &pose_H_anchorPose : nullptr,
|
|
H1 ? &pose_H_basePose : nullptr);
|
|
|
|
const OrientedPlane3 predicted_plane = plane.transform(relativePose,
|
|
H2 ? &predicted_H_plane : nullptr,
|
|
(H1 || H3) ? &predicted_H_pose : nullptr);
|
|
|
|
const Vector3 err = measured_p_.error(predicted_plane,
|
|
boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr);
|
|
|
|
// const Vector3 err = predicted_plane.errorVector(measured_p_,
|
|
// (H1 || H2 || H3) ? &error_H_predicted : nullptr);
|
|
|
|
// Apply the chain rule to calculate the derivatives.
|
|
if (H1) {
|
|
*H1 = error_H_predicted * predicted_H_pose * pose_H_basePose;
|
|
// std::cout << "H1:\n" << *H1 << std::endl;
|
|
}
|
|
|
|
if (H2) {
|
|
*H2 = error_H_predicted * predicted_H_pose * pose_H_anchorPose;
|
|
// std::cout << "H2:\n" << *H2 << std::endl;
|
|
}
|
|
|
|
if (H3) {
|
|
*H3 = error_H_predicted * predicted_H_plane;
|
|
// std::cout << "H3:\n" << *H3 << std::endl;
|
|
|
|
// measured_p_.print();
|
|
// predicted_plane.print();
|
|
|
|
// std::cout << "pose_H_anchorPose:\n" << pose_H_anchorPose << std::endl;
|
|
// std::cout << "pose_H_basePose:\n" << pose_H_basePose << std::endl;
|
|
// std::cout << "predicted_H_pose:\n" << predicted_H_pose << std::endl;
|
|
// std::cout << "error_H_predicted:\n" << error_H_predicted << std::endl;
|
|
// std::cout << "predicted_H_plane:\n" << predicted_H_plane << std::endl;
|
|
|
|
std::cout << "H3^T x error:\n" << (*H3).transpose() * err << std::endl;
|
|
// std::cout << "H3:\n" << *H3 << std::endl;
|
|
}
|
|
|
|
// std::cout << "Error: " << err.transpose() << std::endl;
|
|
|
|
return err;
|
|
}
|
|
|
|
} // namespace gtsam
|