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

Merged in feature/EigenPoint2 (pull request #256) 

Point2's turn to be Eigenized
release/4.3a0
Frank Dellaert 2016-06-08 22:46:40 -07:00
parent e555990c0a a79be00dd6
commit bd8129d3f7
79 changed files with 559 additions and 531 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
CMakeLists.txt
View File

@ -66,7 +66,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
@ -91,8 +91,8 @@ if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
# Flags for choosing default packaging tools
@ -491,7 +491,7 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")

3
examples/CameraResectioning.cpp
View File

@ -48,8 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
return reprojectionError.vector();
return camera.project(P_, H, boost::none, boost::none) - p_;
}
};

12
examples/easyPoint2KalmanFilter.cpp
View File

@ -70,7 +70,7 @@ int main() {
// Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1);
x1_predict.print("X1 Predict");
traits<Point2>::Print(x1_predict, "X1 Predict");
@ -91,7 +91,7 @@ int main() {
// Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2);
x1_update.print("X1 Update");
traits<Point2>::Print(x1_update, "X1 Update");
@ -101,13 +101,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict");
traits<Point2>::Print(x2_predict, "X2 Predict");
// Update
Point2 z2(2.0, 0.0);
PriorFactor<Point2> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update");
traits<Point2>::Print(x2_update, "X2 Update");
@ -117,13 +117,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict");
traits<Point2>::Print(x3_predict, "X3 Predict");
// Update
Point2 z3(3.0, 0.0);
PriorFactor<Point2> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update");
traits<Point2>::Print(x3_update, "X3 Update");
return 0;
}

13
gtsam.h
View File

@ -266,23 +266,12 @@ class Point2 {
// Group
static gtsam::Point2 identity();
gtsam::Point2 inverse() const;
gtsam::Point2 compose(const gtsam::Point2& p2) const;
gtsam::Point2 between(const gtsam::Point2& p2) const;
// Manifold
gtsam::Point2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point2& p) const;
// Lie Group
static gtsam::Point2 Expmap(Vector v);
static Vector Logmap(const gtsam::Point2& p);
// Standard Interface
double x() const;
double y() const;
Vector vector() const;
double dist(const gtsam::Point2& p2) const;
double distance(const gtsam::Point2& p2) const;
double norm() const;
// enabling serialization functionality

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 (distance2(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 (distance2(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);

2
gtsam/geometry/CalibratedCamera.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)

5
gtsam/geometry/CalibratedCamera.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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:

8
gtsam/geometry/CameraSet.h
View File

@ -56,8 +56,7 @@ protected:
// Project and fill error vector
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
Z e = predicted[i] - measured[i];
b.segment<ZDim>(row) = e.vector();
b.segment<ZDim>(row) = traits<Z>::Local(measured[i], predicted[i]);
}
return b;
}
@ -107,7 +106,8 @@ public:
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
std::vector<Z> z;
z.reserve(m);
// Allocate derivatives
if (E) E->resize(ZDim * m, N);
@ -117,7 +117,7 @@ public:
for (size_t i = 0; i < m; i++) {
MatrixZD Fi;
Eigen::Matrix<double, ZDim, N> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
z.emplace_back(this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0));
if (Fs) (*Fs)[i] = Fi;
if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei;
}

104
gtsam/geometry/Point2.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -23,21 +23,11 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
}
/* ************************************************************************* */
double Point2::norm(OptionalJacobian<1,2> H) const {
double r = sqrt(x_ * x_ + y_ * y_);
double norm2(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 << x_ / r, y_ / r;
*H << p.x() / r, p.y() / r;
else
*H << 1, 1; // really infinity, why 1 ?
}
@ -45,34 +35,66 @@ double Point2::norm(OptionalJacobian<1,2> H) const {
}
/* ************************************************************************* */
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
Point2 d = point - *this;
double distance2(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 = d.norm(H);
double r = norm2(d, H);
if (H1) *H1 = -H;
if (H2) *H2 = H;
return r;
} else
} else {
return d.norm();
}
}
/*
* 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
*/
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol);
}
/* ************************************************************************* */
double Point2::norm(OptionalJacobian<1,2> H) const {
return gtsam::norm2(*this, H);
}
/* ************************************************************************* */
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
return gtsam::distance2(*this, point, H1, H2);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol) {
return circleCircleIntersection(R_d, r_d, tol);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh) {
return circleCircleIntersection(c1, c2, fh);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) {
return circleCircleIntersection(c1, r1, c2, r2, tol);
}
#endif
#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 !
@ -83,11 +105,11 @@ boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
// Hence, there are only solutions if >=0
if (h2<-tol) return boost::none; // allow *slightly* negative
else if (h2<tol) return Point2(f,0.0); // one solution
else return Point2(f,sqrt(h2)); // two solutions
else return Point2(f,std::sqrt(h2)); // two solutions
}
/* ************************************************************************* */
list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
list<Point2> circleCircleIntersection(Point2 c1, Point2 c2,
boost::optional<Point2> fh) {
list<Point2> solutions;
@ -116,27 +138,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.dist(c2);
double d = distance2(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);
}
/* ************************************************************************* */

