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

BIG change: SmartFactorBase and SmartProjectionFactor now templated on CAMERA

release/4.3a0
dellaert 2015-02-23 12:43:43 +01:00
parent d5261e2e8d
commit d6f54475c3
7 changed files with 352 additions and 438 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
gtsam/slam/SmartFactorBase.h
View File

@ -45,6 +45,9 @@ template<class CAMERA>
class SmartFactorBase: public NonlinearFactor {
private:
typedef NonlinearFactor Base;
typedef SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z;
/**
* As of Feb 22, 2015, the noise model is the same for all measurements and
@ -56,9 +59,6 @@ private:
protected:
// Figure out the measurement type
typedef typename CAMERA::Measurement Z;
/**
* 2D measurement and noise model for each of the m views
* We keep a copy of measurements for I/O and computing the error.
@ -66,19 +66,11 @@ protected:
*/
std::vector<Z> measured_;
/// shorthand for base class type
typedef NonlinearFactor Base;
/// shorthand for this class
typedef SmartFactorBase<CAMERA> This;
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, Dim> Matrix2D; // F
// Definitions for block matrices used internally
typedef Eigen::Matrix<double, Dim, ZDim> MatrixD2; // F'
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
typedef Eigen::Matrix<double, Dim, Dim> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
typedef Eigen::Matrix<double, Dim, 1> VectorD;
@ -105,6 +97,10 @@ protected:
public:
// Definitions for blocks of F, externally visible
typedef Eigen::Matrix<double, ZDim, Dim> Matrix2D; // F
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor
@ -256,57 +252,19 @@ public:
}
/**
* Compute whitenedError, returning only the derivative E, i.e.,
* the stacked ZDim*3 derivatives of project with respect to the point.
* Assumes non-degenerate ! TODO explain this
*/
Vector reprojectionErrors(const Cameras& cameras, const Point3& point,
Matrix& E) const {
// Project into CameraSet, calculating derivatives
std::vector<Z> predicted;
predicted = cameras.project2(point, boost::none, E);
// if needed, whiten
size_t m = keys_.size();
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Calculate error
const Z& zi = measured_.at(i);
Vector bi = (zi - predicted[i]).vector();
b.segment<ZDim>(row) = bi;
}
return b;
}
/**
* Compute F, E, and optionally H, where F and E are the stacked derivatives
* with respect to the cameras, point, and calibration, respectively.
* The value of cameras/point are passed as parameters.
* Returns error vector b
* Compute reprojection errors and derivatives
* TODO: the treatment of body_P_sensor_ is weird: the transformation
* is applied in the caller, but the derivatives are computed here.
*/
Vector reprojectionErrors(const Cameras& cameras, const Point3& point,
typename Cameras::FBlocks& F, Matrix& E) const {
// Project into CameraSet, calculating derivatives
std::vector<Z> predicted;
predicted = cameras.project2(point, F, E);
Vector b = cameras.reprojectionErrors(point, measured_, F, E);
// Calculate error and whiten derivatives if needed
size_t m = keys_.size();
Vector b(ZDim * m);
for (size_t i = 0; i < m; i++) {
// Calculate error
const Z& zi = measured_.at(i);
Vector bi = (zi - predicted[i]).vector();
// if we have a sensor offset, correct camera derivatives
// Apply sensor chain rule if needed TODO: no simpler way ??
if (body_P_sensor_) {
// TODO: no simpler way ??
size_t m = keys_.size();
for (size_t i = 0; i < m; i++) {
const Pose3& pose_i = cameras[i].pose();
Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
Matrix66 J;
@ -314,6 +272,7 @@ public:
F[i].leftCols(6) *= J;
}
}
return b;
}
@ -344,7 +303,7 @@ public:
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
const Point3& point) const {
reprojectionErrors(cameras, point, E);
cameras.reprojectionErrors(point, measured_, boost::none, E);
P = PointCov(E);
}

36
gtsam/slam/SmartProjectionCameraFactor.h
View File

