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

Cleanup / Cleanup flags

release/4.3a0
Rahul Sawhney 2010-10-21 18:54:00 +00:00
parent 125c612c09
commit 6b553ce4fa
6 changed files with 13 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
nonlinear/LieValues.h
View File

@ -119,8 +119,6 @@ namespace gtsam {
/** Add a delta config to current config and returns a new config */
LieValues expmap(const VectorValues& delta, const Ordering& ordering) const;
// /** Add a delta vector to current config and returns a new config, uses iterator order */
// LieValues expmap(const Vector& delta) const;
/** Get a delta config about a linearization point c0 (*this) */
VectorValues logmap(const LieValues& cp, const Ordering& ordering) const;

2
nonlinear/NonlinearFactorGraph-inl.h
View File

@ -75,7 +75,7 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
// Permute the Ordering and VariableIndex with the COLAMD ordering
ordering->permuteWithInverse(*colamdPerm->inverse());
// variableIndex.permute(*colamdPerm);
// variableIndex.permute(*colamdPerm);
// SL-FIX: fix permutation
// Return the Ordering and VariableIndex to be re-used during linearization

2
nonlinear/NonlinearOptimization-inl.h
View File

@ -88,7 +88,7 @@ namespace gtsam {
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters);
case SPCG:
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
// return optimizeSPCG<G,T>(graph, initialEstimate, parameters) ;
break;
}
throw runtime_error("optimize: undefined solver");

2
nonlinear/NonlinearOptimizer-inl.h
View File

@ -239,7 +239,7 @@ namespace gtsam {
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterateLM(
Parameters::verbosityLevel verbosity, double lambdaFactor, Parameters::LambdaMode lambdaMode) const {
// maybe show output
// show output
if (verbosity >= Parameters::CONFIG)
config_->print("config");
if (verbosity >= Parameters::ERROR)

101
slam/smallExample.cpp
View File

@ -423,107 +423,6 @@ namespace example {
return config;
}
/* ************************************************************************* */
//GaussianFactorGraph createConstrainedGaussianFactorGraph()
//{
// GaussianFactorGraph graph;
//
// // add an equality factor
// Vector v1(2); v1(0)=1.;v1(1)=2.;
// GaussianFactor::shared_ptr f1(new GaussianFactor(v1, "x0"));
// graph.push_back_eq(f1);
//
// // add a normal linear factor
// Matrix A21 = -1 * eye(2);
//
// Matrix A22 = eye(2);
//
// Vector b(2);
// b(0) = 2 ; b(1) = 3;
//
// double sigma = 0.1;
// GaussianFactor::shared_ptr f2(new GaussianFactor("x0", A21/sigma, _x1_, A22/sigma, b/sigma));
// graph.push_back(f2);
// return graph;
//}
/* ************************************************************************* */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorValues> , VectorValues> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorValues> , VectorValues> graph;
// VectorValues c = createConstrainedValues();
//
// // equality constraint for initial pose
// GaussianFactor::shared_ptr f1(new GaussianFactor(c["x0"], "x0"));
// graph.push_back_eq(f1);
//
// // odometry between x0 and x1
// double sigma = 0.1;
// shared f2(new Simulated2DOdometry(c[_x1_] - c["x0"], sigma, "x0", _x1_));
// graph.push_back(f2); // TODO
// return graph;
// }
/* ************************************************************************* */
//VectorValues createConstrainedValues()
//{
// VectorValues config;
//
// Vector x0(2); x0(0)=1.0; x0(1)=2.0;
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)=3.0; x1(1)=5.0;
// config.insert(_x1_, x1);
//
// return config;
//}
/* ************************************************************************* */
//VectorValues createConstrainedLinValues()
//{
// VectorValues config;
//
// Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)=2.3; x1(1)=5.3;
// config.insert(_x1_, x1);
//
// return config;
//}
/* ************************************************************************* */
//VectorValues createConstrainedCorrectDelta()
//{
// VectorValues config;
//
// Vector x0(2); x0(0)=0.; x0(1)=0.;
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)= 0.7; x1(1)= -0.3;
// config.insert(_x1_, x1);
//
// return config;
//}
/* ************************************************************************* */
//ConstrainedGaussianBayesNet createConstrainedGaussianBayesNet()
//{
// ConstrainedGaussianBayesNet cbn;
// VectorValues c = createConstrainedValues();
//
// // add regular conditional gaussian - no parent
// Matrix R = eye(2);
// Vector d = c[_x1_];
// double sigma = 0.1;
// GaussianConditional::shared_ptr f1(new GaussianConditional(d/sigma, R/sigma));
// cbn.insert(_x1_, f1);
//
// // add a delta function to the cbn
// ConstrainedGaussianConditional::shared_ptr f2(new ConstrainedGaussianConditional); //(c["x0"], "x0"));
// cbn.insert_df("x0", f2);
//
// return cbn;
//}
/* ************************************************************************* */
// Create key for simulated planar graph

25
slam/visualSLAM.h
View File

@ -50,8 +50,8 @@ namespace gtsam { namespace visualSLAM {
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
*/
template <class Cfg=Values, class LmK=PointKey, class PosK=PoseKey>
class GenericProjectionFactor : public NonlinearFactor2<Cfg, PosK, LmK>, Testable<GenericProjectionFactor<Cfg, LmK, PosK> > {
template <class CFG=Values, class LMK=PointKey, class POSK=PoseKey>
class GenericProjectionFactor : public NonlinearFactor2<CFG, POSK, LMK>, Testable<GenericProjectionFactor<CFG, LMK, POSK> > {
protected:
// Keep a copy of measurement and calibration for I/O
@ -61,10 +61,10 @@ namespace gtsam { namespace visualSLAM {
public:
// shorthand for base class type
typedef NonlinearFactor2<Cfg, PosK, LmK> Base;
typedef NonlinearFactor2<CFG, POSK, LMK> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<Cfg, LmK, PosK> > shared_ptr;
typedef boost::shared_ptr<GenericProjectionFactor<CFG, LMK, POSK> > shared_ptr;
/**
* Default constructor
@ -80,8 +80,8 @@ namespace gtsam { namespace visualSLAM {
* @param K the constant calibration
*/
GenericProjectionFactor(const Point2& z,
const SharedGaussian& model, PosK j_pose,
LmK j_landmark, const shared_ptrK& K) :
const SharedGaussian& model, POSK j_pose,
LMK j_landmark, const shared_ptrK& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
}
@ -97,15 +97,12 @@ namespace gtsam { namespace visualSLAM {
/**
* equals
*/
bool equals(const GenericProjectionFactor<Cfg, LmK, PosK>& p, double tol = 1e-9) const {
bool equals(const GenericProjectionFactor<CFG, LMK, POSK>& p, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol)
&& this->K_->equals(*p.K_, tol);
}
// /** h(x) */
// Point2 predict(const Pose3& pose, const Point3& point) const {
// return SimpleCamera(*K_, pose).project(point);
// }
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
@ -118,10 +115,8 @@ namespace gtsam { namespace visualSLAM {
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
//ar & BOOST_SERIALIZATION_NVP(key1_);
//ar & BOOST_SERIALIZATION_NVP(key2_);
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(K_);
}