195
gtsam/geometry/Point2.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -22,6 +22,14 @@
namespace gtsam {
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point2 to Vector2
typedef Vector2 Point2;
#else
/**
* A 2D point
* Complies with the Testable Concept
@ -29,69 +37,30 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point2 {
class GTSAM_EXPORT Point2 : public Vector2 {
private:
double x_, y_;
public:
enum { dimension = 2 };
/// @name Standard Constructors
/// @{
/// default constructor
Point2(): x_(0), y_(0) {}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Deprecated default constructor initializes to zero, in contrast to new behavior below
Point2() { setZero(); }
#else
Point2() {}
#endif
/// construct from doubles
Point2(double x, double y): x_(x), y_(y) {}
using Vector2::Vector2;
/// @}
/// @name Advanced Constructors
/// @{
/// construct from 2D vector
explicit Point2(const Vector2& v) {
x_ = v(0);
y_ = v(1);
}
/*
* @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);
explicit Point2(const Vector2& v):Vector2(v) {}
/// @}
/// @name Testable
/// @{
@ -107,21 +76,7 @@ public:
/// @{
/// identity
inline static Point2 identity() {return Point2();}
/// inverse
inline Point2 operator- () const {return Point2(-x_,-y_);}
/// add vector on right
inline Point2 operator +(const Vector2& v) const {
return Point2(x_ + v[0], y_ + v[1]);
}
/// add
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
/// subtract
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
inline static Point2 identity() {return Point2(0,0);}
/// @}
/// @name Vector Space
@ -137,51 +92,44 @@ public:
double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
OptionalJacobian<1,2> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point2& p2) const {
return (p2 - *this).norm();
}
/// multiply with a scalar
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
/// divide by a scalar
inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
/// @}
/// @name Standard Interface
/// @{
/// equality
inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;}
inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();}
/// get x
double x() const {return x_;}
inline double x() const {return (*this)[0];}
/// get y
double y() const {return y_;}
inline double y() const {return (*this)[1];}
/// return vectorized form (column-wise). TODO: why does this function exist?
Vector2 vector() const { return Vector2(x_, y_); }
/// return vectorized form (column-wise).
const Vector2& vector() const { return *this; }
/// @}
/// @name Deprecated
/// @{
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
inline void operator *= (double s) {x_*=s;y_*=s;}
Point2 inverse() const { return -(*this);}
Point2 compose(const Point2& q) const { return (*this)+q;}
Point2 between(const Point2& q) const { return q-(*this);}
Vector2 localCoordinates(const Point2& q) const { return between(q).vector();}
Point2 retract(const Vector2& v) const { return compose(Point2(v));}
static Vector2 Logmap(const Point2& p) { return p.vector();}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
/// @}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point2 inverse() const { return -(*this); }
Point2 compose(const Point2& q) const { return (*this)+q;}
Point2 between(const Point2& q) const { return q-(*this);}
Vector2 localCoordinates(const Point2& q) const { return between(q);}
Point2 retract(const Vector2& v) const { return compose(Point2(v));}
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);
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
/// @}
#endif
private:
/// @name Advanced Interface
@ -192,13 +140,25 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
}
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);}
/// @}
/// @}
};
template<>
struct traits<Point2> : public internal::VectorSpace<Point2> {
};
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// Distance of the point from the origin, with Jacobian
double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
/// distance between two points
double distance2(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);
@ -207,10 +167,45 @@ std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
typedef std::vector<Point2> Point2Vector;
/// multiply with scalar
inline Point2 operator*(double s, const Point2& p) {return p*s;}
inline Point2 operator*(double s, const Point2& p) {
return p * s;
}
template<>
struct traits<Point2> : public internal::VectorSpace<Point2> {};
/*
* @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

14
gtsam/geometry/Point3.cpp
View File

@ -21,7 +21,7 @@ using namespace std;
namespace gtsam {
#ifndef GTSAM_USE_VECTOR3_POINTS
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
bool Point3::equals(const Point3 &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
fabs(z() - q.z()) < tol);
@ -34,11 +34,11 @@ void Point3::print(const string& s) const {
/* ************************************************************************* */
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::distance(*this,q,H1,H2);
return gtsam::distance3(*this,q,H1,H2);
}
double Point3::norm(OptionalJacobian<1,3> H) const {
return gtsam::norm(*this, H);
return gtsam::norm3(*this, H);
}
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
@ -80,8 +80,8 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
#endif
/* ************************************************************************* */
double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
double d = (q - p1).norm();
if (H1) {
*H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
@ -94,7 +94,7 @@ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
return d;
}
double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
double norm3(const Point3 &p, OptionalJacobian<1, 3> H) {
double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
if (H) {
if (fabs(r) > 1e-10)
@ -106,7 +106,7 @@ double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
}
Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
Point3 normalized = p / norm(p);
Point3 normalized = p / p.norm();
if (H) {
// 3*3 Derivative
double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();

18
gtsam/geometry/Point3.h
View File

@ -29,7 +29,7 @@
namespace gtsam {
#ifdef GTSAM_USE_VECTOR3_POINTS
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
@ -124,9 +124,9 @@ class GTSAM_EXPORT Point3 : public Vector3 {
Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);}
Vector3 localCoordinates(const Point3& q) const { return between(q).vector();}
Vector3 localCoordinates(const Point3& q) const { return between(q);}
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p.vector();}
static Vector3 Logmap(const Point3& p) { return p;}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
inline double dist(const Point3& q) const { return (q - *this).norm(); }
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
@ -153,19 +153,19 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
#endif
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// distance between two points
double distance(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
double distance3(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
/// Distance of the point from the origin, with Jacobian
double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
/// normalize, with optional Jacobian
Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
@ -193,7 +193,7 @@ struct Range<Point3, Point3> {
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return distance(p, q, H1, H2);
return distance3(p, q, H1, H2);
}
};

20
gtsam/geometry/Pose2.cpp
View File

@ -53,21 +53,21 @@ 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);
}
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) {
if (H) *H = Pose2::ExpmapDerivative(xi);
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
double w = xi(2);
if (H) *H = Pose2::ExpmapDerivative(xi);
const Point2 v(xi(0),xi(1));
const double w = xi(2);
if (std::abs(w) < 1e-10)
return Pose2(xi[0], xi[1], xi[2]);
else {
Rot2 R(Rot2::fromAngle(w));
Point2 v_ortho = R_PI_2 * v; // points towards rot center
Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
const Rot2 R(Rot2::fromAngle(w));
const Point2 v_ortho = R_PI_2 * v; // points towards rot center
const Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
return Pose2(R, t);
}
}
@ -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 = norm2(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 = norm2(d, D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose <<
@ -311,7 +311,7 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
if (n<2) return boost::none; // we need at least two pairs
// calculate centroids
Point2 cp,cq;
Point2 cp(0,0), cq(0,0);
for(const Point2Pair& pair: pairs) {
cp += pair.first;
cq += pair.second;

8
gtsam/geometry/Pose2.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -52,7 +52,9 @@ public:
/// @{
/** default constructor = origin */
Pose2() {} // default is origin
Pose2() :
r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
}
/** copy constructor */
Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
@ -86,7 +88,7 @@ public:
/// @{
/** Construct from canonical coordinates \f$ [T_x,T_y,\theta] \f$ (Lie algebra) */
Pose2(const Vector& v) {
Pose2(const Vector& v) : Pose2() {
*this = Expmap(v);
}

2
gtsam/geometry/Pose3.cpp
View File

@ -345,7 +345,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
return local.norm();
} else {
Matrix13 D_r_local;
const double r = norm(local, D_r_local);
const double r = norm3(local, D_r_local);
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;

2
gtsam/geometry/Rot3.h
View File

@ -179,7 +179,7 @@ namespace gtsam {
*/
static Rot3 AxisAngle(const Point3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector()));
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return Rot3(SO3::AxisAngle(axis,angle));
#endif

2
gtsam/geometry/Unit3.h
View File

@ -78,7 +78,7 @@ public:
/// Construct from 2D point in plane at focal length f
/// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) {
explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
p_.normalize();
}

2
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -52,7 +52,7 @@ TEST( Cal3Bundler, calibrate )
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( pn.equals(pn_hat, 1e-5));
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -48,7 +48,7 @@ TEST( Cal3DS2, calibrate )
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( pn.equals(pn_hat, 1e-5));
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }

2
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -59,7 +59,7 @@ TEST( Cal3Unified, calibrate)
{
Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi);
CHECK( p.equals(pn_hat, 1e-8));
CHECK( traits<Point2>::Equals(p, pn_hat, 1e-8));
}
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -152,7 +152,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity)
Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
CHECK(assert_equal(Point2(), result));
CHECK(assert_equal(Point2(0,0), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}

4
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -44,7 +44,7 @@ TEST(CameraSet, Pinhole) {
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
Point2 expected(0,0);
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
@ -117,7 +117,7 @@ TEST(CameraSet, Pinhole) {
Unit3 pointAtInfinity(0, 0, 1000);
EXPECT(
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
camera.backprojectPointAtInfinity(Point2(0,0))));
actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
EXPECT(assert_equal(expectedV, actualV));
LONGS_EQUAL(2, Fs.size());

14
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -153,12 +153,12 @@ TEST( PinholeCamera, backproject2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 actual = camera.backproject(Point2(0,0), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(assert_equal(Point2(0,0), x.first));
EXPECT(x.second);
}
@ -169,12 +169,12 @@ TEST( PinholeCamera, backprojectInfinity2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
Unit3 expected(0., 1., 0.);
Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
EXPECT(assert_equal(Point2(0,0), x));
}
/* ************************************************************************* */
@ -184,12 +184,12 @@ TEST( PinholeCamera, backprojectInfinity3)
Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
Camera camera(Pose3(rot, origin), K);
Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
Unit3 expected(0., 0., 1.);
Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
EXPECT(assert_equal(Point2(0,0), x));
}
/* ************************************************************************* */
@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) {
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result,
EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));

