Moved all linearize logic into SmartProjectionFactor and removed two subclasses
dellaert
parent
1eec6748cb
commit
a0470b7e1c
|
|
@ -189,7 +189,7 @@ public:
|
|||
std::cout << "measurement, p = " << measured_[k] << "\t";
|
||||
noiseModel_->print("noise model = ");
|
||||
}
|
||||
Base::print("", keyFormatter);
|
||||
print("", keyFormatter);
|
||||
}
|
||||
|
||||
/// equals
|
||||
|
|
@ -202,12 +202,10 @@ public:
|
|||
areMeasurementsEqual = false;
|
||||
break;
|
||||
}
|
||||
return e && Base::equals(p, tol) && areMeasurementsEqual;
|
||||
return e && equals(p, tol) && areMeasurementsEqual;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives
|
||||
*/
|
||||
///Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives
|
||||
template<class POINT>
|
||||
Vector unwhitenedError(const Cameras& cameras, const POINT& point,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
|
||||
|
|
@ -294,9 +292,7 @@ public:
|
|||
Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize to a Hessianfactor
|
||||
*/
|
||||
/// Linearize to a Hessianfactor
|
||||
boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
|
||||
const Cameras& cameras, const Point3& point, const double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
|
|
@ -341,9 +337,7 @@ public:
|
|||
F[i] = noiseModel_->Whiten(F[i]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return Jacobians as RegularImplicitSchurFactor with raw access
|
||||
*/
|
||||
/// Return Jacobians as RegularImplicitSchurFactor with raw access
|
||||
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
|
||||
createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
|
||||
double lambda = 0.0, bool diagonalDamping = false) const {
|
||||
|
|
@ -374,7 +368,7 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Return Jacobians as JacobianFactor
|
||||
* Return Jacobians as JacobianFactorSVD
|
||||
* TODO lambda is currently ignored
|
||||
*/
|
||||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
|
|
|
|||
|
|
@ -1,161 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SmartProjectionCameraFactor.h
|
||||
* @brief Produces an Hessian factors on CAMERAS (Pose3+CALIBRATION) from monocular measurements of a landmark
|
||||
* @author Luca Carlone
|
||||
* @author Chris Beall
|
||||
* @author Zsolt Kira
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/slam/SmartProjectionFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class CALIBRATION>
|
||||
class SmartProjectionCameraFactor: public SmartProjectionFactor<
|
||||
PinholeCamera<CALIBRATION> > {
|
||||
|
||||
private:
|
||||
typedef PinholeCamera<CALIBRATION> Camera;
|
||||
typedef SmartProjectionFactor<Camera> Base;
|
||||
typedef SmartProjectionCameraFactor<CALIBRATION> This;
|
||||
|
||||
protected:
|
||||
|
||||
static const int Dim = traits<Camera>::dimension; ///< CAMERA dimension
|
||||
|
||||
bool isImplicit_;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
// A set of cameras
|
||||
typedef CameraSet<Camera> Cameras;
|
||||
|
||||
/**
|
||||
* 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 isImplicit if set to true linearize the factor to an implicit Schur factor
|
||||
*/
|
||||
SmartProjectionCameraFactor(const double rankTol = 1,
|
||||
const double linThreshold = -1, const bool manageDegeneracy = false,
|
||||
const bool enableEPI = false, const bool isImplicit = false) :
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI), isImplicit_(
|
||||
isImplicit) {
|
||||
if (linThreshold != -1) {
|
||||
std::cout << "SmartProjectionCameraFactor: linThreshold " << linThreshold
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionCameraFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
std::cout << s << "SmartProjectionCameraFactor, z = \n ";
|
||||
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 && Base::equals(p, tol);
|
||||
}
|
||||
|
||||
/// get the dimension of the factor (number of rows on linearization)
|
||||
virtual size_t dim() const {
|
||||
return Dim * this->keys_.size(); // 6 for the pose and 3 for the calibration
|
||||
}
|
||||
|
||||
/// linearize and adds damping on the points
|
||||
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
|
||||
const double lambda=0.0) const {
|
||||
if (!isImplicit_)
|
||||
return Base::createHessianFactor(Base::cameras(values), lambda);
|
||||
else
|
||||
return Base::createRegularImplicitSchurFactor(Base::cameras(values));
|
||||
}
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
virtual boost::shared_ptr<RegularHessianFactor<Dim> > linearizeToHessian(
|
||||
const Values& values, double lambda=0.0) const {
|
||||
return Base::createHessianFactor(Base::cameras(values),lambda);
|
||||
}
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
virtual boost::shared_ptr<RegularImplicitSchurFactor<Camera> > linearizeToImplicit(
|
||||
const Values& values, double lambda=0.0) const {
|
||||
return Base::createRegularImplicitSchurFactor(Base::cameras(values),lambda);
|
||||
}
|
||||
|
||||
/// linearize returns a Jacobianfactor that is an approximation of error(p)
|
||||
virtual boost::shared_ptr<JacobianFactorQ<Dim, 2> > linearizeToJacobian(
|
||||
const Values& values, double lambda=0.0) const {
|
||||
return Base::createJacobianQFactor(Base::cameras(values),lambda);
|
||||
}
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
virtual boost::shared_ptr<GaussianFactor> linearizeWithLambda(
|
||||
const Values& values, double lambda) const {
|
||||
if (isImplicit_)
|
||||
return linearizeToImplicit(values,lambda);
|
||||
else
|
||||
return linearizeToHessian(values,lambda);
|
||||
}
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& values) const {
|
||||
return linearizeWithLambda(values,0.0);
|
||||
}
|
||||
|
||||
/// Calculare total reprojection error
|
||||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
return Base::totalReprojectionError(Base::cameras(values));
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -46,22 +46,27 @@ struct SmartProjectionFactorState {
|
|||
double f;
|
||||
};
|
||||
|
||||
enum LinearizationMode {
|
||||
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
|
||||
};
|
||||
|
||||
/**
|
||||
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
|
||||
*/
|
||||
template<class CAMERA>
|
||||
class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
|
||||
|
||||
public:
|
||||
|
||||
/// Linearization mode: what factor to linearize to
|
||||
enum LinearizationMode {
|
||||
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
|
||||
};
|
||||
|
||||
private:
|
||||
typedef SmartFactorBase<CAMERA> Base;
|
||||
typedef SmartProjectionFactor<CAMERA> This;
|
||||
|
||||
protected:
|
||||
|
||||
LinearizationMode linearizeTo_; ///< How to linearize the factor
|
||||
|
||||
/// @name Caching triangulation
|
||||
/// @{
|
||||
const TriangulationParameters parameters_;
|
||||
|
|
@ -104,16 +109,16 @@ public:
|
|||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
*/
|
||||
SmartProjectionFactor(const double rankTolerance, const double linThreshold,
|
||||
const bool manageDegeneracy, const bool enableEPI,
|
||||
SmartProjectionFactor(LinearizationMode linearizationMode = HESSIAN,
|
||||
double rankTolerance = 1, double linThreshold = -1,
|
||||
bool manageDegeneracy = false, bool enableEPI = false,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
|
||||
SmartFactorStatePtr(new SmartProjectionFactorState())) :
|
||||
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
|
||||
dynamicOutlierRejectionThreshold), //
|
||||
linearizeTo_(linearizationMode), parameters_(rankTolerance, enableEPI,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), //
|
||||
result_(TriangulationResult::Degenerate()), //
|
||||
retriangulationThreshold_(1e-5), //
|
||||
manageDegeneracy_(manageDegeneracy), //
|
||||
retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), //
|
||||
throwCheirality_(false), verboseCheirality_(false), //
|
||||
state_(state), linearizationThreshold_(linThreshold) {
|
||||
}
|
||||
|
|
@ -135,6 +140,12 @@ public:
|
|||
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 && 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
|
||||
|
|
@ -358,6 +369,53 @@ public:
|
|||
return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
|
||||
}
|
||||
|
||||
/// 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);
|
||||
}
|
||||
|
||||
/// linearize to an Implicit Schur factor
|
||||
virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > 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, 2> > linearizeToJacobian(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createJacobianQFactor(this->cameras(values), lambda);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
switch (linearizeTo_) {
|
||||
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("SmartFactorlinearize: unknown mode");
|
||||
}
|
||||
}
|
||||
|
||||
/// 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
|
||||
|
|
@ -380,7 +438,8 @@ public:
|
|||
if (!result_) {
|
||||
// Handle degeneracy
|
||||
// TODO check flag whether we should do this
|
||||
Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured_.at(0));
|
||||
Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
|
||||
this->measured_.at(0));
|
||||
Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
|
||||
} else {
|
||||
// valid result: just return Base version
|
||||
|
|
@ -447,6 +506,15 @@ public:
|
|||
return 0.0;
|
||||
}
|
||||
|
||||
/// 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 */
|
||||
TriangulationResult point() const {
|
||||
return result_;
|
||||
|
|
@ -495,4 +563,10 @@ private:
|
|||
}
|
||||
};
|
||||
|
||||
/// traits
|
||||
template<class CAMERA>
|
||||
struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
|
||||
SmartProjectionFactor<CAMERA> > {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
|
|||
|
|
@ -1,146 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SmartProjectionPoseFactor.h
|
||||
* @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark
|
||||
* @author Luca Carlone
|
||||
* @author Chris Beall
|
||||
* @author Zsolt Kira
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/SmartProjectionFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
* If you are using the factor, please cite:
|
||||
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
|
||||
* independent sets in factor graphs: a unifying perspective based on smart factors,
|
||||
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class CALIBRATION>
|
||||
class SmartProjectionPoseFactor: public SmartProjectionFactor<
|
||||
PinholePose<CALIBRATION> > {
|
||||
|
||||
private:
|
||||
typedef PinholePose<CALIBRATION> Camera;
|
||||
typedef SmartProjectionFactor<Camera> Base;
|
||||
typedef SmartProjectionPoseFactor<CALIBRATION> This;
|
||||
|
||||
protected:
|
||||
|
||||
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
SmartProjectionPoseFactor(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) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionPoseFactor() {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
std::cout << s << "SmartProjectionPoseFactor, z = \n ";
|
||||
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 && Base::equals(p, tol);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(Base::cameras(values), 0.0);
|
||||
break;
|
||||
case JACOBIAN_Q :
|
||||
return this->createJacobianQFactor(Base::cameras(values), 0.0);
|
||||
break;
|
||||
default:
|
||||
return this->createHessianFactor(Base::cameras(values));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* error calculates the error of the factor.
|
||||
*/
|
||||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
return this->totalReprojectionError(Base::cameras(values));
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
|
||||
}; // end of class declaration
|
||||
|
||||
/// traits
|
||||
template<class CALIBRATION>
|
||||
struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
|
||||
SmartProjectionPoseFactor<CALIBRATION> > {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -17,6 +17,7 @@
|
|||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/slam/SmartProjectionFactor.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
|
|
@ -48,6 +49,7 @@ static size_t w = 640, h = 480;
|
|||
// default Cal3_S2 cameras
|
||||
namespace vanilla {
|
||||
typedef PinholeCamera<Cal3_S2> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
static Cal3_S2 K(fov, w, h);
|
||||
static Cal3_S2 K2(1500, 1200, 0, w, h);
|
||||
Camera level_camera(level_pose, K2);
|
||||
|
|
@ -64,6 +66,7 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
|||
// default Cal3_S2 poses
|
||||
namespace vanillaPose {
|
||||
typedef PinholePose<Cal3_S2> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
|
||||
Camera level_camera(level_pose, sharedK);
|
||||
Camera level_camera_right(pose_right, sharedK);
|
||||
|
|
@ -76,6 +79,7 @@ Camera cam3(pose_above, sharedK);
|
|||
// default Cal3_S2 poses
|
||||
namespace vanillaPose2 {
|
||||
typedef PinholePose<Cal3_S2> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
Camera level_camera(level_pose, sharedK2);
|
||||
Camera level_camera_right(pose_right, sharedK2);
|
||||
|
|
@ -88,6 +92,7 @@ Camera cam3(pose_above, sharedK2);
|
|||
// Cal3Bundler cameras
|
||||
namespace bundler {
|
||||
typedef PinholeCamera<Cal3Bundler> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
|
||||
Camera level_camera(level_pose, K);
|
||||
Camera level_camera_right(pose_right, K);
|
||||
|
|
@ -103,6 +108,7 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
|||
// Cal3Bundler poses
|
||||
namespace bundlerPose {
|
||||
typedef PinholePose<Cal3Bundler> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
|
||||
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
|
||||
Camera level_camera(level_pose, sharedBundlerK);
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
*/
|
||||
|
||||
#include "smartFactorScenarios.h"
|
||||
#include <gtsam/slam/SmartProjectionCameraFactor.h>
|
||||
#include <gtsam/slam/SmartProjectionFactor.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/assign/std/map.hpp>
|
||||
|
|
@ -43,9 +43,6 @@ Key c1 = 1, c2 = 2, c3 = 3;
|
|||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
|
||||
typedef SmartProjectionCameraFactor<Cal3_S2> SmartFactor;
|
||||
typedef SmartProjectionCameraFactor<Cal3Bundler> SmartFactorBundler;
|
||||
|
||||
template<class CALIBRATION>
|
||||
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
|
||||
const PinholeCamera<CALIBRATION>& camera) {
|
||||
|
|
@ -82,7 +79,7 @@ TEST( SmartProjectionCameraFactor, Constructor) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor2) {
|
||||
using namespace vanilla;
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -95,7 +92,7 @@ TEST( SmartProjectionCameraFactor, Constructor3) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor4) {
|
||||
using namespace vanilla;
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
|
||||
factor1.add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
|
|
@ -457,19 +454,19 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
|||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor4(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor5(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
|
||||
|
|
@ -533,19 +530,19 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor4(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor5(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
|
||||
|
|
@ -597,7 +594,7 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
|
|||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
|
||||
|
|
@ -626,7 +623,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
|
|||
values.insert(c2, level_camera_right);
|
||||
|
||||
NonlinearFactorGraph smartGraph;
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
smartGraph.push_back(factor1);
|
||||
|
|
@ -667,7 +664,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
values.insert(c2, level_camera_right);
|
||||
|
||||
NonlinearFactorGraph smartGraph;
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
smartGraph.push_back(factor1);
|
||||
|
|
@ -710,7 +707,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
// values.insert(c2, level_camera_right);
|
||||
//
|
||||
// NonlinearFactorGraph smartGraph;
|
||||
// SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
// SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
// factor1->add(level_uv, c1, unit2);
|
||||
// factor1->add(level_uv_right, c2, unit2);
|
||||
// smartGraph.push_back(factor1);
|
||||
|
|
@ -758,7 +755,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
|
|||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
Matrix expectedE;
|
||||
|
|
@ -803,8 +800,8 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
bool isImplicit = false;
|
||||
|
||||
// Hessian version
|
||||
SmartFactorBundler::shared_ptr explicitFactor(
|
||||
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy, useEPI,
|
||||
SmartFactor::shared_ptr explicitFactor(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold, manageDegeneracy, useEPI,
|
||||
isImplicit));
|
||||
explicitFactor->add(level_uv, c1, unit2);
|
||||
explicitFactor->add(level_uv_right, c2, unit2);
|
||||
|
|
@ -816,8 +813,8 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
|
||||
// Implicit Schur version
|
||||
isImplicit = true;
|
||||
SmartFactorBundler::shared_ptr implicitFactor(
|
||||
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy, useEPI,
|
||||
SmartFactor::shared_ptr implicitFactor(
|
||||
new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol, linThreshold, manageDegeneracy, useEPI,
|
||||
isImplicit));
|
||||
implicitFactor->add(level_uv, c1, unit2);
|
||||
implicitFactor->add(level_uv_right, c2, unit2);
|
||||
|
|
|
|||
|
|
@ -20,7 +20,6 @@
|
|||
*/
|
||||
|
||||
#include "smartFactorScenarios.h"
|
||||
#include <gtsam/slam/SmartProjectionPoseFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam/slam/PoseTranslationPrior.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
|
@ -49,8 +48,6 @@ static Symbol x2('X', 2);
|
|||
static Symbol x3('X', 3);
|
||||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
|
||||
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
// Make more verbose like so (in tests):
|
||||
|
|
@ -58,12 +55,14 @@ LevenbergMarquardtParams params;
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor2) {
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
using namespace vanillaPose;
|
||||
SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -76,7 +75,7 @@ TEST( SmartProjectionPoseFactor, Constructor3) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor4) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
|
||||
factor1.add(measurement1, x1, model);
|
||||
}
|
||||
|
||||
|
|
@ -227,6 +226,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
|
||||
graph.push_back(PriorFactor<Camera>(x2, cam2, noisePrior));
|
||||
|
||||
Values groundTruth;
|
||||
groundTruth.insert(x1, cam1);
|
||||
groundTruth.insert(x2, cam2);
|
||||
groundTruth.insert(x3, cam3);
|
||||
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
|
|
@ -257,7 +262,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactor, Factors ) {
|
||||
|
||||
typedef PinholePose<Cal3_S2> Camera;
|
||||
using namespace vanillaPose;
|
||||
|
||||
// Default cameras for simple derivatives
|
||||
Rot3 R;
|
||||
|
|
@ -479,15 +484,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -534,17 +539,17 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
|
|
@ -599,22 +604,22 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor4->add(measurements_cam4, views, model);
|
||||
|
||||
|
|
@ -658,15 +663,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
new SmartFactor(SmartFactor::JACOBIAN_Q, 1, -1, false, false));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
new SmartFactor(SmartFactor::JACOBIAN_Q, 1, -1, false, false));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
new SmartFactor(SmartFactor::JACOBIAN_Q, 1, -1, false, false));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -743,8 +748,11 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
|
|||
|
||||
DOUBLES_EQUAL(48406055, graph.error(values), 1);
|
||||
|
||||
cout << "SUCCEEDS : ==============================================" << endl;
|
||||
params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
Values result = optimizer.optimize();
|
||||
cout << "=========================================================" << endl;
|
||||
|
||||
DOUBLES_EQUAL(0, graph.error(result), 1e-9);
|
||||
|
||||
|
|
@ -777,13 +785,16 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
|
|||
|
||||
double rankTol = 10;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -858,11 +869,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
|
|||
|
||||
double rankTol = 50;
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy));
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy));
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -937,15 +950,18 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
double rankTol = 10;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy));
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy));
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy));
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -1142,7 +1158,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
|
||||
SmartFactorBundler factor(rankTol, linThreshold);
|
||||
using namespace bundlerPose;
|
||||
SmartFactor factor(SmartFactor::HESSIAN, rankTol, linThreshold);
|
||||
factor.add(measurement1, x1, model);
|
||||
}
|
||||
|
||||
|
|
@ -1167,13 +1184,13 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
|
|||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -1237,16 +1254,19 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
|
|||
|
||||
double rankTol = 10;
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor1(
|
||||
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor2(
|
||||
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor3(
|
||||
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold,
|
||||
manageDegeneracy));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
|
|||
Loading…
Reference in New Issue