From a62bdd45e8959a9b9c147c94a54d957beeb0cc2a Mon Sep 17 00:00:00 2001 From: David Wisth Date: Mon, 15 Feb 2021 13:15:11 +0000 Subject: [PATCH] Add new oriented plane 3 factors with local linearisation point --- .../slam/LocalOrientedPlane3Factor.cpp | 82 +++++ .../slam/LocalOrientedPlane3Factor.h | 69 +++++ .../tests/testLocalOrientedPlane3Factor.cpp | 289 ++++++++++++++++++ 3 files changed, 440 insertions(+) create mode 100644 gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp create mode 100644 gtsam_unstable/slam/LocalOrientedPlane3Factor.h create mode 100644 gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp new file mode 100644 index 000000000..87d94d957 --- /dev/null +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp @@ -0,0 +1,82 @@ +/* + * 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 H1, boost::optional H2, + boost::optional 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 diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h new file mode 100644 index 000000000..7bec7e12e --- /dev/null +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -0,0 +1,69 @@ +/* + * @file LocalOrientedPlane3Factor.h + * @brief LocalOrientedPlane3 Factor class + * @author David Wisth + * @date February 12, 2021 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor to measure a planar landmark from a given pose, with a given local + * linearization point. + * + * This factor is based on the relative plane factor formulation proposed in: + * Equation 25, + * M. Kaess, "Simultaneous Localization and Mapping with Infinite Planes", + * IEEE International Conference on Robotics and Automation, 2015. + * + * Note: This uses the retraction from the OrientedPlane3, not the quaternion- + * based representation proposed by Kaess. + */ +class LocalOrientedPlane3Factor: public NoiseModelFactor3 { +protected: + OrientedPlane3 measured_p_; + typedef NoiseModelFactor3 Base; +public: + /// Constructor + LocalOrientedPlane3Factor() {} + + virtual ~LocalOrientedPlane3Factor() {} + + /** Constructor with measured plane (a,b,c,d) coefficients + * @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 landmarkKey Key or symbol for unknown planar landmark + * + * Note: The anchorPoseKey can simply be chosen as the first pose a plane + * is observed. + */ + LocalOrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel, + Key poseKey, Key anchorPoseKey, Key landmarkKey) + : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} + + LocalOrientedPlane3Factor(const OrientedPlane3& z, const SharedGaussian& noiseModel, + Key poseKey, Key anchorPoseKey, Key landmarkKey) + : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} + + /// print + 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, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override; +}; + +} // gtsam + diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp new file mode 100644 index 000000000..e3eec80c7 --- /dev/null +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -0,0 +1,289 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testOrientedPlane3Factor.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3Factor class + */ + +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +using symbol_shorthand::P; //< Planes +using symbol_shorthand::X; //< Pose3 + +// ************************************************************************* +TEST(LocalOrientedPlane3Factor, lm_translation_error) { + // Tests one pose, two measurements of the landmark that differ in range only. + // Normal along -x, 3m away + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + NonlinearFactorGraph graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement + // does not fully constrain the pose + Pose3 init_pose = Pose3::identity(); + graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); + + // Add two landmark measurements, differing in range + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; + LocalOrientedPlane3Factor factor0( + measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + LocalOrientedPlane3Factor factor1( + measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + graph.add(factor0); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); + + // Optimize + ISAM2 isam2; + isam2.update(graph, values); + Values result_values = isam2.calculateEstimate(); + auto optimized_plane_landmark = result_values.at(P(0)); + + // Given two noisy measurements of equal weight, expect result between the two + OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST (LocalOrientedPlane3Factor, lm_rotation_error) { + // Tests one pose, two measurements of the landmark that differ in angle only. + // Normal along -x, 3m away + OrientedPlane3 test_lm0(-1.0/sqrt(1.01), -0.1/sqrt(1.01), 0.0, 3.0); + + NonlinearFactorGraph graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + Pose3 init_pose = Pose3::identity(); + graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); + + // Add two landmark measurements, differing in angle + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + Vector4 measurement1 {0.0, -1.0, 0.0, 3.0}; + LocalOrientedPlane3Factor factor0(measurement0, + noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + LocalOrientedPlane3Factor factor1(measurement1, + noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + graph.add(factor0); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); + + // Optimize + ISAM2 isam2; + isam2.update(graph, values); + Values result_values = isam2.calculateEstimate(); + 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); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST(LocalOrientedPlane3Factor, Derivatives) { + // Measurement + OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); + + // Linearisation point + OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7); + Pose3 poseLin(Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI), Point3(1, 2, -4)); + Pose3 anchorPoseLin(Rot3::RzRyRx(-0.1*M_PI, 0.2*M_PI, 1.0*M_PI), Point3(-5, 0, 1)); + + // Factor + Key planeKey(1), poseKey(2), anchorPoseKey(3); + SharedGaussian noise = noiseModel::Isotropic::Sigma(3, 0.1); + LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey); + + // Calculate numerical derivatives + auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor, + _1, _2, _3, boost::none, boost::none, boost::none); + Matrix numericalH1 = numericalDerivative31(f, poseLin, anchorPoseLin, pLin); + Matrix numericalH2 = numericalDerivative32(f, poseLin, anchorPoseLin, pLin); + Matrix numericalH3 = numericalDerivative33(f, poseLin, anchorPoseLin, pLin); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2, actualH3; + factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2, actualH3); + + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); + EXPECT(assert_equal(numericalH3, actualH3, 1e-8)); +} + + +// /* ************************************************************************* */ +// // Simplified version of the test by Marco Camurri to debug issue #561 +// TEST(OrientedPlane3Factor, Issue561Simplified) { +// // Typedefs +// using Plane = OrientedPlane3; + +// NonlinearFactorGraph graph; + +// // Setup prior factors +// Pose3 x0(Rot3::identity(), Vector3(0, 0, 10)); +// auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); +// graph.addPrior(X(0), x0, x0_noise); + +// // Two horizontal planes with different heights, in the world frame. +// const Plane p1(0,0,1,1), p2(0,0,1,2); + +// auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); +// graph.addPrior(P(1), p1, p1_noise); + +// // ADDING THIS PRIOR MAKES OPTIMIZATION FAIL +// auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); +// graph.addPrior(P(2), p2, p2_noise); + +// // First plane factor +// auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement +// const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); +// graph.emplace_shared( +// p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); + +// // Second plane factor +// auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement +// const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); +// graph.emplace_shared( +// p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); + +// GTSAM_PRINT(graph); + +// // Initial values +// // Just offset the initial pose by 1m. This is what we are trying to optimize. +// Values initialEstimate; +// Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); +// initialEstimate.insert(P(1), p1); +// initialEstimate.insert(P(2), p2); +// initialEstimate.insert(X(0), x0_initial); + +// // Print Jacobian +// cout << graph.linearize(initialEstimate)->augmentedJacobian() << endl << endl; + +// // For testing only +// HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); +// const auto hessian = hessianFactor->hessianBlockDiagonal(); + +// Matrix hessianP1 = hessian.at(P(1)), +// hessianP2 = hessian.at(P(2)), +// hessianX0 = hessian.at(X(0)); + +// Eigen::JacobiSVD svdP1(hessianP1, Eigen::ComputeThinU), +// svdP2(hessianP2, Eigen::ComputeThinU), +// svdX0(hessianX0, Eigen::ComputeThinU); + +// double conditionNumberP1 = svdP1.singularValues()[0] / svdP1.singularValues()[2], +// conditionNumberP2 = svdP2.singularValues()[0] / svdP2.singularValues()[2], +// conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; + +// std::cout << "Hessian P1:\n" << hessianP1 << "\n" +// << "Condition number:\n" << conditionNumberP1 << "\n" +// << "Singular values:\n" << svdP1.singularValues().transpose() << "\n" +// << "SVD U:\n" << svdP1.matrixU() << "\n" << std::endl; + +// std::cout << "Hessian P2:\n" << hessianP2 << "\n" +// << "Condition number:\n" << conditionNumberP2 << "\n" +// << "Singular values:\n" << svdP2.singularValues().transpose() << "\n" +// << "SVD U:\n" << svdP2.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; + +// // std::cout << "Hessian P2:\n" << hessianP2 << std::endl; +// // std::cout << "Hessian X0:\n" << hessianX0 << std::endl; + +// // For testing only + +// // Optimize +// try { +// GaussNewtonParams params; +// //GTSAM_PRINT(graph); +// //Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first +// //params.setOrdering(ordering); +// // params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution +// params.setVerbosity("TERMINATION"); // show info about stopping conditions +// GaussNewtonOptimizer optimizer(graph, initialEstimate, params); +// Values result = optimizer.optimize(); +// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); +// EXPECT(x0.equals(result.at(X(0)))); +// EXPECT(p1.equals(result.at(P(1)))); +// EXPECT(p2.equals(result.at(P(2)))); +// } catch (const IndeterminantLinearSystemException &e) { +// std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; +// EXPECT(false); // fail if this happens +// } +// } + +/* ************************************************************************* */ +int main() { + srand(time(nullptr)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */