Everything moved to triangulation, PinholeSet eviscerated.
dellaert
parent
262b42e829
commit
69e56cee1c
|
|
@ -13,8 +13,6 @@
|
|||
* @file PinholeSet.h
|
||||
* @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
* @author Zsolt Kira
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -38,50 +36,8 @@ private:
|
|||
|
||||
protected:
|
||||
|
||||
/// @name Triangulation parameters
|
||||
/// @{
|
||||
|
||||
const double rankTolerance_; ///< threshold to decide whether triangulation is result.degenerate
|
||||
const bool enableEPI_; ///< if set to true, will refine triangulation using LM
|
||||
|
||||
/**
|
||||
* if the landmark is triangulated at distance larger than this,
|
||||
* result is flagged as degenerate.
|
||||
*/
|
||||
const double landmarkDistanceThreshold_; //
|
||||
|
||||
/**
|
||||
* If this is nonnegative the we will check if the average reprojection error
|
||||
* is smaller than this threshold after triangulation, otherwise result is
|
||||
* flagged as degenerate.
|
||||
*/
|
||||
const double dynamicOutlierRejectionThreshold_;
|
||||
/// @}
|
||||
|
||||
public:
|
||||
|
||||
/// @name Triangulation result
|
||||
/// @{
|
||||
struct Result {
|
||||
Point3 point;
|
||||
bool degenerate;
|
||||
bool cheiralityException;
|
||||
};
|
||||
/// @}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @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
|
||||
*/
|
||||
PinholeSet(const double rankTol = 1.0, const bool enableEPI = false,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
rankTolerance_(rankTol), enableEPI_(enableEPI), landmarkDistanceThreshold_(
|
||||
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
|
||||
dynamicOutlierRejectionThreshold) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~PinholeSet() {
|
||||
}
|
||||
|
|
@ -92,8 +48,6 @@ public:
|
|||
/// print
|
||||
virtual void print(const std::string& s = "") const {
|
||||
Base::print(s);
|
||||
std::cout << s << "PinholeSet\n";
|
||||
std::cout << "rankTolerance = " << rankTolerance_ << std::endl;
|
||||
}
|
||||
|
||||
/// equals
|
||||
|
|
@ -104,64 +58,10 @@ public:
|
|||
/// @}
|
||||
|
||||
/// triangulateSafe
|
||||
Result triangulateSafe(const std::vector<typename Base::Z>& measured) const {
|
||||
|
||||
Result result;
|
||||
|
||||
size_t m = this->size();
|
||||
|
||||
// if we have a single pose the corresponding factor is uninformative
|
||||
if (m < 2) {
|
||||
result.degenerate = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
|
||||
result.point = triangulatePoint3<CAMERA>(*this, measured, rankTolerance_,
|
||||
enableEPI_);
|
||||
result.degenerate = false;
|
||||
result.cheiralityException = false;
|
||||
|
||||
// Check landmark distance and reprojection errors to avoid outliers
|
||||
double totalReprojError = 0.0;
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(const CAMERA& camera, *this) {
|
||||
Point3 cameraTranslation = camera.pose().translation();
|
||||
// we discard smart factors corresponding to points that are far away
|
||||
if (cameraTranslation.distance(result.point)
|
||||
> landmarkDistanceThreshold_) {
|
||||
result.degenerate = true;
|
||||
break;
|
||||
}
|
||||
const Point2& zi = measured.at(i);
|
||||
try {
|
||||
Point2 reprojectionError(camera.project(result.point) - zi);
|
||||
totalReprojError += reprojectionError.vector().norm();
|
||||
} catch (CheiralityException) {
|
||||
result.cheiralityException = true;
|
||||
}
|
||||
i += 1;
|
||||
}
|
||||
// we discard smart factors that have large reprojection error
|
||||
if (dynamicOutlierRejectionThreshold_ > 0
|
||||
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
|
||||
result.degenerate = true;
|
||||
|
||||
} 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 *this (or motion towards the landmark)
|
||||
// in the second case we want to use a rotation-only smart factor
|
||||
result.degenerate = true;
|
||||
result.cheiralityException = false;
|
||||
} catch (TriangulationCheiralityException&) {
|
||||
// point is behind one of the *this: can be the case of close-to-parallel *this or may depend on outliers
|
||||
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
|
||||
result.cheiralityException = true;
|
||||
}
|
||||
return result;
|
||||
TriangulationResult triangulateSafe(
|
||||
const std::vector<Point2>& measured,
|
||||
const TriangulationParameters& params) const {
|
||||
return gtsam::triangulateSafe(*this, measured, params);
|
||||
}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -127,7 +127,8 @@ TEST(PinholeSet, Pinhole) {
|
|||
EXPECT(assert_equal(expectedV, actualV));
|
||||
|
||||
// Instantiate triangulateSafe
|
||||
PinholeSet<Camera>::Result actual = set.triangulateSafe(z);
|
||||
TriangulationParameters params;
|
||||
TriangulationResult actual = set.triangulateSafe(z,params);
|
||||
CHECK(actual.degenerate);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -221,7 +221,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|||
BOOST_FOREACH(const Pose3& pose, poses) {
|
||||
const Point3& p_local = pose.transform_to(point);
|
||||
if (p_local.z() <= 0)
|
||||
throw(TriangulationCheiralityException());
|
||||
throw(TriangulationCheiralityException());
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -269,7 +269,7 @@ Point3 triangulatePoint3(const std::vector<CAMERA>& cameras,
|
|||
BOOST_FOREACH(const CAMERA& camera, cameras) {
|
||||
const Point3& p_local = camera.pose().transform_to(point);
|
||||
if (p_local.z() <= 0)
|
||||
throw(TriangulationCheiralityException());
|
||||
throw(TriangulationCheiralityException());
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -286,5 +286,107 @@ Point3 triangulatePoint3(
|
|||
(cameras, measurements, rank_tol, optimize);
|
||||
}
|
||||
|
||||
struct TriangulationParameters {
|
||||
|
||||
const double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
|
||||
const bool enableEPI; ///< if set to true, will refine triangulation using LM
|
||||
|
||||
/**
|
||||
* if the landmark is triangulated at distance larger than this,
|
||||
* result is flagged as degenerate.
|
||||
*/
|
||||
const double landmarkDistanceThreshold; //
|
||||
|
||||
/**
|
||||
* If this is nonnegative the we will check if the average reprojection error
|
||||
* is smaller than this threshold after triangulation, otherwise result is
|
||||
* flagged as degenerate.
|
||||
*/
|
||||
const double dynamicOutlierRejectionThreshold;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @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
|
||||
*/
|
||||
TriangulationParameters(const double _rankTolerance = 1.0,
|
||||
const bool _enableEPI = false, double _landmarkDistanceThreshold = 1e10,
|
||||
double _dynamicOutlierRejectionThreshold = -1) :
|
||||
rankTolerance(_rankTolerance), enableEPI(_enableEPI), landmarkDistanceThreshold(
|
||||
_landmarkDistanceThreshold), dynamicOutlierRejectionThreshold(
|
||||
_dynamicOutlierRejectionThreshold) {
|
||||
}
|
||||
};
|
||||
|
||||
struct TriangulationResult {
|
||||
Point3 point;
|
||||
bool degenerate;
|
||||
bool cheiralityException;
|
||||
};
|
||||
|
||||
/// triangulateSafe: extensive checking of the outcome
|
||||
template<class CAMERA>
|
||||
TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
|
||||
const std::vector<Point2>& measured,
|
||||
const TriangulationParameters& params) {
|
||||
|
||||
TriangulationResult result;
|
||||
|
||||
size_t m = cameras.size();
|
||||
|
||||
// if we have a single pose the corresponding factor is uninformative
|
||||
if (m < 2) {
|
||||
result.degenerate = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
|
||||
result.point = triangulatePoint3<CAMERA>(cameras, measured,
|
||||
params.rankTolerance, params.enableEPI);
|
||||
result.degenerate = false;
|
||||
result.cheiralityException = false;
|
||||
|
||||
// Check landmark distance and reprojection errors to avoid outliers
|
||||
double totalReprojError = 0.0;
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(const CAMERA& camera, cameras) {
|
||||
Point3 cameraTranslation = camera.pose().translation();
|
||||
// we discard smart factors corresponding to points that are far away
|
||||
if (cameraTranslation.distance(result.point)
|
||||
> params.landmarkDistanceThreshold) {
|
||||
result.degenerate = true;
|
||||
break;
|
||||
}
|
||||
const Point2& zi = measured.at(i);
|
||||
try {
|
||||
Point2 reprojectionError(camera.project(result.point) - zi);
|
||||
totalReprojError += reprojectionError.vector().norm();
|
||||
} catch (CheiralityException) {
|
||||
result.cheiralityException = true;
|
||||
}
|
||||
i += 1;
|
||||
}
|
||||
// we discard smart factors that have large reprojection error
|
||||
if (params.dynamicOutlierRejectionThreshold > 0
|
||||
&& totalReprojError / m > params.dynamicOutlierRejectionThreshold)
|
||||
result.degenerate = true;
|
||||
|
||||
} 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
|
||||
result.degenerate = true;
|
||||
result.cheiralityException = false;
|
||||
} 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
|
||||
result.cheiralityException = true;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue