diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index b67b1725e..10a1a2401 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -278,22 +278,6 @@ public: return Point2(P.x() / P.z(), P.y() / P.z()); } - /** - * projects a 3-dimensional point at infinity (direction-only) in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * TODO: Frank says: this function seems to be identical as the above - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P - */ - inline static Point2 projectPointAtInfinityToCamera(const Point3& P, - boost::optional Dpoint = boost::none) { - if (Dpoint) { - double d = 1.0 / P.z(), d2 = d * d; - *Dpoint = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); - } - return Point2(P.x() / P.z(), P.y() / P.z()); - } - /// Project a point into the image and check depth inline std::pair projectSafe(const Point3& pw) const { const Point3 pc = pose_.transform_to(pw); @@ -365,8 +349,8 @@ public: boost::optional Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = pose_.rotation().unrotate(pw); - const Point2 pn = projectPointAtInfinityToCamera(pc); + const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) + const Point2 pn = project_to_camera(pc); // project the point to the camera return K_.uncalibrate(pn); } @@ -379,7 +363,7 @@ public: // camera to normalized image coordinate Matrix Dpn_pc; // 2*3 - const Point2 pn = projectPointAtInfinityToCamera(pc, Dpn_pc); + const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration Matrix Dpi_pn; // 2*2 @@ -390,7 +374,7 @@ public: if (Dpose) *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 3, 2); + *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only) return pi; } @@ -434,7 +418,7 @@ public: /// backproject a 2-dimensional point to a 3-dimensional point at infinity inline Point3 backprojectPointAtInfinity(const Point2& p) const { const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); + const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 return pose_.rotation().rotate(pc); } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 3a8075c0c..49391ccc3 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -44,6 +44,11 @@ static const Point3 point2(-0.08, 0.08, 0.0); static const Point3 point3( 0.08, 0.08, 0.0); static const Point3 point4( 0.08,-0.08, 0.0); +static const Point3 point1_inf(-0.16,-0.16, -1.0); +static const Point3 point2_inf(-0.16, 0.16, -1.0); +static const Point3 point3_inf( 0.16, 0.16, -1.0); +static const Point3 point4_inf( 0.16,-0.16, -1.0); + /* ************************************************************************* */ TEST( PinholeCamera, constructor) { @@ -103,6 +108,15 @@ TEST( PinholeCamera, backproject) CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); } +/* ************************************************************************* */ +TEST( PinholeCamera, backprojectInfinity) +{ + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); +} + /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { @@ -119,11 +133,47 @@ TEST( PinholeCamera, backproject2) CHECK(x.second); } +/* ************************************************************************* */ +TEST( PinholeCamera, backprojectInfinity2) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backprojectPointAtInfinity(Point2()); + Point3 expected(0., 1., 0.); + Point2 x = camera.projectPointAtInfinity(expected); + + CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(Point2(), x)); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, backprojectInfinity3) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backprojectPointAtInfinity(Point2()); + Point3 expected(0., 0., 1.); + Point2 x = camera.projectPointAtInfinity(expected); + + CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(Point2(), x)); +} + /* ************************************************************************* */ static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& cal) { return Camera(pose,cal).project(point); } +/* ************************************************************************* */ +static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { + Point3 point(point2D.x(), point2D.y(), 1.0); + return Camera(pose,cal).projectPointAtInfinity(point); +} + /* ************************************************************************* */ TEST( PinholeCamera, Dproject_point_pose) { @@ -138,6 +188,21 @@ TEST( PinholeCamera, Dproject_point_pose) CHECK(assert_equal(Dcal, numerical_cal,1e-7)); } +/* ************************************************************************* */ +TEST( PinholeCamera, Dproject_point_pose_Infinity) +{ + Matrix Dpose, Dpoint, Dcal; + Point2 point2D(-0.08,-0.08); + Point3 point3D(point1.x(), point1.y(), 1.0); + Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); + Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K); + CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); + CHECK(assert_equal(Dpoint, numerical_point,1e-7)); + CHECK(assert_equal(Dcal, numerical_cal,1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 9ebec9c53..18535f7ec 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -26,7 +26,7 @@ #include #include #include -#include +//#include namespace gtsam { @@ -472,7 +472,8 @@ namespace gtsam { Pose3 pose = cameraPoses.at(i); PinholeCamera camera(pose, *K_); if(i==0){ // first pose - state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity + state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); + // 3D parametrization of point at infinity: [px py 1] // std::cout << "point_ " << state_->point<< std::endl; } Matrix Hxi, Hli; diff --git a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h index 5ab4984f5..60b080b93 100644 --- a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h +++ b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h @@ -71,8 +71,8 @@ namespace gtsam { if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end()); - views += poseKey; - measurements += measurement; + views.push_back(poseKey); + measurements.push_back(measurement); // This is a new landmark, create a new factor and add to mapping boost::shared_ptr smartFactorState(new SmartProjectionFactorState()); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 706586039..8a025e277 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -40,8 +40,6 @@ #include #include -#ifdef DEVELOP - #include using namespace std; @@ -62,12 +60,14 @@ using symbol_shorthand::L; typedef SmartProjectionFactor TestSmartProjectionFactor; +#ifdef DEVELOP + /* ************************************************************************* */ TEST( SmartProjectionFactor, Constructor) { Key poseKey(X(1)); std::vector views; - views += poseKey; + views.push_back(poseKey); std::vector measurements; measurements.push_back(Point2(323.0, 240.0)); @@ -80,7 +80,7 @@ TEST( SmartProjectionFactor, ConstructorWithTransform) { Key poseKey(X(1)); std::vector views; - views += poseKey; + views.push_back(poseKey); std::vector measurements; measurements.push_back(Point2(323.0, 240.0)); @@ -96,7 +96,7 @@ TEST( SmartProjectionFactor, Equals ) { measurements.push_back(Point2(323.0, 240.0)); std::vector views; - views += X(1); + views.push_back(X(1)); TestSmartProjectionFactor factor1(views, measurements, model, K); TestSmartProjectionFactor factor2(views, measurements, model, K); @@ -111,7 +111,7 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) { Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); std::vector views; - views += X(1); + views.push_back(X(1)); TestSmartProjectionFactor factor1(views, measurements, model, K, body_P_sensor); TestSmartProjectionFactor factor2(views, measurements, model, K, body_P_sensor); @@ -121,7 +121,7 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) { /* *************************************************************************/ TEST( SmartProjectionFactor, noisy ){ - cout << " ************************ SmartProjectionFactor: noisy ****************************" << endl; + // cout << " ************************ SmartProjectionFactor: noisy ****************************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -129,7 +129,8 @@ TEST( SmartProjectionFactor, noisy ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2; //, x3; + views.push_back(x1); + views.push_back(x2); Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); @@ -155,13 +156,14 @@ TEST( SmartProjectionFactor, noisy ){ values.insert(x2, level_pose_right.compose(noise_pose)); vector measurements; - measurements += level_uv, level_uv_right; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); SmartProjectionFactor::shared_ptr - smartFactor(new SmartProjectionFactor(views, measurements, noiseProjection, K)); + smartFactor(new SmartProjectionFactor(views, measurements, noiseProjection, K)); double actualError = smartFactor->error(values); - std::cout << "Error: " << actualError << std::endl; + // std::cout << "Error: " << actualError << std::endl; // we do not expect to be able to predict the error, since the error on the pixel will change // the triangulation of the landmark which is internal to the factor. @@ -179,7 +181,9 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2, x3; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); @@ -206,19 +210,19 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; // TODO: change to push_back // Point2 cam1_uv2 = cam1.project(landmark2); Point2 cam2_uv2 = cam2.project(landmark2); Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; + measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; TODO: change to push_back Point2 cam1_uv3 = cam1.project(landmark3); Point2 cam2_uv3 = cam2.project(landmark3); Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; + measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; TODO: change to push_back typedef SmartProjectionFactor SmartFactor; typedef GenericProjectionFactor ProjectionFactor; @@ -312,7 +316,7 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ /* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ - cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl; + // cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -321,7 +325,9 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2, x3; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); @@ -348,19 +354,28 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + // measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; // Point2 cam1_uv2 = cam1.project(landmark2); Point2 cam2_uv2 = cam2.project(landmark2); Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + // measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; Point2 cam1_uv3 = cam1.project(landmark3); Point2 cam2_uv3 = cam2.project(landmark3); Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; + measurements_cam3.push_back(cam1_uv3); + measurements_cam3.push_back(cam2_uv3); + measurements_cam3.push_back(cam3_uv3); + // measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; typedef SmartProjectionFactor SmartFactor; @@ -377,7 +392,7 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -406,7 +421,7 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ /* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){ - cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl; + // cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -415,7 +430,10 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2, x3; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + // views += x1, x2, x3; Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); @@ -442,19 +460,28 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){ Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + // measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; // Point2 cam1_uv2 = cam1.project(landmark2); Point2 cam2_uv2 = cam2.project(landmark2); Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + // measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; Point2 cam1_uv3 = cam1.project(landmark3); Point2 cam2_uv3 = cam2.project(landmark3); Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; + measurements_cam3.push_back(cam1_uv3); + measurements_cam3.push_back(cam2_uv3); + measurements_cam3.push_back(cam3_uv3); + // measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; typedef SmartProjectionFactor SmartFactor; @@ -482,7 +509,7 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -511,7 +538,7 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){ /* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_projection_factor ){ -// cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; + // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -520,7 +547,9 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2, x3; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) @@ -569,22 +598,22 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){ values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); -// values.at(x3).print("Pose3 before optimization: "); + // values.at(x3).print("Pose3 before optimization: "); LevenbergMarquardtParams params; -// params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; -// params.verbosity = NonlinearOptimizerParams::ERROR; + // params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + // params.verbosity = NonlinearOptimizerParams::ERROR; LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); -// result.at(x3).print("Pose3 after optimization: "); + // result.at(x3).print("Pose3 after optimization: "); EXPECT(assert_equal(pose3,result.at(x3))); } /* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor ){ - cout << " ************************ SmartProjectionFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; + // cout << " ************************ SmartProjectionFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -593,7 +622,9 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2, x3; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); @@ -620,13 +651,19 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + // measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; // Point2 cam1_uv2 = cam1.project(landmark2); Point2 cam2_uv2 = cam2.project(landmark2); Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + // measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; typedef SmartProjectionFactor SmartFactor; @@ -673,7 +710,7 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor /* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ - cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; + // cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -682,7 +719,10 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2, x3; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + // views += x1, x2, x3; Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); @@ -709,19 +749,26 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + // measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; - // Point2 cam1_uv2 = cam1.project(landmark2); Point2 cam2_uv2 = cam2.project(landmark2); Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; - + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + // measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; Point2 cam1_uv3 = cam1.project(landmark3); Point2 cam2_uv3 = cam2.project(landmark3); Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; + measurements_cam3.push_back(cam1_uv3); + measurements_cam3.push_back(cam2_uv3); + measurements_cam3.push_back(cam3_uv3); + // measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; typedef SmartProjectionFactor SmartFactor; @@ -743,7 +790,7 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -772,7 +819,7 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ /* *************************************************************************/ TEST( SmartProjectionFactor, Hessian ){ - cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl; + // cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -780,7 +827,9 @@ TEST( SmartProjectionFactor, Hessian ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2; + views.push_back(x1); + views.push_back(x2); + // views += x1, x2; Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) @@ -798,7 +847,9 @@ TEST( SmartProjectionFactor, Hessian ){ Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); vector measurements_cam1; - measurements_cam1 += cam1_uv1, cam2_uv1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + // measurements_cam1 += cam1_uv1, cam2_uv1; SmartProjectionFactor smartFactor(views, measurements_cam1, noiseProjection, K); @@ -821,158 +872,170 @@ TEST( SmartProjectionFactor, Hessian ){ /* *************************************************************************/ TEST( SmartProjectionFactor, HessianWithRotation ){ - cout << " ************************ SmartProjectionFactor: rotated Hessian **********************" << endl; + // cout << " ************************ SmartProjectionFactor: rotated Hessian **********************" << endl; - Symbol x1('X', 1); - Symbol x2('X', 2); - Symbol x3('X', 3); + Symbol x1('X', 1); + Symbol x2('X', 2); + Symbol x3('X', 3); - const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - std::vector views; - views += x1, x2, x3; + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + // views += x1, x2, x3; - Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); - Point3 landmark1(5, 0.5, 1.2); + Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1, measurements_cam2, measurements_cam3; + vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + // measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; - typedef SmartProjectionFactor SmartFactor; + typedef SmartProjectionFactor SmartFactor; - SmartFactor::shared_ptr smartFactor(new SmartFactor(noiseProjection, K)); - smartFactor->add(cam1_uv1, views[0]); - smartFactor->add(cam2_uv1, views[1]); - smartFactor->add(cam3_uv1, views[2]); + SmartFactor::shared_ptr smartFactor(new SmartFactor(noiseProjection, K)); + smartFactor->add(cam1_uv1, views[0]); + smartFactor->add(cam2_uv1, views[1]); + smartFactor->add(cam3_uv1, views[2]); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - // hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize(values); + // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); - Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - // hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); + // hessianFactorRot->print("Hessian factor \n"); - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); } /* *************************************************************************/ TEST( SmartProjectionFactor, HessianWithRotationDegenerate ){ - cout << " ************************ SmartProjectionFactor: rotated Hessian (degenerate) **********************" << endl; + // cout << " ************************ SmartProjectionFactor: rotated Hessian (degenerate) **********************" << endl; - Symbol x1('X', 1); - Symbol x2('X', 2); - Symbol x3('X', 3); + Symbol x1('X', 1); + Symbol x2('X', 2); + Symbol x3('X', 3); - const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - std::vector views; - views += x1, x2, x3; + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + // views += x1, x2, x3; - Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K); - Point3 landmark1(5, 0.5, 1.2); + Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1, measurements_cam2, measurements_cam3; + vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + // measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; - typedef SmartProjectionFactor SmartFactor; + typedef SmartProjectionFactor SmartFactor; - SmartFactor::shared_ptr smartFactor(new SmartFactor(noiseProjection, K)); - smartFactor->add(cam1_uv1, views[0]); - smartFactor->add(cam2_uv1, views[1]); - smartFactor->add(cam3_uv1, views[2]); + SmartFactor::shared_ptr smartFactor(new SmartFactor(noiseProjection, K)); + smartFactor->add(cam1_uv1, views[0]); + smartFactor->add(cam2_uv1, views[1]); + smartFactor->add(cam3_uv1, views[2]); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - // hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize(values); + // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); - Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - // hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); + // hessianFactorRot->print("Hessian factor \n"); - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); } #endif