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

Make changes needed to deal with typdef flag activation

release/4.3a0
Frank 2016-06-06 19:02:52 -07:00
parent be21889e9d
commit aa40b266f6
9 changed files with 140 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/config.h.in
View File

@ -64,7 +64,7 @@
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4 #cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Publish flag about Eigen typedef // Publish flag about Eigen typedef
#cmakedefine GTSAM_USE_VECTOR3_POINTS #cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS
// Support Metis-based nested dissection // Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION

2
gtsam/geometry/Cal3Bundler.cpp
View File

@ -106,7 +106,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
const int maxIterations = 10; const int maxIterations = 10;
int iteration; int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) { for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) if (distance(uncalibrate(pn), pi) <= tol)
break; break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
const double rr = xx + yy; const double rr = xx + yy;

2
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -144,7 +144,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
const int maxIterations = 10; const int maxIterations = 10;
int iteration; int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) { for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break; if (distance(uncalibrate(pn), pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy; const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr); const double g = (1 + k1_ * rr + k2_ * rr * rr);

28
gtsam/geometry/CalibratedCamera.cpp
View File

@ -24,7 +24,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { Matrix26 PinholeBase::Dpose(const Vector2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v; double uv = u * v, uu = u * u, vv = v * v;
@ -34,7 +34,7 @@ Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) { Matrix23 PinholeBase::Dpoint(const Vector2& pn, double d, const Matrix3& Rt) {
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point; Matrix23 Dpn_point;
@ -85,20 +85,20 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { Vector2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) {
double d = 1.0 / pc.z(); double d = 1.0 / pc.z();
const double u = pc.x() * d, v = pc.y() * d; const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint) if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v); return Vector2(u, v);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { Vector2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
if (Dpoint) { if (Dpoint) {
Matrix32 Dpoint3_pc; Matrix32 Dpoint3_pc;
Matrix23 Duv_point3; Matrix23 Duv_point3;
Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); Vector2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3);
*Dpoint = Duv_point3 * Dpoint3_pc; *Dpoint = Duv_point3 * Dpoint3_pc;
return uv; return uv;
} else } else
@ -106,14 +106,14 @@ Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const { pair<Vector2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw); const Point3 pc = pose().transform_to(pw);
const Point2 pn = Project(pc); const Vector2 pn = Project(pc);
return make_pair(pn, pc.z() > 0); return make_pair(pn, pc.z() > 0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const { OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed Matrix3 Rt; // calculated by transform_to if needed
@ -122,7 +122,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
if (q.z() <= 0) if (q.z() <= 0)
throw CheiralityException(); throw CheiralityException();
#endif #endif
const Point2 pn = Project(q); const Vector2 pn = Project(q);
if (Dpose || Dpoint) { if (Dpose || Dpoint) {
const double d = 1.0 / q.z(); const double d = 1.0 / q.z();
@ -135,7 +135,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint) const { OptionalJacobian<2, 2> Dpoint) const {
// world to camera coordinate // world to camera coordinate
@ -146,7 +146,7 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
// camera to normalized image coordinate // camera to normalized image coordinate
Matrix2 Dpn_pc; Matrix2 Dpn_pc;
const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); const Vector2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
// chain the Jacobian matrices // chain the Jacobian matrices
if (Dpose) { if (Dpose) {
@ -161,7 +161,7 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
return pn; return pn;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p, Point3 PinholeBase::backproject_from_camera(const Vector2& p,
const double depth) { const double depth) {
return Point3(p.x() * depth, p.y() * depth, depth); return Point3(p.x() * depth, p.y() * depth, depth);
} }
@ -178,7 +178,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point, Vector2 CalibratedCamera::project(const Point3& point,
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
return project2(point, Dcamera, Dpoint); return project2(point, Dcamera, Dpoint);
} }

25
gtsam/geometry/CalibratedCamera.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <gtsam/geometry/BearingRange.h> #include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
@ -28,8 +29,6 @@
namespace gtsam { namespace gtsam {
class Point2;
class GTSAM_EXPORT CheiralityException: public ThreadsafeException< class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
CheiralityException> { CheiralityException> {
public: public:
@ -55,7 +54,7 @@ public:
* Some classes template on either PinholeCamera or StereoCamera, * Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns. * and this typedef informs those classes what "project" returns.
*/ */
typedef Point2 Measurement; typedef Vector2 Measurement;
private: private:
@ -71,7 +70,7 @@ protected:
* @param pn projection in normalized coordinates * @param pn projection in normalized coordinates
* @param d disparity (inverse depth) * @param d disparity (inverse depth)
*/ */
static Matrix26 Dpose(const Point2& pn, double d); static Matrix26 Dpose(const Vector2& pn, double d);
/** /**
* Calculate Jacobian with respect to point * Calculate Jacobian with respect to point
@ -79,7 +78,7 @@ protected:
* @param d disparity (inverse depth) * @param d disparity (inverse depth)
* @param Rt transposed rotation matrix * @param Rt transposed rotation matrix
*/ */
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); static Matrix23 Dpoint(const Vector2& pn, double d, const Matrix3& Rt);
/// @} /// @}
@ -170,7 +169,7 @@ public:
* Does *not* throw a CheiralityException, even if pc behind image plane * Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates * @param pc point in camera coordinates
*/ */
static Point2 Project(const Point3& pc, // static Vector2 Project(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none); OptionalJacobian<2, 3> Dpoint = boost::none);
/** /**
@ -178,18 +177,18 @@ public:
* Does *not* throw a CheiralityException, even if pc behind image plane * Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates * @param pc point in camera coordinates
*/ */
static Point2 Project(const Unit3& pc, // static Vector2 Project(const Unit3& pc, //
OptionalJacobian<2, 2> Dpoint = boost::none); OptionalJacobian<2, 2> Dpoint = boost::none);
/// Project a point into the image and check depth /// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const; std::pair<Vector2, bool> projectSafe(const Point3& pw) const;
/** Project point into the image /** Project point into the image
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
* @param point 3D point in world coordinates * @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = Vector2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** Project point at infinity into the image /** Project point at infinity into the image
@ -197,12 +196,12 @@ public:
* @param point 3D point in world coordinates * @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Point2 project2(const Unit3& point, Vector2 project2(const Unit3& point,
OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const; OptionalJacobian<2, 2> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
static Point3 backproject_from_camera(const Point2& p, const double depth); static Point3 backproject_from_camera(const Vector2& p, const double depth);
/// @} /// @}
/// @name Advanced interface /// @name Advanced interface
@ -326,11 +325,11 @@ public:
* @deprecated * @deprecated
* Use project2, which is more consistently named across Pinhole cameras * Use project2, which is more consistently named across Pinhole cameras
*/ */
Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = Vector2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& pn, double depth) const { Point3 backproject(const Vector2& pn, double depth) const {
return pose().transform_from(backproject_from_camera(pn, depth)); return pose().transform_from(backproject_from_camera(pn, depth));
} }

86
gtsam/geometry/Point2.cpp
View File

@ -22,6 +22,35 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
double norm(const Point2& p, OptionalJacobian<1,2> H) {
double r = std::sqrt(p.x() * p.x() + p.y() * p.y());
if (H) {
if (fabs(r) > 1e-10)
*H << p.x() / r, p.y() / r;
else
*H << 1, 1; // really infinity, why 1 ?
}
return r;
}
/* ************************************************************************* */
double distance(const Point2& p, const Point2& q, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) {
Point2 d = q - p;
if (H1 || H2) {
Matrix12 H;
double r = norm(d, H);
if (H1) *H1 = -H;
if (H2) *H2 = H;
return r;
} else {
return norm(d);
}
}
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */ /* ************************************************************************* */
void Point2::print(const string& s) const { void Point2::print(const string& s) const {
cout << s << *this << endl; cout << s << *this << endl;
@ -34,45 +63,26 @@ bool Point2::equals(const Point2& q, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
double Point2::norm(OptionalJacobian<1,2> H) const { double Point2::norm(OptionalJacobian<1,2> H) const {
double r = std::sqrt(x() * x() + y() * y()); return norm(*this, H);
if (H) {
if (fabs(r) > 1e-10)
*H << x() / r, y() / r;
else
*H << 1, 1; // really infinity, why 1 ?
}
return r;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const { OptionalJacobian<1,2> H2) const {
Point2 d = point - *this; return distance(point, H1, H2);
if (H1 || H2) {
Matrix12 H;
double r = d.norm(H);
if (H1) *H1 = -H;
if (H2) *H2 = H;
return r;
} else
return d.norm();
} }
/* /* ************************************************************************* */
* Calculate f and h, respectively the parallel and perpendicular distance of ostream &operator<<(ostream &os, const Point2& p) {
* the intersections of two circles along and from the line connecting the centers. os << '(' << p.x() << ", " << p.y() << ')';
* Both are dimensionless fractions of the distance d between the circle centers. return os;
* If the circles do not intersect or they are identical, returns boost::none. }
* If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch #endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles
*/
/* ************************************************************************* */ /* ************************************************************************* */
// Math inspired by http://paulbourke.net/geometry/circlesphere/ // Math inspired by http://paulbourke.net/geometry/circlesphere/
boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d, boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
double tol) { double tol) {
double R2_d2 = R_d*R_d; // Yes, RD-D2 ! double R2_d2 = R_d*R_d; // Yes, RD-D2 !
@ -87,7 +97,7 @@ boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
} }
/* ************************************************************************* */ /* ************************************************************************* */
list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2, list<Point2> circleCircleIntersection(Point2 c1, Point2 c2,
boost::optional<Point2> fh) { boost::optional<Point2> fh) {
list<Point2> solutions; list<Point2> solutions;
@ -116,27 +126,21 @@ list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
} }
/* ************************************************************************* */ /* ************************************************************************* */
list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
double r2, double tol) { double r2, double tol) {
// distance between circle centers. // distance between circle centers.
double d = c1.distance(c2); double d = distance(c1, c2);
// centers coincide, either no solution or infinite number of solutions. // centers coincide, either no solution or infinite number of solutions.
if (d<1e-9) return list<Point2>(); if (d<1e-9) return list<Point2>();
// Calculate f and h given normalized radii // Calculate f and h given normalized radii
double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d; double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d;
boost::optional<Point2> fh = CircleCircleIntersection(R_d,r_d); boost::optional<Point2> fh = circleCircleIntersection(R_d,r_d);
// Call version that takes fh // Call version that takes fh
return CircleCircleIntersection(c1, c2, fh); return circleCircleIntersection(c1, c2, fh);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
} }
/* ************************************************************************* */ /* ************************************************************************* */

95
gtsam/geometry/Point2.h
View File

@ -61,43 +61,13 @@ public:
/// construct from 2D vector /// construct from 2D vector
explicit Point2(const Vector2& v):Vector2(v) {} explicit Point2(const Vector2& v):Vector2(v) {}
/// @}
/// @name Declare circle intersection functionality
/// @{
/* friend boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol);
* @brief Circle-circle intersection, given normalized radii. friend std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
* Calculate f and h, respectively the parallel and perpendicular distance of friend std::list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol);
* the intersections of two circles along and from the line connecting the centers.
* Both are dimensionless fractions of the distance d between the circle centers.
* If the circles do not intersect or they are identical, returns boost::none.
* If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles
* @return optional Point2 with f and h, boost::none if no solution.
*/
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d,
double tol = 1e-9);
/*
* @brief Circle-circle intersection, from the normalized radii solution.
* @param c1 center of first circle
* @param c2 center of second circle
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>);
/**
* @brief Intersect 2 circles
* @param c1 center of first circle
* @param r1 radius of first circle
* @param c2 center of second circle
* @param r2 radius of second circle
* @param tol: absolute tolerance below which we consider touching circles
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);
/// @} /// @}
/// @name Testable /// @name Testable
@ -162,6 +132,15 @@ public:
static Vector2 Logmap(const Point2& p) { return p;} static Vector2 Logmap(const Point2& p) { return p;}
static Point2 Expmap(const Vector2& v) { return Point2(v);} static Point2 Expmap(const Vector2& v) { return Point2(v);}
inline double dist(const Point2& p2) const {return distance(p2);} inline double dist(const Point2& p2) const {return distance(p2);}
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9) {
return circleCircleIntersection( R_d, r_d, tol);
}
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>) {
return circleCircleIntersection( c1, c2, boost::optional<Point2>);
}
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9) {
return CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
}
/// @} /// @}
#endif #endif
@ -186,6 +165,14 @@ struct traits<Point2> : public internal::VectorSpace<Point2> {
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS #endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// Distance of the point from the origin, with Jacobian
double norm(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
/// distance between two points
double distance(const Point2& p1, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none);
// Convenience typedef // Convenience typedef
typedef std::pair<Point2, Point2> Point2Pair; typedef std::pair<Point2, Point2> Point2Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
@ -198,5 +185,41 @@ inline Point2 operator*(double s, const Point2& p) {
return p * s; return p * s;
} }
/*
* @brief Circle-circle intersection, given normalized radii.
* Calculate f and h, respectively the parallel and perpendicular distance of
* the intersections of two circles along and from the line connecting the centers.
* Both are dimensionless fractions of the distance d between the circle centers.
* If the circles do not intersect or they are identical, returns boost::none.
* If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles
* @return optional Point2 with f and h, boost::none if no solution.
*/
boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
/*
* @brief Circle-circle intersection, from the normalized radii solution.
* @param c1 center of first circle
* @param c2 center of second circle
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
/**
* @brief Intersect 2 circles
* @param c1 center of first circle
* @param r1 radius of first circle
* @param c2 center of second circle
* @param r2 radius of second circle
* @param tol: absolute tolerance below which we consider touching circles
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);
} // \ namespace gtsam } // \ namespace gtsam

6
gtsam/geometry/Pose2.cpp
View File

@ -53,7 +53,7 @@ void Pose2::print(const string& s) const {
/* ************************************************************************* */ /* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const { bool Pose2::equals(const Pose2& q, double tol) const {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); return equal_with_abs_tol(t_, q.t_, tol) && r_.equals(q.r_, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -249,7 +249,7 @@ double Pose2::range(const Point2& point,
Point2 d = point - t_; Point2 d = point - t_;
if (!Hpose && !Hpoint) return d.norm(); if (!Hpose && !Hpoint) return d.norm();
Matrix12 D_r_d; Matrix12 D_r_d;
double r = d.norm(D_r_d); double r = norm(d, D_r_d);
if (Hpose) { if (Hpose) {
Matrix23 D_d_pose; Matrix23 D_d_pose;
D_d_pose << -r_.c(), r_.s(), 0.0, D_d_pose << -r_.c(), r_.s(), 0.0,
@ -267,7 +267,7 @@ double Pose2::range(const Pose2& pose,
Point2 d = pose.t() - t_; Point2 d = pose.t() - t_;
if (!Hpose && !Hother) return d.norm(); if (!Hpose && !Hother) return d.norm();
Matrix12 D_r_d; Matrix12 D_r_d;
double r = d.norm(D_r_d); double r = norm(d, D_r_d);
if (Hpose) { if (Hpose) {
Matrix23 D_d_pose; Matrix23 D_d_pose;
D_d_pose << D_d_pose <<

2
gtsam/navigation/ManifoldPreintegration.h
View File

@ -75,7 +75,7 @@ public:
/// @{ /// @{
NavState deltaXij() const override { return deltaXij_; } NavState deltaXij() const override { return deltaXij_; }
Rot3 deltaRij() const override { return deltaXij_.attitude(); } Rot3 deltaRij() const override { return deltaXij_.attitude(); }
Vector3 deltaPij() const override { return deltaXij_.position().vector(); } Vector3 deltaPij() const override { return deltaXij_.position(); }
Vector3 deltaVij() const override { return deltaXij_.velocity(); } Vector3 deltaVij() const override { return deltaXij_.velocity(); }
Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }