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

Compiles and passes unit tests

release/4.3a0
Richard Roberts 2012-03-19 14:32:37 +00:00
parent c695b23e36
commit 670117cfe7
9 changed files with 175 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.project
View File

@ -31,7 +31,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildLocation</key>
<value>${workspace_loc:/gtsam/build-timing}</value>
<value>${ProjDirPath}/build</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>

12
gtsam/linear/GaussianConditional.cpp
View File

@ -194,7 +194,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES
static const bool debug = false;
if(debug) conditional.print("Solving conditional in place");
Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
xS = conditional.get_d() - conditional.get_S() * xS;
Vector soln = conditional.permutation().transpose() *
conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
@ -202,7 +202,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES
gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
gtsam::print(soln, "full back-substitution solution: ");
}
writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
}
/* ************************************************************************* */
@ -217,7 +217,7 @@ void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
// TODO: verify permutation
frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
GaussianConditional::const_iterator it;
@ -225,14 +225,14 @@ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
const Index i = *it;
transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
}
writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
/* ************************************************************************* */
void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
frontalVec = emul(frontalVec, get_sigmas());
writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
}

16
gtsam/linear/VectorValues.cpp
View File

@ -156,3 +156,19 @@ void VectorValues::operator+=(const VectorValues& c) {
assert(this->hasSameStructure(c));
this->values_ += c.values_;
}
/* ************************************************************************* */
VectorValues& VectorValues::operator=(const Permuted<VectorValues>& rhs) {
if(this->size() != rhs.size())
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
for(size_t j=0; j<this->size(); ++j) {
if(exists(j)) {
SubVector& l(this->at(j));
const SubVector& r(rhs[j]);
if(l.rows() != r.rows())
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
l = r;
}
}
return *this;
}

6
gtsam/linear/VectorValues.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/Permutation.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
@ -273,6 +274,11 @@ namespace gtsam {
*/
void operator+=(const VectorValues& c);
/** Assignment operator from Permuted<VectorValues>, requires the dimensions
* of the assignee to already be properly pre-allocated.
*/
VectorValues& operator=(const Permuted<VectorValues>& rhs);
/// @}
private:

68
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -22,8 +22,8 @@ namespace gtsam {
/* ************************************************************************* */
void ISAM2::Impl::AddVariables(
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch,
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, vector<bool>& replacedKeys,
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
@ -48,6 +48,8 @@ void ISAM2::Impl::AddVariables(
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
deltaNewton.permutation()[nextVar] = nextVar;
deltaGradSearch.permutation()[nextVar] = nextVar;
ordering.insert(key_value.key, nextVar);
if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
++ nextVar;
@ -301,21 +303,47 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
/* ************************************************************************* */
namespace internal {
size_t updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, vector<bool>& updated) {
void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, size_t& varsUpdated) {
// Check if any frontal or separator keys were recalculated, if so, we need
// update deltas and recurse to children, but if not, we do not need to
// recurse further because of the running separator property.
bool anyReplaced = false;
BOOST_FOREACH(Index j, *clique->conditional()) {
if(replacedKeys[j]) {
anyReplaced = true;
break;
}
}
if(anyReplaced) {
// Update the current variable
// Get VectorValues slice corresponding to current variables
Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals());
Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents());
// Update the current variable
// Get VectorValues slice corresponding to current variables
Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals());
Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents());
// Compute R*g and S*g for this clique
Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS;
// Compute R*g and S*g for this clique
Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS;
// Write into RgProd vector
internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals());
// Write into RgProd vector
internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->begin(), (*clique)->end());
// Now solve the part of the Newton's method point for this clique (back-substitution)
(*clique)->solveInPlace(deltaNewton);
// If debugging, set recalculated keys to false so we can check them later
#ifndef NDEBUG
BOOST_FOREACH(Index j, (*clique)->frontals()) {
replacedKeys[j] = false; }
#endif
varsUpdated += (*clique)->nrFrontals();
// Recurse to children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
updateDoglegDeltas(child, replacedKeys, grad, deltaNewton, RgProd, varsUpdated); }
}
}
}
@ -323,15 +351,23 @@ size_t updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vec
size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector<bool>& replacedKeys,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd) {
// Keep a set of flags for whether each variable has been updated.
vector<bool> updated(replacedKeys.size());
// Get gradient
VectorValues grad = *allocateVectorValues(isam);
gradientAtZero(isam, grad);
// Update variables
return internal::updateDoglegDeltas(root, replacedKeys, grad, deltaNewton, RgProd, updated);
size_t varsUpdated = 0;
internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated);
// Make sure all were updated
#ifndef NDEBUG
for(size_t j=0; j<replacedKeys.size(); ++j)
assert(!replacedKeys[j]);
#else
replacedKeys.assign(replacedKeys.size(), false);
#endif
return varsUpdated;
}
}

