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

remove the error() function from OrientedPlane3 (it had incorrect derivatives)

release/4.3a0
David Wisth 2021-02-16 11:26:16 +00:00
parent 1771a5699a
commit 71f39ab2e0
3 changed files with 0 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/geometry/OrientedPlane3.cpp
View File

@ -17,9 +17,7 @@
* @brief A plane, represented by a normal direction and perpendicular distance
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
@ -59,24 +57,6 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr,
return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Vector2 n_error = -n_.localCoordinates(other.n_);
const auto f = boost::bind(&Unit3::localCoordinates, _1, _2);
if (H1) {
Matrix2 H_n_error_this = -numericalDerivative21<Vector2, Unit3, Unit3>(f, n_, other.n_);
*H1 << H_n_error_this, Z_2x1, 0, 0, 1;
}
if (H2) {
Matrix H_n_error_other = -numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, other.n_);
*H2 << H_n_error_other, Z_2x1, 0, 0, -1;
}
return Vector3(n_error(0), n_error(1), d_ - other.d_);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1,

12
gtsam/geometry/OrientedPlane3.h
View File

@ -20,13 +20,9 @@
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <algorithm>
#include <string>
namespace gtsam {
/**
@ -94,14 +90,6 @@ public:
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
/** Computes the error between two planes.
* The error is a norm 1 difference in tangent space.
* @param other The other plane
*/
Vector3 error(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = 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.

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

@ -144,27 +144,6 @@ TEST (OrientedPlane3, errorVector) {
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
//*******************************************************************************
TEST (OrientedPlane3, error) {
// Hard-coded regression values, to ensure the result doesn't change.
OrientedPlane3 plane1(-1, 0.1, 0.2, 5);
OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
// Test the jacobians of transform
Matrix33 actualH1, expectedH1, actualH2, expectedH2;
Vector3 actual = plane1.error(plane2, actualH1, actualH2);
EXPECT(assert_equal((Vector) Z_3x1, plane1.error(plane1), 1e-8));
EXPECT(assert_equal(Vector3(0.0678852, 0.0761865, -0.4), actual, 1e-5));
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = //
boost::bind(&OrientedPlane3::error, _1, _2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
//*******************************************************************************
TEST (OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);