Some formatting/cleanup before fixing bug
dellaert
parent
4909fef21a
commit
2c99f68ed7
|
|
@ -51,295 +51,295 @@ class access;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement.
|
||||
* The calibration is unknown here compared to GenericProjectionFactor
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template <class CAMERA, class LANDMARK>
|
||||
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement.
|
||||
* The calibration is unknown here compared to GenericProjectionFactor
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class CAMERA, class LANDMARK>
|
||||
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA);
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK);
|
||||
|
||||
static const int DimC = FixedDimension<CAMERA>::value;
|
||||
static const int DimL = FixedDimension<LANDMARK>::value;
|
||||
typedef Eigen::Matrix<double, 2, DimC> JacobianC;
|
||||
typedef Eigen::Matrix<double, 2, DimL> JacobianL;
|
||||
static const int DimC = FixedDimension<CAMERA>::value;
|
||||
static const int DimL = FixedDimension<LANDMARK>::value;
|
||||
typedef Eigen::Matrix<double, 2, DimC> JacobianC;
|
||||
typedef Eigen::Matrix<double, 2, DimL> JacobianL;
|
||||
|
||||
protected:
|
||||
protected:
|
||||
|
||||
Point2 measured_; ///< the 2D measurement
|
||||
Point2 measured_; ///< the 2D measurement
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
typedef GeneralSFMFactor<CAMERA, LANDMARK> This; ///< typedef for this object
|
||||
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base; ///< typedef for the base class
|
||||
typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
|
||||
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;///< typedef for the base class
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||
* @param model is the standard deviation of the measurements
|
||||
* @param cameraKey is the index of the camera
|
||||
* @param landmarkKey is the index of the landmark
|
||||
*/
|
||||
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
|
||||
Base(model, cameraKey, landmarkKey), measured_(measured) {}
|
||||
|
||||
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
|
||||
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
|
||||
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
|
||||
|
||||
virtual ~GeneralSFMFactor() {} ///< destructor
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter for printing out Symbols
|
||||
*/
|
||||
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
|
||||
const This* e = dynamic_cast<const This*>(&p);
|
||||
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
|
||||
}
|
||||
|
||||
/** h(x)-z */
|
||||
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
|
||||
try {
|
||||
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
|
||||
return reprojError.vector();
|
||||
}
|
||||
catch( CheiralityException& e) {
|
||||
if (H1) *H1 = JacobianC::Zero();
|
||||
if (H2) *H2 = JacobianL::Zero();
|
||||
// TODO warn if verbose output asked for
|
||||
return zero(2);
|
||||
}
|
||||
}
|
||||
|
||||
class LinearizedFactor : public JacobianFactor {
|
||||
// Fixed size matrices
|
||||
// TODO: implement generic BinaryJacobianFactor<N,M1,M2> next to
|
||||
// JacobianFactor
|
||||
JacobianC AC_;
|
||||
JacobianL AL_;
|
||||
Vector2 b_;
|
||||
|
||||
public:
|
||||
/// Constructor
|
||||
LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2,
|
||||
const Vector2& b,
|
||||
const SharedDiagonal& model = SharedDiagonal())
|
||||
: JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {}
|
||||
|
||||
// Fixed-size matrix update
|
||||
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
|
||||
gttic(updateHessian_LinearizedFactor);
|
||||
|
||||
// Whiten the factor if it has a noise model
|
||||
const SharedDiagonal& model = get_model();
|
||||
if (model && !model->isUnit()) {
|
||||
if (model->isConstrained())
|
||||
throw std::invalid_argument(
|
||||
"JacobianFactor::updateHessian: cannot update information with "
|
||||
"constrained noise model");
|
||||
JacobianFactor whitenedFactor = whiten();
|
||||
whitenedFactor.updateHessian(infoKeys, info);
|
||||
} else {
|
||||
// First build an array of slots
|
||||
DenseIndex slotC = Slot(infoKeys, keys_.front());
|
||||
DenseIndex slotL = Slot(infoKeys, keys_.back());
|
||||
DenseIndex slotB = info->nBlocks() - 1;
|
||||
|
||||
// We perform I += A'*A to the upper triangle
|
||||
(*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose());
|
||||
(*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_;
|
||||
(*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_;
|
||||
(*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose());
|
||||
(*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_;
|
||||
(*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose());
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/// Linearize using fixed-size matrices
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
|
||||
// Only linearize if the factor is active
|
||||
if (!this->active(values)) return boost::shared_ptr<LinearizedFactor>();
|
||||
|
||||
const Key key1 = this->key1(), key2 = this->key2();
|
||||
JacobianC H1;
|
||||
JacobianL H2;
|
||||
Vector2 b;
|
||||
try {
|
||||
const CAMERA& camera = values.at<CAMERA>(key1);
|
||||
const LANDMARK& point = values.at<LANDMARK>(key2);
|
||||
Point2 reprojError(camera.project2(point, H1, H2) - measured());
|
||||
b = -reprojError.vector();
|
||||
} catch (CheiralityException& e) {
|
||||
H1.setZero();
|
||||
H2.setZero();
|
||||
b.setZero();
|
||||
// TODO warn if verbose output asked for
|
||||
}
|
||||
|
||||
// Whiten the system if needed
|
||||
const SharedNoiseModel& noiseModel = this->get_noiseModel();
|
||||
if (noiseModel && !noiseModel->isUnit()) {
|
||||
// TODO: implement WhitenSystem for fixed size matrices and include
|
||||
// above
|
||||
H1 = noiseModel->Whiten(H1);
|
||||
H2 = noiseModel->Whiten(H2);
|
||||
b = noiseModel->Whiten(b);
|
||||
}
|
||||
|
||||
if (noiseModel && noiseModel->isConstrained()) {
|
||||
using noiseModel::Constrained;
|
||||
return boost::make_shared<LinearizedFactor>(
|
||||
key1, H1, key2, H2, b,
|
||||
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
|
||||
} else {
|
||||
return boost::make_shared<LinearizedFactor>(key1, H1, key2, H2, b);
|
||||
}
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
inline const Point2 measured() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor2",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||
}
|
||||
};
|
||||
|
||||
template<class CAMERA, class LANDMARK>
|
||||
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
|
||||
GeneralSFMFactor<CAMERA, LANDMARK> > {
|
||||
};
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement.
|
||||
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
|
||||
* Constructor
|
||||
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||
* @param model is the standard deviation of the measurements
|
||||
* @param cameraKey is the index of the camera
|
||||
* @param landmarkKey is the index of the landmark
|
||||
*/
|
||||
template <class CALIBRATION>
|
||||
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
|
||||
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
|
||||
Base(model, cameraKey, landmarkKey), measured_(measured) {}
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
|
||||
static const int DimK = FixedDimension<CALIBRATION>::value;
|
||||
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
|
||||
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
|
||||
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
|
||||
|
||||
protected:
|
||||
virtual ~GeneralSFMFactor() {} ///< destructor
|
||||
|
||||
Point2 measured_; ///< the 2D measurement
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
|
||||
|
||||
public:
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter for printing out Symbols
|
||||
*/
|
||||
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
typedef GeneralSFMFactor2<CALIBRATION> This;
|
||||
typedef PinholeCamera<CALIBRATION> Camera; ///< typedef for camera type
|
||||
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base; ///< typedef for the base class
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
|
||||
const This* e = dynamic_cast<const This*>(&p);
|
||||
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
|
||||
}
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||
* @param model is the standard deviation of the measurements
|
||||
* @param poseKey is the index of the camera
|
||||
* @param landmarkKey is the index of the landmark
|
||||
* @param calibKey is the index of the calibration
|
||||
*/
|
||||
GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
|
||||
Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
|
||||
GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor
|
||||
|
||||
virtual ~GeneralSFMFactor2() {} ///< destructor
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
/** h(x)-z */
|
||||
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
|
||||
try {
|
||||
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
|
||||
return reprojError.vector();
|
||||
}
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
|
||||
const This* e = dynamic_cast<const This*>(&p);
|
||||
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
|
||||
}
|
||||
|
||||
/** h(x)-z */
|
||||
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none,
|
||||
boost::optional<Matrix&> H3=boost::none) const
|
||||
{
|
||||
try {
|
||||
Camera camera(pose3,calib);
|
||||
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
|
||||
return reprojError.vector();
|
||||
}
|
||||
catch( CheiralityException& e) {
|
||||
if (H1) *H1 = zeros(2, 6);
|
||||
if (H2) *H2 = zeros(2, 3);
|
||||
if (H3) *H3 = zeros(2, DimK);
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
|
||||
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||
}
|
||||
catch( CheiralityException& e) {
|
||||
if (H1) *H1 = JacobianC::Zero();
|
||||
if (H2) *H2 = JacobianL::Zero();
|
||||
// TODO warn if verbose output asked for
|
||||
return zero(2);
|
||||
}
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
inline const Point2 measured() const {
|
||||
return measured_;
|
||||
}
|
||||
class LinearizedFactor : public JacobianFactor {
|
||||
// Fixed size matrices
|
||||
// TODO: implement generic BinaryJacobianFactor<N,M1,M2> next to
|
||||
// JacobianFactor
|
||||
JacobianC AC_;
|
||||
JacobianL AL_;
|
||||
Vector2 b_;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor3",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||
public:
|
||||
/// Constructor
|
||||
LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2,
|
||||
const Vector2& b,
|
||||
const SharedDiagonal& model = SharedDiagonal())
|
||||
: JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {}
|
||||
|
||||
// Fixed-size matrix update
|
||||
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
|
||||
gttic(updateHessian_LinearizedFactor);
|
||||
|
||||
// Whiten the factor if it has a noise model
|
||||
const SharedDiagonal& model = get_model();
|
||||
if (model && !model->isUnit()) {
|
||||
if (model->isConstrained())
|
||||
throw std::invalid_argument(
|
||||
"JacobianFactor::updateHessian: cannot update information with "
|
||||
"constrained noise model");
|
||||
JacobianFactor whitenedFactor = whiten();
|
||||
whitenedFactor.updateHessian(infoKeys, info);
|
||||
} else {
|
||||
// First build an array of slots
|
||||
DenseIndex slotC = Slot(infoKeys, keys_.front());
|
||||
DenseIndex slotL = Slot(infoKeys, keys_.back());
|
||||
DenseIndex slotB = info->nBlocks() - 1;
|
||||
|
||||
// We perform I += A'*A to the upper triangle
|
||||
(*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose());
|
||||
(*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_;
|
||||
(*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_;
|
||||
(*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose());
|
||||
(*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_;
|
||||
(*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose());
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<class CALIBRATION>
|
||||
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
|
||||
GeneralSFMFactor2<CALIBRATION> > {
|
||||
};
|
||||
/// Linearize using fixed-size matrices
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
|
||||
// Only linearize if the factor is active
|
||||
if (!this->active(values)) return boost::shared_ptr<LinearizedFactor>();
|
||||
|
||||
const Key key1 = this->key1(), key2 = this->key2();
|
||||
JacobianC H1;
|
||||
JacobianL H2;
|
||||
Vector2 b;
|
||||
try {
|
||||
const CAMERA& camera = values.at<CAMERA>(key1);
|
||||
const LANDMARK& point = values.at<LANDMARK>(key2);
|
||||
Point2 reprojError(camera.project2(point, H1, H2) - measured());
|
||||
b = -reprojError.vector();
|
||||
} catch (CheiralityException& e) {
|
||||
H1.setZero();
|
||||
H2.setZero();
|
||||
b.setZero();
|
||||
// TODO warn if verbose output asked for
|
||||
}
|
||||
|
||||
// Whiten the system if needed
|
||||
const SharedNoiseModel& noiseModel = this->get_noiseModel();
|
||||
if (noiseModel && !noiseModel->isUnit()) {
|
||||
// TODO: implement WhitenSystem for fixed size matrices and include
|
||||
// above
|
||||
H1 = noiseModel->Whiten(H1);
|
||||
H2 = noiseModel->Whiten(H2);
|
||||
b = noiseModel->Whiten(b);
|
||||
}
|
||||
|
||||
if (noiseModel && noiseModel->isConstrained()) {
|
||||
using noiseModel::Constrained;
|
||||
return boost::make_shared<LinearizedFactor>(
|
||||
key1, H1, key2, H2, b,
|
||||
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
|
||||
} else {
|
||||
return boost::make_shared<LinearizedFactor>(key1, H1, key2, H2, b);
|
||||
}
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
inline const Point2 measured() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor2",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||
}
|
||||
};
|
||||
|
||||
template<class CAMERA, class LANDMARK>
|
||||
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
|
||||
GeneralSFMFactor<CAMERA, LANDMARK> > {
|
||||
};
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement.
|
||||
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
|
||||
*/
|
||||
template<class CALIBRATION>
|
||||
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
|
||||
static const int DimK = FixedDimension<CALIBRATION>::value;
|
||||
|
||||
protected:
|
||||
|
||||
Point2 measured_; ///< the 2D measurement
|
||||
|
||||
public:
|
||||
|
||||
typedef GeneralSFMFactor2<CALIBRATION> This;
|
||||
typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
|
||||
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||
* @param model is the standard deviation of the measurements
|
||||
* @param poseKey is the index of the camera
|
||||
* @param landmarkKey is the index of the landmark
|
||||
* @param calibKey is the index of the calibration
|
||||
*/
|
||||
GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
|
||||
Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
|
||||
GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor
|
||||
|
||||
virtual ~GeneralSFMFactor2() {} ///< destructor
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
|
||||
const This* e = dynamic_cast<const This*>(&p);
|
||||
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
|
||||
}
|
||||
|
||||
/** h(x)-z */
|
||||
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none,
|
||||
boost::optional<Matrix&> H3=boost::none) const
|
||||
{
|
||||
try {
|
||||
Camera camera(pose3,calib);
|
||||
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
|
||||
return reprojError.vector();
|
||||
}
|
||||
catch( CheiralityException& e) {
|
||||
if (H1) *H1 = zeros(2, 6);
|
||||
if (H2) *H2 = zeros(2, 3);
|
||||
if (H3) *H3 = zeros(2, DimK);
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
|
||||
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||
}
|
||||
return zero(2);
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
inline const Point2 measured() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor3",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||
}
|
||||
};
|
||||
|
||||
template<class CALIBRATION>
|
||||
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
|
||||
GeneralSFMFactor2<CALIBRATION> > {
|
||||
};
|
||||
|
||||
} //namespace
|
||||
|
|
|
|||
|
|
@ -49,7 +49,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
|
|||
|
||||
class Graph: public NonlinearFactorGraph {
|
||||
public:
|
||||
void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
|
||||
void addMeasurement(int i, int j, const Point2& z,
|
||||
const SharedNoiseModel& model) {
|
||||
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
|
||||
}
|
||||
|
||||
|
|
@ -65,98 +66,100 @@ public:
|
|||
|
||||
};
|
||||
|
||||
static double getGaussian()
|
||||
{
|
||||
double S,V1,V2;
|
||||
// Use Box-Muller method to create gauss noise from uniform noise
|
||||
do
|
||||
{
|
||||
double U1 = rand() / (double)(RAND_MAX);
|
||||
double U2 = rand() / (double)(RAND_MAX);
|
||||
V1 = 2 * U1 - 1; /* V1=[-1,1] */
|
||||
V2 = 2 * U2 - 1; /* V2=[-1,1] */
|
||||
S = V1 * V1 + V2 * V2;
|
||||
} while(S>=1);
|
||||
return sqrt(-2.0f * (double)log(S) / S) * V1;
|
||||
static double getGaussian() {
|
||||
double S, V1, V2;
|
||||
// Use Box-Muller method to create gauss noise from uniform noise
|
||||
do {
|
||||
double U1 = rand() / (double) (RAND_MAX);
|
||||
double U2 = rand() / (double) (RAND_MAX);
|
||||
V1 = 2 * U1 - 1; /* V1=[-1,1] */
|
||||
V2 = 2 * U2 - 1; /* V2=[-1,1] */
|
||||
S = V1 * V1 + V2 * V2;
|
||||
} while (S >= 1);
|
||||
return sqrt(-2.f * (double) log(S) / S) * V1;
|
||||
}
|
||||
|
||||
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GeneralSFMFactor, equals )
|
||||
{
|
||||
TEST( GeneralSFMFactor, equals ) {
|
||||
// Create two identical factors and make sure they're equal
|
||||
Point2 z(323.,240.);
|
||||
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
|
||||
Point2 z(323., 240.);
|
||||
const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
|
||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||
boost::shared_ptr<Projection>
|
||||
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
boost::shared_ptr<Projection> factor1(
|
||||
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
|
||||
boost::shared_ptr<Projection>
|
||||
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
boost::shared_ptr<Projection> factor2(
|
||||
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
|
||||
EXPECT(assert_equal(*factor1, *factor2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GeneralSFMFactor, error ) {
|
||||
Point2 z(3.,0.);
|
||||
Point2 z(3., 0.);
|
||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(2));
|
||||
Projection factor(z, sigma, X(1), L(1));
|
||||
// For the following configuration, the factor predicts 320,240
|
||||
Values values;
|
||||
Rot3 R;
|
||||
Point3 t1(0,0,-6);
|
||||
Pose3 x1(R,t1);
|
||||
Point3 t1(0, 0, -6);
|
||||
Pose3 x1(R, t1);
|
||||
values.insert(X(1), GeneralCamera(x1));
|
||||
Point3 l1; values.insert(L(1), l1);
|
||||
EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor.unwhitenedError(values)));
|
||||
Point3 l1;
|
||||
values.insert(L(1), l1);
|
||||
EXPECT(
|
||||
assert_equal(((Vector ) Vector2(-3., 0.)),
|
||||
factor.unwhitenedError(values)));
|
||||
}
|
||||
|
||||
static const double baseline = 5.0 ;
|
||||
static const double baseline = 5.;
|
||||
|
||||
/* ************************************************************************* */
|
||||
static vector<Point3> genPoint3() {
|
||||
const double z = 5;
|
||||
vector<Point3> landmarks ;
|
||||
landmarks.push_back(Point3 (-1.0,-1.0, z));
|
||||
landmarks.push_back(Point3 (-1.0, 1.0, z));
|
||||
landmarks.push_back(Point3 ( 1.0, 1.0, z));
|
||||
landmarks.push_back(Point3 ( 1.0,-1.0, z));
|
||||
landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
|
||||
landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
|
||||
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
|
||||
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
|
||||
return landmarks ;
|
||||
vector<Point3> landmarks;
|
||||
landmarks.push_back(Point3(-1., -1., z));
|
||||
landmarks.push_back(Point3(-1., 1., z));
|
||||
landmarks.push_back(Point3(1., 1., z));
|
||||
landmarks.push_back(Point3(1., -1., z));
|
||||
landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(-2., -2., 2 * z));
|
||||
landmarks.push_back(Point3(-2., 2., 2 * z));
|
||||
landmarks.push_back(Point3(2., 2., 2 * z));
|
||||
landmarks.push_back(Point3(2., -2., 2 * z));
|
||||
return landmarks;
|
||||
}
|
||||
|
||||
static vector<GeneralCamera> genCameraDefaultCalibration() {
|
||||
vector<GeneralCamera> X ;
|
||||
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
|
||||
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
|
||||
return X ;
|
||||
vector<GeneralCamera> X;
|
||||
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.))));
|
||||
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.))));
|
||||
return X;
|
||||
}
|
||||
|
||||
static vector<GeneralCamera> genCameraVariableCalibration() {
|
||||
const Cal3_S2 K(640,480,0.01,320,240);
|
||||
vector<GeneralCamera> X ;
|
||||
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
|
||||
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
|
||||
return X ;
|
||||
const Cal3_S2 K(640, 480, 0.1, 320, 240);
|
||||
vector<GeneralCamera> X;
|
||||
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K));
|
||||
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K));
|
||||
return X;
|
||||
}
|
||||
|
||||
static boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
|
||||
static boost::shared_ptr<Ordering> getOrdering(
|
||||
const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
|
||||
boost::shared_ptr<Ordering> ordering(new Ordering);
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
|
||||
return ordering ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i)
|
||||
ordering->push_back(L(i));
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
ordering->push_back(X(i));
|
||||
return ordering;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GeneralSFMFactor, optimize_defaultK ) {
|
||||
|
||||
|
|
@ -165,32 +168,32 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
|
|||
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = cameras.size()*landmarks.size() ;
|
||||
const size_t nMeasurements = cameras.size() * landmarks.size();
|
||||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
values.insert(L(i), pt) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
values.insert(L(i), pt);
|
||||
}
|
||||
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
|
||||
// Create an ordering of the variables
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
|
||||
|
|
@ -202,38 +205,37 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
|
|||
vector<GeneralCamera> cameras = genCameraVariableCalibration();
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = cameras.size()*landmarks.size() ;
|
||||
const size_t nMeasurements = cameras.size() * landmarks.size();
|
||||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
if ( i == 0 ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
values.insert(L(i), pt) ;
|
||||
}
|
||||
else {
|
||||
values.insert(L(i), landmarks[i]) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
if (i == 0) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
values.insert(L(i), pt);
|
||||
} else {
|
||||
values.insert(L(i), landmarks[i]);
|
||||
}
|
||||
}
|
||||
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
|
|
@ -246,35 +248,35 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
|
|||
vector<GeneralCamera> cameras = genCameraVariableCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = landmarks.size()*cameras.size();
|
||||
const size_t nMeasurements = landmarks.size() * cameras.size();
|
||||
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
//Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
|
||||
values.insert(L(i), pt) ;
|
||||
values.insert(L(i), pt);
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
graph.addCameraConstraint(i, cameras[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
|
|
@ -288,50 +290,45 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
|
|||
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = landmarks.size()*cameras.size();
|
||||
const size_t nMeasurements = landmarks.size() * cameras.size();
|
||||
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i ) {
|
||||
const double
|
||||
rot_noise = 1e-5,
|
||||
trans_noise = 1e-3,
|
||||
focal_noise = 1,
|
||||
skew_noise = 1e-5;
|
||||
if ( i == 0 ) {
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
}
|
||||
else {
|
||||
for (size_t i = 0; i < cameras.size(); ++i) {
|
||||
const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
|
||||
skew_noise = 1e-5;
|
||||
if (i == 0) {
|
||||
values.insert(X(i), cameras[i]);
|
||||
} else {
|
||||
|
||||
Vector delta = (Vector(11) <<
|
||||
rot_noise, rot_noise, rot_noise, // rotation
|
||||
trans_noise, trans_noise, trans_noise, // translation
|
||||
focal_noise, focal_noise, // f_x, f_y
|
||||
skew_noise, // s
|
||||
trans_noise, trans_noise // ux, uy
|
||||
Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation
|
||||
trans_noise, trans_noise, trans_noise, // translation
|
||||
focal_noise, focal_noise, // f_x, f_y
|
||||
skew_noise, // s
|
||||
trans_noise, trans_noise // ux, uy
|
||||
).finished();
|
||||
values.insert(X(i), cameras[i].retract(delta)) ;
|
||||
values.insert(X(i), cameras[i].retract(delta));
|
||||
}
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
values.insert(L(i), landmarks[i]) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
values.insert(L(i), landmarks[i]);
|
||||
}
|
||||
|
||||
// fix X0 and all landmarks, allow only the cameras[1] to move
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i )
|
||||
for (size_t i = 0; i < landmarks.size(); ++i)
|
||||
graph.addPoint3Constraint(i, landmarks[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
|
|
@ -344,38 +341,40 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
|
|||
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = cameras.size()*landmarks.size() ;
|
||||
const size_t nMeasurements = cameras.size() * landmarks.size();
|
||||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
values.insert(L(i), pt) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
values.insert(L(i), pt);
|
||||
}
|
||||
|
||||
// Constrain position of system with the first camera constrained to the origin
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
|
||||
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
|
||||
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
|
||||
graph.push_back(
|
||||
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
|
||||
noiseModel::Isotropic::Sigma(1, 10.)));
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
|
|
@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
|
|||
// Tests range factor between a GeneralCamera and a Pose3
|
||||
Graph graph;
|
||||
graph.addCameraConstraint(0, GeneralCamera());
|
||||
graph.push_back(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
|
||||
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
|
||||
graph.push_back(
|
||||
RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.,
|
||||
noiseModel::Isotropic::Sigma(1, 1.)));
|
||||
graph.push_back(
|
||||
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
|
||||
noiseModel::Isotropic::Sigma(6, 1.)));
|
||||
|
||||
Values init;
|
||||
init.insert(X(0), GeneralCamera());
|
||||
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
|
||||
init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
|
||||
|
||||
// The optimal value between the 2m range factor and 1m prior is 1.5m
|
||||
Values expected;
|
||||
expected.insert(X(0), GeneralCamera());
|
||||
expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0)));
|
||||
expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.)));
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
params.absoluteErrorTol = 1e-9;
|
||||
|
|
@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
|
|||
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
|
||||
// Tests range factor between a CalibratedCamera and a Pose3
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0)));
|
||||
graph.push_back(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
|
||||
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
|
||||
graph.push_back(
|
||||
PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(),
|
||||
noiseModel::Isotropic::Sigma(6, 1.)));
|
||||
graph.push_back(
|
||||
RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.,
|
||||
noiseModel::Isotropic::Sigma(1, 1.)));
|
||||
graph.push_back(
|
||||
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
|
||||
noiseModel::Isotropic::Sigma(6, 1.)));
|
||||
|
||||
Values init;
|
||||
init.insert(X(0), CalibratedCamera());
|
||||
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
|
||||
init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
|
||||
|
||||
Values expected;
|
||||
expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
|
||||
expected.insert(X(0),
|
||||
CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
|
||||
expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
|
|
@ -432,50 +442,58 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GeneralSFMFactor, Linearize) {
|
||||
Point2 z(3.,0.);
|
||||
Point2 z(3., 0.);
|
||||
|
||||
// Create Values
|
||||
Values values;
|
||||
Rot3 R;
|
||||
Point3 t1(0,0,-6);
|
||||
Pose3 x1(R,t1);
|
||||
Point3 t1(0, 0, -6);
|
||||
Pose3 x1(R, t1);
|
||||
values.insert(X(1), GeneralCamera(x1));
|
||||
Point3 l1; values.insert(L(1), l1);
|
||||
Point3 l1;
|
||||
values.insert(L(1), l1);
|
||||
|
||||
// Test with Empty Model
|
||||
{
|
||||
const SharedNoiseModel model;
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected,*actual,1e-9));
|
||||
const SharedNoiseModel model;
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
|
||||
values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected, *actual, 1e-9));
|
||||
}
|
||||
// Test with Unit Model
|
||||
{
|
||||
const SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected,*actual,1e-9));
|
||||
const SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
|
||||
values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected, *actual, 1e-9));
|
||||
}
|
||||
// Test with Isotropic noise
|
||||
{
|
||||
const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5));
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected,*actual,1e-9));
|
||||
const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.5));
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
|
||||
values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected, *actual, 1e-9));
|
||||
}
|
||||
// Test with Constrained Model
|
||||
{
|
||||
const SharedNoiseModel model(noiseModel::Constrained::All(2));
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected,*actual,1e-9));
|
||||
const SharedNoiseModel model(noiseModel::Constrained::All(2));
|
||||
Projection factor(z, model, X(1), L(1));
|
||||
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
|
||||
values);
|
||||
GaussianFactor::shared_ptr actual = factor.linearize(values);
|
||||
EXPECT(assert_equal(*expected, *actual, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -49,7 +49,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
|
|||
/* ************************************************************************* */
|
||||
class Graph: public NonlinearFactorGraph {
|
||||
public:
|
||||
void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
|
||||
void addMeasurement(const int& i, const int& j, const Point2& z,
|
||||
const SharedNoiseModel& model) {
|
||||
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
|
||||
}
|
||||
|
||||
|
|
@ -65,97 +66,101 @@ public:
|
|||
|
||||
};
|
||||
|
||||
static double getGaussian()
|
||||
{
|
||||
double S,V1,V2;
|
||||
// Use Box-Muller method to create gauss noise from uniform noise
|
||||
do
|
||||
{
|
||||
double U1 = rand() / (double)(RAND_MAX);
|
||||
double U2 = rand() / (double)(RAND_MAX);
|
||||
V1 = 2 * U1 - 1; /* V1=[-1,1] */
|
||||
V2 = 2 * U2 - 1; /* V2=[-1,1] */
|
||||
S = V1 * V1 + V2 * V2;
|
||||
} while(S>=1);
|
||||
return sqrt(-2.0f * (double)log(S) / S) * V1;
|
||||
static double getGaussian() {
|
||||
double S, V1, V2;
|
||||
// Use Box-Muller method to create gauss noise from uniform noise
|
||||
do {
|
||||
double U1 = rand() / (double) (RAND_MAX);
|
||||
double U2 = rand() / (double) (RAND_MAX);
|
||||
V1 = 2 * U1 - 1; /* V1=[-1,1] */
|
||||
V2 = 2 * U2 - 1; /* V2=[-1,1] */
|
||||
S = V1 * V1 + V2 * V2;
|
||||
} while (S >= 1);
|
||||
return sqrt(-2.f * (double) log(S) / S) * V1;
|
||||
}
|
||||
|
||||
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GeneralSFMFactor_Cal3Bundler, equals )
|
||||
{
|
||||
TEST( GeneralSFMFactor_Cal3Bundler, equals ) {
|
||||
// Create two identical factors and make sure they're equal
|
||||
Point2 z(323.,240.);
|
||||
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
|
||||
Point2 z(323., 240.);
|
||||
const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
|
||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||
boost::shared_ptr<Projection>
|
||||
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
boost::shared_ptr<Projection> factor1(
|
||||
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
|
||||
boost::shared_ptr<Projection>
|
||||
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
boost::shared_ptr<Projection> factor2(
|
||||
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||
|
||||
EXPECT(assert_equal(*factor1, *factor2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GeneralSFMFactor_Cal3Bundler, error ) {
|
||||
Point2 z(3.,0.);
|
||||
Point2 z(3., 0.);
|
||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||
boost::shared_ptr<Projection>
|
||||
factor(new Projection(z, sigma, X(1), L(1)));
|
||||
boost::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), L(1)));
|
||||
// For the following configuration, the factor predicts 320,240
|
||||
Values values;
|
||||
Rot3 R;
|
||||
Point3 t1(0,0,-6);
|
||||
Pose3 x1(R,t1);
|
||||
Point3 t1(0, 0, -6);
|
||||
Pose3 x1(R, t1);
|
||||
values.insert(X(1), GeneralCamera(x1));
|
||||
Point3 l1; values.insert(L(1), l1);
|
||||
EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values)));
|
||||
Point3 l1;
|
||||
values.insert(L(1), l1);
|
||||
EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values)));
|
||||
}
|
||||
|
||||
|
||||
static const double baseline = 5.0 ;
|
||||
static const double baseline = 5.;
|
||||
|
||||
/* ************************************************************************* */
|
||||
static vector<Point3> genPoint3() {
|
||||
const double z = 5;
|
||||
vector<Point3> landmarks ;
|
||||
landmarks.push_back(Point3 (-1.0,-1.0, z));
|
||||
landmarks.push_back(Point3 (-1.0, 1.0, z));
|
||||
landmarks.push_back(Point3 ( 1.0, 1.0, z));
|
||||
landmarks.push_back(Point3 ( 1.0,-1.0, z));
|
||||
landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z));
|
||||
landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
|
||||
landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
|
||||
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
|
||||
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
|
||||
return landmarks ;
|
||||
vector<Point3> landmarks;
|
||||
landmarks.push_back(Point3(-1., -1., z));
|
||||
landmarks.push_back(Point3(-1., 1., z));
|
||||
landmarks.push_back(Point3(1., 1., z));
|
||||
landmarks.push_back(Point3(1., -1., z));
|
||||
landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
|
||||
landmarks.push_back(Point3(-2., -2., 2 * z));
|
||||
landmarks.push_back(Point3(-2., 2., 2 * z));
|
||||
landmarks.push_back(Point3(2., 2., 2 * z));
|
||||
landmarks.push_back(Point3(2., -2., 2 * z));
|
||||
return landmarks;
|
||||
}
|
||||
|
||||
static vector<GeneralCamera> genCameraDefaultCalibration() {
|
||||
vector<GeneralCamera> cameras ;
|
||||
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
|
||||
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
|
||||
return cameras ;
|
||||
vector<GeneralCamera> cameras;
|
||||
cameras.push_back(
|
||||
GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
|
||||
cameras.push_back(
|
||||
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
|
||||
return cameras;
|
||||
}
|
||||
|
||||
static vector<GeneralCamera> genCameraVariableCalibration() {
|
||||
const Cal3Bundler K(500,1e-3,1e-3);
|
||||
vector<GeneralCamera> cameras ;
|
||||
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
|
||||
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
|
||||
return cameras ;
|
||||
const Cal3Bundler K(500, 1e-3, 1e-3);
|
||||
vector<GeneralCamera> cameras;
|
||||
cameras.push_back(
|
||||
GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
|
||||
cameras.push_back(
|
||||
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
|
||||
return cameras;
|
||||
}
|
||||
|
||||
static boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
|
||||
static boost::shared_ptr<Ordering> getOrdering(
|
||||
const std::vector<GeneralCamera>& cameras,
|
||||
const std::vector<Point3>& landmarks) {
|
||||
boost::shared_ptr<Ordering> ordering(new Ordering);
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
|
||||
return ordering ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i)
|
||||
ordering->push_back(L(i));
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
ordering->push_back(X(i));
|
||||
return ordering;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -166,32 +171,32 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) {
|
|||
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = cameras.size()*landmarks.size() ;
|
||||
const size_t nMeasurements = cameras.size() * landmarks.size();
|
||||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
values.insert(L(i), pt) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
values.insert(L(i), pt);
|
||||
}
|
||||
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
|
||||
// Create an ordering of the variables
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
|
||||
|
|
@ -203,38 +208,37 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
|
|||
vector<GeneralCamera> cameras = genCameraVariableCalibration();
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = cameras.size()*landmarks.size() ;
|
||||
const size_t nMeasurements = cameras.size() * landmarks.size();
|
||||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
if ( i == 0 ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
values.insert(L(i), pt) ;
|
||||
}
|
||||
else {
|
||||
values.insert(L(i), landmarks[i]) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
if (i == 0) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
values.insert(L(i), pt);
|
||||
} else {
|
||||
values.insert(L(i), landmarks[i]);
|
||||
}
|
||||
}
|
||||
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
|
|
@ -247,35 +251,35 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) {
|
|||
vector<GeneralCamera> cameras = genCameraVariableCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = landmarks.size()*cameras.size();
|
||||
const size_t nMeasurements = landmarks.size() * cameras.size();
|
||||
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
//Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
|
||||
values.insert(L(i), pt) ;
|
||||
values.insert(L(i), pt);
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
graph.addCameraConstraint(i, cameras[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
|
|
@ -289,46 +293,43 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
|
|||
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = landmarks.size()*cameras.size();
|
||||
const size_t nMeasurements = landmarks.size() * cameras.size();
|
||||
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i ) {
|
||||
const double
|
||||
rot_noise = 1e-5, trans_noise = 1e-3,
|
||||
focal_noise = 1, distort_noise = 1e-3;
|
||||
if ( i == 0 ) {
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
}
|
||||
else {
|
||||
for (size_t i = 0; i < cameras.size(); ++i) {
|
||||
const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
|
||||
distort_noise = 1e-3;
|
||||
if (i == 0) {
|
||||
values.insert(X(i), cameras[i]);
|
||||
} else {
|
||||
|
||||
Vector delta = (Vector(9) <<
|
||||
rot_noise, rot_noise, rot_noise, // rotation
|
||||
trans_noise, trans_noise, trans_noise, // translation
|
||||
focal_noise, distort_noise, distort_noise // f, k1, k2
|
||||
Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation
|
||||
trans_noise, trans_noise, trans_noise, // translation
|
||||
focal_noise, distort_noise, distort_noise // f, k1, k2
|
||||
).finished();
|
||||
values.insert(X(i), cameras[i].retract(delta)) ;
|
||||
values.insert(X(i), cameras[i].retract(delta));
|
||||
}
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
values.insert(L(i), landmarks[i]) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
values.insert(L(i), landmarks[i]);
|
||||
}
|
||||
|
||||
// fix X0 and all landmarks, allow only the cameras[1] to move
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i )
|
||||
for (size_t i = 0; i < landmarks.size(); ++i)
|
||||
graph.addPoint3Constraint(i, landmarks[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
|
|
@ -341,43 +342,48 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
|
|||
|
||||
// add measurement with noise
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]) ;
|
||||
for (size_t j = 0; j < cameras.size(); ++j) {
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point2 pt = cameras[j].project(landmarks[i]);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = cameras.size()*landmarks.size() ;
|
||||
const size_t nMeasurements = cameras.size() * landmarks.size();
|
||||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
const double noise = baseline * 0.1;
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < cameras.size() ; ++i )
|
||||
values.insert(X(i), cameras[i]) ;
|
||||
for (size_t i = 0; i < cameras.size(); ++i)
|
||||
values.insert(X(i), cameras[i]);
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
|
||||
Point3 pt(landmarks[i].x()+noise*getGaussian(),
|
||||
landmarks[i].y()+noise*getGaussian(),
|
||||
landmarks[i].z()+noise*getGaussian());
|
||||
values.insert(L(i), pt) ;
|
||||
for (size_t i = 0; i < landmarks.size(); ++i) {
|
||||
Point3 pt(landmarks[i].x() + noise * getGaussian(),
|
||||
landmarks[i].y() + noise * getGaussian(),
|
||||
landmarks[i].z() + noise * getGaussian());
|
||||
values.insert(L(i), pt);
|
||||
}
|
||||
|
||||
// Constrain position of system with the first camera constrained to the origin
|
||||
graph.addCameraConstraint(0, cameras[0]);
|
||||
|
||||
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
|
||||
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
|
||||
graph.push_back(
|
||||
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
|
||||
noiseModel::Isotropic::Sigma(1, 10.)));
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
Ordering ordering = *getOrdering(cameras,landmarks);
|
||||
Ordering ordering = *getOrdering(cameras, landmarks);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
|
||||
Values final = optimizer.optimize();
|
||||
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue