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

re-enable triangulation hack

release/4.3a0
cbeall3 2015-07-15 23:16:45 -04:00
parent bd4dd84933
commit 93f7eafaa8
1 changed files with 67 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -241,76 +241,74 @@ public:
bool retriangulate = decideIfTriangulate(cameras); bool retriangulate = decideIfTriangulate(cameras);
if (retriangulate) { if (retriangulate) {
// We triangulate the 3D position of the landmark // We triangulate the 3D position of the landmark
// try { try {
// // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
//
// //TODO: Chris will replace this with something else for stereo //TODO: Chris will replace this with something else for stereo
//// point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_, // point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
//// rankTolerance_, enableEPI_); // rankTolerance_, enableEPI_);
//
// // // // Temporary hack to use monocular triangulation // // // Temporary hack to use monocular triangulation
// std::vector<Point2> mono_measurements; std::vector<Point2> mono_measurements;
// BOOST_FOREACH(const StereoPoint2& sp, this->measured_) { BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
// mono_measurements.push_back(sp.point2()); mono_measurements.push_back(sp.point2());
// } }
//
// std::vector<PinholeCamera<Cal3_S2> > mono_cameras; std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
// BOOST_FOREACH(const StereoCamera& camera, cameras) { BOOST_FOREACH(const StereoCamera& camera, cameras) {
// const Pose3& pose = camera.pose(); const Pose3& pose = camera.pose();
// const Cal3_S2& K = camera.calibration()->calibration(); const Cal3_S2& K = camera.calibration()->calibration();
// mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K)); mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
// } }
// point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements, Point3 point = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
// parameters_.rankTolerance, parameters_.enableEPI); params_.triangulation.rankTolerance, params_.triangulation.enableEPI);
//
// // // // End temporary hack // // // End temporary hack
//
// // FIXME: temporary: triangulation using only first camera // FIXME: temporary: triangulation using only first camera
//// const StereoPoint2& z0 = this->measured_.at(0); // const StereoPoint2& z0 = this->measured_.at(0);
//// point_ = cameras[0].backproject(z0); // point_ = cameras[0].backproject(z0);
//
// degenerate_ = false; // Check landmark distance and reprojection errors to avoid outliers
// cheiralityException_ = false; double totalReprojError = 0.0;
// size_t i = 0;
// // Check landmark distance and reprojection errors to avoid outliers BOOST_FOREACH(const StereoCamera& camera, cameras) {
// double totalReprojError = 0.0; Point3 cameraTranslation = camera.pose().translation();
// size_t i = 0; // we discard smart factors corresponding to points that are far away
// BOOST_FOREACH(const StereoCamera& camera, cameras) { if (cameraTranslation.distance(point) > params_.triangulation.landmarkDistanceThreshold) {
// Point3 cameraTranslation = camera.pose().translation(); return TriangulationResult::Degenerate();
// // we discard smart factors corresponding to points that are far away }
// if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) { const StereoPoint2& zi = this->measured_.at(i);
// degenerate_ = true; try {
// break; StereoPoint2 reprojectionError(camera.project(point) - zi);
// } totalReprojError += reprojectionError.vector().norm();
// const StereoPoint2& zi = this->measured_.at(i); } catch (CheiralityException) {
// try { return TriangulationResult::BehindCamera();
// StereoPoint2 reprojectionError(camera.project(point_) - zi); }
// totalReprojError += reprojectionError.vector().norm(); i += 1;
// } catch (CheiralityException) { }
// cheiralityException_ = true; //std::cout << "totalReprojError error: " << totalReprojError << std::endl;
// } // we discard smart factors that have large reprojection error
// i += 1; if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
// } && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold)
// //std::cout << "totalReprojError error: " << totalReprojError << std::endl; return TriangulationResult::Degenerate();
// // we discard smart factors that have large reprojection error
// if (parameters_.dynamicOutlierRejectionThreshold > 0 result_ = TriangulationResult(point);
// && totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
// degenerate_ = true; } catch (TriangulationUnderconstrainedException&) {
// // if TriangulationUnderconstrainedException can be
// } catch (TriangulationUnderconstrainedException&) { // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// // if TriangulationUnderconstrainedException can be // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before // in the second case we want to use a rotation-only smart factor
// // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) return TriangulationResult::Degenerate();
// // in the second case we want to use a rotation-only smart factor } catch (TriangulationCheiralityException&) {
// degenerate_ = true; // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// cheiralityException_ = false; // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
// } catch (TriangulationCheiralityException&) { return TriangulationResult::BehindCamera();
// // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers }
// // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
// cheiralityException_ = true;
// }
} }
return TriangulationResult(Point3()); return result_;
} }
/// triangulate /// triangulate