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

Derive Point2 from Vector2 and deprecate a number of methods

release/4.3a0
dellaert 2016-06-05 23:52:04 -07:00
parent 489966a95d
commit bdbbe0203d
31 changed files with 117 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

10
gtsam/geometry/Point2.cpp
View File

@ -29,15 +29,15 @@ void Point2::print(const string& s) const {
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
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 r = std::sqrt(x() * x() + y() * y());
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r;
*H << x() / r, y() / r;
else
*H << 1, 1; // really infinity, why 1 ?
}
@ -83,7 +83,7 @@ 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
}
/* ************************************************************************* */
@ -120,7 +120,7 @@ list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2,
double r2, double tol) {
// distance between circle centers.
double d = c1.dist(c2);
double d = c1.distance(c2);
// centers coincide, either no solution or infinite number of solutions.
if (d<1e-9) return list<Point2>();

80
gtsam/geometry/Point2.h
View File

@ -29,31 +29,32 @@ 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() {
throw std::runtime_error("Point2 default");
}
#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);
}
explicit Point2(const Vector2& v):Vector2(v) {}
/*
* @brief Circle-circle intersection, given normalized radii.
@ -107,21 +108,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,35 +124,28 @@ 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; }
/// @}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
@ -177,10 +157,9 @@ public:
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);}
inline double dist(const Point2& p2) const {return distance();}
/// @}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
#endif
private:
@ -192,11 +171,9 @@ 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);}
/// @}
/// @}
};
// Convenience typedef
@ -207,10 +184,13 @@ 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> {};
struct traits<Point2> : public internal::VectorSpace<Point2> {
};
} // \ namespace gtsam

14
gtsam/geometry/Pose2.cpp
View File

@ -58,16 +58,16 @@ bool Pose2::equals(const Pose2& q, double tol) const {
/* ************************************************************************* */
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);
}
}
@ -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;

4
gtsam/geometry/Pose2.h
View File

@ -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_) {}

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

12
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));
}
/* ************************************************************************* */

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

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

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

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

@ -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,12 +100,12 @@ 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));
}
/* ************************************************************************* */

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

6
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) <<

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;

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

3
gtsam/slam/TriangulationFactor.h
View File

@ -83,7 +83,6 @@ public:
"TriangulationFactor must be created with "
+ boost::lexical_cast<std::string>((int) Measurement::dimension)
+ "-dimensional noise model.");
}
/** Virtual destructor */
@ -160,7 +159,7 @@ public:
// 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);

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

8
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

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

6
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -101,7 +101,7 @@ public:
// loop over all circles
for(const Circle2& it: circles) {
// distance between circle centers.
double d = circle1.center.dist(it.center);
double d = circle1.center.distance(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
@ -123,8 +123,8 @@ public:
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 += it.center.distance(p1);
error2 += it.center.distance(p2);
}
return (error1 < error2) ? p1 : p2;
//gttoc_(triangulate);

4
gtsam_unstable/slam/TSAMFactors.h
View File

@ -99,12 +99,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_;
}
}
};

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

@ -28,6 +28,7 @@ using namespace gtsam;
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)
void exportPoint2(){
@ -35,13 +36,12 @@ void exportPoint2(){
.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__
@ -55,4 +55,4 @@ void exportPoint2(){
.def(self == self)
;
}
}

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 a.distance(b); }
/**
* Binary inequality constraint forcing the range between points

4
tests/smallExample.h
View File

@ -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,7 +593,7 @@ 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++)

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

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

2
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()));
}
//******************************************************************************

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