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

68
gtsam/geometry/Pose3.h
View File

@ -75,18 +75,18 @@ public:
/// Named constructor with derivatives /// Named constructor with derivatives
static Pose3 Create(const Rot3& R, const Point3& t, static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> HR = boost::none, OptionalJacobian<6, 3> HR = {},
OptionalJacobian<6, 3> Ht = boost::none); OptionalJacobian<6, 3> Ht = {});
/** /**
* Create Pose3 by aligning two point pairs * 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 * 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. * 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. // 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 /// @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$ /// 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 /// 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 * 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() * Note that H_xib = AdjointMap()
*/ */
Vector6 Adjoint(const Vector6& xi_b, Vector6 Adjoint(const Vector6& xi_b,
OptionalJacobian<6, 6> H_this = boost::none, OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_xib = boost::none) const; OptionalJacobian<6, 6> H_xib = {}) const;
/// The dual version of Adjoint /// The dual version of Adjoint
Vector6 AdjointTranspose(const Vector6& x, Vector6 AdjointTranspose(const Vector6& x,
OptionalJacobian<6, 6> H_this = boost::none, OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_x = boost::none) const; OptionalJacobian<6, 6> H_x = {}) const;
/** /**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 * 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 * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/ */
static Vector6 adjoint(const Vector6& xi, const Vector6& y, static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none, OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = boost::none); OptionalJacobian<6, 6> H_y = {});
// temporary fix for wrappers until case issue is resolved // temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} 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. * 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, static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none, OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = boost::none); OptionalJacobian<6, 6> H_y = {});
/// Derivative of Expmap /// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi); static Matrix6 ExpmapDerivative(const Vector6& xi);
@ -208,8 +208,8 @@ public:
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin { struct ChartAtOrigin {
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none); static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
}; };
/** /**
@ -250,7 +250,7 @@ public:
* @return point in world coordinates * @return point in world coordinates
*/ */
Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself = 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. * @brief transform many points in Pose coordinates and transform to world.
@ -272,7 +272,7 @@ public:
* @return point in Pose coordinates * @return point in Pose coordinates
*/ */
Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself = 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. * @brief transform many points in world coordinates and transform to Pose.
@ -286,10 +286,10 @@ public:
/// @{ /// @{
/// get rotation /// get rotation
const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const; const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const;
/// get translation /// get translation
const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const; const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const;
/// get x /// get x
double x() const { double x() const {
@ -314,39 +314,39 @@ public:
* and transforms it to world coordinates wTb = wTa * aTb. * and transforms it to world coordinates wTb = wTa * aTb.
* This is identical to compose. * This is identical to compose.
*/ */
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none, Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HaTb = boost::none) const; OptionalJacobian<6, 6> HaTb = {}) const;
/** /**
* Assuming self == wTa, takes a pose wTb in world coordinates * Assuming self == wTa, takes a pose wTb in world coordinates
* and transforms it to local coordinates aTb = inv(wTa) * wTb * and transforms it to local coordinates aTb = inv(wTa) * wTb
*/ */
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none, Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HwTb = boost::none) const; OptionalJacobian<6, 6> HwTb = {}) const;
/** /**
* Calculate range to a landmark * Calculate range to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none, double range(const Point3& point, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 3> Hpoint = boost::none) const; OptionalJacobian<1, 3> Hpoint = {}) const;
/** /**
* Calculate range to another pose * Calculate range to another pose
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none, double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 6> Hpose = boost::none) const; OptionalJacobian<1, 6> Hpose = {}) const;
/** /**
* Calculate bearing to a landmark * Calculate bearing to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @return bearing (Unit3) * @return bearing (Unit3)
*/ */
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none, Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 3> Hpoint = boost::none) const; OptionalJacobian<2, 3> Hpoint = {}) const;
/** /**
* Calculate bearing to another pose * Calculate bearing to another pose
@ -354,8 +354,8 @@ public:
* information is ignored. * information is ignored.
* @return bearing (Unit3) * @return bearing (Unit3)
*/ */
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none, Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 6> Hpose = boost::none) const; OptionalJacobian<2, 6> Hpose = {}) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
@ -384,8 +384,8 @@ public:
* @param s a value between 0 and 1.5 * @param s a value between 0 and 1.5
* @param other final point of interpolation geodesic on manifold * @param other final point of interpolation geodesic on manifold
*/ */
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
OptionalJacobian<6, 6> Hy = boost::none) const; OptionalJacobian<6, 6> Hy = {}) const;
/// Output stream operator /// Output stream operator
GTSAM_EXPORT GTSAM_EXPORT

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

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