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

huge refactor. Compiles again, but triangulation still broken, SmartStereo test fails

release/4.3a0
cbeall3 2015-07-15 16:53:04 -04:00
parent 9c2ab0ce3b
commit bd4dd84933
3 changed files with 429 additions and 470 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

742
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -38,51 +38,114 @@ namespace gtsam {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD 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> template<class CALIBRATION>
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> { class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
private:
typedef SmartFactorBase<StereoCamera> Base;
typedef SmartStereoProjectionFactor<CALIBRATION> This;
protected: protected:
/// @name Parameters
/// @{
const SmartStereoProjectionParams params_;
/// @}
/// @name Caching triangulation /// @name Caching triangulation
/// @{ /// @{
const TriangulationParameters parameters_; mutable TriangulationResult result_; ///< result from triangulateSafe
// TODO:
// mutable TriangulationResult result_; ///< result from triangulateSafe
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses 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: public:
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
@ -93,22 +156,12 @@ public:
/** /**
* Constructor * Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate * @param params internal parameters of the smart factors
* @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)
*/ */
SmartStereoProjectionFactor(const double rankTolerance, SmartStereoProjectionFactor(const SmartStereoProjectionParams& params =
const double linThreshold, const bool manageDegeneracy, SmartStereoProjectionParams()) :
const bool enableEPI, double landmarkDistanceThreshold = 1e10, params_(params), //
double dynamicOutlierRejectionThreshold = -1) : result_(TriangulationResult::Degenerate()) {
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
dynamicOutlierRejectionThreshold), //
retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false) {
} }
/** Virtual destructor */ /** Virtual destructor */
@ -123,14 +176,19 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionFactor\n"; std::cout << s << "SmartStereoProjectionFactor\n";
std::cout << "triangulationParameters:\n" << parameters_ << std::endl; std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl; std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; std::cout << "result:\n" << result_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_
<< std::endl;
Base::print("", keyFormatter); 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 /// Check if the new linearization point_ is the same as the one used for previous triangulation
bool decideIfTriangulate(const Cameras& cameras) const { 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 // 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) { if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) { for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
retriangulationThreshold_)) { params_.retriangulationThreshold)) {
retriangulate = true; // at least two poses are different, hence we retriangulate retriangulate = true; // at least two poses are different, hence we retriangulate
break; 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 return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
} }
/// triangulateSafe // /// triangulateSafe
size_t triangulateSafe(const Values& values) const { // size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values)); // return triangulateSafe(this->cameras(values));
} // }
/// triangulateSafe /// triangulateSafe
size_t triangulateSafe(const Cameras& cameras) const { TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size(); size_t m = cameras.size();
if (m < 2) { // if we have a single pose the corresponding factor is uninformative if (m < 2) { // if we have a single pose the corresponding factor is uninformative
degenerate_ = true; return TriangulationResult::Degenerate();
return m;
} }
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, // point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
parameters_.rankTolerance, parameters_.enableEPI); // parameters_.rankTolerance, parameters_.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; // degenerate_ = false;
cheiralityException_ = false; // cheiralityException_ = false;
//
// 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_) > parameters_.landmarkDistanceThreshold) { // if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
degenerate_ = true; // degenerate_ = true;
break; // break;
} // }
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(point_) - zi);
totalReprojError += reprojectionError.vector().norm(); // totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) { // } catch (CheiralityException) {
cheiralityException_ = true; // cheiralityException_ = true;
} // }
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 (parameters_.dynamicOutlierRejectionThreshold > 0 // if (parameters_.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold) // && totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
degenerate_ = true; // degenerate_ = true;
//
} catch (TriangulationUnderconstrainedException&) { // } catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be // // if TriangulationUnderconstrainedException can be
// 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 // // in the second case we want to use a rotation-only smart factor
degenerate_ = true; // degenerate_ = true;
cheiralityException_ = false; // cheiralityException_ = false;
} 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 // // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
cheiralityException_ = true; // cheiralityException_ = true;
} // }
} }
return m; return TriangulationResult(Point3());
} }
/// triangulate /// triangulate
bool triangulateForLinearize(const Cameras& cameras) const { bool triangulateForLinearize(const Cameras& cameras) const {
triangulateSafe(cameras); // imperative, might reset result_
bool isDebug = false; return (result_);
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;
}
} }
/// linearize returns a Hessianfactor that is an approximation of error(p) /// linearize returns a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor( 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(); size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors // Create structures for Hessian Factors
std::vector<Key> js; std::vector<Key> js;
@ -293,146 +332,142 @@ public:
if (this->measured_.size() != cameras.size()) { if (this->measured_.size() != cameras.size()) {
std::cout std::cout
<< "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" << "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input"
<< std::endl; << std::endl;
exit(1); exit(1);
} }
triangulateSafe(cameras); triangulateSafe(cameras);
if (isDebug)
std::cout << "point_ = " << point_ << std::endl;
if (numKeys < 2 if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
|| (!this->manageDegeneracy_ // failed: return"empty" Hessian
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug)
std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(Matrix& m, Gs) BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim); m = zeros(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs) BOOST_FOREACH(Vector& v, gs)
v = zero(Base::Dim); v = zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
0.0); Gs, gs, 0.0);
} }
// instead, if we want to manage the exception.. // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
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();
// ==================================================================
std::vector<typename Base::MatrixZD> Fblocks; std::vector<typename Base::MatrixZD> Fblocks;
Matrix F, E; Matrix F, E;
Vector b; Vector b;
computeJacobians(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
Base::FillDiagonalF(Fblocks, F); // expensive !!!
// Schur complement trick // Whiten using noise model
// Frank says: should be possible to do this more efficiently? Base::whitenJacobians(Fblocks, E, b);
// And we care, as in grouped factors this is called repeatedly
Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
Vector gs_vector;
Matrix3 P = Cameras::PointCov(E, lambda); // build augmented hessian
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); SymmetricBlockMatrix augmentedHessian = //
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
if (isDebug) return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
std::cout << "gs_vector size " << gs_vector.size() << std::endl; augmentedHessian);
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);
} }
// // create factor // create factor
// boost::shared_ptr<ImplicitSchurFactor<Base::Dim> > createImplicitSchurFactor( // boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
// const Cameras& cameras, double lambda) const { // const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras)) // if (triangulateForLinearize(cameras))
// return Base::createImplicitSchurFactor(cameras, point_, lambda); // return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
// else // else
// return boost::shared_ptr<ImplicitSchurFactor<Base::Dim> >(); // // failed: return empty
// return boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
// } // }
// //
// /// create factor // /// create factor
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor( // boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
// const Cameras& cameras, double lambda) const { // const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras)) // if (triangulateForLinearize(cameras))
// return Base::createJacobianQFactor(cameras, point_, lambda); // return Base::createJacobianQFactor(cameras, *result_, lambda);
// else // 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 // /// 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 { // const Values& values, double lambda) const {
// Cameras cameras; // return createJacobianQFactor(this->cameras(values), lambda);
// // TODO triangulate twice ??
// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
// if (nonDegenerate)
// return createJacobianQFactor(cameras, lambda);
// else
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
// } // }
//
/// different (faster) way to compute Jacobian factor /// different (faster) way to compute Jacobian factor
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda); return Base::createJacobianSVDFactor(cameras, *result_, lambda);
else else
return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_); return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
} }
/// Returns true if nonDegenerate /// linearize to a Hessianfactor
bool computeCamerasAndTriangulate(const Values& values, virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
Cameras& cameras) const { const Values& values, double lambda = 0.0) const {
Values valuesFactor; return createHessianFactor(this->cameras(values), lambda);
}
// Select only the cameras // /// linearize to an Implicit Schur factor
BOOST_FOREACH(const Key key, this->keys_) // virtual boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
valuesFactor.insert(key, values.at(key)); // 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); * Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
if (nrCameras < 2 * @return a Gaussian factor
|| (!this->manageDegeneracy_ */
&& (this->cheiralityException_ || this->degenerate_))) boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
return false; const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
// instead, if we want to manage the exception.. switch (params_.linearizationMode) {
if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors case HESSIAN:
this->degenerate_ = true; return createHessianFactor(cameras, lambda);
// case IMPLICIT_SCHUR:
if (this->degenerate_) { // return createRegularImplicitSchurFactor(cameras, lambda);
std::cout << "SmartStereoProjectionFactor: this is not ready" // case JACOBIAN_SVD:
<< std::endl; // return createJacobianSVDFactor(cameras, lambda);
std::cout << "this->cheiralityException_ " << this->cheiralityException_ // case JACOBIAN_Q:
<< std::endl; // return createJacobianQFactor(cameras, lambda);
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; 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 * @return whether triangulation worked
*/ */
bool triangulateAndComputeE(Matrix& E, const Values& values) const { bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras; Cameras cameras = this->cameras(values);
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); return triangulateAndComputeE(E, cameras);
if (nonDegenerate)
cameras.project2(point_, boost::none, E);
return nonDegenerate;
} }
/// 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) /// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks, void computeJacobiansWithTriangulatedPoint(
Matrix& E, Vector& b, const Cameras& cameras) const { std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
if (this->degenerate_) { const Cameras& cameras) const {
throw("FIXME: computeJacobians degenerate case commented out!");
// std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; if (!result_) {
// std::cout << "point " << point_ << std::endl; throw ("computeJacobiansWithTriangulatedPoint");
// std::cout // // Handle degeneracy
// << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used" // // TODO check flag whether we should do this
// << std::endl; // Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
// if (D > 6) { // this->measured_.at(0)); */
// std::cout
// << "Management of degeneracy is not yet ready when one also optimizes for the calibration "
// << std::endl;
// }
// //
// int numKeys = this->keys_.size(); // Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
// 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;
} else { } else {
// nondegenerate: just return Base version // valid result: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, point_); Base::computeJacobians(Fblocks, E, b, cameras, *result_);
} // end else }
}
/// 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 /// takes values
bool triangulateAndComputeJacobiansSVD( bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b, std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const { const Values& values) const {
typename Base::Cameras cameras; Cameras cameras = this->cameras(values);
double good = computeCamerasAndTriangulate(values, cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (good) if (nonDegenerate)
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
return true; return nonDegenerate;
} }
/// Calculate vector of re-projection errors, before applying noise model /// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const { Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras; Cameras cameras = this->cameras(values);
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
return Base::unwhitenedError(cameras, point_); return Base::unwhitenedError(cameras, *result_);
else else
return zero(cameras.size() * 3); return zero(cameras.size() * Base::ZDim);
} }
/** /**
@ -532,84 +542,60 @@ public:
double totalReprojectionError(const Cameras& cameras, double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const { boost::optional<Point3> externalPoint = boost::none) const {
size_t nrCameras; if (externalPoint)
if (externalPoint) { result_ = TriangulationResult(*externalPoint);
nrCameras = this->keys_.size(); else
point_ = *externalPoint; result_ = triangulateSafe(cameras);
degenerate_ = false;
cheiralityException_ = false;
} else {
nrCameras = this->triangulateSafe(cameras);
}
if (nrCameras < 2 if (result_)
|| (!this->manageDegeneracy_ // All good, just use version in base class
&& (this->cheiralityException_ || this->degenerate_))) { 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 // if we don't want to manage the exceptions we discard the factor
// std::cout << "In error evaluation: exception" << std::endl;
return 0.0; return 0.0;
} }
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors /// Calculate total reprojection error
std::cout virtual double error(const Values& values) const {
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" if (this->active(values)) {
<< std::endl; return totalReprojectionError(Base::cameras(values));
this->degenerate_ = true; } else { // else of active flag
} return 0.0;
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_);
} }
} }
/** return the landmark */ /** return the landmark */
boost::optional<Point3> point() const { TriangulationResult point() const {
return point_; return result_;
} }
/** COMPUTE the landmark */ /** COMPUTE the landmark */
boost::optional<Point3> point(const Values& values) const { TriangulationResult point(const Values& values) const {
triangulateSafe(values); Cameras cameras = this->cameras(values);
return point_; return triangulateSafe(cameras);
} }
/** return the degenerate state */ /// Is result valid?
inline bool isDegenerate() const { bool isValid() const {
return (cheiralityException_ || degenerate_); return result_;
} }
/** return the cheirality status flag */ /** return the degenerate state */
inline bool isPointBehindCamera() const { bool isDegenerate() const {
return cheiralityException_; return result_.degenerate();
} }
/** return chirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}
/** return flag for throwing cheirality exceptions */ /** return the cheirality status flag */
inline bool throwCheirality() const { bool isPointBehindCamera() const {
return throwCheirality_; return result_.behindCamera();
} }
private: private:
@ -618,8 +604,8 @@ private:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
} }
}; };

