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

Address Frank's comments and clean up changes

release/4.3a0
David 2020-06-23 19:02:51 +10:00
parent 75eb859ee7
commit 58a0f82cba
5 changed files with 62 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/OrientedPlane3.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
#include <gtsam/base/numericalDerivative.h>
using namespace std;
@ -58,7 +59,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
Vector3 OrientedPlane3::error(const OrientedPlane3& plane,
OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
// Numerically calculate the derivative since this function doesn't provide one.
auto f = boost::bind(&OrientedPlane3::Error, _1, _2);
if (H1) *H1 = numericalDerivative21<Vector3, OrientedPlane3, OrientedPlane3>(f, *this, plane);
if (H2) *H2 = numericalDerivative22<Vector3, OrientedPlane3, OrientedPlane3>(f, *this, plane);
Vector2 n_error = -n_.localCoordinates(plane.n_);
return Vector3(n_error(0), n_error(1), d_ - plane.d_);
}

4
gtsam/geometry/OrientedPlane3.h
View File

@ -112,7 +112,9 @@ public:
* The error is a norm 1 difference in tangent space.
* @param the other plane
*/
Vector3 error(const OrientedPlane3& plane) const;
Vector3 error(const OrientedPlane3& plane,
OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) {
return plane1.error(plane2);

27
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -19,6 +19,33 @@ void OrientedPlane3Factor::print(const string& s,
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
const OrientedPlane3& plane, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Vector err(3);
if (H1 || H2) {
Matrix36 predicted_H_pose;
Matrix33 predicted_H_plane, error_H_predicted;
OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose);
err << predicted_plane.error(measured_p_, error_H_predicted);
// Apply the chain rule to calculate the derivatives.
if (H1) {
*H1 = error_H_predicted * predicted_H_pose;
}
if (H2) {
*H2 = error_H_predicted * predicted_H_plane;
}
} else {
OrientedPlane3 predicted_plane = plane.transform(pose);
err << predicted_plane.error(measured_p_);
}
return (err);
}
//***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const {

27
gtsam/slam/OrientedPlane3Factor.h
View File

@ -7,7 +7,6 @@
#pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -48,31 +47,7 @@ public:
/// evaluateError
virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
Vector err(3);
if (H1 || H2) {
Matrix H1_1, H2_1;
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1);
err << predicted_plane.error(measured_p_);
// Numerically calculate the derivative since this function doesn't provide one.
auto f = boost::bind(&OrientedPlane3::Error, _1, _2);
Matrix H1_2 = numericalDerivative21<Vector3, OrientedPlane3, OrientedPlane3>(f, predicted_plane, measured_p_);
// Apply the chain rule to calculate the derivatives.
if (H1) {
*H1 = H1_2 * H1_1;
}
if (H2) {
*H2 = H1_2 * H2_1;
}
} else {
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2);
err << predicted_plane.error(measured_p_);
}
return (err);
}
;
boost::none) const;
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior

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

@ -122,40 +122,34 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
double randDouble(double max = 1) {
return static_cast<double>(rand()) / RAND_MAX * max;
}
TEST( OrientedPlane3Factor, Derivatives ) {
for (int i=0; i<100; i++) {
// Random measurement
OrientedPlane3 p(randDouble(), randDouble(), randDouble(), randDouble());
// Measurement
OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5);
// Random linearisation point
OrientedPlane3 pLin(randDouble(), randDouble(), randDouble(), randDouble());
gtsam::Point3 pointLin = gtsam::Point3(randDouble(100), randDouble(100), randDouble(100));
gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(randDouble(2*M_PI), randDouble(2*M_PI), randDouble(2*M_PI));
Pose3 poseLin(rotationLin, pointLin);
// Linearisation point
OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7);
gtsam::Point3 pointLin = gtsam::Point3(1, 2, -4);
gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI);
Pose3 poseLin(rotationLin, pointLin);
// Factor
Key planeKey(1), poseKey(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
// Factor
Key planeKey(1), poseKey(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
// Calculate numerical derivatives
boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
&OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
// Calculate numerical derivatives
boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
&OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2;
factor.evaluateError(poseLin, pLin, actualH1, actualH2);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2;
factor.evaluateError(poseLin, pLin, actualH1, actualH2);
// Verify we get the expected error
EXPECT(assert_equal(numericalH1, actualH1, 1e-7));
EXPECT(assert_equal(numericalH2, actualH2, 1e-7));
}
// Verify we get the expected error
EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
}
// *************************************************************************