6
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -124,12 +124,12 @@ TEST( PinholePose, backproject2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 actual = camera.backproject(Point2(0,0), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(assert_equal(Point2(0,0), x.first));
EXPECT(x.second);
}
@ -212,7 +212,7 @@ TEST( PinholePose, range0) {
double result = camera.range(point1, D1, D2);
Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
Matrix expectedDpoint = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9);
EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9);
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}

4
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -84,7 +84,7 @@ TEST(PinholeSet, Pinhole) {
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
Point2 expected(0,0);
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
@ -131,7 +131,7 @@ TEST(PinholeSet, Pinhole) {
}
EXPECT(
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
camera.backprojectPointAtInfinity(Point2(0,0))));
{
PinholeSet<Camera>::FBlocks Fs;
Matrix E;

61
gtsam/geometry/tests/testPoint2.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -27,6 +27,11 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_LIE_INST(Point2)
//******************************************************************************
TEST(Point2 , Constructor) {
Point2 p;
}
//******************************************************************************
TEST(Double , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<double>));
@ -95,26 +100,26 @@ TEST( Point2, expmap) {
/* ************************************************************************* */
TEST( Point2, arithmetic) {
EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) ));
EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1)));
EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) ));
EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2));
EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3)));
EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2));
EXPECT(assert_equal<Point2>( Point2(-5,-6), -Point2(5,6) ));
EXPECT(assert_equal<Point2>( Point2(5,6), Point2(4,5)+Point2(1,1)));
EXPECT(assert_equal<Point2>( Point2(3,4), Point2(4,5)-Point2(1,1) ));
EXPECT(assert_equal<Point2>( Point2(8,6), Point2(4,3)*2));
EXPECT(assert_equal<Point2>( Point2(4,6), 2*Point2(2,3)));
EXPECT(assert_equal<Point2>( Point2(2,3), Point2(4,6)/2));
}
/* ************************************************************************* */
TEST( Point2, unit) {
Point2 p0(10, 0), p1(0, -10), p2(10, 10);
EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6));
EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
EXPECT(assert_equal(Point2(1, 0), Point2(p0.normalized()), 1e-6));
EXPECT(assert_equal(Point2(0,-1), Point2(p1.normalized()), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), Point2(p2.normalized()), 1e-6));
}
namespace {
/* ************************************************************************* */
// some shared test values
Point2 x1, x2(1, 1), x3(1, 1);
Point2 x1(0,0), x2(1, 1), x3(1, 1);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */
@ -126,19 +131,19 @@ TEST( Point2, norm ) {
Point2 p0(cos(5.0), sin(5.0));
DOUBLES_EQUAL(1, p0.norm(), 1e-6);
Point2 p1(4, 5), p2(1, 1);
DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6);
DOUBLES_EQUAL( 5, distance2(p1, p2), 1e-6);
DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6);
Matrix expectedH, actualH;
double actual;
// exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
actual = x1.norm(actualH);
actual = norm2(x1, actualH);
EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
expectedH = (Matrix(1, 2) << 1.0, 1.0).finished();
EXPECT(assert_equal(expectedH,actualH));
actual = x2.norm(actualH);
actual = norm2(x2, actualH);
EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
expectedH = numericalDerivative11(norm_proxy, x2);
EXPECT(assert_equal(expectedH,actualH));
@ -151,20 +156,20 @@ TEST( Point2, norm ) {
/* ************************************************************************* */
namespace {
double distance_proxy(const Point2& location, const Point2& point) {
return location.distance(point);
return distance2(location, point);
}
}
TEST( Point2, distance ) {
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish distance is indeed zero
EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9);
EXPECT_DOUBLES_EQUAL(1, distance2(x1, l1), 1e-9);
// establish distance is indeed 45 degrees
EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9);
EXPECT_DOUBLES_EQUAL(sqrt(2.0), distance2(x1, l2), 1e-9);
// Another pair
double actual23 = x2.distance(l3, actualH1, actualH2);
double actual23 = distance2(x2, l3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9);
// Check numerical derivatives
@ -174,7 +179,7 @@ TEST( Point2, distance ) {
EXPECT(assert_equal(expectedH2,actualH2));
// Another test
double actual34 = x3.distance(l4, actualH1, actualH2);
double actual34 = distance2(x3, l4, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9);
// Check numerical derivatives
@ -190,42 +195,42 @@ TEST( Point2, circleCircleIntersection) {
double offset = 0.994987;
// Test intersections of circle moving from inside to outside
list<Point2> inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
list<Point2> inside = circleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
EXPECT_LONGS_EQUAL(0,inside.size());
list<Point2> touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
list<Point2> touching1 = circleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
EXPECT_LONGS_EQUAL(1,touching1.size());
EXPECT(assert_equal(Point2(5,0), touching1.front()));
list<Point2> common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
list<Point2> common = circleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
EXPECT_LONGS_EQUAL(2,common.size());
EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6));
EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
list<Point2> touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
list<Point2> touching2 = circleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
EXPECT_LONGS_EQUAL(1,touching2.size());
EXPECT(assert_equal(Point2(5,0), touching2.front()));
// test rotated case
list<Point2> rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
list<Point2> rotated = circleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
EXPECT_LONGS_EQUAL(2,rotated.size());
EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
// test r1<r2
list<Point2> smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
list<Point2> smaller = circleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
EXPECT_LONGS_EQUAL(2,smaller.size());
EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6));
EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
// test offset case, r1>r2
list<Point2> offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
list<Point2> offset1 = circleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
EXPECT_LONGS_EQUAL(2,offset1.size());
EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6));
EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6));
// test offset case, r1<r2
list<Point2> offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
list<Point2> offset2 = circleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
EXPECT_LONGS_EQUAL(2,offset2.size());
EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6));
EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6));
@ -233,12 +238,14 @@ TEST( Point2, circleCircleIntersection) {
}
/* ************************************************************************* */
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
TEST( Point2, stream) {
Point2 p(1, 2);
std::ostringstream os;
os << p;
EXPECT(os.str() == "(1, 2)");
}
#endif
/* ************************************************************************* */
int main () {

13
gtsam/geometry/tests/testPoint3.cpp
View File

@ -26,6 +26,11 @@ GTSAM_CONCEPT_LIE_INST(Point3)
static Point3 P(0.2, 0.7, -2);
//******************************************************************************
TEST(Point3 , Constructor) {
Point3 p;
}
//******************************************************************************
TEST(Point3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Point3>));
@ -149,7 +154,7 @@ TEST( Point3, cross2) {
}
/* ************************************************************************* */
#ifndef GTSAM_USE_VECTOR3_POINTS
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
TEST( Point3, stream) {
Point3 p(1, 2, -3);
std::ostringstream os;
@ -178,20 +183,20 @@ TEST (Point3, norm) {
Matrix actualH;
Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8);
EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) {
return distance(P,Q);
return distance3(P, Q);
}
TEST (Point3, distance) {
Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
Matrix H1, H2;
double d = distance(P, Q, H1, H2);
double d = distance3(P, Q, H1, H2);
double expectedDistance = 55.542686;
Matrix numH1 = numericalDerivative21(testFunc, P, Q);
Matrix numH2 = numericalDerivative22(testFunc, P, Q);

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

@ -43,7 +43,7 @@ TEST(Pose2 , Concept) {
/* ************************************************************************* */
TEST(Pose2, constructors) {
Point2 p;
Point2 p(0,0);
Pose2 pose(0,p);
Pose2 origin;
assert_equal(pose,origin);
@ -371,7 +371,7 @@ TEST(Pose2, compose_c)
/* ************************************************************************* */
TEST(Pose2, inverse )
{
Point2 origin, t(1,2);
Point2 origin(0,0), t(1,2);
Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Pose2 identity, lTg = gTl.inverse();
@ -409,7 +409,7 @@ namespace {
/* ************************************************************************* */
TEST( Pose2, matrix )
{
Point2 origin, t(1,2);
Point2 origin(0,0), t(1,2);
Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl);
EXPECT(assert_equal((Matrix(3,3) <<
@ -743,7 +743,7 @@ namespace {
/* ************************************************************************* */
struct Triangle { size_t i_,j_,k_;};
boost::optional<Pose2> align(const vector<Point2>& ps, const vector<Point2>& qs,
boost::optional<Pose2> align2(const vector<Point2>& ps, const vector<Point2>& qs,
const pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
vector<Point2Pair> correspondences;
@ -762,7 +762,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 = align(ps, qs, make_pair(t1,t2));
boost::optional<Pose2> actual = align2(ps, qs, make_pair(t1,t2));
EXPECT(assert_equal(expected, *actual));
}

4
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -104,12 +104,12 @@ TEST( SimpleCamera, backproject2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
SimpleCamera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 actual = camera.backproject(Point2(0,0), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x.first));
CHECK(assert_equal(Point2(0,0), x.first));
CHECK(x.second);
}

4
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -273,7 +273,7 @@ TEST( triangulation, onePose) {
vector<Point2> measurements;
poses += Pose3();
measurements += Point2();
measurements += Point2(0,0);
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException);
@ -282,7 +282,7 @@ TEST( triangulation, onePose) {
//******************************************************************************
TEST( triangulation, StereotriangulateNonlinear ) {
Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);
// two camera poses m1, m2
Matrix4 m1, m2;

7
gtsam/geometry/triangulation.h
View File

@ -112,7 +112,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension));
static SharedNoiseModel unit(noiseModel::Unit::Create(
traits<typename CAMERA::Measurement>::dimension));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.push_back(TriangulationFactor<CAMERA> //
@ -457,7 +458,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
for(const CAMERA& camera: cameras) {
const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0
&& distance(pose.translation(), point)
&& distance3(pose.translation(), point)
> params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
@ -471,7 +472,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
if (params.dynamicOutlierRejectionThreshold > 0) {
const Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi);
totalReprojError += reprojectionError.vector().norm();
totalReprojError += reprojectionError.norm();
}
i += 1;
}

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_; }

14
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -61,16 +61,14 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUE>
ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(Key key_initial, T x_initial,
noiseModel::Gaussian::shared_ptr P_initial) {
// Set the initial linearization point to the provided mean
x_ = x_initial;
template <class VALUE>
ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
: x_(x_initial) // Set the initial linearization point
{
// Create a Jacobian Prior Factor directly P_initial.
// Since x0 is set to the provided mean, the b vector in the prior will be zero
// TODO Frank asks: is there a reason why noiseModel is not simply P_initial ?
// TODO(Frank): is there a reason why noiseModel is not simply P_initial?
int n = traits<T>::GetDimension(x_initial);
priorFactor_ = JacobianFactor::shared_ptr(
new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -260,7 +260,7 @@ public:
std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
<< ")," << "\n";
this->noiseModel_->print();
value_.print("Value");
traits<X>::Print(value_, "Value");
}
private:

35
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -80,7 +80,7 @@ TEST(Expression, Leaves) {
// Unary(Leaf)
namespace unary {
Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) {
return Point2();
return Point2(0,0);
}
double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
@ -111,24 +111,43 @@ TEST(Expression, Unary3) {
}
/* ************************************************************************* */
class Class : public Point3 {
public:
enum {dimension = 3};
using Point3::Point3;
const Vector3& vector() const { return *this; }
inline static Class identity() { return Class(0,0,0); }
double norm(OptionalJacobian<1,3> H = boost::none) const {
return norm3(*this, H);
}
bool equals(const Class &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol);
}
void print(const string& s) const { cout << s << *this << endl;}
};
namespace gtsam {
template<> struct traits<Class> : public internal::VectorSpace<Class> {};
}
// Nullary Method
TEST(Expression, NullaryMethod) {
// Create expression
Expression<Point3> p(67);
Expression<double> norm(&gtsam::norm, p);
Expression<Class> p(67);
Expression<double> norm_(p, &Class::norm);
// Create Values
Values values;
values.insert(67, Point3(3, 4, 5));
values.insert(67, Class(3, 4, 5));
// Check dims as map
std::map<Key, int> map;
norm.dims(map);
norm_.dims(map);
LONGS_EQUAL(1, map.size());
// Get value and Jacobians
std::vector<Matrix> H(1);
double actual = norm.value(values, H);
double actual = norm_.value(values, H);
// Check all
EXPECT(actual == sqrt(50));
@ -370,7 +389,7 @@ TEST(Expression, TripleSum) {
/* ************************************************************************* */
TEST(Expression, SumOfUnaries) {
const Key key(67);
const Double_ norm_(&gtsam::norm, Point3_(key));
const Double_ norm_(&gtsam::norm3, Point3_(key));
const Double_ sum_ = norm_ + norm_;
Values values;
@ -389,7 +408,7 @@ TEST(Expression, SumOfUnaries) {
TEST(Expression, UnaryOfSum) {
const Key key1(42), key2(67);
const Point3_ sum_ = Point3_(key1) + Point3_(key2);
const Double_ norm_(&gtsam::norm, sum_);
const Double_ norm_(&gtsam::norm3, sum_);
map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
norm_.dims(actual_dims);

6
gtsam/slam/EssentialMatrixFactor.h
View File

@ -139,7 +139,7 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.transpose() << ")' and (" << pn_.vector().transpose()
<< dP1_.transpose() << ")' and (" << pn_.transpose()
<< ")'" << std::endl;
}
@ -162,7 +162,7 @@ public:
// The point d*P1 = (x,y,1) is computed in constructor as dP1_
// Project to normalized image coordinates, then uncalibrate
Point2 pn;
Point2 pn(0,0);
if (!DE && !Dd) {
Point3 _1T2 = E.direction().point3();
@ -195,7 +195,7 @@ public:
}
Point2 reprojectionError = pn - pn_;
return f_ * reprojectionError.vector();
return f_ * reprojectionError;
}
};

17
gtsam/slam/GeneralSFMFactor.h
View File

@ -107,7 +107,7 @@ public:
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/**
@ -115,15 +115,14 @@ public:
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
return reprojError.vector();
return camera.project2(point,H1,H2) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero();
@ -145,8 +144,7 @@ public:
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - measured());
b = -reprojError.vector();
b = measured() - camera.project2(point, H1, H2);
} catch (CheiralityException& e) {
H1.setZero();
H2.setZero();
@ -243,7 +241,7 @@ public:
*/
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/**
@ -251,7 +249,7 @@ public:
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/** h(x)-z */
@ -262,8 +260,7 @@ public:
{
try {
Camera camera(pose3,calib);
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
return reprojError.vector();
return camera.project(point, H1, H2, H3) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2, 6);

16
gtsam/slam/ProjectionFactor.h
View File

@ -56,7 +56,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
GenericProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -108,7 +110,7 @@ namespace gtsam {
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "GenericProjectionFactor, z = ";
measured_.print();
traits<Point2>::Print(measured_);
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
@ -119,7 +121,7 @@ namespace gtsam {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
@ -134,16 +136,14 @@ namespace gtsam {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H2, boost::none) - measured_;
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H2, boost::none) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

4
gtsam/slam/ReferenceFrameFactor.h
View File

@ -84,7 +84,7 @@ public:
* each degree of freedom.
*/
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
: Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma),
: Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
globalKey, transKey, localKey) {}
virtual ~ReferenceFrameFactor(){}
@ -100,7 +100,7 @@ public:
boost::optional<Matrix&> Dlocal = boost::none) const {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal)
*Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension);
*Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
return traits<Point>::Local(local,newlocal);
}

2
gtsam/slam/SmartFactorBase.h
View File

@ -189,7 +189,7 @@ public:
bool areMeasurementsEqual = true;
for (size_t i = 0; i < measured_.size(); i++) {
if (this->measured_.at(i).equals(e->measured_.at(i), tol) == false)
if (traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false)
areMeasurementsEqual = false;
break;
}

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -499,8 +499,8 @@ public:
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
// Otherwise, manage the exceptions with rotation-only factors
const Point2& z0 = this->measured_.at(0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
this->measured_.at(0));
return Base::totalReprojectionError(cameras, backprojected);
} else
// if we don't want to manage the exceptions we discard the factor

24
gtsam/slam/TriangulationFactor.h
View File

@ -78,12 +78,11 @@ public:
bool verboseCheirality = false) :
Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
throwCheirality), verboseCheirality_(verboseCheirality) {
if (model && model->dim() != Measurement::dimension)
if (model && model->dim() != traits<Measurement>::dimension)
throw std::invalid_argument(
"TriangulationFactor must be created with "
+ boost::lexical_cast<std::string>((int) Measurement::dimension)
+ boost::lexical_cast<std::string>((int) traits<Measurement>::dimension)
+ "-dimensional noise model.");
}
/** Virtual destructor */
@ -105,7 +104,7 @@ public:
DefaultKeyFormatter) const {
std::cout << s << "TriangulationFactor,";
camera_.print("camera");
measured_.print("z");
traits<Measurement>::Print(measured_, "z");
Base::print("", keyFormatter);
}
@ -113,25 +112,24 @@ public:
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
&& this->measured_.equals(e->measured_, tol);
&& traits<Measurement>::Equals(this->measured_, e->measured_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const {
try {
Measurement error(camera_.project2(point, boost::none, H2) - measured_);
return error.vector();
return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
} catch (CheiralityException& e) {
if (H2)
*H2 = Matrix::Zero(Measurement::dimension, 3);
*H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera"
<< std::endl;
if (throwCheirality_)
throw e;
return Eigen::Matrix<double,Measurement::dimension,1>::Constant(2.0 * camera_.calibration().fx());
return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * camera_.calibration().fx());
}
}
@ -153,14 +151,14 @@ public:
// Allocate memory for Jacobian factor, do only once
if (Ab.rows() == 0) {
std::vector<size_t> dimensions(1, 3);
Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true);
A.resize(Measurement::dimension,3);
b.resize(Measurement::dimension);
Ab = VerticalBlockMatrix(dimensions, traits<Measurement>::dimension, true);
A.resize(traits<Measurement>::dimension,3);
b.resize(traits<Measurement>::dimension);
}
// Would be even better if we could pass blocks to project
const Point3& point = x.at<Point3>(key());
b = -(camera_.project2(point, boost::none, A) - measured_).vector();
b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_);
if (noiseModel_)
this->noiseModel_->WhitenSystem(A, b);

12
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -222,8 +222,7 @@ TEST (EssentialMatrixFactor2, factor) {
// Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = PinholeBase::Project(P2);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
@ -296,8 +295,7 @@ TEST (EssentialMatrixFactor3, factor) {
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
@ -438,8 +436,7 @@ TEST (EssentialMatrixFactor2, extraTest) {
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
@ -507,8 +504,7 @@ TEST (EssentialMatrixFactor3, extraTest) {
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());

4
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -55,8 +55,8 @@ struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(), 1);
f.add(Point2(), 2);
f.add(Point2(0,0), 1);
f.add(Point2(0,0), 2);
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}

10
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -300,20 +300,14 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
SfM_Track track1;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam1.at(i);
track1.measurements.push_back(measures);
track1.measurements.emplace_back(i + 1, measurements_cam1.at(i));
}
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(track1);
SfM_Track track2;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam2.at(i);
track2.measurements.push_back(measures);
track2.measurements.emplace_back(i + 1, measurements_cam2.at(i));
}
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(track2);

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other,
const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
Matrix13 D_d_t1, D_d_t2;
double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
return d;

2
gtsam_unstable/geometry/Event.h
View File

@ -85,7 +85,7 @@ public:
OptionalJacobian<1, 3> H2 = boost::none) const {
static const double Speed = 330;
Matrix13 D1, D2;
double distance = gtsam::distance(location_, microphone, D1, D2);
double distance = gtsam::distance3(location_, microphone, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / Speed;

18
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -46,7 +46,7 @@ SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, doubl
bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
if (p.size() != size()) return false;
for (size_t i=0; i<size(); ++i)
if (!landmarks_[i].equals(p.landmarks_[i], tol))
if (!traits<Point2>::Equals(landmarks_[i], p.landmarks_[i], tol))
return false;
return true;
}
@ -55,7 +55,7 @@ bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
void SimPolygon2D::print(const string& s) const {
cout << "SimPolygon " << s << ": " << endl;
for(const Point2& p: landmarks_)
p.print(" ");
traits<Point2>::Print(p, " ");
}
/* ************************************************************************* */
@ -151,7 +151,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC);
// use triangle equality to verify non-degenerate triangle
double dAC = xA.t().distance(xC.t());
double dAC = distance2(xA.t(), xC.t());
// form a triangle and test if it meets requirements
SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());
@ -164,7 +164,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
insideBox(side_len, test_tri.landmark(0)) &&
insideBox(side_len, test_tri.landmark(1)) &&
insideBox(side_len, test_tri.landmark(2)) &&
test_tri.landmark(1).distance(test_tri.landmark(2)) > min_side_len &&
distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len &&
!nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
!nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
!nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
@ -260,7 +260,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -272,7 +272,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -285,7 +285,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -303,7 +303,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -320,7 +320,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) {
bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
const Point2& p, double threshold) {
for(const Point2& Sp: S)
if (Sp.distance(p) < threshold)
if (distance2(Sp, p) < threshold)
return true;
return false;
}

