From 7480d149c8d5dd1ba074f61be622585709e7d51d Mon Sep 17 00:00:00 2001 From: David Wisth Date: Mon, 15 Feb 2021 15:08:57 +0000 Subject: [PATCH] Update documentation on new factor --- .../slam/LocalOrientedPlane3Factor.cpp | 48 ++++++------------- .../slam/LocalOrientedPlane3Factor.h | 19 ++++++-- .../tests/testLocalOrientedPlane3Factor.cpp | 23 +-------- 3 files changed, 32 insertions(+), 58 deletions(-) diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp index 87d94d957..5a92dd7ef 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp @@ -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 H1, boost::optional H2, boost::optional 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; } diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h index 7bec7e12e..586874248 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const override; diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index e3eec80c7..7c00824eb 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -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(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 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);