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

Almost everything compiles and passes in windows

release/4.3a0
Richard Roberts 2012-05-23 18:51:49 +00:00
parent 7cdd8e19da
commit c2c9c4a982
24 changed files with 113 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/PlanarSLAMExample_easy.cpp
View File

@ -53,7 +53,7 @@ int main(int argc, char** argv) {
Rot2 bearing11 = Rot2::fromDegrees(45), Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90); bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4), double range11 = std::sqrt(4.0+4.0),
range21 = 2.0, range21 = 2.0,
range32 = 2.0; range32 = 2.0;

2
gtsam/base/tests/testVector.cpp
View File

@ -251,7 +251,7 @@ TEST( TestVector, axpy )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, equals ) TEST( TestVector, equals )
{ {
Vector v1 = Vector_(1, 0.0/0.0); //testing nan Vector v1 = Vector_(1, 0.0/std::numeric_limits<double>::quiet_NaN()); //testing nan
Vector v2 = Vector_(1, 1.0); Vector v2 = Vector_(1, 1.0);
double tol = 1.; double tol = 1.;
EXPECT(!equal_with_abs_tol(v1, v2, tol)); EXPECT(!equal_with_abs_tol(v1, v2, tol));

2
gtsam/discrete/DecisionTree-inl.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
bool equals(const Node& q, double tol) const { bool equals(const Node& q, double tol) const {
const Leaf* other = dynamic_cast<const Leaf*> (&q); const Leaf* other = dynamic_cast<const Leaf*> (&q);
if (!other) return false; if (!other) return false;
return fabs(this->constant_ - other->constant_) < tol; return fabs(double(this->constant_ - other->constant_)) < tol;
} }
/** print */ /** print */

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

@ -65,7 +65,7 @@ TEST( CalibratedCamera, level1)
TEST( CalibratedCamera, level2) TEST( CalibratedCamera, level2)
{ {
// Create a level camera, looking in Y-direction // Create a level camera, looking in Y-direction
Pose2 pose2(0.4,0.3,M_PI_2); Pose2 pose2(0.4,0.3,M_PI/2.0);
CalibratedCamera camera = CalibratedCamera::level(pose2, 0.1); CalibratedCamera camera = CalibratedCamera::level(pose2, 0.1);
// expected // expected

8
gtsam/geometry/tests/testFundamental.cpp
View File

@ -34,11 +34,11 @@ using namespace tensors;
/* ************************************************************************* */ /* ************************************************************************* */
// Indices // Indices
Index<3, 'a'> a; tensors::Index<3, 'a'> a;
Index<3, 'b'> b; tensors::Index<3, 'b'> b;
Index<4, 'A'> A; tensors::Index<4, 'A'> A;
Index<4, 'B'> B; tensors::Index<4, 'B'> B;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Tensors, FundamentalMatrix) TEST( Tensors, FundamentalMatrix)

18
gtsam/geometry/tests/testHomography2.cpp
View File

@ -36,9 +36,9 @@ using namespace tensors;
/* ************************************************************************* */ /* ************************************************************************* */
// Indices // Indices
Index<3, 'a'> a, _a; tensors::Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b; tensors::Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c; tensors::Index<3, 'c'> c, _c;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Homography2, RealImages) TEST( Homography2, RealImages)
@ -120,8 +120,8 @@ Homography2 patchH(const Pose3& tEc) {
namespace gtsam { namespace gtsam {
// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;} // size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
Vector toVector(const tensors::Tensor2<3, 3>& H) { Vector toVector(const tensors::Tensor2<3, 3>& H) {
Index<3, 'T'> _T; // covariant 2D template tensors::Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera tensors::Index<3, 'C'> I; // contravariant 2D camera
return toVector(H(I,_T)); return toVector(H(I,_T));
} }
Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
@ -134,8 +134,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Homography2, patchH) TEST( Homography2, patchH)
{ {
Index<3, 'T'> _T; // covariant 2D template tensors::Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera tensors::Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I] // data[_T][I]
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
@ -160,8 +160,8 @@ TEST( Homography2, patchH)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Homography2, patchH2) TEST( Homography2, patchH2)
{ {
Index<3, 'T'> _T; // covariant 2D template tensors::Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera tensors::Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I] // data[_T][I]
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};

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

@ -72,7 +72,7 @@ TEST( Point2, arithmetic)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point2, norm) TEST( Point2, norm)
{ {
Point2 p0(cos(5), sin(5)); Point2 p0(cos(5.0), sin(5.0));
DOUBLES_EQUAL(1,p0.norm(),1e-6); DOUBLES_EQUAL(1,p0.norm(),1e-6);
Point2 p1(4, 5), p2(1, 1); Point2 p1(4, 5), p2(1, 1);
DOUBLES_EQUAL( 5,p1.dist(p2),1e-6); DOUBLES_EQUAL( 5,p1.dist(p2),1e-6);
@ -85,7 +85,7 @@ TEST( Point2, unit)
Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0); Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0);
EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6)); EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6));
EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6)); EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2)/2.0, sqrt(2)/2.0), p2.unit(), 1e-6)); EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -44,14 +44,14 @@ TEST(Pose2, constructors) {
Pose2 pose(0,p); Pose2 pose(0,p);
Pose2 origin; Pose2 origin;
assert_equal(pose,origin); assert_equal(pose,origin);
Pose2 t(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
EXPECT(assert_equal(t,Pose2(t.matrix()))); EXPECT(assert_equal(t,Pose2(t.matrix())));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, manifold) { TEST(Pose2, manifold) {
Pose2 t1(M_PI_2, Point2(1, 2)); Pose2 t1(M_PI/2.0, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 t2(M_PI/2.0+0.018, Point2(1.015, 2.01));
Pose2 origin; Pose2 origin;
Vector d12 = t1.localCoordinates(t2); Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
@ -63,11 +63,11 @@ TEST(Pose2, manifold) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, retract) { TEST(Pose2, retract) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifdef SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
#else #else
Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01)); Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
#endif #endif
Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -75,7 +75,7 @@ TEST(Pose2, retract) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap) { TEST(Pose2, expmap) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -83,7 +83,7 @@ TEST(Pose2, expmap) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap2) { TEST(Pose2, expmap2) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -110,11 +110,11 @@ TEST(Pose2, expmap3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0) { TEST(Pose2, expmap0) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
//#ifdef SLOW_BUT_CORRECT_EXPMAP //#ifdef SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
//#else //#else
// Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); // Pose2 expected(M_PI/2.0+0.018, Point2(1.015, 2.01));
//#endif //#endif
Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018))); Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -122,7 +122,7 @@ TEST(Pose2, expmap0) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0_full) { TEST(Pose2, expmap0_full) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -130,7 +130,7 @@ TEST(Pose2, expmap0_full) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0_full2) { TEST(Pose2, expmap0_full2) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -170,8 +170,8 @@ TEST(Pose3, expmap_c_full)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, logmap) { TEST(Pose2, logmap) {
Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifdef SLOW_BUT_CORRECT_EXPMAP
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
#else #else
@ -183,8 +183,8 @@ TEST(Pose2, logmap) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, logmap_full) { TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
Vector actual = logmap_default<Pose2>(pose0, pose); Vector actual = logmap_default<Pose2>(pose0, pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -197,7 +197,7 @@ Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
TEST( Pose2, transform_to ) TEST( Pose2, transform_to )
{ {
Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 pose(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Point2 point(-1,4); // landmark at (-1,4) Point2 point(-1,4); // landmark at (-1,4)
// expected // expected
@ -226,7 +226,7 @@ Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
TEST (Pose2, transform_from) TEST (Pose2, transform_from)
{ {
Pose2 pose(1., 0., M_PI_2); Pose2 pose(1., 0., M_PI/2.0);
Point2 pt(2., 1.); Point2 pt(2., 1.);
Matrix H1, H2; Matrix H1, H2;
Point2 actual = pose.transform_from(pt, H1, H2); Point2 actual = pose.transform_from(pt, H1, H2);
@ -326,7 +326,7 @@ TEST(Pose2, compose_c)
TEST(Pose2, inverse ) TEST(Pose2, inverse )
{ {
Point2 origin, t(1,2); Point2 origin, t(1,2);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Pose2 identity, lTg = gTl.inverse(); Pose2 identity, lTg = gTl.inverse();
EXPECT(assert_equal(identity,lTg.compose(gTl))); EXPECT(assert_equal(identity,lTg.compose(gTl)));
@ -362,7 +362,7 @@ Matrix matrix(const Pose2& gTl) {
TEST( Pose2, matrix ) TEST( Pose2, matrix )
{ {
Point2 origin, t(1,2); Point2 origin, t(1,2);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl); Matrix gMl = matrix(gTl);
EXPECT(assert_equal(Matrix_(3,3, EXPECT(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0, 0.0, -1.0, 1.0,
@ -393,7 +393,7 @@ TEST( Pose2, matrix )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2, compose_matrix ) TEST( Pose2, compose_matrix )
{ {
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2)); Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
@ -407,11 +407,11 @@ TEST( Pose2, between )
// ^ // ^
// //
// *--0--*--* // *--0--*--*
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2; Matrix actualH1,actualH2;
Pose2 expected(M_PI_2, Point2(2,2)); Pose2 expected(M_PI/2.0, Point2(2,2));
Pose2 actual1 = gT1.between(gT2); Pose2 actual1 = gT1.between(gT2);
Pose2 actual2 = gT1.between(gT2,actualH1,actualH2); Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
EXPECT(assert_equal(expected,actual1)); EXPECT(assert_equal(expected,actual1));
@ -443,7 +443,7 @@ TEST( Pose2, between )
// reverse situation for extra test // reverse situation for extra test
TEST( Pose2, between2 ) TEST( Pose2, between2 )
{ {
Pose2 p2(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 p2(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2; Matrix actualH1,actualH2;
@ -472,7 +472,7 @@ TEST(Pose2, members)
/* ************************************************************************* */ /* ************************************************************************* */
// some shared test values // some shared test values
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */ /* ************************************************************************* */
@ -488,11 +488,11 @@ TEST( Pose2, bearing )
EXPECT(assert_equal(Rot2(),x1.bearing(l1))); EXPECT(assert_equal(Rot2(),x1.bearing(l1)));
// establish bearing is indeed 45 degrees // establish bearing is indeed 45 degrees
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(l2))); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(l2)));
// establish bearing is indeed 45 degrees even if shifted // establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = x2.bearing(l3, actualH1, actualH2); Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
@ -502,7 +502,7 @@ TEST( Pose2, bearing )
// establish bearing is indeed 45 degrees even if rotated // establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(l4, actualH1, actualH2); Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
@ -518,7 +518,7 @@ Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
TEST( Pose2, bearing_pose ) TEST( Pose2, bearing_pose )
{ {
Pose2 xl1(1, 0, M_PI_2), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI_2), xl4(1, 3, 0); Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);
Matrix expectedH1, actualH1, expectedH2, actualH2; Matrix expectedH1, actualH1, expectedH2, actualH2;
@ -526,11 +526,11 @@ TEST( Pose2, bearing_pose )
EXPECT(assert_equal(Rot2(),x1.bearing(xl1))); EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));
// establish bearing is indeed 45 degrees // establish bearing is indeed 45 degrees
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(xl2))); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(xl2)));
// establish bearing is indeed 45 degrees even if shifted // establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2); Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3); expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
@ -540,7 +540,7 @@ TEST( Pose2, bearing_pose )
// establish bearing is indeed 45 degrees even if rotated // establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2); Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4); expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
@ -561,11 +561,11 @@ TEST( Pose2, range )
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// establish range is indeed 45 degrees // establish range is indeed 45 degrees
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(l3, actualH1, actualH2); double actual23 = x2.range(l3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x2, l3); expectedH1 = numericalDerivative21(range_proxy, x2, l3);
@ -590,7 +590,7 @@ LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
} }
TEST( Pose2, range_pose ) TEST( Pose2, range_pose )
{ {
Pose2 xl1(1, 0, M_PI_2), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI_2), xl4(1, 3, 0); Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);
Matrix expectedH1, actualH1, expectedH2, actualH2; Matrix expectedH1, actualH1, expectedH2, actualH2;
@ -598,11 +598,11 @@ TEST( Pose2, range_pose )
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
// establish range is indeed 45 degrees // establish range is indeed 45 degrees
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(xl3, actualH1, actualH2); double actual23 = x2.range(xl3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3); expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
@ -637,7 +637,7 @@ TEST(Pose2, align_1) {
TEST(Pose2, align_2) { TEST(Pose2, align_2) {
Point2 t(20,10); Point2 t(20,10);
Rot2 R = Rot2::fromAngle(M_PI_2); Rot2 R = Rot2::fromAngle(M_PI/2.0);
Pose2 expected(R, t); Pose2 expected(R, t);
vector<Point2Pair> correspondences; vector<Point2Pair> correspondences;

12
gtsam/geometry/tests/testPose3.cpp
View File

@ -535,7 +535,7 @@ TEST( Pose3, between )
/* ************************************************************************* */ /* ************************************************************************* */
// some shared test values - pulled from equivalent test in Pose2 // some shared test values - pulled from equivalent test in Pose2
Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4); Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4);
Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI_4, 0.0, 0.0), l2); Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI/4.0, 0.0, 0.0), l2);
Pose3 Pose3
xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)), xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
@ -554,11 +554,11 @@ TEST( Pose3, range )
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// establish range is indeed sqrt2 // establish range is indeed sqrt2
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(l3, actualH1, actualH2); double actual23 = x2.range(l3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x2, l3); expectedH1 = numericalDerivative21(range_proxy, x2, l3);
@ -589,11 +589,11 @@ TEST( Pose3, range_pose )
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
// establish range is indeed sqrt2 // establish range is indeed sqrt2
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(xl3, actualH1, actualH2); double actual23 = x2.range(xl3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3); expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
@ -619,7 +619,7 @@ TEST( Pose3, unicycle )
Vector x_step = delta(6,3,1.0); Vector x_step = delta(6,3,1.0);
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2) * x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/geometry/tests/testRot2.cpp
View File

@ -43,7 +43,7 @@ TEST( Rot2, constructors_and_angle)
TEST( Rot2, unit) TEST( Rot2, unit)
{ {
EXPECT(assert_equal(Point2(1.0, 0.0), Rot2::fromAngle(0).unit())); EXPECT(assert_equal(Point2(1.0, 0.0), Rot2::fromAngle(0).unit()));
EXPECT(assert_equal(Point2(0.0, 1.0), Rot2::fromAngle(M_PI_2).unit())); EXPECT(assert_equal(Point2(0.0, 1.0), Rot2::fromAngle(M_PI/2.0).unit()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -94,9 +94,9 @@ TEST( Rot2, expmap)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot2, logmap) TEST(Rot2, logmap)
{ {
Rot2 rot0(Rot2::fromAngle(M_PI_2)); Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
Rot2 rot(Rot2::fromAngle(M_PI)); Rot2 rot(Rot2::fromAngle(M_PI));
Vector expected = Vector_(1, M_PI_2); Vector expected = Vector_(1, M_PI/2.0);
Vector actual = rot0.localCoordinates(rot); Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
@ -146,7 +146,7 @@ TEST( Rot2, relativeBearing )
// establish relativeBearing is indeed 45 degrees // establish relativeBearing is indeed 45 degrees
Rot2 actual2 = Rot2::relativeBearing(l2, actualH); Rot2 actual2 = Rot2::relativeBearing(l2, actualH);
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2)); CHECK(assert_equal(Rot2::fromAngle(M_PI/4.0),actual2));
// Check numerical derivative // Check numerical derivative
expectedH = numericalDerivative11(relativeBearing_, l2); expectedH = numericalDerivative11(relativeBearing_, l2);

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

@ -131,7 +131,7 @@ TEST( Rot3, rodriguez3)
TEST( Rot3, rodriguez4) TEST( Rot3, rodriguez4)
{ {
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
double angle = M_PI_2; double angle = M_PI/2.0;
Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle); double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0, Rot3 expected(c,-s, 0,

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

@ -53,7 +53,7 @@ TEST( SimpleCamera, constructor)
TEST( SimpleCamera, level2) TEST( SimpleCamera, level2)
{ {
// Create a level camera, looking in Y-direction // Create a level camera, looking in Y-direction
Pose2 pose2(0.4,0.3,M_PI_2); Pose2 pose2(0.4,0.3,M_PI/2.0);
SimpleCamera camera = SimpleCamera::level(K, pose2, 0.1); SimpleCamera camera = SimpleCamera::level(K, pose2, 0.1);
// expected // expected

12
gtsam/geometry/tests/testTensors.cpp
View File

@ -34,12 +34,12 @@ using namespace tensors;
/* ************************************************************************* */ /* ************************************************************************* */
// Indices // Indices
Index<3, 'a'> a, _a; tensors::Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b; tensors::Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c; tensors::Index<3, 'c'> c, _c;
Index<4, 'A'> A; tensors::Index<4, 'A'> A;
Index<4, 'B'> B; tensors::Index<4, 'B'> B;
/* ************************************************************************* */ /* ************************************************************************* */
// Tensor1 // Tensor1
@ -58,7 +58,7 @@ TEST(Tensor1, Basics)
CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent
// and you can take a norm, typically for normalization to the sphere // and you can take a norm, typically for normalization to the sphere
DOUBLES_EQUAL(sqrt(14),norm(p(a)),1e-9) DOUBLES_EQUAL(sqrt(14.0),norm(p(a)),1e-9)
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/inference/graph-inl.h
View File

@ -196,8 +196,8 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
PoseVertex v2 = key2vertex.find(key2)->second; PoseVertex v2 = key2vertex.find(key2)->second;
POSE l1Xl2 = factor->measured(); POSE l1Xl2 = factor->measured();
tie(edge12, found1) = boost::edge(v1, v2, g); boost::tie(edge12, found1) = boost::edge(v1, v2, g);
tie(edge21, found2) = boost::edge(v2, v1, g); boost::tie(edge21, found2) = boost::edge(v2, v1, g);
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree"); if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
if (!found1 && !found2) continue; if (!found1 && !found2) continue;
if (found1) if (found1)

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

@ -144,8 +144,8 @@ vector<GeneralCamera> genCameraVariableCalibration() {
return X ; return X ;
} }
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) { boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
shared_ptr<Ordering> ordering(new Ordering); boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ;
for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ;
return ordering ; return ordering ;

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

@ -146,8 +146,8 @@ vector<GeneralCamera> genCameraVariableCalibration() {
return X ; return X ;
} }
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) { boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
shared_ptr<Ordering> ordering(new Ordering); boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ;
for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ;
return ordering ; return ordering ;

16
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -26,7 +26,7 @@ using namespace gtsam;
using namespace planarSLAM; using namespace planarSLAM;
// some shared test values // some shared test values
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0);
static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
SharedNoiseModel SharedNoiseModel
@ -47,7 +47,7 @@ TEST( planarSLAM, PriorFactor_equals )
TEST( planarSLAM, BearingFactor ) TEST( planarSLAM, BearingFactor )
{ {
// Create factor // Create factor
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 Rot2 z = Rot2::fromAngle(M_PI/4.0 + 0.1); // h(x) - z = -0.1
planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma); planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma);
// create config // create config
@ -75,7 +75,7 @@ TEST( planarSLAM, BearingFactor_equals )
TEST( planarSLAM, RangeFactor ) TEST( planarSLAM, RangeFactor )
{ {
// Create factor // Create factor
double z(sqrt(2) - 0.22); // h(x) - z = 0.22 double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22
planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma); planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma);
// create config // create config
@ -101,8 +101,8 @@ TEST( planarSLAM, RangeFactor_equals )
TEST( planarSLAM, BearingRangeFactor ) TEST( planarSLAM, BearingRangeFactor )
{ {
// Create factor // Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 Rot2 r = Rot2::fromAngle(M_PI/4.0 + 0.1); // h(x) - z = -0.1
double b(sqrt(2) - 0.22); // h(x) - z = 0.22 double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22
planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2); planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2);
// create config // create config
@ -158,14 +158,14 @@ TEST( planarSLAM, constructor )
G.addPoseConstraint(2, x2); // make it feasible :-) G.addPoseConstraint(2, x2); // make it feasible :-)
// Add odometry // Add odometry
G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3); G.addOdometry(2, 3, Pose2(0, 0, M_PI/4.0), I3);
// Create bearing factor // Create bearing factor
Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 Rot2 z1 = Rot2::fromAngle(M_PI/4.0 + 0.1); // h(x) - z = -0.1
G.addBearing(2, 3, z1, sigma); G.addBearing(2, 3, z1, sigma);
// Create range factor // Create range factor
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 double z2(sqrt(2.0) - 0.22); // h(x) - z = 0.22
G.addRange(2, 3, z2, sigma); G.addRange(2, 3, z2, sigma);
Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); Vector expected0 = Vector_(3, 0.0, 0.0, 0.0);

32
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -72,7 +72,7 @@ TEST( Pose2SLAM, constraint1 )
/* ************************************************************************* */ /* ************************************************************************* */
// Test constraint, large displacement // Test constraint, large displacement
Vector f2(const Pose2& pose1, const Pose2& pose2) { Vector f2(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2,2,M_PI_2); Pose2 z(2,2,M_PI/2.0);
Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
return constraint.evaluateError(pose1, pose2); return constraint.evaluateError(pose1, pose2);
} }
@ -80,7 +80,7 @@ Vector f2(const Pose2& pose1, const Pose2& pose2) {
TEST( Pose2SLAM, constraint2 ) TEST( Pose2SLAM, constraint2 )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2,2,M_PI_2); Pose2 pose1, pose2(2,2,M_PI/2.0);
Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
Matrix H1, H2; Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -96,7 +96,7 @@ TEST( Pose2SLAM, constraint2 )
TEST( Pose2SLAM, constructor ) TEST( Pose2SLAM, constructor )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2); Pose2 measured(2,2,M_PI/2.0);
pose2SLAM::Graph graph; pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance); graph.addOdometry(1,2,measured, covariance);
// get the size of the graph // get the size of the graph
@ -110,13 +110,13 @@ TEST( Pose2SLAM, constructor )
TEST( Pose2SLAM, linearization ) TEST( Pose2SLAM, linearization )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2); Pose2 measured(2,2,M_PI/2.0);
Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance);
pose2SLAM::Graph graph; pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance); graph.addOdometry(1,2,measured, covariance);
// Choose a linearization point // Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p1(1.1,2,M_PI/2.0); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
pose2SLAM::Values config; pose2SLAM::Values config;
config.insert(pose2SLAM::PoseKey(1),p1); config.insert(pose2SLAM::PoseKey(1),p1);
@ -151,7 +151,7 @@ TEST(Pose2SLAM, optimize) {
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg; pose2SLAM::Graph fg;
fg.addPoseConstraint(0, Pose2(0,0,0)); fg.addPoseConstraint(0, Pose2(0,0,0));
fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); fg.addOdometry(0, 1, Pose2(1,2,M_PI/2.0), covariance);
// Create initial config // Create initial config
Values initial; Values initial;
@ -170,7 +170,7 @@ TEST(Pose2SLAM, optimize) {
// Check with expected config // Check with expected config
Values expected; Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI/2.0));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// Check marginals // Check marginals
@ -348,9 +348,9 @@ TEST(Pose2Values, pose2Circle )
{ {
// expected is 4 poses tangent to circle with radius 1m // expected is 4 poses tangent to circle with radius 1m
pose2SLAM::Values expected; pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2)); expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI/2.0));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI )); expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2)); expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI/2.0));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 )); expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 ));
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0); pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
@ -362,9 +362,9 @@ TEST(Pose2SLAM, expmap )
{ {
// expected is circle shifted to right // expected is circle shifted to right
pose2SLAM::Values expected; pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2)); expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI/2.0));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI )); expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2)); expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI/2.0));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 )); expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
@ -418,7 +418,7 @@ TEST( Pose2Prior, error )
/* ************************************************************************* */ /* ************************************************************************* */
// common Pose2Prior for tests below // common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2); static gtsam::Pose2 priorVal(2,2,M_PI/2.0);
static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas); static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas);
/* ************************************************************************* */ /* ************************************************************************* */
@ -484,14 +484,14 @@ TEST( Pose2Factor, error )
/* ************************************************************************* */ /* ************************************************************************* */
// common Pose2Factor for tests below // common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2); static Pose2 measured(2,2,M_PI/2.0);
static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance); static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2Factor, rhs ) TEST( Pose2Factor, rhs )
{ {
// Choose a linearization point // Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p1(1.1,2,M_PI/2.0); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1); x0.insert(pose2SLAM::PoseKey(1),p1);
@ -504,7 +504,7 @@ TEST( Pose2Factor, rhs )
// Check RHS // Check RHS
Pose2 hx0 = p1.between(p2); Pose2 hx0 = p1.between(p2);
CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); CHECK(assert_equal(Pose2(2.1, 2.1, M_PI/2.0),hx0));
Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0);
CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); CHECK(assert_equal(expected_b,-factor.whitenedError(x0)));
CHECK(assert_equal(expected_b,linear->getb())); CHECK(assert_equal(expected_b,linear->getb()));
@ -521,7 +521,7 @@ LieVector h(const Pose2& p1,const Pose2& p2) {
TEST( Pose2Factor, linearize ) TEST( Pose2Factor, linearize )
{ {
// Choose a linearization point at ground truth // Choose a linearization point at ground truth
Pose2 p1(1,2,M_PI_2); Pose2 p1(1,2,M_PI/2.0);
Pose2 p2(-1,4,M_PI); Pose2 p2(-1,4,M_PI);
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1); x0.insert(pose2SLAM::PoseKey(1),p1);

5
tests/testDoglegOptimizer.cpp
View File

@ -31,7 +31,6 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::shared_ptr;
/* ************************************************************************* */ /* ************************************************************************* */
double computeError(const GaussianBayesNet& gbn, const LieVector& values) { double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
@ -381,7 +380,7 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) { TEST(DoglegOptimizer, Iterate) {
// really non-linear factor graph // really non-linear factor graph
shared_ptr<example::Graph> fg(new example::Graph( boost::shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph())); example::createReallyNonlinearFactorGraph()));
// config far from minimum // config far from minimum
@ -390,7 +389,7 @@ TEST(DoglegOptimizer, Iterate) {
config->insert(simulated2D::PoseKey(1), x0); config->insert(simulated2D::PoseKey(1), x0);
// ordering // ordering
shared_ptr<Ordering> ord(new Ordering()); boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(simulated2D::PoseKey(1)); ord->push_back(simulated2D::PoseKey(1));
double Delta = 1.0; double Delta = 1.0;

4
tests/testGaussianFactor.cpp
View File

@ -263,7 +263,7 @@ TEST( GaussianFactor, matrix )
EQUALITY(b_act2,b2); EQUALITY(b_act2,b2);
// Ensure that whitening is consistent // Ensure that whitening is consistent
shared_ptr<noiseModel::Gaussian> model = lf->get_model(); boost::shared_ptr<noiseModel::Gaussian> model = lf->get_model();
model->WhitenSystem(A_act2, b_act2); model->WhitenSystem(A_act2, b_act2);
EQUALITY(A_act1, A_act2); EQUALITY(A_act1, A_act2);
EQUALITY(b_act1, b_act2); EQUALITY(b_act1, b_act2);
@ -307,7 +307,7 @@ TEST( GaussianFactor, matrix_aug )
EQUALITY(Ab_act2,Ab2); EQUALITY(Ab_act2,Ab2);
// Ensure that whitening is consistent // Ensure that whitening is consistent
shared_ptr<noiseModel::Gaussian> model = lf->get_model(); boost::shared_ptr<noiseModel::Gaussian> model = lf->get_model();
model->WhitenInPlace(Ab_act1); model->WhitenInPlace(Ab_act1);
EQUALITY(Ab_act1, Ab_act2); EQUALITY(Ab_act1, Ab_act2);
} }

4
tests/testGaussianFactorGraphB.cpp
View File

@ -232,7 +232,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
// create expected Conditional Gaussian // create expected Conditional Gaussian
double sig = sqrt(2)/10.; double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
@ -295,7 +295,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first;
// create expected Conditional Gaussian // create expected Conditional Gaussian
double sig = sqrt(2)/10.; double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);

2
tests/testGraph.cpp
View File

@ -70,7 +70,7 @@ TEST( Graph, predecessorMap2Graph )
p_map.insert(kx(1), kx(2)); p_map.insert(kx(1), kx(2));
p_map.insert(kx(2), kx(2)); p_map.insert(kx(2), kx(2));
p_map.insert(kx(3), kx(2)); p_map.insert(kx(3), kx(2));
tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map); boost::tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
LONGS_EQUAL(3, boost::num_vertices(graph)); LONGS_EQUAL(3, boost::num_vertices(graph));
CHECK(root == key2vertex[kx(2)]); CHECK(root == key2vertex[kx(2)]);

2
tests/testInferenceB.cpp
View File

@ -41,7 +41,7 @@ TEST( inference, marginals )
*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate(); *GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate();
// expected is just scalar Gaussian on x // expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2)); GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2.0));
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

2
tests/testMarginals.cpp
View File

@ -71,7 +71,7 @@ TEST(Marginals, planarSLAMmarginals) {
Rot2 bearing11 = Rot2::fromDegrees(45), Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90); bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4), double range11 = sqrt(4.0+4.0),
range21 = 2.0, range21 = 2.0,
range32 = 2.0; range32 = 2.0;