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

Migrated to non-keyed Fblocks

release/4.3a0
dellaert 2015-03-04 23:11:01 -08:00
parent 485fabeae6
commit e6a90db2d5
1 changed files with 44 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -106,7 +106,9 @@ protected:
/// shorthand for this class
typedef SmartStereoProjectionFactor<CALIBRATION, D> This;
enum {ZDim = 3}; ///< Dimension trait of measurement type
enum {
ZDim = 3
}; ///< Dimension trait of measurement type
public:
@ -156,7 +158,8 @@ public:
std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_
<< std::endl;
Base::print("", keyFormatter);
}
@ -299,8 +302,8 @@ public:
}
//std::cout << "totalReprojError error: " << totalReprojError << std::endl;
// we discard smart factors that have large reprojection error
if(dynamicOutlierRejectionThreshold_ > 0 &&
totalReprojError/m > dynamicOutlierRejectionThreshold_)
if (dynamicOutlierRejectionThreshold_ > 0
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
@ -362,12 +365,14 @@ public:
}
this->triangulateSafe(cameras);
if (isDebug) std::cout << "point_ = " << point_ << std::endl;
if (isDebug)
std::cout << "point_ = " << point_ << std::endl;
if (numKeys < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) std::cout << "In linearize: exception" << std::endl;
if (isDebug)
std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(D, D);
BOOST_FOREACH(Vector& v, gs)
@ -379,12 +384,14 @@ public:
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (isDebug) std::cout << "degenerate_ = true" << std::endl;
if (isDebug)
std::cout << "degenerate_ = true" << std::endl;
}
bool doLinearize = this->decideIfLinearize(cameras);
if (isDebug) std::cout << "doLinearize = " << doLinearize << std::endl;
if (isDebug)
std::cout << "doLinearize = " << doLinearize << std::endl;
if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
for (size_t i = 0; i < cameras.size(); i++)
@ -405,7 +412,7 @@ public:
}
// ==================================================================
std::vector<typename Base::KeyMatrix2D> Fblocks;
std::vector<typename Base::MatrixZD> Fblocks;
Matrix F, E;
Vector b;
double f = computeJacobians(Fblocks, E, b, cameras);
@ -421,8 +428,10 @@ public:
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl;
if (isDebug) std::cout << "H:\n" << H << std::endl;
if (isDebug)
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
if (isDebug)
std::cout << "H:\n" << H << std::endl;
// Populate Gs and gs
int GsCount2 = 0;
@ -476,8 +485,8 @@ public:
// }
//
/// different (faster) way to compute Jacobian factor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
double lambda) const {
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
else
@ -506,7 +515,8 @@ public:
this->degenerate_ = true;
if (this->degenerate_) {
std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl;
std::cout << "SmartStereoProjectionFactor: this is not ready"
<< std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
@ -527,7 +537,7 @@ public:
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
bool computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
@ -539,7 +549,7 @@ public:
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
double computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Cameras& cameras) const {
if (this->degenerate_) {
throw("FIXME: computeJacobians degenerate case commented out!");
@ -570,7 +580,7 @@ public:
//
// this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
// f += bi.squaredNorm();
// Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi));
// E.block < 2, 2 > (2 * i, 0) = Ei;
// subInsert(b, bi, 2 * i);
// }
@ -583,8 +593,8 @@ public:
/// takes values
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Values& values) const {
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const {
typename Base::Cameras cameras;
double good = computeCamerasAndTriangulate(values, cameras);
if (good)
@ -707,8 +717,8 @@ private:
/// traits
template<class CALIBRATION, size_t D>
struct traits<SmartStereoProjectionFactor<CALIBRATION, D> > :
public Testable<SmartStereoProjectionFactor<CALIBRATION, D> > {
struct traits<SmartStereoProjectionFactor<CALIBRATION, D> > : public Testable<
SmartStereoProjectionFactor<CALIBRATION, D> > {
};
} // \ namespace gtsam