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

uncomment/fix reprojectionError and computeEP methods

release/4.3a0
cbeall3 2014-11-17 20:09:19 -05:00
parent f3d42a8487
commit 1c6dc8a77d
1 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
gtsam/slam/SmartFactorBase.h
View File

@ -186,26 +186,26 @@ public:
// **************************************************************************************************** // ****************************************************************************************************
// /// Calculate vector of re-projection errors, before applying noise model // /// Calculate vector of re-projection errors, before applying noise model
// Vector reprojectionError(const Cameras& cameras, const Point3& point) const { Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
//
// Vector b = zero(2 * cameras.size()); Vector b = zero(ZDim_t::value * cameras.size());
//
// size_t i = 0; size_t i = 0;
// BOOST_FOREACH(const Camera& camera, cameras) { BOOST_FOREACH(const CAMERA& camera, cameras) {
// const Z& zi = this->measured_.at(i); const Z& zi = this->measured_.at(i);
// try { try {
// Z e(camera.project(point) - zi); Z e(camera.project(point) - zi);
// b[2 * i] = e.x(); b[ZDim_t::value * i] = e.x();
// b[2 * i + 1] = e.y(); b[ZDim_t::value * i + 1] = e.y();
// } catch (CheiralityException& e) { } catch (CheiralityException& e) {
// std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
// exit(EXIT_FAILURE); exit(EXIT_FAILURE);
// } }
// i += 1; i += 1;
// } }
//
// return b; return b;
// } }
// **************************************************************************************************** // ****************************************************************************************************
/** /**
@ -238,28 +238,28 @@ public:
// **************************************************************************************************** // ****************************************************************************************************
/// Assumes non-degenerate ! /// Assumes non-degenerate !
// void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras,
// const Point3& point) const { const Point3& point) const {
//
// int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
// E = zeros(2 * numKeys, 3); E = zeros(ZDim_t::value * numKeys, 3);
// Vector b = zero(2 * numKeys); Vector b = zero(2 * numKeys);
//
// Matrix Ei(2, 3); Matrix Ei(ZDim_t::value, 3);
// for (size_t i = 0; i < this->measured_.size(); i++) { for (size_t i = 0; i < this->measured_.size(); i++) {
// try { try {
// cameras[i].project(point, boost::none, Ei); cameras[i].project(point, boost::none, Ei);
// } catch (CheiralityException& e) { } catch (CheiralityException& e) {
// std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
// exit(EXIT_FAILURE); exit(EXIT_FAILURE);
// } }
// this->noise_.at(i)->WhitenSystem(Ei, b); this->noise_.at(i)->WhitenSystem(Ei, b);
// E.block<2, 3>(2 * i, 0) = Ei; E.block<ZDim_t::value, 3>(ZDim_t::value * i, 0) = Ei;
// } }
//
// // Matrix PointCov; // Matrix PointCov;
// PointCov.noalias() = (E.transpose() * E).inverse(); PointCov.noalias() = (E.transpose() * E).inverse();
// } }
// **************************************************************************************************** // ****************************************************************************************************
/// Compute F, E only (called below in both vanilla and SVD versions) /// Compute F, E only (called below in both vanilla and SVD versions)