67
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -46,8 +46,6 @@ public:
protected: 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) std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
public: public:
@ -71,14 +69,9 @@ public:
* otherwise the factor is simply neglected * otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
*/ */
SmartStereoProjectionPoseFactor(const double rankTol = 1, SmartStereoProjectionPoseFactor(const SmartStereoProjectionParams& params =
const double linThreshold = -1, const bool manageDegeneracy = false, SmartStereoProjectionParams()) :
const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN, Base(params) {
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
linearizeTo) {
} }
/** Virtual destructor */ /** Virtual destructor */
@ -149,6 +142,22 @@ public:
return e && Base::equals(p, tol); 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 * Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding * @param values Values structure which must contain camera poses corresponding
@ -166,44 +175,6 @@ public:
return cameras; 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: private:
/// Serialization function /// Serialization function

90
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -43,8 +43,11 @@ static Cal3_S2Stereo::shared_ptr K2(
static boost::shared_ptr<Cal3Bundler> Kbundler( static boost::shared_ptr<Cal3Bundler> Kbundler(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
static double rankTol = 1.0; //static double rankTol = 1.0;
static double linThreshold = -1.0; //static double linThreshold = -1.0;
static SmartStereoProjectionParams params;
// static bool manageDegeneracy = true; // static bool manageDegeneracy = true;
// Create a noise model for the pixel error // Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
@ -80,7 +83,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
return measurements_cam; return measurements_cam;
} }
LevenbergMarquardtParams params; LevenbergMarquardtParams lm_params;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) { TEST( SmartStereoProjectionPoseFactor, Constructor) {
@ -89,7 +92,7 @@ TEST( SmartStereoProjectionPoseFactor, Constructor) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor2) { TEST( SmartStereoProjectionPoseFactor, Constructor2) {
SmartFactor factor1(rankTol, linThreshold); SmartFactor factor1(params);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -100,7 +103,7 @@ TEST( SmartStereoProjectionPoseFactor, Constructor3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor4) { TEST( SmartStereoProjectionPoseFactor, Constructor4) {
SmartFactor factor1(rankTol, linThreshold); SmartFactor factor1(params);
factor1.add(measurement1, poseKey1, model, K); factor1.add(measurement1, poseKey1, model, K);
} }
@ -278,7 +281,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
Values result; Values result;
gttic_(SmartStereoProjectionPoseFactor); gttic_(SmartStereoProjectionPoseFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize(); result = optimizer.optimize();
gttoc_(SmartStereoProjectionPoseFactor); gttoc_(SmartStereoProjectionPoseFactor);
tictoc_finishedIteration_(); tictoc_finishedIteration_();
@ -325,16 +328,16 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3); cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1( SmartStereoProjectionParams params;
new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); params.setLinearizationMode(JACOBIAN_SVD);
SmartFactor::shared_ptr smartFactor1( new SmartFactor(params));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -355,7 +358,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
values.insert(x3, pose3 * noise_pose); values.insert(x3, pose3 * noise_pose);
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3))); EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
} }
@ -363,7 +366,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
double excludeLandmarksFutherThanDist = 2; // double excludeLandmarksFutherThanDist = 2;
vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
@ -393,19 +396,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3); cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1( SmartStereoProjectionParams params;
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, params.setLinearizationMode(JACOBIAN_SVD);
excludeLandmarksFutherThanDist)); params.setLandmarkDistanceThreshold(2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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 // All factors are disabled and pose should remain where it is
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3))); 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 measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
SmartFactor::shared_ptr smartFactor1( SmartStereoProjectionParams params;
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, params.setLinearizationMode(JACOBIAN_SVD);
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4( SmartFactor::shared_ptr smartFactor4(new SmartFactor(params));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K); smartFactor4->add(measurements_cam4, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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 // All factors are disabled and pose should remain where it is
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3))); EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
} }
@ -571,7 +570,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(x3, pose3*noise_pose); // values.insert(x3, pose3*noise_pose);
// //
//// Values result; //// Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, params); // LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize(); // result = optimizer.optimize();
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
//} //}
@ -630,7 +629,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(L(2), landmark2); // values.insert(L(2), landmark2);
// values.insert(L(3), landmark3); // values.insert(L(3), landmark3);
// //
// LevenbergMarquardtOptimizer optimizer(graph, values, params); // LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// Values result = optimizer.optimize(); // Values result = optimizer.optimize();
// //
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
@ -672,13 +671,16 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// Create smartfactors // Create smartfactors
double rankTol = 10; 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); 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); 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); smartFactor3->add(measurements_cam3, views, model, K);
// Create graph to optimize // Create graph to optimize
@ -781,7 +783,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// //
// Values result; // Values result;
// gttic_(SmartStereoProjectionPoseFactor); // gttic_(SmartStereoProjectionPoseFactor);
// LevenbergMarquardtOptimizer optimizer(graph, values, params); // LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize(); // result = optimizer.optimize();
// gttoc_(SmartStereoProjectionPoseFactor); // gttoc_(SmartStereoProjectionPoseFactor);
// tictoc_finishedIteration_(); // tictoc_finishedIteration_();
@ -855,7 +857,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// //
// Values result; // Values result;
// gttic_(SmartStereoProjectionPoseFactor); // gttic_(SmartStereoProjectionPoseFactor);
// LevenbergMarquardtOptimizer optimizer(graph, values, params); // LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize(); // result = optimizer.optimize();
// gttoc_(SmartStereoProjectionPoseFactor); // gttoc_(SmartStereoProjectionPoseFactor);
// tictoc_finishedIteration_(); // tictoc_finishedIteration_();