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

Fixed errors introduced by previous changes

release/4.3a0
Richard Roberts 2010-07-10 22:59:50 +00:00
parent ad0bd147f0
commit 9b89a32880
9 changed files with 28 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/ISAM2-inl.h
View File

@ -75,11 +75,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
list<int> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const { list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
FactorGraph<NonlinearFactor<Config> > allAffected; FactorGraph<NonlinearFactor<Config> > allAffected;
list<int> indices; list<size_t> indices;
BOOST_FOREACH(const Symbol& key, keys) { BOOST_FOREACH(const Symbol& key, keys) {
const list<int> l = nonlinearFactors_.factors(key); const list<size_t> l = nonlinearFactors_.factors(key);
indices.insert(indices.begin(), l.begin(), l.end()); indices.insert(indices.begin(), l.begin(), l.end());
} }
indices.sort(); indices.sort();
@ -96,11 +96,11 @@ namespace gtsam {
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list... list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
list<int> candidates = getAffectedFactors(affectedKeysList); list<size_t> candidates = getAffectedFactors(affectedKeysList);
NonlinearFactorGraph<Config> nonlinearAffectedFactors; NonlinearFactorGraph<Config> nonlinearAffectedFactors;
BOOST_FOREACH(int idx, candidates) { BOOST_FOREACH(size_t idx, candidates) {
bool inside = true; bool inside = true;
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) { BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
if (affectedKeys.find(key) == affectedKeys.end()) { if (affectedKeys.find(key) == affectedKeys.end()) {
@ -168,7 +168,7 @@ namespace gtsam {
// mark variables that have to be removed as invalid (removeFATtop) // mark variables that have to be removed as invalid (removeFATtop)
// basically calculate all the keys contained in the factors that contain any of the keys... // basically calculate all the keys contained in the factors that contain any of the keys...
// the goal is to relinearize all variables directly affected by new factors // the goal is to relinearize all variables directly affected by new factors
list<int> allAffected = getAffectedFactors(marked_); list<size_t> allAffected = getAffectedFactors(marked_);
set<Symbol> accumulate; set<Symbol> accumulate;
BOOST_FOREACH(int idx, allAffected) { BOOST_FOREACH(int idx, allAffected) {
list<Symbol> tmp = nonlinearFactors_[idx]->keys(); list<Symbol> tmp = nonlinearFactors_[idx]->keys();

2
cpp/ISAM2.h
View File

@ -97,7 +97,7 @@ namespace gtsam {
private: private:
std::list<int> getAffectedFactors(const std::list<Symbol>& keys) const; std::list<size_t> getAffectedFactors(const std::list<Symbol>& keys) const;
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const; boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans); FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);

2
cpp/simulated2DOriented.h
View File

@ -71,7 +71,7 @@ namespace gtsam {
GenericOdometry(const Pose2& z, const SharedGaussian& model, GenericOdometry(const Pose2& z, const SharedGaussian& model,
const Key& i1, const Key& i2) : const Key& i1, const Key& i2) :
z_(z), NonlinearFactor2<Cfg, Key, Pose2, Key, Pose2> (model, i1, i2) { NonlinearFactor2<Cfg, Key, Pose2, Key, Pose2> (model, i1, i2), z_(z) {
} }
Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional< Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional<

2
cpp/testConstraintOptimizer.cpp
View File

@ -376,7 +376,7 @@ TEST( matrix, line_search ) {
double init_error = penalty(x0), double init_error = penalty(x0),
final_error = penalty(x0 + actual); final_error = penalty(x0 + actual);
double actual_stepsize = dot(actual, delta)/dot(delta, delta); //double actual_stepsize = dot(actual, delta)/dot(delta, delta);
// cout << "actual_stepsize: " << actual_stepsize << endl; // cout << "actual_stepsize: " << actual_stepsize << endl;
CHECK(final_error <= init_error); CHECK(final_error <= init_error);

12
cpp/testGaussianFactorGraph.cpp
View File

@ -528,15 +528,15 @@ TEST( GaussianFactorGraph, factor_lookup)
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// ask for all factor indices connected to x1 // ask for all factor indices connected to x1
list<int> x1_factors = fg.factors("x1"); list<size_t> x1_factors = fg.factors("x1");
int x1_indices[] = { 0, 1, 2 }; size_t x1_indices[] = { 0, 1, 2 };
list<int> x1_expected(x1_indices, x1_indices + 3); list<size_t> x1_expected(x1_indices, x1_indices + 3);
CHECK(x1_factors==x1_expected); CHECK(x1_factors==x1_expected);
// ask for all factor indices connected to x2 // ask for all factor indices connected to x2
list<int> x2_factors = fg.factors("x2"); list<size_t> x2_factors = fg.factors("x2");
int x2_indices[] = { 1, 3 }; size_t x2_indices[] = { 1, 3 };
list<int> x2_expected(x2_indices, x2_indices + 2); list<size_t> x2_expected(x2_indices, x2_indices + 2);
CHECK(x2_factors==x2_expected); CHECK(x2_factors==x2_expected);
} }

6
cpp/testIterative.cpp
View File

@ -80,8 +80,8 @@ TEST( Iterative, conjugateGradientDescent )
CHECK(assert_equal(expected,actual2,1e-2)); CHECK(assert_equal(expected,actual2,1e-2));
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( Iterative, conjugateGradientDescent_hard_constraint ) /*TEST( Iterative, conjugateGradientDescent_hard_constraint )
{ {
typedef Pose2Config::Key Key; typedef Pose2Config::Key Key;
@ -105,7 +105,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
expected.insert("x1", zero(3)); expected.insert("x1", zero(3));
expected.insert("x2", Vector_(-0.5,0.,0.)); expected.insert("x2", Vector_(-0.5,0.,0.));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }*/
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Iterative, conjugateGradientDescent_soft_constraint ) TEST( Iterative, conjugateGradientDescent_soft_constraint )

12
cpp/testLieConfig.cpp
View File

@ -162,8 +162,8 @@ TEST(LieConfig, expmap_c)
CHECK(assert_equal(expected, expmap(config0, increment))); CHECK(assert_equal(expected, expmap(config0, increment)));
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST(LieConfig, expmap_d) /*TEST(LieConfig, expmap_d)
{ {
LieConfig<string,Vector> config0; LieConfig<string,Vector> config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
@ -178,10 +178,10 @@ TEST(LieConfig, expmap_d)
//poseconfig.print("poseconfig"); //poseconfig.print("poseconfig");
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
} }*/
/* ************************************************************************* * /* ************************************************************************* */
TEST(LieConfig, extract_keys) /*TEST(LieConfig, extract_keys)
{ {
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
LieConfig<PoseKey, Pose2> config; LieConfig<PoseKey, Pose2> config;
@ -200,7 +200,7 @@ TEST(LieConfig, extract_keys)
for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
CHECK(assert_equal(*itExp, *itAct)); CHECK(assert_equal(*itExp, *itAct));
} }
} }*/
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, exists_) TEST(LieConfig, exists_)

1
cpp/testSQP.cpp
View File

@ -767,7 +767,6 @@ TEST (SQP, stereo_sqp ) {
* with noise in the initial estimate * with noise in the initial estimate
*/ */
TEST (SQP, stereo_sqp_noisy ) { TEST (SQP, stereo_sqp_noisy ) {
bool verbose = false;
// get a graph // get a graph
boost::shared_ptr<VGraph> graph = stereoExampleGraph(); boost::shared_ptr<VGraph> graph = stereoExampleGraph();

8
cpp/testSymbolicFactorGraph.cpp
View File

@ -66,15 +66,15 @@ TEST( SymbolicFactorGraph, factors)
SymbolicFactorGraph fg(factorGraph); SymbolicFactorGraph fg(factorGraph);
// ask for all factor indices connected to x1 // ask for all factor indices connected to x1
list<int> x1_factors = fg.factors("x1"); list<size_t> x1_factors = fg.factors("x1");
int x1_indices[] = { 0, 1, 2 }; int x1_indices[] = { 0, 1, 2 };
list<int> x1_expected(x1_indices, x1_indices + 3); list<size_t> x1_expected(x1_indices, x1_indices + 3);
CHECK(x1_factors==x1_expected); CHECK(x1_factors==x1_expected);
// ask for all factor indices connected to x2 // ask for all factor indices connected to x2
list<int> x2_factors = fg.factors("x2"); list<size_t> x2_factors = fg.factors("x2");
int x2_indices[] = { 1, 3 }; int x2_indices[] = { 1, 3 };
list<int> x2_expected(x2_indices, x2_indices + 2); list<size_t> x2_expected(x2_indices, x2_indices + 2);
CHECK(x2_factors==x2_expected); CHECK(x2_factors==x2_expected);
} }