21
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -14,13 +14,14 @@ using namespace std;
/* ************************************************************************* */
void SimWall2D::print(const std::string& s) const {
std::cout << "SimWall2D " << s << ":" << std::endl;
a_.print(" a");
b_.print(" b");
traits<Point2>::Print(a_, " a");
traits<Point2>::Print(b_, " b");
}
/* ************************************************************************* */
bool SimWall2D::equals(const SimWall2D& other, double tol) const {
return a_.equals(other.a_, tol) && b_.equals(other.b_, tol);
return traits<Point2>::Equals(a_, other.a_, tol) &&
traits<Point2>::Equals(b_, other.b_, tol);
}
/* ************************************************************************* */
@ -37,8 +38,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
if (debug) cout << "len: " << len << endl;
Point2 Ba = transform.transform_to(B.a()),
Bb = transform.transform_to(B.b());
if (debug) Ba.print("Ba");
if (debug) Bb.print("Bb");
if (debug) traits<Point2>::Print(Ba, "Ba");
if (debug) traits<Point2>::Print(Bb, "Bb");
// check sides of line
if (Ba.y() * Bb.y() > 0.0 ||
@ -73,7 +74,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
}
// find lower point by y
Point2 low, high;
Point2 low(0,0), high(0,0);
if (Ba.y() > Bb.y()) {
high = Ba;
low = Bb;
@ -81,8 +82,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
high = Bb;
low = Ba;
}
if (debug) high.print("high");
if (debug) low.print("low");
if (debug) traits<Point2>::Print(high, "high");
if (debug) traits<Point2>::Print(low, "low");
// find x-intercept
double slope = (high.y() - low.y())/(high.x() - low.x());
@ -138,7 +139,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
Point2 cur_intersection;
if (wall.intersects(traj,cur_intersection)) {
collision = true;
if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) {
if (distance2(cur_pose.t(), cur_intersection) < distance2(cur_pose.t(), intersection)) {
intersection = cur_intersection;
closest_wall = wall;
}
@ -154,7 +155,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
norm = norm / norm.norm();
// Simple check to make sure norm is on side closest robot
if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm))
if (distance2(cur_pose.t(), intersection + norm) > distance2(cur_pose.t(), intersection - norm))
norm = - norm;
// using the reflection

