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

pose2.h

release/4.3a0
kartik arcot 2023-01-11 15:27:26 -08:00
parent 02b5485c76
commit 273e2eb9ef
3 changed files with 30 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Pose2.cpp
View File

@ -327,10 +327,10 @@ double Pose2::range(const Pose2& pose,
* as they also satisfy ca = t + R*cb, hence t = ca - R*cb
*/
boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
std::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
const size_t n = ab_pairs.size();
if (n < 2) {
return boost::none; // we need at least 2 pairs
return {}; // we need at least 2 pairs
}
// calculate centroids
@ -359,7 +359,7 @@ boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
return Pose2(R, t);
}
boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
std::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
throw std::invalid_argument(
"Pose2:Align expects 2*N matrices of equal shape.");
@ -372,7 +372,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
boost::optional<Pose2> align(const Point2Pairs& ba_pairs) {
std::optional<Pose2> align(const Point2Pairs& ba_pairs) {
Point2Pairs ab_pairs;
for (const Point2Pair &baPair : ba_pairs) {
ab_pairs.emplace_back(baPair.second, baPair.first);

37
gtsam/geometry/Pose2.h
View File

@ -25,6 +25,9 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
#include <gtsam/base/std_optional_serialization.h>
#include <optional>
namespace gtsam {
@ -99,10 +102,10 @@ public:
* Note this allows for noise on the points but in that case the mapping
* will not be exact.
*/
static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
static std::optional<Pose2> Align(const Point2Pairs& abPointPairs);
// Version of Pose2::Align that takes 2 matrices.
static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
static std::optional<Pose2> Align(const Matrix& a, const Matrix& b);
/// @}
/// @name Testable
@ -134,10 +137,10 @@ public:
/// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
/**
* Calculate Adjoint map
@ -196,8 +199,8 @@ public:
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
};
using LieGroup<Pose2, 3>::inverse; // version with derivative
@ -208,8 +211,8 @@ public:
/** Return point coordinates in pose coordinate frame */
GTSAM_EXPORT Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
/**
* @brief transform many points in world coordinates and transform to Pose.
@ -220,8 +223,8 @@ public:
/** Return point coordinates in global frame */
GTSAM_EXPORT Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
/**
* @brief transform many points in Pose coordinates and transform to world.
@ -269,7 +272,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
/**
* Calculate bearing to another pose
@ -277,7 +280,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
/**
* Calculate range to a landmark
@ -285,8 +288,8 @@ public:
* @return range (double)
*/
GTSAM_EXPORT double range(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 2> H2=boost::none) const;
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 2> H2={}) const;
/**
* Calculate range to another pose
@ -294,8 +297,8 @@ public:
* @return range (double)
*/
GTSAM_EXPORT double range(const Pose2& point,
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 3> H2=boost::none) const;
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 3> H2={}) const;
/// @}
/// @name Advanced Interface
@ -349,7 +352,7 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
* Calculate pose between a vector of 2D point correspondences (p,q)
* where q = Pose2::transformFrom(p) = t + R*p
*/
GTSAM_EXPORT boost::optional<Pose2>
GTSAM_EXPORT std::optional<Pose2>
GTSAM_DEPRECATED align(const Point2Pairs& pairs);
#endif

12
gtsam/geometry/tests/testPose2.cpp
View File

@ -23,7 +23,7 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot2.h>
#include <boost/optional.hpp>
#include <optional>
#include <cmath>
#include <iostream>
@ -718,7 +718,7 @@ TEST(Pose2, align_1) {
Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
{Point2(30, 20), Point2(20, 10)}};
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}
@ -731,7 +731,7 @@ TEST(Pose2, align_2) {
Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
{expected.transformFrom(b2), b2}};
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}
@ -752,7 +752,7 @@ TEST(Pose2, align_3) {
Point2Pair ab3(make_pair(a3, b3));
const Point2Pairs ab_pairs{ab1, ab2, ab3};
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}
@ -762,7 +762,7 @@ namespace {
/* ************************************************************************* */
struct Triangle { size_t i_, j_, k_;};
boost::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
std::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
const pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]},
@ -780,7 +780,7 @@ TEST(Pose2, align_4) {
Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;
boost::optional<Pose2> actual = align2(as, bs, {t1, t2});
std::optional<Pose2> actual = align2(as, bs, {t1, t2});
EXPECT(assert_equal(expected, *actual));
}