huge refactor. Compiles again, but triangulation still broken, SmartStereo test fails
cbeall3
parent
9c2ab0ce3b
commit
bd4dd84933
|
|
@ -38,51 +38,114 @@ namespace gtsam {
|
|||
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
|
||||
};
|
||||
|
||||
/// How to manage degeneracy
|
||||
enum DegeneracyMode {
|
||||
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
|
||||
};
|
||||
|
||||
/*
|
||||
* Parameters for the smart stereo projection factors
|
||||
*/
|
||||
class GTSAM_EXPORT SmartStereoProjectionParams {
|
||||
|
||||
public:
|
||||
|
||||
LinearizationMode linearizationMode; ///< How to linearize the factor
|
||||
DegeneracyMode degeneracyMode; ///< How to linearize the factor
|
||||
|
||||
/// @name Parameters governing the triangulation
|
||||
/// @{
|
||||
mutable TriangulationParameters triangulation;
|
||||
const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
|
||||
/// @}
|
||||
|
||||
/// @name Parameters governing how triangulation result is treated
|
||||
/// @{
|
||||
const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
|
||||
const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
|
||||
/// @}
|
||||
|
||||
|
||||
/// Constructor
|
||||
SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN,
|
||||
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
|
||||
bool verboseCheirality = false) :
|
||||
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
|
||||
1e-5), throwCheirality(throwCheirality), verboseCheirality(
|
||||
verboseCheirality) {
|
||||
}
|
||||
|
||||
virtual ~SmartStereoProjectionParams() {
|
||||
}
|
||||
|
||||
void print(const std::string& str) const {
|
||||
std::cout << "linearizationMode: " << linearizationMode << "\n";
|
||||
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
|
||||
std::cout << triangulation << std::endl;
|
||||
}
|
||||
|
||||
LinearizationMode getLinearizationMode() const {
|
||||
return linearizationMode;
|
||||
}
|
||||
DegeneracyMode getDegeneracyMode() const {
|
||||
return degeneracyMode;
|
||||
}
|
||||
TriangulationParameters getTriangulationParameters() const {
|
||||
return triangulation;
|
||||
}
|
||||
bool getVerboseCheirality() const {
|
||||
return verboseCheirality;
|
||||
}
|
||||
bool getThrowCheirality() const {
|
||||
return throwCheirality;
|
||||
}
|
||||
void setLinearizationMode(LinearizationMode linMode) {
|
||||
linearizationMode = linMode;
|
||||
}
|
||||
void setDegeneracyMode(DegeneracyMode degMode) {
|
||||
degeneracyMode = degMode;
|
||||
}
|
||||
void setRankTolerance(double rankTol) {
|
||||
triangulation.rankTolerance = rankTol;
|
||||
}
|
||||
void setEnableEPI(bool enableEPI) {
|
||||
triangulation.enableEPI = enableEPI;
|
||||
}
|
||||
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
|
||||
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
|
||||
}
|
||||
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
|
||||
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* SmartStereoProjectionFactor: triangulates point
|
||||
*/
|
||||
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
|
||||
* This factor operates with StereoCamrea. This factor requires that values
|
||||
* contains the involved camera poses. Calibration is assumed to be fixed.
|
||||
*/
|
||||
template<class CALIBRATION>
|
||||
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
|
||||
private:
|
||||
|
||||
typedef SmartFactorBase<StereoCamera> Base;
|
||||
typedef SmartStereoProjectionFactor<CALIBRATION> This;
|
||||
|
||||
protected:
|
||||
|
||||
/// @name Parameters
|
||||
/// @{
|
||||
const SmartStereoProjectionParams params_;
|
||||
/// @}
|
||||
|
||||
/// @name Caching triangulation
|
||||
/// @{
|
||||
const TriangulationParameters parameters_;
|
||||
// TODO:
|
||||
// mutable TriangulationResult result_; ///< result from triangulateSafe
|
||||
|
||||
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
|
||||
mutable TriangulationResult result_; ///< result from triangulateSafe
|
||||
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
|
||||
/// @}
|
||||
|
||||
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
|
||||
|
||||
const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
|
||||
mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
|
||||
|
||||
mutable Point3 point_; ///< Current estimate of the 3D point
|
||||
|
||||
mutable bool degenerate_;
|
||||
mutable bool cheiralityException_;
|
||||
|
||||
|
||||
/// shorthand for base class type
|
||||
typedef SmartFactorBase<StereoCamera> Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef SmartStereoProjectionFactor<CALIBRATION> This;
|
||||
|
||||
enum {
|
||||
ZDim = 3
|
||||
}; ///< Dimension trait of measurement type
|
||||
|
||||
/// @name Parameters governing how triangulation result is treated
|
||||
/// @{
|
||||
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
|
||||
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
|
||||
/// @}
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
|
|
@ -93,22 +156,12 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param rankTol tolerance used to check if point triangulation is degenerate
|
||||
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
|
||||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
SmartStereoProjectionFactor(const double rankTolerance,
|
||||
const double linThreshold, const bool manageDegeneracy,
|
||||
const bool enableEPI, double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
|
||||
dynamicOutlierRejectionThreshold), //
|
||||
retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_(
|
||||
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
|
||||
false), verboseCheirality_(false) {
|
||||
SmartStereoProjectionFactor(const SmartStereoProjectionParams& params =
|
||||
SmartStereoProjectionParams()) :
|
||||
params_(params), //
|
||||
result_(TriangulationResult::Degenerate()) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
|
|
@ -123,14 +176,19 @@ public:
|
|||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
std::cout << s << "SmartStereoProjectionFactor\n";
|
||||
std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
|
||||
std::cout << "degenerate_ = " << degenerate_ << std::endl;
|
||||
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
|
||||
std::cout << "linearizationThreshold_ = " << linearizationThreshold_
|
||||
<< std::endl;
|
||||
std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
|
||||
std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
|
||||
std::cout << "result:\n" << result_ << std::endl;
|
||||
Base::print("", keyFormatter);
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
return e && params_.linearizationMode == e->params_.linearizationMode
|
||||
&& Base::equals(p, tol);
|
||||
}
|
||||
|
||||
/// Check if the new linearization point_ is the same as the one used for previous triangulation
|
||||
bool decideIfTriangulate(const Cameras& cameras) const {
|
||||
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
|
||||
|
|
@ -149,7 +207,7 @@ public:
|
|||
if (!retriangulate) {
|
||||
for (size_t i = 0; i < cameras.size(); i++) {
|
||||
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
|
||||
retriangulationThreshold_)) {
|
||||
params_.retriangulationThreshold)) {
|
||||
retriangulate = true; // at least two poses are different, hence we retriangulate
|
||||
break;
|
||||
}
|
||||
|
|
@ -167,124 +225,105 @@ public:
|
|||
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
|
||||
}
|
||||
|
||||
/// triangulateSafe
|
||||
size_t triangulateSafe(const Values& values) const {
|
||||
return triangulateSafe(this->cameras(values));
|
||||
}
|
||||
// /// triangulateSafe
|
||||
// size_t triangulateSafe(const Values& values) const {
|
||||
// return triangulateSafe(this->cameras(values));
|
||||
// }
|
||||
|
||||
/// triangulateSafe
|
||||
size_t triangulateSafe(const Cameras& cameras) const {
|
||||
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
|
||||
degenerate_ = true;
|
||||
return m;
|
||||
return TriangulationResult::Degenerate();
|
||||
}
|
||||
bool retriangulate = decideIfTriangulate(cameras);
|
||||
|
||||
bool retriangulate = decideIfTriangulate(cameras);
|
||||
if (retriangulate) {
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
|
||||
|
||||
//TODO: Chris will replace this with something else for stereo
|
||||
// point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
|
||||
// rankTolerance_, enableEPI_);
|
||||
|
||||
// // // Temporary hack to use monocular triangulation
|
||||
std::vector<Point2> mono_measurements;
|
||||
BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
|
||||
mono_measurements.push_back(sp.point2());
|
||||
}
|
||||
|
||||
std::vector<PinholeCamera<Cal3_S2> > 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<Cal3_S2>(pose, K));
|
||||
}
|
||||
point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
|
||||
parameters_.rankTolerance, parameters_.enableEPI);
|
||||
|
||||
// // // End temporary hack
|
||||
|
||||
// FIXME: temporary: triangulation using only first camera
|
||||
// const StereoPoint2& z0 = this->measured_.at(0);
|
||||
// point_ = cameras[0].backproject(z0);
|
||||
|
||||
degenerate_ = false;
|
||||
cheiralityException_ = false;
|
||||
|
||||
// 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(point_) > parameters_.landmarkDistanceThreshold) {
|
||||
degenerate_ = true;
|
||||
break;
|
||||
}
|
||||
const StereoPoint2& zi = this->measured_.at(i);
|
||||
try {
|
||||
StereoPoint2 reprojectionError(camera.project(point_) - zi);
|
||||
totalReprojError += reprojectionError.vector().norm();
|
||||
} catch (CheiralityException) {
|
||||
cheiralityException_ = true;
|
||||
}
|
||||
i += 1;
|
||||
}
|
||||
//std::cout << "totalReprojError error: " << totalReprojError << std::endl;
|
||||
// we discard smart factors that have large reprojection error
|
||||
if (parameters_.dynamicOutlierRejectionThreshold > 0
|
||||
&& totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
|
||||
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
|
||||
degenerate_ = true;
|
||||
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
|
||||
cheiralityException_ = true;
|
||||
}
|
||||
// try {
|
||||
// // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
|
||||
//
|
||||
// //TODO: Chris will replace this with something else for stereo
|
||||
//// point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
|
||||
//// rankTolerance_, enableEPI_);
|
||||
//
|
||||
// // // // Temporary hack to use monocular triangulation
|
||||
// std::vector<Point2> mono_measurements;
|
||||
// BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
|
||||
// mono_measurements.push_back(sp.point2());
|
||||
// }
|
||||
//
|
||||
// std::vector<PinholeCamera<Cal3_S2> > 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<Cal3_S2>(pose, K));
|
||||
// }
|
||||
// point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
|
||||
// parameters_.rankTolerance, parameters_.enableEPI);
|
||||
//
|
||||
// // // // End temporary hack
|
||||
//
|
||||
// // FIXME: temporary: triangulation using only first camera
|
||||
//// const StereoPoint2& z0 = this->measured_.at(0);
|
||||
//// point_ = cameras[0].backproject(z0);
|
||||
//
|
||||
// degenerate_ = false;
|
||||
// cheiralityException_ = false;
|
||||
//
|
||||
// // 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(point_) > parameters_.landmarkDistanceThreshold) {
|
||||
// degenerate_ = true;
|
||||
// break;
|
||||
// }
|
||||
// const StereoPoint2& zi = this->measured_.at(i);
|
||||
// try {
|
||||
// StereoPoint2 reprojectionError(camera.project(point_) - zi);
|
||||
// totalReprojError += reprojectionError.vector().norm();
|
||||
// } catch (CheiralityException) {
|
||||
// cheiralityException_ = true;
|
||||
// }
|
||||
// i += 1;
|
||||
// }
|
||||
// //std::cout << "totalReprojError error: " << totalReprojError << std::endl;
|
||||
// // we discard smart factors that have large reprojection error
|
||||
// if (parameters_.dynamicOutlierRejectionThreshold > 0
|
||||
// && totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
|
||||
// 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
|
||||
// degenerate_ = true;
|
||||
// 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
|
||||
// cheiralityException_ = true;
|
||||
// }
|
||||
}
|
||||
return m;
|
||||
return TriangulationResult(Point3());
|
||||
}
|
||||
|
||||
/// triangulate
|
||||
bool triangulateForLinearize(const Cameras& cameras) const {
|
||||
|
||||
bool isDebug = false;
|
||||
size_t nrCameras = this->triangulateSafe(cameras);
|
||||
|
||||
if (nrCameras < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_))) {
|
||||
if (isDebug) {
|
||||
std::cout << "createImplicitSchurFactor: degenerate configuration"
|
||||
<< std::endl;
|
||||
}
|
||||
return false;
|
||||
} else {
|
||||
|
||||
// instead, if we want to manage the exception..
|
||||
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
|
||||
this->degenerate_ = true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
triangulateSafe(cameras); // imperative, might reset result_
|
||||
return (result_);
|
||||
}
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
|
||||
const Cameras& cameras, const double lambda = 0.0) const {
|
||||
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
|
||||
false) const {
|
||||
|
||||
bool isDebug = false;
|
||||
size_t numKeys = this->keys_.size();
|
||||
// Create structures for Hessian Factors
|
||||
std::vector<Key> js;
|
||||
|
|
@ -293,146 +332,142 @@ public:
|
|||
|
||||
if (this->measured_.size() != cameras.size()) {
|
||||
std::cout
|
||||
<< "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
|
||||
<< "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input"
|
||||
<< std::endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
triangulateSafe(cameras);
|
||||
if (isDebug)
|
||||
std::cout << "point_ = " << point_ << std::endl;
|
||||
|
||||
if (numKeys < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_))) {
|
||||
if (isDebug)
|
||||
std::cout << "In linearize: exception" << std::endl;
|
||||
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
|
||||
// failed: return"empty" Hessian
|
||||
BOOST_FOREACH(Matrix& m, Gs)
|
||||
m = zeros(Base::Dim, Base::Dim);
|
||||
BOOST_FOREACH(Vector& v, gs)
|
||||
v = zero(Base::Dim);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs,
|
||||
0.0);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
|
||||
Gs, gs, 0.0);
|
||||
}
|
||||
|
||||
// instead, if we want to manage the exception..
|
||||
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
|
||||
this->degenerate_ = true;
|
||||
if (isDebug)
|
||||
std::cout << "degenerate_ = true" << std::endl;
|
||||
}
|
||||
|
||||
if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
|
||||
for (size_t i = 0; i < cameras.size(); i++)
|
||||
this->cameraPosesLinearization_[i] = cameras[i].pose();
|
||||
|
||||
// ==================================================================
|
||||
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
|
||||
std::vector<typename Base::MatrixZD> Fblocks;
|
||||
Matrix F, E;
|
||||
Vector b;
|
||||
computeJacobians(Fblocks, E, b, cameras);
|
||||
Base::FillDiagonalF(Fblocks, F); // expensive !!!
|
||||
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
|
||||
|
||||
// Schur complement trick
|
||||
// Frank says: should be possible to do this more efficiently?
|
||||
// And we care, as in grouped factors this is called repeatedly
|
||||
Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
|
||||
Vector gs_vector;
|
||||
// Whiten using noise model
|
||||
Base::whitenJacobians(Fblocks, E, b);
|
||||
|
||||
Matrix3 P = Cameras::PointCov(E, lambda);
|
||||
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
|
||||
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
|
||||
// build augmented hessian
|
||||
SymmetricBlockMatrix augmentedHessian = //
|
||||
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
|
||||
|
||||
if (isDebug)
|
||||
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
|
||||
if (isDebug)
|
||||
std::cout << "H:\n" << H << std::endl;
|
||||
|
||||
// Populate Gs and gs
|
||||
int GsCount2 = 0;
|
||||
for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
|
||||
DenseIndex i1D = i1 * Base::Dim;
|
||||
gs.at(i1) = gs_vector.segment<Base::Dim>(i1D);
|
||||
for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
|
||||
if (i2 >= i1) {
|
||||
Gs.at(GsCount2) = H.block<Base::Dim, Base::Dim>(i1D, i2 * Base::Dim);
|
||||
GsCount2++;
|
||||
}
|
||||
}
|
||||
}
|
||||
// ==================================================================
|
||||
double f = b.squaredNorm();
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
|
||||
augmentedHessian);
|
||||
}
|
||||
|
||||
// // create factor
|
||||
// boost::shared_ptr<ImplicitSchurFactor<Base::Dim> > createImplicitSchurFactor(
|
||||
// create factor
|
||||
// boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
|
||||
// const Cameras& cameras, double lambda) const {
|
||||
// if (triangulateForLinearize(cameras))
|
||||
// return Base::createImplicitSchurFactor(cameras, point_, lambda);
|
||||
// return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
|
||||
// else
|
||||
// return boost::shared_ptr<ImplicitSchurFactor<Base::Dim> >();
|
||||
// // failed: return empty
|
||||
// return boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
|
||||
// }
|
||||
//
|
||||
// /// create factor
|
||||
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
|
||||
// boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
|
||||
// const Cameras& cameras, double lambda) const {
|
||||
// if (triangulateForLinearize(cameras))
|
||||
// return Base::createJacobianQFactor(cameras, point_, lambda);
|
||||
// return Base::createJacobianQFactor(cameras, *result_, lambda);
|
||||
// else
|
||||
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
|
||||
// // failed: return empty
|
||||
// return boost::make_shared<JacobianFactorQ<Base::Dim, Base::ZDim> >(this->keys_);
|
||||
// }
|
||||
//
|
||||
// /// Create a factor, takes values
|
||||
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
|
||||
// boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
|
||||
// const Values& values, double lambda) const {
|
||||
// Cameras cameras;
|
||||
// // TODO triangulate twice ??
|
||||
// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
||||
// if (nonDegenerate)
|
||||
// return createJacobianQFactor(cameras, lambda);
|
||||
// else
|
||||
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
|
||||
// return createJacobianQFactor(this->cameras(values), lambda);
|
||||
// }
|
||||
//
|
||||
|
||||
/// different (faster) way to compute Jacobian factor
|
||||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
return Base::createJacobianSVDFactor(cameras, point_, lambda);
|
||||
return Base::createJacobianSVDFactor(cameras, *result_, lambda);
|
||||
else
|
||||
return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
|
||||
}
|
||||
|
||||
/// Returns true if nonDegenerate
|
||||
bool computeCamerasAndTriangulate(const Values& values,
|
||||
Cameras& cameras) const {
|
||||
Values valuesFactor;
|
||||
/// linearize to a Hessianfactor
|
||||
virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createHessianFactor(this->cameras(values), lambda);
|
||||
}
|
||||
|
||||
// Select only the cameras
|
||||
BOOST_FOREACH(const Key key, this->keys_)
|
||||
valuesFactor.insert(key, values.at(key));
|
||||
// /// linearize to an Implicit Schur factor
|
||||
// virtual boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
|
||||
// const Values& values, double lambda = 0.0) const {
|
||||
// return createRegularImplicitSchurFactor(this->cameras(values), lambda);
|
||||
// }
|
||||
//
|
||||
// /// linearize to a JacobianfactorQ
|
||||
// virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > linearizeToJacobian(
|
||||
// const Values& values, double lambda = 0.0) const {
|
||||
// return createJacobianQFactor(this->cameras(values), lambda);
|
||||
// }
|
||||
|
||||
cameras = this->cameras(valuesFactor);
|
||||
size_t nrCameras = this->triangulateSafe(cameras);
|
||||
|
||||
if (nrCameras < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_)))
|
||||
return false;
|
||||
|
||||
// instead, if we want to manage the exception..
|
||||
if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
|
||||
this->degenerate_ = true;
|
||||
|
||||
if (this->degenerate_) {
|
||||
std::cout << "SmartStereoProjectionFactor: this is not ready"
|
||||
<< std::endl;
|
||||
std::cout << "this->cheiralityException_ " << this->cheiralityException_
|
||||
<< std::endl;
|
||||
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
|
||||
/**
|
||||
* Linearize to Gaussian Factor
|
||||
* @param values Values structure which must contain camera poses for this factor
|
||||
* @return a Gaussian factor
|
||||
*/
|
||||
boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
|
||||
const double lambda = 0.0) const {
|
||||
// depending on flag set on construction we may linearize to different linear factors
|
||||
switch (params_.linearizationMode) {
|
||||
case HESSIAN:
|
||||
return createHessianFactor(cameras, lambda);
|
||||
// case IMPLICIT_SCHUR:
|
||||
// return createRegularImplicitSchurFactor(cameras, lambda);
|
||||
// case JACOBIAN_SVD:
|
||||
// return createJacobianSVDFactor(cameras, lambda);
|
||||
// case JACOBIAN_Q:
|
||||
// return createJacobianQFactor(cameras, lambda);
|
||||
default:
|
||||
throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize to Gaussian Factor
|
||||
* @param values Values structure which must contain camera poses for this factor
|
||||
* @return a Gaussian factor
|
||||
*/
|
||||
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
|
||||
const double lambda = 0.0) const {
|
||||
// depending on flag set on construction we may linearize to different linear factors
|
||||
Cameras cameras = this->cameras(values);
|
||||
return linearizeDamped(cameras, lambda);
|
||||
}
|
||||
|
||||
/// linearize
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& values) const {
|
||||
return linearizeDamped(values);
|
||||
}
|
||||
|
||||
/**
|
||||
* Triangulate and compute derivative of error with respect to point
|
||||
* @return whether triangulation worked
|
||||
*/
|
||||
bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
cameras.project2(*result_, boost::none, E);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -440,87 +475,62 @@ public:
|
|||
* @return whether triangulation worked
|
||||
*/
|
||||
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
|
||||
Cameras cameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
||||
if (nonDegenerate)
|
||||
cameras.project2(point_, boost::none, E);
|
||||
return nonDegenerate;
|
||||
Cameras cameras = this->cameras(values);
|
||||
return triangulateAndComputeE(E, cameras);
|
||||
}
|
||||
|
||||
/// Version that takes values, and creates the point
|
||||
bool computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
|
||||
Matrix& E, Vector& b, const Values& values) const {
|
||||
Cameras cameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
||||
if (nonDegenerate)
|
||||
computeJacobians(Fblocks, E, b, cameras, 0.0);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/// Compute F, E only (called below in both vanilla and SVD versions)
|
||||
/// Assumes the point has been computed
|
||||
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
|
||||
void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
|
||||
Matrix& E, Vector& b, const Cameras& cameras) const {
|
||||
if (this->degenerate_) {
|
||||
throw("FIXME: computeJacobians degenerate case commented out!");
|
||||
// std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
|
||||
// std::cout << "point " << point_ << std::endl;
|
||||
// std::cout
|
||||
// << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used"
|
||||
// << std::endl;
|
||||
// if (D > 6) {
|
||||
// std::cout
|
||||
// << "Management of degeneracy is not yet ready when one also optimizes for the calibration "
|
||||
// << std::endl;
|
||||
// }
|
||||
void computeJacobiansWithTriangulatedPoint(
|
||||
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
|
||||
const Cameras& cameras) const {
|
||||
|
||||
if (!result_) {
|
||||
throw ("computeJacobiansWithTriangulatedPoint");
|
||||
// // Handle degeneracy
|
||||
// // TODO check flag whether we should do this
|
||||
// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
|
||||
// this->measured_.at(0)); */
|
||||
//
|
||||
// int numKeys = this->keys_.size();
|
||||
// E = zeros(2 * numKeys, 2);
|
||||
// b = zero(2 * numKeys);
|
||||
// double f = 0;
|
||||
// for (size_t i = 0; i < this->measured_.size(); i++) {
|
||||
// if (i == 0) { // first pose
|
||||
// this->point_ = cameras[i].backprojectPointAtInfinity(
|
||||
// this->measured_.at(i));
|
||||
// // 3D parametrization of point at infinity: [px py 1]
|
||||
// }
|
||||
// Matrix Fi, Ei;
|
||||
// Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
|
||||
// - this->measured_.at(i)).vector();
|
||||
//
|
||||
// this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
|
||||
// f += bi.squaredNorm();
|
||||
// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi));
|
||||
// E.block < 2, 2 > (2 * i, 0) = Ei;
|
||||
// subInsert(b, bi, 2 * i);
|
||||
// }
|
||||
// return f;
|
||||
// Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
|
||||
} else {
|
||||
// nondegenerate: just return Base version
|
||||
Base::computeJacobians(Fblocks, E, b, cameras, point_);
|
||||
} // end else
|
||||
// valid result: just return Base version
|
||||
Base::computeJacobians(Fblocks, E, b, cameras, *result_);
|
||||
}
|
||||
}
|
||||
|
||||
/// Version that takes values, and creates the point
|
||||
bool triangulateAndComputeJacobians(
|
||||
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
|
||||
const Values& values) const {
|
||||
Cameras cameras = this->cameras(values);
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/// takes values
|
||||
bool triangulateAndComputeJacobiansSVD(
|
||||
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
|
||||
const Values& values) const {
|
||||
typename Base::Cameras cameras;
|
||||
double good = computeCamerasAndTriangulate(values, cameras);
|
||||
if (good)
|
||||
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
|
||||
return true;
|
||||
Cameras cameras = this->cameras(values);
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/// Calculate vector of re-projection errors, before applying noise model
|
||||
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
|
||||
Cameras cameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
||||
Cameras cameras = this->cameras(values);
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
return Base::unwhitenedError(cameras, point_);
|
||||
return Base::unwhitenedError(cameras, *result_);
|
||||
else
|
||||
return zero(cameras.size() * 3);
|
||||
return zero(cameras.size() * Base::ZDim);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -532,84 +542,60 @@ public:
|
|||
double totalReprojectionError(const Cameras& cameras,
|
||||
boost::optional<Point3> externalPoint = boost::none) const {
|
||||
|
||||
size_t nrCameras;
|
||||
if (externalPoint) {
|
||||
nrCameras = this->keys_.size();
|
||||
point_ = *externalPoint;
|
||||
degenerate_ = false;
|
||||
cheiralityException_ = false;
|
||||
} else {
|
||||
nrCameras = this->triangulateSafe(cameras);
|
||||
}
|
||||
if (externalPoint)
|
||||
result_ = TriangulationResult(*externalPoint);
|
||||
else
|
||||
result_ = triangulateSafe(cameras);
|
||||
|
||||
if (nrCameras < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_))) {
|
||||
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");
|
||||
// // Otherwise, manage the exceptions with rotation-only factors
|
||||
// const StereoPoint2& z0 = this->measured_.at(0);
|
||||
// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
|
||||
//
|
||||
// return Base::totalReprojectionError(cameras, backprojected);
|
||||
} else
|
||||
// if we don't want to manage the exceptions we discard the factor
|
||||
// std::cout << "In error evaluation: exception" << std::endl;
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
|
||||
std::cout
|
||||
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
|
||||
<< std::endl;
|
||||
this->degenerate_ = true;
|
||||
}
|
||||
|
||||
if (this->degenerate_) {
|
||||
return 0.0; // TODO: this maybe should be zero?
|
||||
// std::cout
|
||||
// << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
|
||||
// << std::endl;
|
||||
// size_t i = 0;
|
||||
// double overallError = 0;
|
||||
// BOOST_FOREACH(const Camera& camera, cameras) {
|
||||
// const StereoPoint2& zi = this->measured_.at(i);
|
||||
// if (i == 0) // first pose
|
||||
// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
|
||||
// StereoPoint2 reprojectionError(
|
||||
// camera.projectPointAtInfinity(this->point_) - zi);
|
||||
// overallError += 0.5
|
||||
// * this->noise_.at(i)->distance(reprojectionError.vector());
|
||||
// i += 1;
|
||||
// }
|
||||
// return overallError;
|
||||
} else {
|
||||
// Just use version in base class
|
||||
return Base::totalReprojectionError(cameras, point_);
|
||||
/// Calculate total reprojection error
|
||||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
return totalReprojectionError(Base::cameras(values));
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/** return the landmark */
|
||||
boost::optional<Point3> point() const {
|
||||
return point_;
|
||||
}
|
||||
TriangulationResult point() const {
|
||||
return result_;
|
||||
}
|
||||
|
||||
/** COMPUTE the landmark */
|
||||
boost::optional<Point3> point(const Values& values) const {
|
||||
triangulateSafe(values);
|
||||
return point_;
|
||||
}
|
||||
/** COMPUTE the landmark */
|
||||
TriangulationResult point(const Values& values) const {
|
||||
Cameras cameras = this->cameras(values);
|
||||
return triangulateSafe(cameras);
|
||||
}
|
||||
|
||||
/** return the degenerate state */
|
||||
inline bool isDegenerate() const {
|
||||
return (cheiralityException_ || degenerate_);
|
||||
}
|
||||
/// Is result valid?
|
||||
bool isValid() const {
|
||||
return result_;
|
||||
}
|
||||
|
||||
/** return the cheirality status flag */
|
||||
inline bool isPointBehindCamera() const {
|
||||
return cheiralityException_;
|
||||
}
|
||||
/** return chirality verbosity */
|
||||
inline bool verboseCheirality() const {
|
||||
return verboseCheirality_;
|
||||
}
|
||||
/** return the degenerate state */
|
||||
bool isDegenerate() const {
|
||||
return result_.degenerate();
|
||||
}
|
||||
|
||||
/** return flag for throwing cheirality exceptions */
|
||||
inline bool throwCheirality() const {
|
||||
return throwCheirality_;
|
||||
}
|
||||
/** return the cheirality status flag */
|
||||
bool isPointBehindCamera() const {
|
||||
return result_.behindCamera();
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -618,8 +604,8 @@ private:
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
|
||||
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
|
||||
ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
|
||||
ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -46,8 +46,6 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
|
||||
|
||||
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
|
||||
|
||||
public:
|
||||
|
|
@ -71,14 +69,9 @@ public:
|
|||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
*/
|
||||
SmartStereoProjectionPoseFactor(const double rankTol = 1,
|
||||
const double linThreshold = -1, const bool manageDegeneracy = false,
|
||||
const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
|
||||
linearizeTo) {
|
||||
SmartStereoProjectionPoseFactor(const SmartStereoProjectionParams& params =
|
||||
SmartStereoProjectionParams()) :
|
||||
Base(params) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
|
|
@ -149,6 +142,22 @@ public:
|
|||
return e && Base::equals(p, tol);
|
||||
}
|
||||
|
||||
/**
|
||||
* error calculates the error of the factor.
|
||||
*/
|
||||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
return this->totalReprojectionError(cameras(values));
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
|
||||
return K_all_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Collect all cameras involved in this factor
|
||||
* @param values Values structure which must contain camera poses corresponding
|
||||
|
|
@ -166,44 +175,6 @@ public:
|
|||
return cameras;
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize to Gaussian Factor
|
||||
* @param values Values structure which must contain camera poses for this factor
|
||||
* @return
|
||||
*/
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& values) const {
|
||||
// depending on flag set on construction we may linearize to different linear factors
|
||||
switch(linearizeTo_){
|
||||
case JACOBIAN_SVD :
|
||||
return this->createJacobianSVDFactor(cameras(values), 0.0);
|
||||
break;
|
||||
case JACOBIAN_Q :
|
||||
throw("JacobianQ not working yet!");
|
||||
// return this->createJacobianQFactor(cameras(values), 0.0);
|
||||
break;
|
||||
default:
|
||||
return this->createHessianFactor(cameras(values));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* error calculates the error of the factor.
|
||||
*/
|
||||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
return this->totalReprojectionError(cameras(values));
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
|
||||
return K_all_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Serialization function
|
||||
|
|
|
|||
|
|
@ -43,8 +43,11 @@ static Cal3_S2Stereo::shared_ptr K2(
|
|||
static boost::shared_ptr<Cal3Bundler> Kbundler(
|
||||
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
|
||||
|
||||
static double rankTol = 1.0;
|
||||
static double linThreshold = -1.0;
|
||||
//static double rankTol = 1.0;
|
||||
//static double linThreshold = -1.0;
|
||||
|
||||
static SmartStereoProjectionParams params;
|
||||
|
||||
// static bool manageDegeneracy = true;
|
||||
// Create a noise model for the pixel error
|
||||
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
|
||||
|
|
@ -80,7 +83,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
|
|||
return measurements_cam;
|
||||
}
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams lm_params;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Constructor) {
|
||||
|
|
@ -89,7 +92,7 @@ TEST( SmartStereoProjectionPoseFactor, Constructor) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Constructor2) {
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
SmartFactor factor1(params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -100,7 +103,7 @@ TEST( SmartStereoProjectionPoseFactor, Constructor3) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Constructor4) {
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
SmartFactor factor1(params);
|
||||
factor1.add(measurement1, poseKey1, model, K);
|
||||
}
|
||||
|
||||
|
|
@ -278,7 +281,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
|
||||
Values result;
|
||||
gttic_(SmartStereoProjectionPoseFactor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartStereoProjectionPoseFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
|
@ -325,16 +328,16 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
|
|||
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
SmartStereoProjectionParams params;
|
||||
params.setLinearizationMode(JACOBIAN_SVD);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1( new SmartFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -355,7 +358,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
|
|||
values.insert(x3, pose3 * noise_pose);
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
|
||||
}
|
||||
|
|
@ -363,7 +366,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
||||
|
||||
double excludeLandmarksFutherThanDist = 2;
|
||||
// double excludeLandmarksFutherThanDist = 2;
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(x1);
|
||||
|
|
@ -393,19 +396,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
|||
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
SmartStereoProjectionParams params;
|
||||
params.setLinearizationMode(JACOBIAN_SVD);
|
||||
params.setLandmarkDistanceThreshold(2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -427,7 +428,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
|||
|
||||
// All factors are disabled and pose should remain where it is
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
|
||||
}
|
||||
|
|
@ -471,24 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
|
||||
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
SmartStereoProjectionParams params;
|
||||
params.setLinearizationMode(JACOBIAN_SVD);
|
||||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor(params));
|
||||
smartFactor4->add(measurements_cam4, views, model, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -510,7 +509,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
|
||||
// All factors are disabled and pose should remain where it is
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
|
||||
}
|
||||
|
|
@ -571,7 +570,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
// values.insert(x3, pose3*noise_pose);
|
||||
//
|
||||
//// Values result;
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
// result = optimizer.optimize();
|
||||
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
//}
|
||||
|
|
@ -630,7 +629,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
// values.insert(L(2), landmark2);
|
||||
// values.insert(L(3), landmark3);
|
||||
//
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
// Values result = optimizer.optimize();
|
||||
//
|
||||
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
|
|
@ -672,13 +671,16 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
// Create smartfactors
|
||||
double rankTol = 10;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
|
||||
SmartStereoProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
// Create graph to optimize
|
||||
|
|
@ -781,7 +783,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
//
|
||||
// Values result;
|
||||
// gttic_(SmartStereoProjectionPoseFactor);
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
// result = optimizer.optimize();
|
||||
// gttoc_(SmartStereoProjectionPoseFactor);
|
||||
// tictoc_finishedIteration_();
|
||||
|
|
@ -855,7 +857,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
//
|
||||
// Values result;
|
||||
// gttic_(SmartStereoProjectionPoseFactor);
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
// result = optimizer.optimize();
|
||||
// gttoc_(SmartStereoProjectionPoseFactor);
|
||||
// tictoc_finishedIteration_();
|
||||
|
|
|
|||
Loading…
Reference in New Issue