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

Some refactoring and saving of computation under certain parameter combinations

release/4.3a0
dellaert 2015-03-15 07:42:01 -04:00
parent a3b6a47b2e
commit 32c6453ee6
3 changed files with 33 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -231,8 +231,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
SimpleCamera camera4(pose4, K4); SimpleCamera camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark) CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
;, CheiralityException);
cameras += camera4; cameras += camera4;
measurements += Point2(400, 400); measurements += Point2(400, 400);

1
gtsam/geometry/triangulation.cpp
View File

@ -52,7 +52,6 @@ Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
double error; double error;
Vector v; Vector v;
boost::tie(rank, error, v) = DLT(A, rank_tol); boost::tie(rank, error, v) = DLT(A, rank_tol);
// std::cout << "s " << s.transpose() << std:endl;
if (rank < 3) if (rank < 3)
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());

50
gtsam/geometry/triangulation.h
View File

@ -208,16 +208,16 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
projection_matrices.push_back( projection_matrices.push_back(
sharedCal->K() * (pose.inverse().matrix()).block<3, 4>(0, 0)); sharedCal->K() * (pose.inverse().matrix()).block<3, 4>(0, 0));
} }
// Triangulate linearly // Do DLT: throws TriangulationUnderconstrainedException if rank<3
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization // Then refine using non-linear optimization
if (optimize) if (optimize)
point = triangulateNonlinear<CALIBRATION> // point = triangulateNonlinear<CALIBRATION> //
(poses, sharedCal, measurements, point); (poses, sharedCal, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies infront of all cameras // verify that the triangulated point lies in front of all cameras
BOOST_FOREACH(const Pose3& pose, poses) { BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(point); const Point3& p_local = pose.transform_to(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
@ -258,6 +258,7 @@ Point3 triangulatePoint3(const std::vector<CAMERA>& cameras,
projection_matrices.push_back( projection_matrices.push_back(
camera.calibration().K() * (P_.block<3, 4>(0, 0))); camera.calibration().K() * (P_.block<3, 4>(0, 0)));
} }
// Do DLT: throws TriangulationUnderconstrainedException if rank<3
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization // The n refine using non-linear optimization
@ -265,7 +266,7 @@ Point3 triangulatePoint3(const std::vector<CAMERA>& cameras,
point = triangulateNonlinear<CAMERA>(cameras, measurements, point); point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies infront of all cameras // verify that the triangulated point lies in front of all cameras
BOOST_FOREACH(const CAMERA& camera, cameras) { BOOST_FOREACH(const CAMERA& camera, cameras) {
const Point3& p_local = camera.pose().transform_to(point); const Point3& p_local = camera.pose().transform_to(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
@ -307,14 +308,17 @@ struct TriangulationParameters {
/** /**
* Constructor * Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate * @param rankTol tolerance used to check if point triangulation is degenerate
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @param enableEPI if true refine triangulation with embedded LM iterations
* @param landmarkDistanceThreshold flag as degenerate if point further than this
* @param dynamicOutlierRejectionThreshold or if average error larger than this
*
*/ */
TriangulationParameters(const double _rankTolerance = 1.0, TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = 1e10, const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1) : double _dynamicOutlierRejectionThreshold = -1) :
rankTolerance(_rankTolerance), enableEPI(_enableEPI), landmarkDistanceThreshold( rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
_landmarkDistanceThreshold), dynamicOutlierRejectionThreshold( landmarkDistanceThreshold(_landmarkDistanceThreshold), //
_dynamicOutlierRejectionThreshold) { dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
} }
// stream to output // stream to output
@ -386,25 +390,31 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
Point3 point = triangulatePoint3<CAMERA>(cameras, measured, Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
params.rankTolerance, params.enableEPI); params.rankTolerance, params.enableEPI);
// Check landmark distance and reprojection errors to avoid outliers // Check landmark distance and re-projection errors to avoid outliers
size_t i = 0; size_t i = 0;
double totalReprojError = 0.0; double totalReprojError = 0.0;
BOOST_FOREACH(const CAMERA& camera, cameras) { BOOST_FOREACH(const CAMERA& camera, cameras) {
// we discard smart factors corresponding to points that are far away const Pose3& pose = camera.pose();
Point3 cameraTranslation = camera.pose().translation(); if (params.landmarkDistanceThreshold > 0
if (cameraTranslation.distance(point) > params.landmarkDistanceThreshold) && pose.translation().distance(point)
> params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate(); return TriangulationResult::Degenerate();
// Also flag if point is behind any of the cameras #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
try { // verify that the triangulated point lies in front of all cameras
// Only needed if this was not yet handled by exception
const Point3& p_local = pose.transform_to(point);
if (p_local.z() <= 0)
return TriangulationResult::BehindCamera();
#endif
// Check reprojection error
if (params.dynamicOutlierRejectionThreshold > 0) {
const Point2& zi = measured.at(i); const Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi); Point2 reprojectionError(camera.project(point) - zi);
totalReprojError += reprojectionError.vector().norm(); totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
return TriangulationResult::BehindCamera();
} }
i += 1; i += 1;
} }
// we discard smart factors that have large reprojection error // Flag as degenerate if average reprojection error is too large
if (params.dynamicOutlierRejectionThreshold > 0 if (params.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params.dynamicOutlierRejectionThreshold) && totalReprojError / m > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Degenerate(); return TriangulationResult::Degenerate();
@ -412,14 +422,12 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
// all good! // all good!
return TriangulationResult(point); return TriangulationResult(point);
} catch (TriangulationUnderconstrainedException&) { } catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be // This exception is thrown if
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before // 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) // 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(); return TriangulationResult::Degenerate();
} catch (TriangulationCheiralityException&) { } catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers // 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 TriangulationResult::BehindCamera();
} }
} }