diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 8c546b9b0..d57d1ca25 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -234,67 +234,58 @@ public: TriangulationResult triangulateSafe(const Cameras& cameras) const { size_t m = cameras.size(); - if (m < 2) { // if we have a single pose the corresponding factor is uninformative - return TriangulationResult::Degenerate(); - } +// if (m < 2) { // if we have a single pose the corresponding factor is uninformative +// return TriangulationResult::Degenerate(); +// } bool retriangulate = decideIfTriangulate(cameras); if (retriangulate) { - // We triangulate the 3D position of the landmark - std::cout << "triangulateSafe i \n" << std::endl; - //TODO: Chris will replace this with something else for stereo -// point_ = triangulatePoint3(cameras, this->measured_, -// rankTolerance_, enableEPI_); - - // // // Temporary hack to use monocular triangulation - std::vector mono_measurements; - BOOST_FOREACH(const StereoPoint2& sp, this->measured_) { - mono_measurements.push_back(sp.point2()); + std::vector reprojections; + reprojections.reserve(m); + for(size_t i = 0; i < m; i++) { + reprojections.push_back(cameras[i].backproject(measured_[i])); } - std::vector > mono_cameras; - BOOST_FOREACH(const StereoCamera& camera, cameras) { - const Pose3& pose = camera.pose(); - const Cal3_S2& K = camera.calibration()->calibration(); - mono_cameras.push_back(PinholeCamera(pose, K)); + Point3 pw_sum; + BOOST_FOREACH(const Point3& pw, reprojections) { + pw_sum = pw_sum + pw; } -// Point3 point = triangulatePoint3 >(mono_cameras, mono_measurements, -// params_.triangulation.rankTolerance, params_.triangulation.enableEPI); - result_ = gtsam::triangulateSafe(mono_cameras, mono_measurements, - params_.triangulation); + // average reprojected landmark + Point3 pw_avg = pw_sum / double(m); - // // // End temporary hack + // check if it lies in front of all cameras + bool cheirality_ok = true; + double totalReprojError = 0; + for(size_t i = 0; i < m; i++) { + const Pose3& pose = cameras[i].pose(); + const Point3& pl = pose.transform_to(pw_avg); + if (pl.z() <= 0) { + cheirality_ok = false; + break; + } - // FIXME: temporary: triangulation using only first camera -// const StereoPoint2& z0 = this->measured_.at(0); -// point_ = cameras[0].backproject(z0); + // check landmark distance + if (params_.triangulation.landmarkDistanceThreshold > 0 && + pl.norm() > params_.triangulation.landmarkDistanceThreshold) { + return TriangulationResult::Degenerate(); + } - // Check landmark distance and reprojection errors to avoid outliers -// double totalReprojError = 0.0; -// size_t i = 0; -// BOOST_FOREACH(const StereoCamera& camera, cameras) { -// Point3 cameraTranslation = camera.pose().translation(); -// // we discard smart factors corresponding to points that are far away -// if (cameraTranslation.distance(*result_) > params_.triangulation.landmarkDistanceThreshold) { -// return TriangulationResult::Degenerate(); -// } -// const StereoPoint2& zi = this->measured_.at(i); -// try { -// StereoPoint2 reprojectionError(camera.project(*result_) - zi); -// totalReprojError += reprojectionError.vector().norm(); -// } catch (CheiralityException) { -// return TriangulationResult::BehindCamera(); -// } -// i += 1; -// } - //std::cout << "totalReprojError error: " << totalReprojError << std::endl; - // we discard smart factors that have large reprojection error -// if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 -// && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) -// return TriangulationResult::Degenerate(); + if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { + const StereoPoint2& zi = measured_[i]; + StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi); + totalReprojError += reprojectionError.vector().norm(); + } + } // for -// result_ = TriangulationResult(point); + if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); + + if(cheirality_ok == false) { + result_ = TriangulationResult::BehindCamera(); + } + result_ = TriangulationResult(pw_avg); } return result_; @@ -389,11 +380,11 @@ public: return boost::make_shared >(this->keys_); } - /// linearize to a Hessianfactor - virtual boost::shared_ptr > linearizeToHessian( - const Values& values, double lambda = 0.0) const { - return createHessianFactor(this->cameras(values), lambda); - } +// /// linearize to a Hessianfactor +// virtual boost::shared_ptr > linearizeToHessian( +// const Values& values, double lambda = 0.0) const { +// return createHessianFactor(this->cameras(values), lambda); +// } // /// linearize to an Implicit Schur factor // virtual boost::shared_ptr > linearizeToImplicit( @@ -420,8 +411,8 @@ public: return createHessianFactor(cameras, lambda); // case IMPLICIT_SCHUR: // return createRegularImplicitSchurFactor(cameras, lambda); -// case JACOBIAN_SVD: -// return createJacobianSVDFactor(cameras, lambda); + case JACOBIAN_SVD: + return createJacobianSVDFactor(cameras, lambda); // case JACOBIAN_Q: // return createJacobianQFactor(cameras, lambda); default: @@ -535,14 +526,11 @@ public: else result_ = triangulateSafe(cameras); - std::cout << "Triangulation result in totalReprojectionError" << std::endl; - std::cout << result_ << std::endl; - if (result_) // All good, just use version in base class return Base::totalReprojectionError(cameras, *result_); else if (params_.degeneracyMode == HANDLE_INFINITY) { - throw("Backproject at infinity"); + throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo.")); // // Otherwise, manage the exceptions with rotation-only factors // const StereoPoint2& z0 = this->measured_.at(0); // Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0); diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index a4a8c9269..f35c82539 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -124,7 +124,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); - + cout << "Test 122 STARTS HERE ----------------------------------------- 122 " << endl; // create second camera 1 meter to the right of first camera Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); @@ -277,7 +277,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1); + EXPECT_DOUBLES_EQUAL(991819.94, graph.error(values), 1); Values result; gttic_(SmartStereoProjectionPoseFactor); @@ -286,7 +286,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { gttoc_(SmartStereoProjectionPoseFactor); tictoc_finishedIteration_(); - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); VectorValues delta = GFG->optimize(); @@ -436,7 +436,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { - double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error vector views; @@ -474,7 +473,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { SmartStereoProjectionParams params; params.setLinearizationMode(JACOBIAN_SVD); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); @@ -668,11 +666,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - // Create smartfactors - double rankTol = 10; - SmartStereoProjectionParams params; - params.setRankTolerance(rankTol); + params.setRankTolerance(10); SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); @@ -979,6 +974,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { + vector views; views.push_back(x1); views.push_back(x2); @@ -1023,7 +1019,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), - hessianFactorRot->information(), 1e-8)); + hessianFactorRot->information(), 1e-7)); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); @@ -1039,7 +1035,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations and translations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), - hessianFactorRotTran->information(), 1e-8)); + hessianFactorRotTran->information(), 1e-6)); } /* ************************************************************************* */