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

start commenting out some unused code

release/4.3a0
cbeall3 2014-06-03 16:51:03 -04:00
parent 6d33b3c24e
commit 49ae04dc47
3 changed files with 80 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/slam/SmartFactorBase.h
View File

@ -25,10 +25,9 @@
#include "RegularHessianFactor.h" #include "RegularHessianFactor.h"
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h> // for Cheirality exception
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>

146
gtsam/slam/SmartStereoProjectionFactor.h
View File

@ -408,44 +408,44 @@ public:
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs, f); return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs, f);
} }
// create factor // // create factor
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor( // boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
const Cameras& cameras, double lambda) const { // const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) // if (triangulateForLinearize(cameras))
return Base::createImplicitSchurFactor(cameras, point_, lambda); // return Base::createImplicitSchurFactor(cameras, point_, lambda);
else // else
return boost::shared_ptr<ImplicitSchurFactor<D> >(); // return boost::shared_ptr<ImplicitSchurFactor<D> >();
} // }
//
/// create factor // /// create factor
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor( // boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
const Cameras& cameras, double lambda) const { // const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) // if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda); // return Base::createJacobianQFactor(cameras, point_, lambda);
else // else
return boost::make_shared< JacobianFactorQ<D> >(this->keys_); // return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
} // }
//
/// Create a factor, takes values // /// Create a factor, takes values
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor( // boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
const Values& values, double lambda) const { // const Values& values, double lambda) const {
Cameras myCameras; // Cameras myCameras;
// TODO triangulate twice ?? // // TODO triangulate twice ??
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); // bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate) // if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda); // return createJacobianQFactor(myCameras, lambda);
else // else
return boost::make_shared< JacobianFactorQ<D> >(this->keys_); // return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
} // }
//
/// different (faster) way to compute Jacobian factor // /// different (faster) way to compute Jacobian factor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, // boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
double lambda) const { // double lambda) const {
if (triangulateForLinearize(cameras)) // if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda); // return Base::createJacobianSVDFactor(cameras, point_, lambda);
else // else
return boost::make_shared< JacobianFactorSVD<D> >(this->keys_); // return boost::make_shared< JacobianFactorSVD<D> >(this->keys_);
} // }
/// Returns true if nonDegenerate /// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values, bool computeCamerasAndTriangulate(const Values& values,
@ -546,41 +546,41 @@ public:
} // end else } // end else
} }
/// Version that computes PointCov, with optional lambda parameter // /// Version that computes PointCov, with optional lambda parameter
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks, // double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, // Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
const double lambda = 0.0) const { // const double lambda = 0.0) const {
//
double f = computeJacobians(Fblocks, E, b, cameras); // double f = computeJacobians(Fblocks, E, b, cameras);
//
// Point covariance inv(E'*E) // // Point covariance inv(E'*E)
PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); // PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
//
return f; // return f;
} // }
//
/// takes values // /// takes values
bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks, // bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Values& values) const { // Matrix& Enull, Vector& b, const Values& values) const {
typename Base::Cameras myCameras; // typename Base::Cameras myCameras;
double good = computeCamerasAndTriangulate(values, myCameras); // double good = computeCamerasAndTriangulate(values, myCameras);
if (good) // if (good)
computeJacobiansSVD(Fblocks, Enull, b, myCameras); // computeJacobiansSVD(Fblocks, Enull, b, myCameras);
return true; // return true;
} // }
//
/// SVD version // /// SVD version
double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks, // double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Cameras& cameras) const { // Matrix& Enull, Vector& b, const Cameras& cameras) const {
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); // return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
} // }
//
/// Returns Matrix, TODO: maybe should not exist -> not sparse ! // /// Returns Matrix, TODO: maybe should not exist -> not sparse !
// TODO should there be a lambda? // // TODO should there be a lambda?
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, // double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
const Cameras& cameras) const { // const Cameras& cameras) const {
return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); // return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
} // }
/// Returns Matrix, TODO: maybe should not exist -> not sparse ! /// Returns Matrix, TODO: maybe should not exist -> not sparse !
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,

12
gtsam/slam/SmartStereoProjectionPoseFactor.h
View File

@ -172,12 +172,12 @@ public:
const Values& values) const { const Values& values) const {
// depending on flag set on construction we may linearize to different linear factors // depending on flag set on construction we may linearize to different linear factors
switch(linearizeTo_){ switch(linearizeTo_){
case JACOBIAN_SVD : // case JACOBIAN_SVD :
return this->createJacobianSVDFactor(cameras(values), 0.0); // return this->createJacobianSVDFactor(cameras(values), 0.0);
break; // break;
case JACOBIAN_Q : // case JACOBIAN_Q :
return this->createJacobianQFactor(cameras(values), 0.0); // return this->createJacobianQFactor(cameras(values), 0.0);
break; // break;
default: default:
return this->createHessianFactor(cameras(values)); return this->createHessianFactor(cameras(values));
break; break;