From 94ccf989857964dc7e517b2cea100721b20110fb Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 11 Feb 2016 16:52:02 -0800 Subject: [PATCH] Avoid default constructor in tests --- gtsam/geometry/tests/testPinholeCamera.cpp | 8 ++++---- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 ++-- .../navigation/tests/testImuFactorSerialization.cpp | 2 +- gtsam/slam/tests/testInitializePose3.cpp | 12 ++++++------ gtsam/slam/tests/testSmartProjectionPoseFactor.cpp | 4 ++-- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 2 +- gtsam_unstable/geometry/tests/testEvent.cpp | 4 ++-- .../slam/tests/testInvDepthFactorVariant1.cpp | 6 +++--- .../slam/tests/testInvDepthFactorVariant2.cpp | 8 ++++---- tests/testNonlinearEquality.cpp | 2 +- 11 files changed, 27 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 4682a6507..4fbdbcbe1 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -112,7 +112,7 @@ TEST( PinholeCamera, lookat) EXPECT(assert_equal(camera.pose(), expected)); Point3 C2(30,0,10); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0,0,1)); + Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -149,7 +149,7 @@ TEST( PinholeCamera, backprojectInfinity) /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -165,7 +165,7 @@ TEST( PinholeCamera, backproject2) /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -180,7 +180,7 @@ TEST( PinholeCamera, backprojectInfinity2) /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity3) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 61550d540..af8e7c6a0 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -306,7 +306,7 @@ TEST( Pose3, Dtransform_from1_b) TEST( Pose3, Dtransform_from1_c) { - Point3 origin; + Point3 origin(0,0,0); Pose3 T0(R,origin); Matrix actualDtransform_from1; T0.transform_from(P, actualDtransform_from1, boost::none); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index f1d761cb0..28ad037c0 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -590,7 +590,7 @@ TEST(ImuFactor, PredictRotation) { // Predict NavState actual = pim.predict(NavState(), bias); - NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1); + NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0,0,0), Z_3x1); EXPECT(assert_equal(expected, actual)); } @@ -708,7 +708,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { auto p = testing::Params(); p->n_gravity = Vector3(0, 0, -kGravity); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index a7796d1b2..9f9781d2c 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -43,7 +43,7 @@ TEST(ImuFactor, serialization) { // Create default parameters with Z-down and above noise paramaters auto p = PreintegrationParams::MakeSharedD(9.81); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 9fd8474eb..e6f08286a 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -148,9 +148,9 @@ TEST( InitializePose3, iterationGradient ) { Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); - givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); - givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; @@ -189,9 +189,9 @@ TEST( InitializePose3, orientationsGradient ) { Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); - givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); - givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 1c1bc3c03..5430e1088 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -959,8 +959,8 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac views.push_back(x3); // Two different cameras, at the same position, but different rotations - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Camera cam2(pose2, sharedK2); Camera cam3(pose3, sharedK2); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index db2f8f7f8..855131042 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -29,7 +29,7 @@ TEST( testPoseRTV, constructors ) { EXPECT(assert_equal(Pose3(rot, pt), state1.pose())); PoseRTV state2; - EXPECT(assert_equal(Point3(), state2.t())); + EXPECT(assert_equal(Point3(0,0,0), state2.t())); EXPECT(assert_equal(Rot3(), state2.R())); EXPECT(assert_equal(kZero3, state2.v())); EXPECT(assert_equal(Pose3(), state2.pose())); diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 0842e2146..ec8ca1f4b 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0; +static const Point3 microphoneAt0(0,0,0); //***************************************************************************** TEST( Event, Constructor ) { diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 3aa758701..96043fb50 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -48,7 +48,7 @@ TEST( InvDepthFactorVariant1, optimize) { Vector6 expected((Vector(6) << x, y, z, theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -89,14 +89,14 @@ TEST( InvDepthFactorVariant1, optimize) { // cout << endl << endl; // Calculate world coordinates of landmark versions - Point3 world_landmarkBefore; + Point3 world_landmarkBefore(0,0,0); { Vector6 landmarkBefore = values.at(landmarkKey); double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2); double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5); world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - Point3 world_landmarkAfter; + Point3 world_landmarkAfter(0,0,0); { Vector6 landmarkAfter = result.at(landmarkKey); double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index d20fc000c..7ac0faa1e 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -46,7 +46,7 @@ TEST( InvDepthFactorVariant2, optimize) { Vector3 expected((Vector(3) << theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -72,7 +72,7 @@ TEST( InvDepthFactorVariant2, optimize) { LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Vector3 actual = result.at(landmarkKey); - + // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); @@ -87,13 +87,13 @@ TEST( InvDepthFactorVariant2, optimize) { // std::cout << std::endl << std::endl; // Calculate world coordinates of landmark versions - Point3 world_landmarkBefore; + Point3 world_landmarkBefore(0,0,0); { Vector3 landmarkBefore = values.at(landmarkKey); double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2); world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - Point3 world_landmarkAfter; + Point3 world_landmarkAfter(0,0,0); { Vector3 landmarkAfter = result.at(landmarkKey); double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index a3c99ece7..130dc1bc8 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -531,7 +531,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates Rot3 faceDownY( (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished()); - Pose3 pose1(faceDownY, Point3()); // origin, left camera + Pose3 pose1(faceDownY, Point3(0,0,0)); // origin, left camera SimpleCamera camera1(pose1, K); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left SimpleCamera camera2(pose2, K);