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

pose3.h

release/4.3a0
kartik arcot 2023-01-12 08:20:25 -08:00
parent 273e2eb9ef
commit a18902563d
3 changed files with 59 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Pose3.cpp
View File

@ -439,14 +439,14 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
Hpose->setZero();
return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
}
return bearing(pose.translation(), Hself, boost::none);
return bearing(pose.translation(), Hself, {});
}
/* ************************************************************************* */
boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
const size_t n = abPointPairs.size();
if (n < 3) {
return boost::none; // we need at least three pairs
return {}; // we need at least three pairs
}
// calculate centroids
@ -466,7 +466,7 @@ boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
return Pose3(aRb, aTb);
}
boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
throw std::invalid_argument(
"Pose3:Align expects 3*N matrices of equal shape.");
@ -479,7 +479,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
std::optional<Pose3> align(const Point3Pairs &baPointPairs) {
Point3Pairs abPointPairs;
for (const Point3Pair &baPair : baPointPairs) {
abPointPairs.emplace_back(baPair.second, baPair.first);

68
gtsam/geometry/Pose3.h
View File

@ -75,18 +75,18 @@ public:
/// Named constructor with derivatives
static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> HR = boost::none,
OptionalJacobian<6, 3> Ht = boost::none);
OptionalJacobian<6, 3> HR = {},
OptionalJacobian<6, 3> Ht = {});
/**
* Create Pose3 by aligning two point pairs
* A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
* Note this allows for noise on the points but in that case the mapping will not be exact.
*/
static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
static std::optional<Pose3> Align(const Point3Pairs& abPointPairs);
// Version of Pose3::Align that takes 2 matrices.
static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
static std::optional<Pose3> Align(const Matrix& a, const Matrix& b);
/// @}
/// @name Testable
@ -139,10 +139,10 @@ public:
/// @{
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {});
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {});
/**
* Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
@ -157,13 +157,13 @@ public:
* Note that H_xib = AdjointMap()
*/
Vector6 Adjoint(const Vector6& xi_b,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_xib = boost::none) const;
OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_xib = {}) const;
/// The dual version of Adjoint
Vector6 AdjointTranspose(const Vector6& x,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_x = boost::none) const;
OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_x = {}) const;
/**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
@ -186,8 +186,8 @@ public:
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/
static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = {});
// temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
@ -197,8 +197,8 @@ public:
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = {});
/// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi);
@ -208,8 +208,8 @@ public:
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin {
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
};
/**
@ -250,7 +250,7 @@ public:
* @return point in world coordinates
*/
Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
{}, OptionalJacobian<3, 3> Hpoint = {}) const;
/**
* @brief transform many points in Pose coordinates and transform to world.
@ -272,7 +272,7 @@ public:
* @return point in Pose coordinates
*/
Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
{}, OptionalJacobian<3, 3> Hpoint = {}) const;
/**
* @brief transform many points in world coordinates and transform to Pose.
@ -286,10 +286,10 @@ public:
/// @{
/// get rotation
const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const;
/// get translation
const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const;
/// get x
double x() const {
@ -314,39 +314,39 @@ public:
* and transforms it to world coordinates wTb = wTa * aTb.
* This is identical to compose.
*/
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HaTb = boost::none) const;
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HaTb = {}) const;
/**
* Assuming self == wTa, takes a pose wTb in world coordinates
* and transforms it to local coordinates aTb = inv(wTa) * wTb
*/
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HwTb = boost::none) const;
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HwTb = {}) const;
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 3> Hpoint = boost::none) const;
double range(const Point3& point, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 3> Hpoint = {}) const;
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 6> Hpose = boost::none) const;
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 6> Hpose = {}) const;
/**
* Calculate bearing to a landmark
* @param point 3D location of landmark
* @return bearing (Unit3)
*/
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 3> Hpoint = boost::none) const;
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 3> Hpoint = {}) const;
/**
* Calculate bearing to another pose
@ -354,8 +354,8 @@ public:
* information is ignored.
* @return bearing (Unit3)
*/
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 6> Hpose = boost::none) const;
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 6> Hpose = {}) const;
/// @}
/// @name Advanced Interface
@ -384,8 +384,8 @@ public:
* @param s a value between 0 and 1.5
* @param other final point of interpolation geodesic on manifold
*/
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
OptionalJacobian<6, 6> Hy = boost::none) const;
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
OptionalJacobian<6, 6> Hy = {}) const;
/// Output stream operator
GTSAM_EXPORT

39
gtsam/geometry/tests/testPose3.cpp
View File

