Almost everything compiles and passes in windows
Richard Roberts
parent
7cdd8e19da
commit
c2c9c4a982
|
|
@ -53,7 +53,7 @@ int main(int argc, char** argv) {
|
|||
Rot2 bearing11 = Rot2::fromDegrees(45),
|
||||
bearing21 = Rot2::fromDegrees(90),
|
||||
bearing32 = Rot2::fromDegrees(90);
|
||||
double range11 = sqrt(4+4),
|
||||
double range11 = std::sqrt(4.0+4.0),
|
||||
range21 = 2.0,
|
||||
range32 = 2.0;
|
||||
|
||||
|
|
|
|||
|
|
@ -251,7 +251,7 @@ TEST( TestVector, axpy )
|
|||
/* ************************************************************************* */
|
||||
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);
|
||||
double tol = 1.;
|
||||
EXPECT(!equal_with_abs_tol(v1, v2, tol));
|
||||
|
|
|
|||
|
|
@ -81,7 +81,7 @@ namespace gtsam {
|
|||
bool equals(const Node& q, double tol) const {
|
||||
const Leaf* other = dynamic_cast<const Leaf*> (&q);
|
||||
if (!other) return false;
|
||||
return fabs(this->constant_ - other->constant_) < tol;
|
||||
return fabs(double(this->constant_ - other->constant_)) < tol;
|
||||
}
|
||||
|
||||
/** print */
|
||||
|
|
|
|||
|
|
@ -65,7 +65,7 @@ TEST( CalibratedCamera, level1)
|
|||
TEST( CalibratedCamera, level2)
|
||||
{
|
||||
// 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);
|
||||
|
||||
// expected
|
||||
|
|
|
|||
|
|
@ -34,11 +34,11 @@ using namespace tensors;
|
|||
/* ************************************************************************* */
|
||||
// Indices
|
||||
|
||||
Index<3, 'a'> a;
|
||||
Index<3, 'b'> b;
|
||||
tensors::Index<3, 'a'> a;
|
||||
tensors::Index<3, 'b'> b;
|
||||
|
||||
Index<4, 'A'> A;
|
||||
Index<4, 'B'> B;
|
||||
tensors::Index<4, 'A'> A;
|
||||
tensors::Index<4, 'B'> B;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Tensors, FundamentalMatrix)
|
||||
|
|
|
|||
|
|
@ -36,9 +36,9 @@ using namespace tensors;
|
|||
/* ************************************************************************* */
|
||||
// Indices
|
||||
|
||||
Index<3, 'a'> a, _a;
|
||||
Index<3, 'b'> b, _b;
|
||||
Index<3, 'c'> c, _c;
|
||||
tensors::Index<3, 'a'> a, _a;
|
||||
tensors::Index<3, 'b'> b, _b;
|
||||
tensors::Index<3, 'c'> c, _c;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Homography2, RealImages)
|
||||
|
|
@ -120,8 +120,8 @@ Homography2 patchH(const Pose3& tEc) {
|
|||
namespace gtsam {
|
||||
// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
|
||||
Vector toVector(const tensors::Tensor2<3, 3>& H) {
|
||||
Index<3, 'T'> _T; // covariant 2D template
|
||||
Index<3, 'C'> I; // contravariant 2D camera
|
||||
tensors::Index<3, 'T'> _T; // covariant 2D template
|
||||
tensors::Index<3, 'C'> I; // contravariant 2D camera
|
||||
return toVector(H(I,_T));
|
||||
}
|
||||
Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
|
||||
|
|
@ -134,8 +134,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
TEST( Homography2, patchH)
|
||||
{
|
||||
Index<3, 'T'> _T; // covariant 2D template
|
||||
Index<3, 'C'> I; // contravariant 2D camera
|
||||
tensors::Index<3, 'T'> _T; // covariant 2D template
|
||||
tensors::Index<3, 'C'> I; // contravariant 2D camera
|
||||
|
||||
// data[_T][I]
|
||||
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
|
||||
|
|
@ -160,8 +160,8 @@ TEST( Homography2, patchH)
|
|||
/* ************************************************************************* */
|
||||
TEST( Homography2, patchH2)
|
||||
{
|
||||
Index<3, 'T'> _T; // covariant 2D template
|
||||
Index<3, 'C'> I; // contravariant 2D camera
|
||||
tensors::Index<3, 'T'> _T; // covariant 2D template
|
||||
tensors::Index<3, 'C'> I; // contravariant 2D camera
|
||||
|
||||
// data[_T][I]
|
||||
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ TEST( Point2, arithmetic)
|
|||
/* ************************************************************************* */
|
||||
TEST( Point2, norm)
|
||||
{
|
||||
Point2 p0(cos(5), sin(5));
|
||||
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.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);
|
||||
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(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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -44,14 +44,14 @@ TEST(Pose2, constructors) {
|
|||
Pose2 pose(0,p);
|
||||
Pose2 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())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, manifold) {
|
||||
Pose2 t1(M_PI_2, Point2(1, 2));
|
||||
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Pose2 t1(M_PI/2.0, Point2(1, 2));
|
||||
Pose2 t2(M_PI/2.0+0.018, Point2(1.015, 2.01));
|
||||
Pose2 origin;
|
||||
Vector d12 = t1.localCoordinates(t2);
|
||||
EXPECT(assert_equal(t2, t1.retract(d12)));
|
||||
|
|
@ -63,11 +63,11 @@ TEST(Pose2, manifold) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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
|
||||
Pose2 expected(1.00811, 2.01528, 2.5608);
|
||||
#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
|
||||
Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
|
|
@ -75,7 +75,7 @@ TEST(Pose2, retract) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
|
|
@ -83,7 +83,7 @@ TEST(Pose2, expmap) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
|
|
@ -110,11 +110,11 @@ TEST(Pose2, expmap3) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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
|
||||
Pose2 expected(1.01491, 2.01013, 1.5888);
|
||||
//#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
|
||||
Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
|
|
@ -122,7 +122,7 @@ TEST(Pose2, expmap0) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
|
|
@ -130,7 +130,7 @@ TEST(Pose2, expmap0_full) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
|
|
@ -170,8 +170,8 @@ TEST(Pose3, expmap_c_full)
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, logmap) {
|
||||
Pose2 pose0(M_PI_2, Point2(1, 2));
|
||||
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Pose2 pose0(M_PI/2.0, Point2(1, 2));
|
||||
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
|
||||
#else
|
||||
|
|
@ -183,8 +183,8 @@ TEST(Pose2, logmap) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, logmap_full) {
|
||||
Pose2 pose0(M_PI_2, Point2(1, 2));
|
||||
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Pose2 pose0(M_PI/2.0, Point2(1, 2));
|
||||
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
|
||||
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
|
||||
Vector actual = logmap_default<Pose2>(pose0, pose);
|
||||
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 )
|
||||
{
|
||||
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)
|
||||
|
||||
// expected
|
||||
|
|
@ -226,7 +226,7 @@ Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
|
|||
|
||||
TEST (Pose2, transform_from)
|
||||
{
|
||||
Pose2 pose(1., 0., M_PI_2);
|
||||
Pose2 pose(1., 0., M_PI/2.0);
|
||||
Point2 pt(2., 1.);
|
||||
Matrix H1, H2;
|
||||
Point2 actual = pose.transform_from(pt, H1, H2);
|
||||
|
|
@ -326,7 +326,7 @@ TEST(Pose2, compose_c)
|
|||
TEST(Pose2, inverse )
|
||||
{
|
||||
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();
|
||||
EXPECT(assert_equal(identity,lTg.compose(gTl)));
|
||||
|
|
@ -362,7 +362,7 @@ Matrix matrix(const Pose2& gTl) {
|
|||
TEST( Pose2, matrix )
|
||||
{
|
||||
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);
|
||||
EXPECT(assert_equal(Matrix_(3,3,
|
||||
0.0, -1.0, 1.0,
|
||||
|
|
@ -393,7 +393,7 @@ TEST( Pose2, 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
|
||||
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
|
||||
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
|
||||
|
|
@ -407,11 +407,11 @@ TEST( Pose2, between )
|
|||
// ^
|
||||
//
|
||||
// *--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
|
||||
|
||||
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 actual2 = gT1.between(gT2,actualH1,actualH2);
|
||||
EXPECT(assert_equal(expected,actual1));
|
||||
|
|
@ -443,7 +443,7 @@ TEST( Pose2, between )
|
|||
// reverse situation for extra test
|
||||
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
|
||||
|
||||
Matrix actualH1,actualH2;
|
||||
|
|
@ -472,7 +472,7 @@ TEST(Pose2, members)
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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);
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -488,11 +488,11 @@ TEST( Pose2, bearing )
|
|||
EXPECT(assert_equal(Rot2(),x1.bearing(l1)));
|
||||
|
||||
// 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
|
||||
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
|
||||
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
|
||||
|
|
@ -502,7 +502,7 @@ TEST( Pose2, bearing )
|
|||
|
||||
// establish bearing is indeed 45 degrees even if rotated
|
||||
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
|
||||
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
|
||||
|
|
@ -518,7 +518,7 @@ Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
|
|||
|
||||
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;
|
||||
|
||||
|
|
@ -526,11 +526,11 @@ TEST( Pose2, bearing_pose )
|
|||
EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));
|
||||
|
||||
// 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
|
||||
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
|
||||
expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
|
||||
|
|
@ -540,7 +540,7 @@ TEST( Pose2, bearing_pose )
|
|||
|
||||
// establish bearing is indeed 45 degrees even if rotated
|
||||
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
|
||||
expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
|
||||
|
|
@ -561,11 +561,11 @@ TEST( Pose2, range )
|
|||
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
|
||||
|
||||
// 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
|
||||
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
|
||||
expectedH1 = numericalDerivative21(range_proxy, x2, l3);
|
||||
|
|
@ -590,7 +590,7 @@ LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
|
|||
}
|
||||
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;
|
||||
|
||||
|
|
@ -598,11 +598,11 @@ TEST( Pose2, range_pose )
|
|||
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
|
||||
|
||||
// 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
|
||||
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
|
||||
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
|
||||
|
|
@ -637,7 +637,7 @@ TEST(Pose2, align_1) {
|
|||
|
||||
TEST(Pose2, align_2) {
|
||||
Point2 t(20,10);
|
||||
Rot2 R = Rot2::fromAngle(M_PI_2);
|
||||
Rot2 R = Rot2::fromAngle(M_PI/2.0);
|
||||
Pose2 expected(R, t);
|
||||
|
||||
vector<Point2Pair> correspondences;
|
||||
|
|
|
|||
|
|
@ -535,7 +535,7 @@ TEST( Pose3, between )
|
|||
/* ************************************************************************* */
|
||||
// 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);
|
||||
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
|
||||
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)),
|
||||
|
|
@ -554,11 +554,11 @@ TEST( Pose3, range )
|
|||
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
|
||||
|
||||
// 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
|
||||
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
|
||||
expectedH1 = numericalDerivative21(range_proxy, x2, l3);
|
||||
|
|
@ -589,11 +589,11 @@ TEST( Pose3, range_pose )
|
|||
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
|
||||
|
||||
// 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
|
||||
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
|
||||
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
|
||||
|
|
@ -619,7 +619,7 @@ TEST( Pose3, unicycle )
|
|||
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), 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ TEST( Rot2, constructors_and_angle)
|
|||
TEST( Rot2, 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)
|
||||
{
|
||||
Rot2 rot0(Rot2::fromAngle(M_PI_2));
|
||||
Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
|
||||
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);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
|
@ -146,7 +146,7 @@ TEST( Rot2, relativeBearing )
|
|||
|
||||
// establish relativeBearing is indeed 45 degrees
|
||||
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
|
||||
expectedH = numericalDerivative11(relativeBearing_, l2);
|
||||
|
|
|
|||
|
|
@ -131,7 +131,7 @@ TEST( Rot3, rodriguez3)
|
|||
TEST( Rot3, rodriguez4)
|
||||
{
|
||||
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);
|
||||
double c=cos(angle),s=sin(angle);
|
||||
Rot3 expected(c,-s, 0,
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ TEST( SimpleCamera, constructor)
|
|||
TEST( SimpleCamera, level2)
|
||||
{
|
||||
// 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);
|
||||
|
||||
// expected
|
||||
|
|
|
|||
|
|
@ -34,12 +34,12 @@ using namespace tensors;
|
|||
/* ************************************************************************* */
|
||||
// Indices
|
||||
|
||||
Index<3, 'a'> a, _a;
|
||||
Index<3, 'b'> b, _b;
|
||||
Index<3, 'c'> c, _c;
|
||||
tensors::Index<3, 'a'> a, _a;
|
||||
tensors::Index<3, 'b'> b, _b;
|
||||
tensors::Index<3, 'c'> c, _c;
|
||||
|
||||
Index<4, 'A'> A;
|
||||
Index<4, 'B'> B;
|
||||
tensors::Index<4, 'A'> A;
|
||||
tensors::Index<4, 'B'> B;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Tensor1
|
||||
|
|
@ -58,7 +58,7 @@ TEST(Tensor1, Basics)
|
|||
CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent
|
||||
|
||||
// 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)
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -196,8 +196,8 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
|
|||
PoseVertex v2 = key2vertex.find(key2)->second;
|
||||
|
||||
POSE l1Xl2 = factor->measured();
|
||||
tie(edge12, found1) = boost::edge(v1, v2, g);
|
||||
tie(edge21, found2) = boost::edge(v2, v1, g);
|
||||
boost::tie(edge12, found1) = boost::edge(v1, v2, g);
|
||||
boost::tie(edge21, found2) = boost::edge(v2, v1, g);
|
||||
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
|
||||
if (!found1 && !found2) continue;
|
||||
if (found1)
|
||||
|
|
|
|||
|
|
@ -144,8 +144,8 @@ vector<GeneralCamera> genCameraVariableCalibration() {
|
|||
return X ;
|
||||
}
|
||||
|
||||
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
|
||||
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 < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ;
|
||||
return ordering ;
|
||||
|
|
|
|||
|
|
@ -146,8 +146,8 @@ vector<GeneralCamera> genCameraVariableCalibration() {
|
|||
return X ;
|
||||
}
|
||||
|
||||
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
|
||||
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 < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ;
|
||||
return ordering ;
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ using namespace gtsam;
|
|||
using namespace planarSLAM;
|
||||
|
||||
// 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);
|
||||
|
||||
SharedNoiseModel
|
||||
|
|
@ -47,7 +47,7 @@ TEST( planarSLAM, PriorFactor_equals )
|
|||
TEST( planarSLAM, BearingFactor )
|
||||
{
|
||||
// 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);
|
||||
|
||||
// create config
|
||||
|
|
@ -75,7 +75,7 @@ TEST( planarSLAM, BearingFactor_equals )
|
|||
TEST( planarSLAM, RangeFactor )
|
||||
{
|
||||
// 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);
|
||||
|
||||
// create config
|
||||
|
|
@ -101,8 +101,8 @@ TEST( planarSLAM, RangeFactor_equals )
|
|||
TEST( planarSLAM, BearingRangeFactor )
|
||||
{
|
||||
// Create factor
|
||||
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
|
||||
double b(sqrt(2) - 0.22); // h(x) - z = 0.22
|
||||
Rot2 r = Rot2::fromAngle(M_PI/4.0 + 0.1); // h(x) - z = -0.1
|
||||
double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22
|
||||
planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2);
|
||||
|
||||
// create config
|
||||
|
|
@ -158,14 +158,14 @@ TEST( planarSLAM, constructor )
|
|||
G.addPoseConstraint(2, x2); // make it feasible :-)
|
||||
|
||||
// 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
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
Vector expected0 = Vector_(3, 0.0, 0.0, 0.0);
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ TEST( Pose2SLAM, constraint1 )
|
|||
/* ************************************************************************* */
|
||||
// Test constraint, large displacement
|
||||
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);
|
||||
return constraint.evaluateError(pose1, pose2);
|
||||
}
|
||||
|
|
@ -80,7 +80,7 @@ Vector f2(const Pose2& pose1, const Pose2& pose2) {
|
|||
TEST( Pose2SLAM, constraint2 )
|
||||
{
|
||||
// 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);
|
||||
Matrix H1, H2;
|
||||
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
|
||||
|
|
@ -96,7 +96,7 @@ TEST( Pose2SLAM, constraint2 )
|
|||
TEST( Pose2SLAM, constructor )
|
||||
{
|
||||
// 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;
|
||||
graph.addOdometry(1,2,measured, covariance);
|
||||
// get the size of the graph
|
||||
|
|
@ -110,13 +110,13 @@ TEST( Pose2SLAM, constructor )
|
|||
TEST( Pose2SLAM, linearization )
|
||||
{
|
||||
// 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);
|
||||
pose2SLAM::Graph graph;
|
||||
graph.addOdometry(1,2,measured, covariance);
|
||||
|
||||
// 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)
|
||||
pose2SLAM::Values config;
|
||||
config.insert(pose2SLAM::PoseKey(1),p1);
|
||||
|
|
@ -151,7 +151,7 @@ TEST(Pose2SLAM, optimize) {
|
|||
// create a Pose graph with one equality constraint and one measurement
|
||||
pose2SLAM::Graph fg;
|
||||
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
|
||||
Values initial;
|
||||
|
|
@ -170,7 +170,7 @@ TEST(Pose2SLAM, optimize) {
|
|||
// Check with expected config
|
||||
Values expected;
|
||||
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 marginals
|
||||
|
|
@ -348,9 +348,9 @@ TEST(Pose2Values, pose2Circle )
|
|||
{
|
||||
// expected is 4 poses tangent to circle with radius 1m
|
||||
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(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 ));
|
||||
|
||||
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
|
||||
|
|
@ -362,9 +362,9 @@ TEST(Pose2SLAM, expmap )
|
|||
{
|
||||
// expected is circle shifted to right
|
||||
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(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 ));
|
||||
|
||||
// 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
|
||||
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);
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -484,14 +484,14 @@ TEST( Pose2Factor, error )
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2Factor, rhs )
|
||||
{
|
||||
// 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)
|
||||
pose2SLAM::Values x0;
|
||||
x0.insert(pose2SLAM::PoseKey(1),p1);
|
||||
|
|
@ -504,7 +504,7 @@ TEST( Pose2Factor, rhs )
|
|||
|
||||
// Check RHS
|
||||
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);
|
||||
CHECK(assert_equal(expected_b,-factor.whitenedError(x0)));
|
||||
CHECK(assert_equal(expected_b,linear->getb()));
|
||||
|
|
@ -521,7 +521,7 @@ LieVector h(const Pose2& p1,const Pose2& p2) {
|
|||
TEST( Pose2Factor, linearize )
|
||||
{
|
||||
// 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);
|
||||
pose2SLAM::Values x0;
|
||||
x0.insert(pose2SLAM::PoseKey(1),p1);
|
||||
|
|
|
|||
|
|
@ -31,7 +31,6 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using boost::shared_ptr;
|
||||
|
||||
/* ************************************************************************* */
|
||||
double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
|
||||
|
|
@ -381,7 +380,7 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
|
|||
/* ************************************************************************* */
|
||||
TEST(DoglegOptimizer, Iterate) {
|
||||
// really non-linear factor graph
|
||||
shared_ptr<example::Graph> fg(new example::Graph(
|
||||
boost::shared_ptr<example::Graph> fg(new example::Graph(
|
||||
example::createReallyNonlinearFactorGraph()));
|
||||
|
||||
// config far from minimum
|
||||
|
|
@ -390,7 +389,7 @@ TEST(DoglegOptimizer, Iterate) {
|
|||
config->insert(simulated2D::PoseKey(1), x0);
|
||||
|
||||
// ordering
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
boost::shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back(simulated2D::PoseKey(1));
|
||||
|
||||
double Delta = 1.0;
|
||||
|
|
|
|||
|
|
@ -263,7 +263,7 @@ TEST( GaussianFactor, matrix )
|
|||
EQUALITY(b_act2,b2);
|
||||
|
||||
// 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);
|
||||
EQUALITY(A_act1, A_act2);
|
||||
EQUALITY(b_act1, b_act2);
|
||||
|
|
@ -307,7 +307,7 @@ TEST( GaussianFactor, matrix_aug )
|
|||
EQUALITY(Ab_act2,Ab2);
|
||||
|
||||
// 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);
|
||||
EQUALITY(Ab_act1, Ab_act2);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -232,7 +232,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
|
|||
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
|
||||
|
||||
// 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;
|
||||
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);
|
||||
|
|
@ -295,7 +295,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
|
|||
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first;
|
||||
|
||||
// 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;
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -70,7 +70,7 @@ TEST( Graph, predecessorMap2Graph )
|
|||
p_map.insert(kx(1), kx(2));
|
||||
p_map.insert(kx(2), 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));
|
||||
CHECK(root == key2vertex[kx(2)]);
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ TEST( inference, marginals )
|
|||
*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate();
|
||||
|
||||
// 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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -71,7 +71,7 @@ TEST(Marginals, planarSLAMmarginals) {
|
|||
Rot2 bearing11 = Rot2::fromDegrees(45),
|
||||
bearing21 = Rot2::fromDegrees(90),
|
||||
bearing32 = Rot2::fromDegrees(90);
|
||||
double range11 = sqrt(4+4),
|
||||
double range11 = sqrt(4.0+4.0),
|
||||
range21 = 2.0,
|
||||
range32 = 2.0;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue