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

Resolved all issues with typedefs

release/4.3a0
Frank Dellaert 2016-06-06 21:57:52 -07:00
parent aa40b266f6
commit a97502f5a1
41 changed files with 153 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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 (distance(uncalibrate(pn), 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 (distance(uncalibrate(pn), 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);

12
gtsam/geometry/Point2.cpp
View File

@ -23,7 +23,7 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
double norm(const Point2& p, OptionalJacobian<1,2> H) {
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)
@ -35,17 +35,17 @@ double norm(const Point2& p, OptionalJacobian<1,2> H) {
}
/* ************************************************************************* */
double distance(const Point2& p, const Point2& q, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) {
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 = norm(d, H);
double r = norm2(d, H);
if (H1) *H1 = -H;
if (H2) *H2 = H;
return r;
} else {
return norm(d);
return d.norm();
}
}
@ -130,7 +130,7 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
double r2, double tol) {
// distance between circle centers.
double d = distance(c1, c2);
double d = distance2(c1, c2);
// centers coincide, either no solution or infinite number of solutions.
if (d<1e-9) return list<Point2>();

8
gtsam/geometry/Point2.h
View File

@ -166,12 +166,12 @@ struct traits<Point2> : public internal::VectorSpace<Point2> {
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// Distance of the point from the origin, with Jacobian
double norm(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
/// distance between two points
double distance(const Point2& p1, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none);
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;

8
gtsam/geometry/Point3.cpp
View File

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

10
gtsam/geometry/Point3.h
View File

@ -160,12 +160,12 @@ 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);
}
};

4
gtsam/geometry/Pose2.cpp
View File

@ -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 = norm(d, 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 = norm(d, D_r_d);
double r = norm2(d, D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose <<

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/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/testPinholeCamera.cpp
View File

@ -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));

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

@ -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));
}

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

@ -111,9 +111,9 @@ TEST( Point2, arithmetic) {
/* ************************************************************************* */
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), p0.normalized(), 1e-6));
EXPECT(assert_equal(Point2(0,-1), p1.normalized(), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.normalized(), 1e-6));
}
namespace {
@ -131,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));
@ -156,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
@ -179,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
@ -195,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));
@ -238,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 () {

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

@ -183,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);

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

@ -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));
}

5
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

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:

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

@ -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);

8
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,7 +115,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 */
@ -241,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");
}
/**
@ -249,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 */

4
gtsam/slam/ProjectionFactor.h
View File

@ -110,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);
@ -121,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_)));
}

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

18
gtsam/slam/TriangulationFactor.h
View File

@ -78,10 +78,10 @@ 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.");
}
@ -104,7 +104,7 @@ public:
DefaultKeyFormatter) const {
std::cout << s << "TriangulationFactor,";
camera_.print("camera");
measured_.print("z");
traits<Measurement>::Print(measured_, "z");
Base::print("", keyFormatter);
}
@ -112,7 +112,7 @@ 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
@ -122,14 +122,14 @@ public:
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());
}
}
@ -151,9 +151,9 @@ 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

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

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;

10
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) &&
@ -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;
}

19
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 ||
@ -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;
/**

4
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -72,13 +72,13 @@ public:
void print(const std::string& s = "InvDepthFactor3",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
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

4
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -68,7 +68,7 @@ public:
void print(const std::string& s = "InvDepthFactorVariant1",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
@ -76,7 +76,7 @@ public:
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);
}

4
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -71,7 +71,7 @@ public:
void print(const std::string& s = "InvDepthFactorVariant2",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
@ -79,7 +79,7 @@ public:
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);
}

8
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -70,7 +70,7 @@ public:
void print(const std::string& s = "InvDepthFactorVariant3a",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
@ -78,7 +78,7 @@ public:
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);
}
@ -190,7 +190,7 @@ public:
void print(const std::string& s = "InvDepthFactorVariant3",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
@ -198,7 +198,7 @@ public:
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);
}

4
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -107,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);
}
@ -116,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);
}

4
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -102,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);
}
@ -111,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

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.distance(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.distance(p1);
error2 += it.center.distance(p2);
error1 += distance2(it.center, p1);
error2 += distance2(it.center, p2);
}
return (error1 < error2) ? p1 : p2;
//gttoc_(triangulate);

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.distance(b); }
double range_trait(const T1& a, const T2& b) { return distance2(a, b); }
/**
* Binary inequality constraint forcing the range between points

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