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
// Publish flag about Eigen typedef
#cmakedefine GTSAM_USE_VECTOR3_POINTS
#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS
// Support Metis-based 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;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol)
if (distance(uncalibrate(pn), pi) <= tol)
break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
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;
int 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 rr = xx + yy;
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 {
/* ************************************************************************* */
Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
Matrix26 PinholeBase::Dpose(const Vector2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
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
const double u = pn.x(), v = pn.y();
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();
const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint)
*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) {
Matrix32 Dpoint3_pc;
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;
return uv;
} 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 Point2 pn = Project(pc);
const Vector2 pn = Project(pc);
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 {
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)
throw CheiralityException();
#endif
const Point2 pn = Project(q);
const Vector2 pn = Project(q);
if (Dpose || Dpoint) {
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 {
// world to camera coordinate
@ -146,7 +146,7 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
// camera to normalized image coordinate
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
if (Dpose) {
@ -161,7 +161,7 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p,
Point3 PinholeBase::backproject_from_camera(const Vector2& p,
const double 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 {
return project2(point, Dcamera, Dpoint);
}

25
gtsam/geometry/CalibratedCamera.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
@ -28,8 +29,6 @@
namespace gtsam {
class Point2;
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
CheiralityException> {
public:
@ -55,7 +54,7 @@ public:
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Point2 Measurement;
typedef Vector2 Measurement;
private:
@ -71,7 +70,7 @@ protected:
* @param pn projection in normalized coordinates
* @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
@ -79,7 +78,7 @@ protected:
* @param d disparity (inverse depth)
* @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
* @param pc point in camera coordinates
*/
static Point2 Project(const Point3& pc, //
static Vector2 Project(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none);
/**
@ -178,18 +177,18 @@ public:
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Point2 Project(const Unit3& pc, //
static Vector2 Project(const Unit3& pc, //
OptionalJacobian<2, 2> Dpoint = boost::none);
/// 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
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
* @param point 3D point in world coordinates
* @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;
/** Project point at infinity into the image
@ -197,12 +196,12 @@ public:
* @param point 3D point in world coordinates
* @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, 2> Dpoint = boost::none) const;
/// 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
@ -326,11 +325,11 @@ public:
* @deprecated
* 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;
/// 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));
}

86
gtsam/geometry/Point2.cpp
View File

@ -22,6 +22,35 @@ using namespace std;
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 {
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 r = std::sqrt(x() * x() + y() * y());
if (H) {
if (fabs(r) > 1e-10)
*H << x() / r, y() / r;
else
*H << 1, 1; // really infinity, why 1 ?
}
return r;
return norm(*this, H);
}
/* ************************************************************************* */
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
Point2 d = point - *this;
if (H1 || H2) {
Matrix12 H;
double r = d.norm(H);
if (H1) *H1 = -H;
if (H2) *H2 = H;
return r;
} else
return d.norm();
return distance(point, H1, H2);
}
/*
* 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
*/
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
}
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */
// 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 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) {
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) {
// distance between circle centers.
double d = c1.distance(c2);
double d = distance(c1, c2);
// centers coincide, either no solution or infinite number of solutions.
if (d<1e-9) return list<Point2>();
// Calculate f and h given normalized radii
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
return CircleCircleIntersection(c1, c2, fh);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
return circleCircleIntersection(c1, c2, fh);
}
/* ************************************************************************* */

95
gtsam/geometry/Point2.h
View File

@ -61,43 +61,13 @@ public:
/// construct from 2D vector
explicit Point2(const Vector2& v):Vector2(v) {}
/// @}
/// @name Declare circle intersection functionality
/// @{
/*
* @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.
*/
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);
friend boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol);
friend std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
friend std::list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol);
/// @}
/// @name Testable
@ -162,6 +132,15 @@ public:
static Vector2 Logmap(const Point2& p) { return p;}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
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
@ -186,6 +165,14 @@ struct traits<Point2> : public internal::VectorSpace<Point2> {
#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
typedef std::pair<Point2, Point2> Point2Pair;
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;
}
/*
* @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

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 {
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_;
if (!Hpose && !Hpoint) return d.norm();
Matrix12 D_r_d;
double r = d.norm(D_r_d);
double r = norm(d, D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose << -r_.c(), r_.s(), 0.0,
@ -267,7 +267,7 @@ double Pose2::range(const Pose2& pose,
Point2 d = pose.t() - t_;
if (!Hpose && !Hother) return d.norm();
Matrix12 D_r_d;
double r = d.norm(D_r_d);
double r = norm(d, D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose <<

2
gtsam/navigation/ManifoldPreintegration.h
View File

@ -75,7 +75,7 @@ public:
/// @{
NavState deltaXij() const override { return deltaXij_; }
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(); }
Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }