12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update documentation on new factor

release/4.3a0
David Wisth 2021-02-15 15:08:57 +00:00
parent 5087d36ab1
commit 7480d149c8
3 changed files with 32 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp
View File

@ -22,60 +22,42 @@ void LocalOrientedPlane3Factor::print(const string& s,
}
//***************************************************************************
Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& basePose,
const Pose3& anchorPose, const OrientedPlane3& plane,
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 pose_H_anchorPose, pose_H_basePose;
Matrix36 predicted_H_pose;
Matrix66 aTai_H_wTwa, aTai_H_wTwi;
Matrix36 predicted_H_aTai;
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);
// 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);
const OrientedPlane3 predicted_plane = plane.transform(relativePose,
// Transform the plane measurement into sensor frame.
const OrientedPlane3 i_plane = a_plane.transform(aTai,
H2 ? &predicted_H_plane : nullptr,
(H1 || H3) ? &predicted_H_pose : nullptr);
(H1 || H3) ? &predicted_H_aTai : nullptr);
const Vector3 err = measured_p_.error(predicted_plane,
// 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);
// 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;
*H1 = error_H_predicted * predicted_H_aTai * aTai_H_wTwi;
}
if (H2) {
*H2 = error_H_predicted * predicted_H_pose * pose_H_anchorPose;
// std::cout << "H2:\n" << *H2 << std::endl;
*H2 = error_H_predicted * predicted_H_aTai * aTai_H_wTwa;
}
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;
}

19
gtsam_unstable/slam/LocalOrientedPlane3Factor.h
View File

@ -39,7 +39,8 @@ public:
* @param z measured plane (a,b,c,d) coefficients as 4D vector
* @param noiseModel noiseModel Gaussian noise model
* @param poseKey Key or symbol for unknown pose
* @param anchorPoseKey Key or symbol for the plane's linearization point.
* @param anchorPoseKey Key or symbol for the plane's linearization point,
(called the "anchor pose").
* @param landmarkKey Key or symbol for unknown planar landmark
*
* Note: The anchorPoseKey can simply be chosen as the first pose a plane
@ -57,9 +58,19 @@ public:
void print(const std::string& s = "LocalOrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// evaluateError
Vector evaluateError(
const Pose3& basePose, const Pose3& anchorPose, const OrientedPlane3& plane,
/***
* Vector of errors
* @brief Error = measured_plane_.error(a_plane.transform(inv(wTwa) * wTwi))
*
* This is the error of the measured and predicted plane in the current
* sensor frame, i. The plane is represented in the anchor pose, a.
*
* @param wTwi The pose of the sensor in world coordinates
* @param wTwa The pose of the anchor frame in world coordinates
* @param a_plane The estimated plane in anchor frame.
*/
Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa,
const OrientedPlane3& a_plane,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;

23
gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
View File

@ -111,32 +111,13 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
ISAM2 isam2;
isam2.update(graph, values);
Values result_values = isam2.calculateEstimate();
isam2.getDelta().print();
auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0));
values.print();
result_values.print();
// HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(values);
// const auto hessian = hessianFactor->hessianBlockDiagonal();
// Matrix hessianP0 = hessian.at(P(0)), hessianX0 = hessian.at(X(0));
// Eigen::JacobiSVD<Matrix> svdP0(hessianP0, Eigen::ComputeThinU),
// svdX0(hessianX0, Eigen::ComputeThinU);
// double conditionNumberP0 = svdP0.singularValues()[0] / svdP0.singularValues()[2],
// conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5];
// std::cout << "Hessian P0:\n" << hessianP0 << "\n"
// << "Condition number:\n" << conditionNumberP0 << "\n"
// << "Singular values:\n" << svdP0.singularValues().transpose() << "\n"
// << "SVD U:\n" << svdP0.matrixU() << "\n" << std::endl;
// std::cout << "Hessian X0:\n" << hessianX0 << "\n"
// << "Condition number:\n" << conditionNumberX0 << "\n"
// << "Singular values:\n" << svdX0.singularValues().transpose() << "\n"
// << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl;
// Given two noisy measurements of equal weight, expect result between the two
OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0,
0.0, 3.0);