@ -25,23 +25,29 @@ namespace gtsam {
/**
* @addtogroup SLAM
*/
template<class CALIBRATION, size_t D>
class SmartProjectionCameraFactor: public SmartProjectionFactor<CALIBRATION, D> {
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 base class type
typedef SmartProjectionFactor<CALIBRATION, D> Base;
/// shorthand for this class
typedef SmartProjectionCameraFactor<CALIBRATION, D> This;
/// 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
@ -88,14 +94,14 @@ public:
/// get the dimension of the factor (number of rows on linearization)
virtual size_t dim() const {
return D * this->keys_.size(); // 6 for the pose and 3 for the calibration
return Dim * this->keys_.size(); // 6 for the pose and 3 for the calibration
}
/// Collect all cameras: important that in key order
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
Cameras cameras(const Values& values) const {
Cameras cameras;
BOOST_FOREACH(const Key& k, this->keys_)
cameras.push_back(values.at<typename Base::Camera>(k));
cameras.push_back(values.at<Camera>(k));
return cameras;
}
@ -109,19 +115,19 @@ public:
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<RegularHessianFactor<D> > linearizeToHessian(
virtual boost::shared_ptr<RegularHessianFactor<Dim> > linearizeToHessian(
const Values& values, double lambda=0.0) const {
return Base::createHessianFactor(cameras(values),lambda);
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<RegularImplicitSchurFactor<D> > linearizeToImplicit(
virtual boost::shared_ptr<RegularImplicitSchurFactor<Dim> > linearizeToImplicit(
const Values& values, double lambda=0.0) const {
return Base::createRegularImplicitSchurFactor(cameras(values),lambda);
}
/// linearize returns a Jacobianfactor that is an approximation of error(p)
virtual boost::shared_ptr<JacobianFactorQ<D, 2> > linearizeToJacobian(
virtual boost::shared_ptr<JacobianFactorQ<Dim, 2> > linearizeToJacobian(
const Values& values, double lambda=0.0) const {
return Base::createJacobianQFactor(cameras(values),lambda);
}

108
gtsam/slam/SmartProjectionFactor.h
View File

@ -60,11 +60,17 @@ enum LinearizationMode {
/**
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
*/
template<class CALIBRATION, size_t D>
class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>,
D> {
template<class CAMERA>
class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
private:
typedef SmartFactorBase<CAMERA> Base;
typedef SmartProjectionFactor<CAMERA> This;
protected:
static const int Dim = traits<CAMERA>::dimension; ///< CAMERA dimension
// Some triangulation parameters
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
@ -91,9 +97,6 @@ protected:
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<PinholeCamera<CALIBRATION>, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
@ -101,17 +104,13 @@ protected:
// average reprojection error is smaller than this threshold after triangulation,
// and the factor is disregarded if the error is large
/// shorthand for this class
typedef SmartProjectionFactor<CALIBRATION, D> This;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a pinhole camera
typedef PinholeCamera<CALIBRATION> Camera;
typedef CameraSet<Camera> Cameras;
/// shorthand for a set of cameras
typedef CameraSet<CAMERA> Cameras;
/**
* Constructor
@ -243,7 +242,7 @@ public:
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
point_ = triangulatePoint3<CAMERA>(cameras, this->measured_,
rankTolerance_, enableEPI_);
degenerate_ = false;
cheiralityException_ = false;
@ -251,7 +250,7 @@ public:
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
BOOST_FOREACH(const CAMERA& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
@ -314,7 +313,7 @@ public:
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0) const {
bool isDebug = false;
@ -338,10 +337,10 @@ public:
&& (this->cheiralityException_ || this->degenerate_))) {
// std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(D, D);
m = zeros(Dim, Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(D);
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs,
v = zero(Dim);
return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_, Gs, gs,
0.0);
}
@ -366,7 +365,7 @@ public:
<< "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
<< std::endl;
exit(1);
return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_,
this->state_->Gs, this->state_->gs, this->state_->f);
}
@ -378,7 +377,7 @@ public:
// Schur complement trick
// Frank says: should be possible to do this more efficiently?
// And we care, as in grouped factors this is called repeatedly
Matrix H(D * numKeys, D * numKeys);
Matrix H(Dim * numKeys, Dim * numKeys);
Vector gs_vector;
Matrix3 P = Base::PointCov(E, lambda);
@ -390,11 +389,11 @@ public:
// Populate Gs and gs
int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D;
gs.at(i1) = gs_vector.segment<D>(i1D);
DenseIndex i1D = i1 * Dim;
gs.at(i1) = gs_vector.segment<Dim>(i1D);
for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
Gs.at(GsCount2) = H.block<Dim, Dim>(i1D, i2 * Dim);
GsCount2++;
}
}
@ -405,37 +404,37 @@ public:
this->state_->gs = gs;
this->state_->f = f;
}
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs, f);
return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_, Gs, gs, f);
}
// create factor
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
boost::shared_ptr<RegularImplicitSchurFactor<Dim> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
else
return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
return boost::shared_ptr<RegularImplicitSchurFactor<Dim> >();
}
/// create factor
boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<Dim, 2> > createJacobianQFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda);
else
return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
return boost::make_shared<JacobianFactorQ<Dim, 2> >(this->keys_);
}
/// Create a factor, takes values
boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<Dim, 2> > createJacobianQFactor(
const Values& values, double lambda) const {
Cameras myCameras;
Cameras cameras;
// TODO triangulate twice ??
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda);
return createJacobianQFactor(cameras, lambda);
else
return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
return boost::make_shared<JacobianFactorQ<Dim, 2> >(this->keys_);
}
/// different (faster) way to compute Jacobian factor
@ -444,20 +443,20 @@ public:
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
else
return boost::make_shared<JacobianFactorSVD<D, 2> >(this->keys_);
return boost::make_shared<JacobianFactorSVD<Dim, 2> >(this->keys_);
}
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& myCameras) const {
Cameras& cameras) const {
Values valuesFactor;
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
myCameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(myCameras);
cameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
@ -477,27 +476,22 @@ public:
return true;
}
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const {
return Base::computeEP(E, P, cameras, point_);
}
/// Takes values
bool computeEP(Matrix& E, Matrix& P, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeEP(E, P, myCameras);
Base::computeEP(E, P, cameras, point_);
return nonDegenerate;
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, b, myCameras);
computeJacobians(Fblocks, E, b, cameras);
return nonDegenerate;
}
@ -513,12 +507,13 @@ public:
std::cout
<< "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used"
<< std::endl;
if (D > 6) {
if (Dim > 6) {
std::cout
<< "Management of degeneracy is not yet ready when one also optimizes for the calibration "
<< std::endl;
}
// TODO replace all this by Call to CameraSet
int numKeys = this->keys_.size();
E = zeros(2 * numKeys, 2);
b = zero(2 * numKeys);
@ -533,7 +528,6 @@ public:
Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
- this->measured_.at(i)).vector();
this->noiseModel_->WhitenSystem(Fi, Ei, bi);
f += bi.squaredNorm();
Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
E.block<2, 2>(2 * i, 0) = Ei;
@ -549,10 +543,10 @@ public:
/// takes values
bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Values& values) const {
typename Base::Cameras myCameras;
double good = computeCamerasAndTriangulate(values, myCameras);
typename Base::Cameras cameras;
double good = computeCamerasAndTriangulate(values, cameras);
if (good)
computeJacobiansSVD(Fblocks, Enull, b, myCameras);
computeJacobiansSVD(Fblocks, Enull, b, cameras);
return true;
}
@ -583,12 +577,12 @@ public:
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
return reprojectionError(myCameras);
return reprojectionError(cameras);
else
return zero(myCameras.size() * 2);
return zero(cameras.size() * 2);
}
/**

33
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -38,21 +38,22 @@ namespace gtsam {
* @addtogroup SLAM
*/
template<class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<CALIBRATION, 6> {
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)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<CALIBRATION> > sharedKs_; ///< shared pointer to calibration object (one for each camera)
public:
/// shorthand for base class type
typedef SmartProjectionFactor<CALIBRATION, 6> Base;
/// shorthand for this class
typedef SmartProjectionPoseFactor<CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -87,7 +88,7 @@ public:
const SharedNoiseModel noise_i,
const boost::shared_ptr<CALIBRATION> K_i) {
Base::add(measured_i, poseKey_i, noise_i);
K_all_.push_back(K_i);
sharedKs_.push_back(K_i);
}
/**
@ -102,7 +103,7 @@ public:
std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
Base::add(measurements, poseKeys, noises);
for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i));
sharedKs_.push_back(Ks.at(i));
}
}
@ -117,7 +118,7 @@ public:
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i), noise);
K_all_.push_back(K);
sharedKs_.push_back(K);
}
}
@ -129,7 +130,7 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartProjectionPoseFactor, z = \n ";
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, sharedKs_)
K->print("calibration = ");
Base::print("", keyFormatter);
}
@ -155,7 +156,7 @@ public:
if(Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
typename Base::Camera camera(pose, *K_all_[i++]);
Camera camera(pose, sharedKs_[i++]);
cameras.push_back(camera);
}
return cameras;
@ -193,9 +194,9 @@ public:
}
}
/** return the calibration object */
/** return calibration shared pointers */
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
return K_all_;
return sharedKs_;
}
private:
@ -205,7 +206,7 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_all_);
ar & BOOST_SERIALIZATION_NVP(sharedKs_);
}
}; // end of class declaration

4
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -72,8 +72,8 @@ static Point2 measurement1(323.0, 240.0);
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
typedef PinholeCamera<Cal3_S2> CameraCal3_S2;
typedef SmartProjectionCameraFactor<Cal3_S2, 11> SmartFactor;
typedef SmartProjectionCameraFactor<Cal3Bundler, 9> SmartFactorBundler;
typedef SmartProjectionCameraFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionCameraFactor<Cal3Bundler> SmartFactorBundler;
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
template<class CALIBRATION>

488
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

File diff suppressed because it is too large Load Diff

48
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -64,7 +64,7 @@ enum LinearizationMode {
* SmartStereoProjectionFactor: triangulates point
*/
template<class CALIBRATION, size_t D>
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera, D> {
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
protected:
// Some triangulation parameters
@ -94,7 +94,7 @@ protected:
typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<StereoCamera, D> Base;
typedef SmartFactorBase<StereoCamera> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
@ -465,11 +465,11 @@ public:
// /// Create a factor, takes values
// boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
// const Values& values, double lambda) const {
// Cameras myCameras;
// Cameras cameras;
// // TODO triangulate twice ??
// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
// if (nonDegenerate)
// return createJacobianQFactor(myCameras, lambda);
// return createJacobianQFactor(cameras, lambda);
// else
// return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
// }
@ -485,15 +485,15 @@ public:
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& myCameras) const {
Cameras& cameras) const {
Values valuesFactor;
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
myCameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(myCameras);
cameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
@ -520,20 +520,20 @@ public:
/// Takes values
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeEP(E, PointCov, myCameras);
computeEP(E, PointCov, cameras);
return nonDegenerate;
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, b, myCameras, 0.0);
computeJacobians(Fblocks, E, b, cameras, 0.0);
return nonDegenerate;
}
@ -598,10 +598,10 @@ public:
// /// takes values
// bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
// Matrix& Enull, Vector& b, const Values& values) const {
// typename Base::Cameras myCameras;
// double good = computeCamerasAndTriangulate(values, myCameras);
// typename Base::Cameras cameras;
// double good = computeCamerasAndTriangulate(values, cameras);
// if (good)
// computeJacobiansSVD(Fblocks, Enull, b, myCameras);
// computeJacobiansSVD(Fblocks, Enull, b, cameras);
// return true;
// }
//
@ -624,20 +624,14 @@ public:
return Base::computeJacobians(F, E, b, cameras, point_);
}
/// Calculate vector of re-projection errors, before applying noise model
/// Assumes triangulation was done and degeneracy handled
Vector reprojectionError(const Cameras& cameras) const {
return Base::reprojectionError(cameras, point_);
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
return reprojectionError(myCameras);
return cameras.reprojectionErrors(point_);
else
return zero(myCameras.size() * 3);
return zero(cameras.size() * 3);
}
/**