2
gtsam/nonlinear/ISAM2-impl.h
View File

@ -127,7 +127,7 @@ struct ISAM2::Impl {
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
static size_t UpdateDoglegDeltas(const ISAM2& isam, std::vector<bool>& replacedKeys,
const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
};

55
gtsam/nonlinear/ISAM2.cpp
View File

@ -38,7 +38,8 @@ static const double batchThreshold = 0.65;
/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2Params& params):
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) {
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@ -46,7 +47,8 @@ ISAM2::ISAM2(const ISAM2Params& params):
/* ************************************************************************* */
ISAM2::ISAM2():
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) {
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), deltaUptodate_(true) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@ -344,6 +346,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
toc(3,"permute global variable index");
tic(4,"permute delta");
delta_.permute(partialSolveResult.fullReordering);
deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_.permute(partialSolveResult.fullReordering);
toc(4,"permute delta");
tic(5,"permute ordering");
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
@ -441,7 +445,7 @@ ISAM2Result ISAM2::update(
tic(2,"add new variables");
// 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_);
Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_);
toc(2,"add new variables");
tic(3,"evaluate error before");
@ -543,6 +547,7 @@ ISAM2Result ISAM2::update(
toc(10,"evaluate error after");
result.cliques = this->nodes().size();
deltaDoglegUptodate_ = false;
deltaUptodate_ = false;
return result;
@ -575,9 +580,6 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
doglegDelta_ = doglegResult.Delta;
delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation
delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
// Clear replaced mask
deltaReplacedMask_.assign(deltaReplacedMask_.size(), false);
}
deltaUptodate_ = true;
@ -618,9 +620,17 @@ VectorValues optimize(const ISAM2& isam) {
/* ************************************************************************* */
void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
tic(1, "optimizeInPlace");
internal::optimizeInPlace<ISAM2::Base>(isam.root(), delta);
toc(1, "optimizeInPlace");
// We may need to update the solution calcaulations
if(!isam.deltaDoglegUptodate_) {
tic(1, "UpdateDoglegDeltas");
ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
isam.deltaDoglegUptodate_ = true;
toc(1, "UpdateDoglegDeltas");
}
tic(2, "copy delta");
delta = isam.deltaNewton_;
toc(2, "copy delta");
}
/* ************************************************************************* */
@ -635,27 +645,30 @@ VectorValues optimizeGradientSearch(const ISAM2& isam) {
}
/* ************************************************************************* */
void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) {
void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
// We may need to update the solution calcaulations
if(!isam.deltaDoglegUptodate_) {
tic(1, "UpdateDoglegDeltas");
ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
isam.deltaDoglegUptodate_ = true;
toc(1, "UpdateDoglegDeltas");
}
tic(1, "Compute Gradient");
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
gradientAtZero(Rd, grad);
gradientAtZero(isam, grad);
double gradientSqNorm = grad.dot(grad);
toc(1, "Compute Gradient");
tic(2, "Compute R*g");
// Compute R * g
FactorGraph<JacobianFactor> Rd_jfg(Rd);
Errors Rg = Rd_jfg * grad;
toc(2, "Compute R*g");
tic(3, "Compute minimizing step size");
tic(2, "Compute minimizing step size");
// Compute minimizing step size
double step = -gradientSqNorm / dot(Rg, Rg);
toc(3, "Compute minimizing step size");
double RgNormSq = isam.RgProd_.container().vector().squaredNorm();
double step = -gradientSqNorm / RgNormSq;
toc(2, "Compute minimizing step size");
tic(4, "Compute point");
// Compute steepest descent point
scal(step, grad);
grad.vector() *= step;
toc(4, "Compute point");
}

