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 * 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(); const size_t n = ab_pairs.size();
if (n < 2) { if (n < 2) {
return boost::none; // we need at least 2 pairs return {}; // we need at least 2 pairs
} }
// calculate centroids // calculate centroids
@ -359,7 +359,7 @@ boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
return Pose2(R, t); 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()) { if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
throw std::invalid_argument( throw std::invalid_argument(
"Pose2:Align expects 2*N matrices of equal shape."); "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 #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; Point2Pairs ab_pairs;
for (const Point2Pair &baPair : ba_pairs) { for (const Point2Pair &baPair : ba_pairs) {
ab_pairs.emplace_back(baPair.second, baPair.first); 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/geometry/Rot2.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/base/std_optional_serialization.h>
#include <optional>
namespace gtsam { namespace gtsam {
@ -99,10 +102,10 @@ public:
* Note this allows for noise on the points but in that case the mapping * Note this allows for noise on the points but in that case the mapping
* will not be exact. * 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. // 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 /// @name Testable
@ -134,10 +137,10 @@ public:
/// @{ /// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ ///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 ///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 * Calculate Adjoint map
@ -196,8 +199,8 @@ public:
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin { struct ChartAtOrigin {
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
}; };
using LieGroup<Pose2, 3>::inverse; // version with derivative using LieGroup<Pose2, 3>::inverse; // version with derivative
@ -208,8 +211,8 @@ public:
/** Return point coordinates in pose coordinate frame */ /** Return point coordinates in pose coordinate frame */
GTSAM_EXPORT Point2 transformTo(const Point2& point, GTSAM_EXPORT Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none) const; OptionalJacobian<2, 2> Dpoint = {}) const;
/** /**
* @brief transform many points in world coordinates and transform to Pose. * @brief transform many points in world coordinates and transform to Pose.
@ -220,8 +223,8 @@ public:
/** Return point coordinates in global frame */ /** Return point coordinates in global frame */
GTSAM_EXPORT Point2 transformFrom(const Point2& point, GTSAM_EXPORT Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none) const; OptionalJacobian<2, 2> Dpoint = {}) const;
/** /**
* @brief transform many points in Pose coordinates and transform to world. * @brief transform many points in Pose coordinates and transform to world.
@ -269,7 +272,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$ * @return 2D rotation \f$ \in SO(2) \f$
*/ */
GTSAM_EXPORT Rot2 bearing(const Point2& point, 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 * Calculate bearing to another pose
@ -277,7 +280,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$ * @return 2D rotation \f$ \in SO(2) \f$
*/ */
GTSAM_EXPORT Rot2 bearing(const Pose2& pose, 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 * Calculate range to a landmark
@ -285,8 +288,8 @@ public:
* @return range (double) * @return range (double)
*/ */
GTSAM_EXPORT double range(const Point2& point, GTSAM_EXPORT double range(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 2> H2=boost::none) const; OptionalJacobian<1, 2> H2={}) const;
/** /**
* Calculate range to another pose * Calculate range to another pose
@ -294,8 +297,8 @@ public:
* @return range (double) * @return range (double)
*/ */
GTSAM_EXPORT double range(const Pose2& point, GTSAM_EXPORT double range(const Pose2& point,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 3> H2=boost::none) const; OptionalJacobian<1, 3> H2={}) const;
/// @} /// @}
/// @name Advanced Interface /// @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) * Calculate pose between a vector of 2D point correspondences (p,q)
* where q = Pose2::transformFrom(p) = t + R*p * where q = Pose2::transformFrom(p) = t + R*p
*/ */
GTSAM_EXPORT boost::optional<Pose2> GTSAM_EXPORT std::optional<Pose2>
GTSAM_DEPRECATED align(const Point2Pairs& pairs); GTSAM_DEPRECATED align(const Point2Pairs& pairs);
#endif #endif

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

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