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

Tidy up comments and use cpplint

release/4.3a0
David Wisth 2021-02-16 11:48:26 +00:00
parent 71f39ab2e0
commit 960a3e1d8e
8 changed files with 132 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/OrientedPlane3.cpp
View File

@ -79,9 +79,9 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other,
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& v,
OptionalJacobian<3,3> H) const {
OptionalJacobian<3, 3> H) const {
Matrix22 H_n;
Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
Unit3 n_retract(n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
if (H) {
*H << H_n, Z_2x1, 0, 0, 1;
}
@ -95,4 +95,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
return Vector3(n_local(0), n_local(1), -d_local);
}
}
} // namespace gtsam

23
gtsam/geometry/OrientedPlane3.h
View File

@ -22,19 +22,19 @@
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <string>
namespace gtsam {
/**
* @brief Represents an infinite plane in 3D, which is composed of a planar normal and its
* perpendicular distance to the origin.
* Currently it provides a transform of the plane, and a norm 1 differencing of two planes.
* @brief Represents an infinite plane in 3D, which is composed of a planar
* normal and its perpendicular distance to the origin.
* Currently it provides a transform of the plane, and a norm 1 differencing of
* two planes.
* Refer to Trevor12iros for more math details.
*/
class GTSAM_EXPORT OrientedPlane3 {
private:
Unit3 n_; ///< The direction of the planar normal
double d_; ///< The perpendicular distance to this plane
@ -91,12 +91,14 @@ public:
OptionalJacobian<3, 6> Hr = boost::none) const;
/** Computes the error between the two planes, with derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
* Unit3::localCoordinates. This one has correct derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this
* class, which uses Unit3::localCoordinates. This one has correct
* derivatives.
* NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
* @param other the other plane
*/
Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
Vector3 errorVector(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Dimensionality of tangent space = 3 DOF
@ -110,7 +112,8 @@ public:
}
/// The retract function
OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const;
OrientedPlane3 retract(const Vector3& v,
OptionalJacobian<3, 3> H = boost::none) const;
/// The local coordinates function
Vector3 localCoordinates(const OrientedPlane3& s) const;
@ -142,5 +145,5 @@ template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
} // namespace gtsam
} // namespace gtsam

20
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -31,7 +31,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
//*******************************************************************************
TEST (OrientedPlane3, getMethods) {
TEST(OrientedPlane3, getMethods) {
Vector4 c;
c << -1, 0, 0, 5;
OrientedPlane3 plane1(c);
@ -92,7 +92,6 @@ inline static Vector randomVector(const Vector& minLimits,
//*******************************************************************************
TEST(OrientedPlane3, localCoordinates_retract) {
size_t numIterations = 10000;
Vector4 minPlaneLimit, maxPlaneLimit;
minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
@ -102,7 +101,6 @@ TEST(OrientedPlane3, localCoordinates_retract) {
minXiLimit << -M_PI, -M_PI, -10.0;
maxXiLimit << M_PI, M_PI, 10.0;
for (size_t i = 0; i < numIterations; i++) {
// Create a Plane
OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit));
Vector v12 = randomVector(minXiLimit, maxXiLimit);
@ -121,22 +119,24 @@ TEST(OrientedPlane3, localCoordinates_retract) {
}
//*******************************************************************************
TEST (OrientedPlane3, errorVector) {
TEST(OrientedPlane3, errorVector) {
OrientedPlane3 plane1(-1, 0.1, 0.2, 5);
OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
// Hard-coded regression values, to ensure the result doesn't change.
EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8));
EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5));
EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4),
plane1.errorVector(plane2), 1e-5));
// Test the jacobians of transform
Matrix33 actualH1, expectedH1, actualH2, expectedH2;
Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2);
EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()),
Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = //
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
@ -145,19 +145,19 @@ TEST (OrientedPlane3, errorVector) {
}
//*******************************************************************************
TEST (OrientedPlane3, jacobian_retract) {
TEST(OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
Matrix33 H_actual;
boost::function<OrientedPlane3(const Vector3&)> f =
boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
{
Vector3 v (-0.1, 0.2, 0.3);
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);
Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));
}
{
Vector3 v (0, 0, 0);
Vector3 v(0, 0, 0);
plane.retract(v, H_actual);
Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));