9
gtsam/nonlinear/ISAM2.h
View File

@ -296,6 +296,7 @@ protected:
mutable Permuted<VectorValues> deltaNewton_;
VectorValues RgProdUnpermuted_;
mutable Permuted<VectorValues> RgProd_;
mutable bool deltaDoglegUptodate_;
/** Indicates whether the current delta is up-to-date, only used
* internally - delta will always be updated if necessary when it is
@ -359,6 +360,11 @@ public:
newISAM2->variableIndex_ = variableIndex_;
newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
newISAM2->delta_ = delta_;
newISAM2->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_;
newISAM2->deltaNewton_ = deltaNewton_;
newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_;
newISAM2->RgProd_ = RgProd_;
newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_;
newISAM2->deltaUptodate_ = deltaUptodate_;
newISAM2->deltaReplacedMask_ = deltaReplacedMask_;
newISAM2->nonlinearFactors_ = nonlinearFactors_;
@ -459,6 +465,9 @@ private:
// void linear_update(const GaussianFactorGraph& newFactors);
void updateDelta(bool forceFullSolve = false) const;
friend void optimizeInPlace(const ISAM2&, VectorValues&);
friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&);
}; // ISAM2
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */

52
tests/testGaussianISAM2.cpp
View File

@ -29,7 +29,7 @@ using boost::shared_ptr;
const double tol = 1e-4;
/* ************************************************************************* */
TEST(ISAM2, AddVariables) {
TEST_UNSAFE(ISAM2, AddVariables) {
// Create initial state
Values theta;
@ -48,6 +48,26 @@ TEST(ISAM2, AddVariables) {
Permuted<VectorValues> delta(permutation, deltaUnpermuted);
VectorValues deltaNewtonUnpermuted;
deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5));
Permutation permutationNewton(2);
permutationNewton[0] = 1;
permutationNewton[1] = 0;
Permuted<VectorValues> deltaNewton(permutationNewton, deltaNewtonUnpermuted);
VectorValues deltaRgUnpermuted;
deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaRgUnpermuted.insert(1, Vector_(2, .4, .5));
Permutation permutationRg(2);
permutationRg[0] = 1;
permutationRg[1] = 0;
Permuted<VectorValues> deltaRg(permutationRg, deltaRgUnpermuted);
vector<bool> replacedKeys(2, false);
Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
@ -78,6 +98,30 @@ TEST(ISAM2, AddVariables) {
Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
VectorValues deltaNewtonUnpermutedExpected;
deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationNewtonExpected(3);
permutationNewtonExpected[0] = 1;
permutationNewtonExpected[1] = 0;
permutationNewtonExpected[2] = 2;
Permuted<VectorValues> deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected);
VectorValues deltaRgUnpermutedExpected;
deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationRgExpected(3);
permutationRgExpected[0] = 1;
permutationRgExpected[1] = 0;
permutationRgExpected[2] = 2;
Permuted<VectorValues> deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected);
vector<bool> replacedKeysExpected(3, false);
Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
@ -86,11 +130,15 @@ TEST(ISAM2, AddVariables) {
3, ISAM2::sharedClique());
// Expand initial state
ISAM2::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes);
ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
EXPECT(assert_equal(thetaExpected, theta));
EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted));
EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation()));
EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted));
EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation()));
EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
EXPECT(assert_equal(orderingExpected, ordering));
}