@ -23,6 +23,7 @@
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <functional>
using namespace std;
using namespace gtsam;
@ -286,8 +287,8 @@ TEST(Pose3, translation) {
Matrix actualH;
EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Point3, Pose3>(
std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T);
std::function<Point3(const Pose3&)> f = [](const Pose3& T) { return T.translation(); };
Matrix numericalH = numericalDerivative11<Point3, Pose3>(f, T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -297,8 +298,8 @@ TEST(Pose3, rotation) {
Matrix actualH;
EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T);
std::function<Rot3(const Pose3&)> f = [](const Pose3& T) { return T.rotation(); };
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(f, T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -396,7 +397,7 @@ Point3 transformFrom_(const Pose3& pose, const Point3& point) {
}
TEST(Pose3, Dtransform_from1_a) {
Matrix actualDtransform_from1;
T.transformFrom(P, actualDtransform_from1, boost::none);
T.transformFrom(P, actualDtransform_from1, {});
Matrix numerical = numericalDerivative21(transformFrom_, T, P);
EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
}
@ -404,7 +405,7 @@ TEST(Pose3, Dtransform_from1_a) {
TEST(Pose3, Dtransform_from1_b) {
Pose3 origin;
Matrix actualDtransform_from1;
origin.transformFrom(P, actualDtransform_from1, boost::none);
origin.transformFrom(P, actualDtransform_from1, {});
Matrix numerical = numericalDerivative21(transformFrom_, origin, P);
EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
}
@ -413,7 +414,7 @@ TEST(Pose3, Dtransform_from1_c) {
Point3 origin(0, 0, 0);
Pose3 T0(R, origin);
Matrix actualDtransform_from1;
T0.transformFrom(P, actualDtransform_from1, boost::none);
T0.transformFrom(P, actualDtransform_from1, {});
Matrix numerical = numericalDerivative21(transformFrom_, T0, P);
EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
}
@ -423,7 +424,7 @@ TEST(Pose3, Dtransform_from1_d) {
Point3 t0(100, 0, 0);
Pose3 T0(I, t0);
Matrix actualDtransform_from1;
T0.transformFrom(P, actualDtransform_from1, boost::none);
T0.transformFrom(P, actualDtransform_from1, {});
// print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transformFrom_, T0, P);
// print(numerical, "Dtransform_from1_d numerical:");
@ -433,7 +434,7 @@ TEST(Pose3, Dtransform_from1_d) {
/* ************************************************************************* */
TEST(Pose3, Dtransform_from2) {
Matrix actualDtransform_from2;
T.transformFrom(P, boost::none, actualDtransform_from2);
T.transformFrom(P, {}, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transformFrom_, T, P);
EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8));
}
@ -444,7 +445,7 @@ Point3 transform_to_(const Pose3& pose, const Point3& point) {
}
TEST(Pose3, Dtransform_to1) {
Matrix computed;
T.transformTo(P, computed, boost::none);
T.transformTo(P, computed, {});
Matrix numerical = numericalDerivative21(transform_to_, T, P);
EXPECT(assert_equal(numerical, computed, 1e-8));
}
@ -452,7 +453,7 @@ TEST(Pose3, Dtransform_to1) {
/* ************************************************************************* */
TEST(Pose3, Dtransform_to2) {
Matrix computed;
T.transformTo(P, boost::none, computed);
T.transformTo(P, {}, computed);
Matrix numerical = numericalDerivative22(transform_to_, T, P);
EXPECT(assert_equal(numerical, computed, 1e-8));
}
@ -812,7 +813,7 @@ TEST(Pose3, Align1) {
Point3Pair ab3(Point3(20,30,0), Point3(10,20,0));
const vector<Point3Pair> correspondences{ab1, ab2, ab3};
boost::optional<Pose3> actual = Pose3::Align(correspondences);
std::optional<Pose3> actual = Pose3::Align(correspondences);
EXPECT(assert_equal(expected, *actual));
}
@ -829,7 +830,7 @@ TEST(Pose3, Align2) {
const Point3Pair ab1{q1, p1}, ab2{q2, p2}, ab3{q3, p3};
const vector<Point3Pair> correspondences{ab1, ab2, ab3};
boost::optional<Pose3> actual = Pose3::Align(correspondences);
std::optional<Pose3> actual = Pose3::Align(correspondences);
EXPECT(assert_equal(expected, *actual, 1e-5));
}
@ -839,7 +840,7 @@ TEST( Pose3, ExpmapDerivative1) {
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
@ -884,7 +885,7 @@ TEST( Pose3, ExpmapDerivativeQr) {
w.head<3>() = w.head<3>() * 0.9e-2;
Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
}
@ -896,7 +897,7 @@ TEST( Pose3, LogmapDerivative) {
Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none);
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
EXPECT(assert_equal(expectedH, actualH));
}
@ -1189,9 +1190,9 @@ TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;
Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
EXPECT(assert_equal(T, actual));
std::function<Pose3(Rot3, Point3)> create =
std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
std::function<Pose3(Rot3, Point3)> create = [](Rot3 R, Point3 t) {
return Pose3::Create(R, t);
};
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}