1
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -27,6 +27,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
boost::optional<Matrix&> H2) const {
Matrix36 predicted_H_pose;
Matrix33 predicted_H_plane, error_H_predicted;
OrientedPlane3 predicted_plane = plane.transform(pose,
H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr);

69
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -83,6 +83,7 @@ TEST(OrientedPlane3Factor, lm_translation_error) {
}
// // *************************************************************************
/*
TEST (OrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away
@ -122,6 +123,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
0.0, 3.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
*/
// *************************************************************************
TEST( OrientedPlane3Factor, Derivatives ) {
@ -211,34 +213,28 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
NonlinearFactorGraph graph;
// Setup prior factors
Pose3 x0(Rot3::identity(), Vector3(0, 0, 10));
// Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
Pose3 x0(Rot3::identity(), Vector3(10, -1, 1));
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior<Pose3>(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<Plane>(P(1), p1, p1_noise);
// ADDING THIS PRIOR MAKES OPTIMIZATION FAIL
auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
graph.addPrior<Plane>(P(1), p1, p1_noise);
graph.addPrior<Plane>(P(2), p2, p2_noise);
// First plane factor
// Plane factors
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<OrientedPlane3Factor>(
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_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05);
const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05);
graph.emplace_shared<OrientedPlane3Factor>(
p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1));
graph.emplace_shared<OrientedPlane3Factor>(
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;
@ -247,54 +243,9 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
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<Matrix> 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);
GaussNewtonOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
EXPECT(x0.equals(result.at<Pose3>(X(0))));

2
gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp
View File

@ -22,7 +22,7 @@ void LocalOrientedPlane3Factor::print(const string& s,
}
//***************************************************************************
Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
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 {

26
gtsam_unstable/slam/LocalOrientedPlane3Factor.h
View File

@ -9,6 +9,7 @@
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <string>
namespace gtsam {
@ -21,11 +22,18 @@ namespace gtsam {
* 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.
*
* The main purpose of this factor is to improve the numerical stability of the
* optimization, especially compared to gtsam::OrientedPlane3Factor. This
* is especially relevant when the sensor is far from the origin (and thus
* the derivatives associated to transforming the plane are large).
*
* x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e.
* a local linearisation point for the plane. The plane is representated and
* optimized in x1 frame in the optimization.
*/
class LocalOrientedPlane3Factor: public NoiseModelFactor3<Pose3, Pose3,
OrientedPlane3> {
class LocalOrientedPlane3Factor: public NoiseModelFactor3<Pose3, Pose3,
OrientedPlane3> {
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> Base;
@ -50,8 +58,9 @@ public:
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)
LocalOrientedPlane3Factor(const OrientedPlane3& z,
const SharedGaussian& noiseModel,
Key poseKey, Key anchorPoseKey, Key landmarkKey)
: Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
/// print
@ -68,6 +77,9 @@ public:
* @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.
*
* Note: The optimized plane is represented in anchor frame, a, not the
* world frame.
*/
Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa,
const OrientedPlane3& a_plane,
@ -76,5 +88,5 @@ public:
boost::optional<Matrix&> H3 = boost::none) const override;
};
} // gtsam
} // namespace gtsam

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

@ -48,12 +48,12 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) {
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};
@ -81,6 +81,7 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) {
}
// *************************************************************************
/*
TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away
@ -123,6 +124,7 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
0.0, 3.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
*/
// *************************************************************************
TEST(LocalOrientedPlane3Factor, Derivatives) {
@ -132,7 +134,7 @@ TEST(LocalOrientedPlane3Factor, Derivatives) {
// 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));
Pose3 anchorPoseLin(Rot3::RzRyRx(-0.1*M_PI, 0.2*M_PI, 1.0), Point3(-5, 0, 1));
// Factor
Key planeKey(1), poseKey(2), anchorPoseKey(3);
@ -142,13 +144,17 @@ TEST(LocalOrientedPlane3Factor, Derivatives) {
// Calculate numerical derivatives
auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor,
_1, _2, _3, boost::none, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3, OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3, OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH3 = numericalDerivative33<Vector3, Pose3, Pose3, OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH3 = numericalDerivative33<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2, actualH3;
factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2, actualH3);
factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2,
actualH3);
// Verify we get the expected error
EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
@ -157,109 +163,75 @@ TEST(LocalOrientedPlane3Factor, Derivatives) {
}
// /* ************************************************************************* */
// // Simplified version of the test by Marco Camurri to debug issue #561
// TEST(OrientedPlane3Factor, Issue561Simplified) {
// // Typedefs
// using Plane = OrientedPlane3;
/* ************************************************************************* */
// Simplified version of the test by Marco Camurri to debug issue #561
//
// This test provides an example of how LocalOrientedPlane3Factor works.
// x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e.
// a local linearisation point for the plane. The plane is representated and
// optimized in x1 frame in the optimization. This greatly improves numerical
// stability when the pose is far from the origin.
//
TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
// Typedefs
using Plane = OrientedPlane3;
// NonlinearFactorGraph graph;
NonlinearFactorGraph graph;
// // Setup prior factors
// Pose3 x0(Rot3::identity(), Vector3(0, 0, 10));
// auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
// graph.addPrior<Pose3>(X(0), x0, x0_noise);
// Setup prior factors
Pose3 x0(Rot3::identity(), Vector3(100, 30, 10)); // the "sensor pose"
Pose3 x1(Rot3::identity(), Vector3(90, 40, 5) ); // the "anchor pose"
// // Two horizontal planes with different heights, in the world frame.
// const Plane p1(0,0,1,1), p2(0,0,1,2);
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior<Pose3>(X(0), x0, x0_noise);
graph.addPrior<Pose3>(X(1), x1, x1_noise);
// auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
// graph.addPrior<Plane>(P(1), p1, p1_noise);
// Two horizontal planes with different heights, in the world frame.
const Plane p1(0, 0, 1, 1), p2(0, 0, 1, 2);
// Transform to x1, the "anchor frame" (i.e. local frame)
auto p1_in_x1 = p1.transform(x1);
auto p2_in_x1 = p2.transform(x1);
auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
graph.addPrior<Plane>(P(1), p1_in_x1, p1_noise);
graph.addPrior<Plane>(P(2), p2_in_x1, p2_noise);
// // ADDING THIS PRIOR MAKES OPTIMIZATION FAIL
// auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
// graph.addPrior<Plane>(P(2), p2, p2_noise);
// Add plane factors, with a local linearization point.
// transform p1 to pose x0 as a measurement
auto p1_measured_from_x0 = p1.transform(x0);
// transform p2 to pose x0 as a measurement
auto p2_measured_from_x0 = p2.transform(x0);
const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05);
const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05);
graph.emplace_shared<LocalOrientedPlane3Factor>(
p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), X(1), P(1));
graph.emplace_shared<LocalOrientedPlane3Factor>(
p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), X(1), P(2));
// // 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<OrientedPlane3Factor>(
// p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1));
// 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_in_x1);
initialEstimate.insert(P(2), p2_in_x1);
initialEstimate.insert(X(0), x0_initial);
initialEstimate.insert(X(1), x1);
// // 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<OrientedPlane3Factor>(
// 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<Matrix> 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<Pose3>(X(0))));
// EXPECT(p1.equals(result.at<Plane>(P(1))));
// EXPECT(p2.equals(result.at<Plane>(P(2))));
// } catch (const IndeterminantLinearSystemException &e) {
// std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl;
// EXPECT(false); // fail if this happens
// }
// }
// Optimize
try {
GaussNewtonOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
EXPECT(x0.equals(result.at<Pose3>(X(0))));
EXPECT(p1_in_x1.equals(result.at<Plane>(P(1))));
EXPECT(p2_in_x1.equals(result.at<Plane>(P(2))));
} catch (const IndeterminantLinearSystemException &e) {
cerr << "CAPTURED THE EXCEPTION: "
<< DefaultKeyFormatter(e.nearbyVariable()) << endl;
EXPECT(false); // fail if this happens
}
}
/* ************************************************************************* */
int main() {