2
gtsam_unstable/geometry/SimWall2D.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
/** geometry */
double length() const { return a_.distance(b_); }
double length() const { return distance2(a_, b_); }
Point2 midpoint() const;
/**

2
gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
View File

@ -16,7 +16,7 @@ const double tol=1e-5;
TEST(testPolygon, triangle_basic) {
// create a triangle from points, extract landmarks/walls, check occupancy
Point2 pA, pB(2.0, 0.0), pC(0.0, 1.0);
Point2 pA(0,0), pB(2.0, 0.0), pC(0.0, 1.0);
// construct and extract data
SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC);

4
gtsam_unstable/geometry/tests/testSimWall2D.cpp
View File

@ -24,7 +24,7 @@ TEST(testSimWall2D2D, construction ) {
/* ************************************************************************* */
TEST(testSimWall2D2D, equals ) {
Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3;
Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3(0,0);
SimWall2D w1(p1, p2), w2(p1, p3);
EXPECT(assert_equal(w1, w1));
EXPECT(assert_inequal(w1, w2));
@ -34,7 +34,7 @@ TEST(testSimWall2D2D, equals ) {
/* ************************************************************************* */
TEST(testSimWall2D2D, intersection1 ) {
SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0);
Point2 pt;
Point2 pt(0,0);
EXPECT(w1.intersects(w2));
EXPECT(w2.intersects(w1));
w1.intersects(w2, pt);

45
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -24,17 +24,17 @@ namespace gtsam {
* Ternary factor representing a visual measurement that includes inverse depth
*/
template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
protected:
// Keep a copy of measurement and calibration for I/O
gtsam::Point2 measured_; ///< 2D measurement
boost::shared_ptr<gtsam::Cal3_S2> K_; ///< shared pointer to calibration object
Point2 measured_; ///< 2D measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
public:
/// shorthand for base class type
typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@ -43,7 +43,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactor3() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -55,8 +57,8 @@ public:
* @param invDepthKey is the index of inverse depth
* @param K shared pointer to the constant calibration
*/
InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const Cal3_S2::shared_ptr& K) :
InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
/** Virtual destructor */
@ -68,44 +70,43 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactor3",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const {
try {
InvDepthCamera3<gtsam::Cal3_S2> camera(pose, K_);
gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
return reprojectionError.vector();
InvDepthCamera3<Cal3_S2> camera(pose, K_);
return camera.project(point, invDepth, H1, H2, H3) - measured_;
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,5);
if (H3) *H2 = Matrix::Zero(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}
/** return the calibration object */
inline const gtsam::Cal3_S2::shared_ptr calibration() const {
inline const Cal3_S2::shared_ptr calibration() const {
return K_;
}

25
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -41,7 +41,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant1() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -64,17 +66,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant1",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -86,22 +88,21 @@ public:
Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector6& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
@ -118,7 +119,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

25
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -43,7 +43,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant2() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -67,17 +69,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant2",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& traits<Point3>::Equals(this->referencePoint_, e->referencePoint_, tol);
}
@ -89,22 +91,21 @@ public:
Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
@ -121,7 +122,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

48
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -41,7 +41,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant3a() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant3a() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -66,17 +68,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant3a",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -89,22 +91,21 @@ public:
Point3 world_P_landmark = pose.transform_from(pose_P_landmark);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) {
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
@ -117,7 +118,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}
@ -160,7 +161,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant3b() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant3b() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -185,17 +188,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant3",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -208,23 +211,22 @@ public:
Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose2, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
<< " moved behind camera " << DefaultKeyFormatter(this->key2())
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const {
if(H1)
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
@ -239,7 +241,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

22
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -101,9 +101,9 @@ namespace gtsam {
virtual ~MultiProjectionFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -143,20 +143,20 @@ namespace gtsam {
//
// if(body_P_sensor_) {
// if(H1) {
// gtsam::Matrix H0;
// Matrix H0;
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// *H1 = *H1 * H0;
// return reprojectionError.vector();
// return reprojectionError;
// } else {
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// return reprojectionError;
// }
// } else {
// PinholeCamera<CALIBRATION> camera(pose, *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// return reprojectionError;
// }
// }
@ -168,20 +168,20 @@ namespace gtsam {
try {
if(body_P_sensor_) {
if(H1) {
gtsam::Matrix H0;
Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
return reprojectionError;
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
return reprojectionError;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

21
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -54,7 +54,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {}
ProjectionFactorPPP() :
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -94,9 +96,9 @@ namespace gtsam {
virtual ~ProjectionFactorPPP() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -105,7 +107,7 @@ namespace gtsam {
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPP, z = ";
measured_.print();
traits<Point2>::Print(measured_);
Base::print("", keyFormatter);
}
@ -114,7 +116,7 @@ namespace gtsam {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -125,16 +127,15 @@ namespace gtsam {
boost::optional<Matrix&> H3 = boost::none) const {
try {
if(H1 || H2 || H3) {
gtsam::Matrix H0, H02;
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H3, boost::none) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

21
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -52,7 +52,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {}
ProjectionFactorPPPC() :
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -89,9 +91,9 @@ namespace gtsam {
virtual ~ProjectionFactorPPPC() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -100,7 +102,7 @@ namespace gtsam {
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPPC, z = ";
measured_.print();
traits<Point2>::Print(measured_);
Base::print("", keyFormatter);
}
@ -109,7 +111,7 @@ namespace gtsam {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol);
&& traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
@ -120,16 +122,15 @@ namespace gtsam {
boost::optional<Matrix&> H4 = boost::none) const {
try {
if(H1 || H2 || H3 || H4) {
gtsam::Matrix H0, H02;
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H3, H4) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

10
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -101,11 +101,11 @@ public:
// loop over all circles
for(const Circle2& it: circles) {
// distance between circle centers.
double d = circle1.center.dist(it.center);
double d = distance2(circle1.center, it.center);
if (d < 1e-9)
continue; // skip circles that are in the same location
// Find f and h, the intersection points in normalized circles
boost::optional<Point2> fh = Point2::CircleCircleIntersection(
boost::optional<Point2> fh = circleCircleIntersection(
circle1.radius / d, it.radius / d);
// Check if this pair is better by checking h = fh->y()
// if h is large, the intersections are well defined.
@ -116,15 +116,15 @@ public:
}
// use best fh to find actual intersection points
std::list<Point2> intersections = Point2::CircleCircleIntersection(
std::list<Point2> intersections = circleCircleIntersection(
circle1.center, best_circle->center, best_fh);
// pick winner based on other measurements
double error1 = 0, error2 = 0;
Point2 p1 = intersections.front(), p2 = intersections.back();
for(const Circle2& it: circles) {
error1 += it.center.dist(p1);
error2 += it.center.dist(p2);
error1 += distance2(it.center, p1);
error2 += distance2(it.center, p2);
}
return (error1 < error2) ? p1 : p2;
//gttoc_(triangulate);

8
gtsam_unstable/slam/TSAMFactors.h
View File

@ -48,9 +48,7 @@ public:
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
Point2 d = pose.transform_to(point, H1, H2);
Point2 e = d - measured_;
return e.vector();
return pose.transform_to(point, H1, H2) - measured_;
}
};
@ -99,12 +97,12 @@ public:
*H3 = D_e_point_g * D_point_g_base2;
if (H4)
*H4 = D_e_point_g * D_point_g_point;
return (d - measured_).vector();
return d - measured_;
} else {
Pose2 pose_g = base1.compose(pose);
Point2 point_g = base2.transform_from(point);
Point2 d = pose_g.transform_to(point_g);
return (d - measured_).vector();
return d - measured_;
}
}
};

2
matlab.h
View File

@ -91,7 +91,7 @@ Matrix extractPoint2(const Values& values) {
Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(), 2);
for(const auto& key_value: points)
result.row(j++) = key_value.value.vector();
result.row(j++) = key_value.value;
return result;
}

13
python/handwritten/geometry/Point2.cpp
View File

@ -25,23 +25,26 @@
using namespace boost::python;
using namespace gtsam;
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(distance_overloads, Point2::distance, 1, 3)
#endif
void exportPoint2(){
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
class_<Point2>("Point2", init<>())
.def(init<double, double>())
.def(init<const Vector2 &>())
.def("identity", &Point2::identity)
.def("dist", &Point2::dist)
.def("distance", &Point2::distance)
.def("distance", &Point2::distance, distance_overloads(args("q","H1","H2")))
.def("equals", &Point2::equals, equals_overloads(args("q","tol")))
.def("norm", &Point2::norm)
.def("print", &Point2::print, print_overloads(args("s")))
.def("unit", &Point2::unit)
.def("vector", &Point2::vector)
.def("vector", &Point2::vector, return_value_policy<copy_const_reference>())
.def("x", &Point2::x)
.def("y", &Point2::y)
.def(self * other<double>()) // __mult__
@ -54,5 +57,5 @@ void exportPoint2(){
.def(repr(self))
.def(self == self)
;
}
#endif
}

30
python/handwritten/geometry/Point3.cpp
View File

@ -25,31 +25,32 @@
using namespace boost::python;
using namespace gtsam;
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(norm_overloads, Point3::norm, 0, 1)
#endif
void exportPoint3(){
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
class_<Point3>("Point3")
.def(init<>())
.def(init<double,double,double>())
.def(init<const Vector3 &>())
.def("identity", &Point3::identity)
.staticmethod("identity")
.def("cross", &Point3::cross)
.def("distance", &Point3::distance)
.def("dot", &Point3::dot)
.def("equals", &Point3::equals, equals_overloads(args("q","tol")))
.def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>")))
.def("normalized", &Point3::normalized)
.def("print", &Point3::print, print_overloads(args("s")))
#ifndef GTSAM_USE_VECTOR3_POINTS
.def("vector", &Point3::vector, return_value_policy<copy_const_reference>())
.def("x", &Point3::x)
.def("y", &Point3::y)
.def("z", &Point3::z)
#endif
.def("print", &Point3::print, print_overloads(args("s")))
.def("equals", &Point3::equals, equals_overloads(args("q","tol")))
.def("distance", &Point3::distance)
.def("cross", &Point3::cross)
.def("dot", &Point3::dot)
.def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>")))
.def("normalized", &Point3::normalized)
.def("identity", &Point3::identity)
.staticmethod("identity")
.def(self * other<double>())
.def(other<double>() * self)
.def(self + self)
@ -58,7 +59,10 @@ class_<Point3>("Point3")
.def(self / other<double>())
.def(self_ns::str(self))
.def(repr(self))
.def(self == self)
;
.def(self == self);
#endif
class_<Point3Pair>("Point3Pair", init<Point3, Point3>())
.def_readwrite("first", &Point3Pair::first)
.def_readwrite("second", &Point3Pair::second);
}

6
tests/simulated2D.h
View File

@ -140,7 +140,7 @@ namespace simulated2D {
/// Return error and optional derivative
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const {
return (prior(x, H) - measured_).vector();
return (prior(x, H) - measured_);
}
virtual ~GenericPrior() {}
@ -186,7 +186,7 @@ namespace simulated2D {
Vector evaluateError(const Pose& x1, const Pose& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (odo(x1, x2, H1, H2) - measured_).vector();
return (odo(x1, x2, H1, H2) - measured_);
}
virtual ~GenericOdometry() {}
@ -233,7 +233,7 @@ namespace simulated2D {
Vector evaluateError(const Pose& x1, const Landmark& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - measured_).vector();
return (mea(x1, x2, H1, H2) - measured_);
}
virtual ~GenericMeasurement() {}

2
tests/simulated2DConstraints.h
View File

@ -111,7 +111,7 @@ namespace simulated2D {
* @return a scalar distance between values
*/
template<class T1, class T2>
double range_trait(const T1& a, const T2& b) { return a.dist(b); }
double range_trait(const T1& a, const T2& b) { return distance2(a, b); }
/**
* Binary inequality constraint forcing the range between points

8
tests/smallExample.h
View File

@ -180,7 +180,7 @@ inline boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph(
new NonlinearFactorGraph);
// prior on x1
Point2 mu;
Point2 mu(0,0);
shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
nlfg->push_back(f1);
@ -337,7 +337,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
if (A) *A = H(x);
return (h(x) - z_).vector();
return (h(x) - z_);
}
};
@ -593,11 +593,11 @@ inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
Values zeros;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
zeros.insert(key(x, y), Point2());
zeros.insert(key(x, y), Point2(0,0));
VectorValues xtrue;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
xtrue.insert(key(x, y), Point2((double)x, (double)y).vector());
xtrue.insert(key(x, y), Point2((double)x, (double)y));
// linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);

6
tests/testBoundingConstraint.cpp
View File

@ -181,7 +181,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_basics) {
gtsam::Key key1 = 1, key2 = 2;
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
EXPECT(!rangeBound.isGreaterThan());
@ -220,7 +220,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
Symbol x1('x',1), x2('x',2);
NonlinearFactorGraph graph;
@ -246,7 +246,7 @@ TEST( testBoundingConstraint, avoid_demo) {
Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
double radius = 1.0;
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 odo(2.0, 0.0);
NonlinearFactorGraph graph;

4
tests/testExpressionFactor.cpp
View File

@ -193,7 +193,7 @@ TEST(ExpressionFactor, Binary) {
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;
Point2 value = binary.traceExecution(values, trace, traceStorage);
EXPECT(assert_equal(Point2(),value, 1e-9));
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
// trace.print();
// Expected Jacobians
@ -248,7 +248,7 @@ TEST(ExpressionFactor, Shallow) {
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;
Point2 value = expression.traceExecution(values, trace, traceStorage);
EXPECT(assert_equal(Point2(),value, 1e-9));
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
// trace.print();
// Expected Jacobians

4
tests/testExtendedKalmanFilter.cpp
View File

@ -226,7 +226,7 @@ public:
*H2 = Matrix::Identity(dim(),dim());
// Return the error between the prediction and the supplied value of p2
return (p2 - prediction).vector();
return (p2 - prediction);
}
};
@ -400,7 +400,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
ExtendedKalmanFilter<Point2> ekf(X(0), x_initial, P_initial);
// Enter Predict-Update Loop
Point2 x_predict, x_update;
Point2 x_predict(0,0), x_update(0,0);
for(unsigned int i = 0; i < 10; ++i){
// Create motion factor
NonlinearMotionModel motionFactor(X(i), X(i+1));

2
tests/testLie.cpp
View File

@ -40,7 +40,7 @@ template<> struct traits<Product> : internal::LieGroupTraits<Product> {
<< m.second.theta() << ")" << endl;
}
static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) {
return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol);
return traits<Point2>::Equals(m1.first, m2.first, tol) && m1.second.equals(m2.second, tol);
}
};
}

4
tests/testManifold.cpp
View File

@ -78,7 +78,7 @@ TEST(Manifold, Identity) {
EXPECT_DOUBLES_EQUAL(0.0, traits<double>::Identity(), 0.0);
EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits<Matrix24>::Identity())));
EXPECT(assert_equal(Pose3(), traits<Pose3>::Identity()));
EXPECT(assert_equal(Point2(), traits<Point2>::Identity()));
EXPECT(assert_equal(Point2(0,0), traits<Point2>::Identity()));
}
//******************************************************************************
@ -166,7 +166,7 @@ template<> struct traits<MyPoint2Pair> : internal::ManifoldTraits<MyPoint2Pair>
TEST(Manifold, ProductManifold) {
BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
MyPoint2Pair pair1;
MyPoint2Pair pair1(Point2(0,0),Point2(0,0));
Vector4 d;
d << 1,2,3,4;
MyPoint2Pair expected(Point2(1,2),Point2(3,4));

4
tests/testNonlinearEquality.cpp
View File

@ -417,7 +417,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
graph.push_back(factor);
Values initValues;
initValues.insert(key1, Point2());
initValues.insert(key1, Point2(0,0));
initValues.insert(key2, badPt);
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
@ -454,7 +454,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
Values initialEstimate;
initialEstimate.insert(x1, pt_x1);
initialEstimate.insert(x2, Point2());
initialEstimate.insert(x2, Point2(0,0));
initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth
initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame

12
tests/testNonlinearISAM.cpp
View File

@ -117,9 +117,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) {
new_factors += PriorFactor<Point2>(lm3, landmark3, model2);
// Initialize to origin
new_init.insert(lm1, Point2());
new_init.insert(lm2, Point2());
new_init.insert(lm3, Point2());
new_init.insert(lm1, Point2(0,0));
new_init.insert(lm2, Point2(0,0));
new_init.insert(lm3, Point2(0,0));
}
isamChol.update(new_factors, new_init);
@ -194,9 +194,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
new_factors += PriorFactor<Point2>(lm3, landmark3, model2);
// Initialize to origin
new_init.insert(lm1, Point2());
new_init.insert(lm2, Point2());
new_init.insert(lm3, Point2());
new_init.insert(lm1, Point2(0,0));
new_init.insert(lm2, Point2(0,0));
new_init.insert(lm3, Point2(0,0));
}
// Reconnect with observation later

2
timing/timeAdaptAutoDiff.cpp
View File

@ -68,7 +68,7 @@ int main() {
values.insert(2,Vector3(0,0,1));
typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> AdaptedSnavely;
Expression<Vector2> expression(AdaptedSnavely(), Expression<Vector9>(1), Expression<Vector3>(2));
f2 = boost::make_shared<ExpressionFactor<Vector2> >(model, z.vector(), expression);
f2 = boost::make_shared<ExpressionFactor<Vector2> >(model, z, expression);
time("Point2_(AdaptedSnavely(), camera, point): ", f2, values);
return 0;

2
timing/timePinholeCamera.cpp
View File

@ -64,7 +64,7 @@ int main()
long timeLog = clock();
Point2 measurement(0,0);
for(int i = 0; i < n; i++)
measurement.localCoordinates(camera.project(point1));
camera.project(point1)-measurement;
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;

4
timing/timeSFMBALautodiff.cpp
View File

@ -62,11 +62,11 @@ int main(int argc, char* argv[]) {
// readBAL converts to GTSAM format, so we need to convert back !
Pose3 openGLpose = gtsam2openGL(camera.pose());
Vector9 v9;
v9 << Pose3::Logmap(openGLpose), camera.calibration().vector();
v9 << Pose3::Logmap(openGLpose), camera.calibration();
initial.insert(C(i++), v9);
}
for (const SfM_Track& track: db.tracks) {
Vector3 v3 = track.p.vector();
Vector3 v3 = track.p;
initial.insert(P(j++), v3);
}