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>
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;
list<int> indices;
list<size_t> indices;
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.sort();
@ -96,11 +96,11 @@ namespace gtsam {
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
list<int> candidates = getAffectedFactors(affectedKeysList);
list<size_t> candidates = getAffectedFactors(affectedKeysList);
NonlinearFactorGraph<Config> nonlinearAffectedFactors;
BOOST_FOREACH(int idx, candidates) {
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
if (affectedKeys.find(key) == affectedKeys.end()) {
@ -168,7 +168,7 @@ namespace gtsam {
// 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...
// 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;
BOOST_FOREACH(int idx, allAffected) {
list<Symbol> tmp = nonlinearFactors_[idx]->keys();

2
cpp/ISAM2.h
View File

@ -97,7 +97,7 @@ namespace gtsam {
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;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);

2
cpp/simulated2DOriented.h
View File

@ -71,7 +71,7 @@ namespace gtsam {
GenericOdometry(const Pose2& z, const SharedGaussian& model,
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<

2
cpp/testConstraintOptimizer.cpp
View File

@ -376,7 +376,7 @@ TEST( matrix, line_search ) {
double init_error = penalty(x0),
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;
CHECK(final_error <= init_error);

12
cpp/testGaussianFactorGraph.cpp
View File

@ -528,15 +528,15 @@ TEST( GaussianFactorGraph, factor_lookup)
GaussianFactorGraph fg = createGaussianFactorGraph();
// ask for all factor indices connected to x1
list<int> x1_factors = fg.factors("x1");
int x1_indices[] = { 0, 1, 2 };
list<int> x1_expected(x1_indices, x1_indices + 3);
list<size_t> x1_factors = fg.factors("x1");
size_t x1_indices[] = { 0, 1, 2 };
list<size_t> x1_expected(x1_indices, x1_indices + 3);
CHECK(x1_factors==x1_expected);
// ask for all factor indices connected to x2
list<int> x2_factors = fg.factors("x2");
int x2_indices[] = { 1, 3 };
list<int> x2_expected(x2_indices, x2_indices + 2);
list<size_t> x2_factors = fg.factors("x2");
size_t x2_indices[] = { 1, 3 };
list<size_t> x2_expected(x2_indices, x2_indices + 2);
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));
}
/* ************************************************************************* *
TEST( Iterative, conjugateGradientDescent_hard_constraint )
/* ************************************************************************* */
/*TEST( Iterative, conjugateGradientDescent_hard_constraint )
{
typedef Pose2Config::Key Key;
@ -105,7 +105,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
expected.insert("x1", zero(3));
expected.insert("x2", Vector_(-0.5,0.,0.));
CHECK(assert_equal(expected, actual));
}
}*/
/* ************************************************************************* */
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)));
}
/* ************************************************************************* *
TEST(LieConfig, expmap_d)
/* ************************************************************************* */
/*TEST(LieConfig, expmap_d)
{
LieConfig<string,Vector> config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
@ -178,10 +178,10 @@ TEST(LieConfig, expmap_d)
//poseconfig.print("poseconfig");
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
}
}*/
/* ************************************************************************* *
TEST(LieConfig, extract_keys)
/* ************************************************************************* */
/*TEST(LieConfig, extract_keys)
{
typedef TypedSymbol<Pose2, 'x'> PoseKey;
LieConfig<PoseKey, Pose2> config;
@ -200,7 +200,7 @@ TEST(LieConfig, extract_keys)
for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
CHECK(assert_equal(*itExp, *itAct));
}
}
}*/
/* ************************************************************************* */
TEST(LieConfig, exists_)

1
cpp/testSQP.cpp
View File

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

8
cpp/testSymbolicFactorGraph.cpp
View File

@ -66,15 +66,15 @@ TEST( SymbolicFactorGraph, factors)
SymbolicFactorGraph fg(factorGraph);
// 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 };
list<int> x1_expected(x1_indices, x1_indices + 3);
list<size_t> x1_expected(x1_indices, x1_indices + 3);
CHECK(x1_factors==x1_expected);
// 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 };
list<int> x2_expected(x2_indices, x2_indices + 2);
list<size_t> x2_expected(x2_indices, x2_indices + 2);
CHECK(x2_factors==x2_expected);
}