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

some tests pass again

release/4.3a0
cbeall3 2015-07-16 11:26:07 -04:00
parent 93f7eafaa8
commit 3f0e695cc9
1 changed files with 43 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -241,27 +241,28 @@ 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 { std::cout << "triangulateSafe i \n" << 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));
} }
Point3 point = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements, // Point3 point = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
params_.triangulation.rankTolerance, params_.triangulation.enableEPI); // params_.triangulation.rankTolerance, params_.triangulation.enableEPI);
result_ = gtsam::triangulateSafe(mono_cameras, mono_measurements,
params_.triangulation);
// // // End temporary hack // // // End temporary hack
@ -270,42 +271,31 @@ public:
// point_ = cameras[0].backproject(z0); // point_ = cameras[0].backproject(z0);
// Check landmark distance and reprojection errors to avoid outliers // Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0; // double totalReprojError = 0.0;
size_t i = 0; // size_t i = 0;
BOOST_FOREACH(const StereoCamera& camera, cameras) { // BOOST_FOREACH(const StereoCamera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation(); // Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away // // we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point) > params_.triangulation.landmarkDistanceThreshold) { // if (cameraTranslation.distance(*result_) > params_.triangulation.landmarkDistanceThreshold) {
return TriangulationResult::Degenerate(); // return TriangulationResult::Degenerate();
} // }
const StereoPoint2& zi = this->measured_.at(i); // const StereoPoint2& zi = this->measured_.at(i);
try { // try {
StereoPoint2 reprojectionError(camera.project(point) - zi); // StereoPoint2 reprojectionError(camera.project(*result_) - zi);
totalReprojError += reprojectionError.vector().norm(); // totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) { // } catch (CheiralityException) {
return TriangulationResult::BehindCamera(); // return TriangulationResult::BehindCamera();
} // }
i += 1; // i += 1;
} // }
//std::cout << "totalReprojError error: " << totalReprojError << std::endl; //std::cout << "totalReprojError error: " << totalReprojError << std::endl;
// we discard smart factors that have large reprojection error // we discard smart factors that have large reprojection error
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 // if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) // && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold)
return TriangulationResult::Degenerate(); // return TriangulationResult::Degenerate();
result_ = TriangulationResult(point); // result_ = TriangulationResult(point);
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
return TriangulationResult::Degenerate();
} catch (TriangulationCheiralityException&) {
// 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
return TriangulationResult::BehindCamera();
}
} }
return result_; return result_;
@ -545,6 +535,9 @@ public:
else else
result_ = triangulateSafe(cameras); result_ = triangulateSafe(cameras);
std::cout << "Triangulation result in totalReprojectionError" << std::endl;
std::cout << result_ << std::endl;
if (result_) if (result_)
// All good, just use version in base class // All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_); return Base::totalReprojectionError(cameras, *result_);
@ -555,9 +548,10 @@ public:
// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0); // Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
// //
// return Base::totalReprojectionError(cameras, backprojected); // return Base::totalReprojectionError(cameras, backprojected);
} else } else {
// if we don't want to manage the exceptions we discard the factor // if we don't want to manage the exceptions we discard the factor
return 0.0; return 0.0;
}
} }
/// Calculate total reprojection error /// Calculate total reprojection error