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

In progress - updating dogleg computations incrementally

release/4.3a0
Richard Roberts 2012-03-18 05:13:40 +00:00
parent 4c2581f40e
commit c695b23e36
6 changed files with 83 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam/linear/GaussianConditional.cpp
View File

@ -27,42 +27,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
// Helper function used only in this file - extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<class VALUES, typename ITERATOR>
static Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
dim += values[*j].rows();
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
ret.segment(varStart, values[*j].rows()) = values[*j];
varStart += values[*j].rows();
}
return ret;
}
/* ************************************************************************* */
// Helper function used only in this file - writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, class VALUES, typename ITERATOR>
static void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
}
assert(varStart == vector.rows());
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional() : rsd_(matrix_) {}

38
gtsam/linear/VectorValues.h
View File

@ -402,4 +402,42 @@ namespace gtsam {
#endif
}
namespace internal {
/* ************************************************************************* */
// Helper function, extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<class VALUES, typename ITERATOR>
Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
dim += values[*j].rows();
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
ret.segment(varStart, values[*j].rows()) = values[*j];
varStart += values[*j].rows();
}
return ret;
}
/* ************************************************************************* */
// Helper function, writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, class VALUES, typename ITERATOR>
void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
}
assert(varStart == vector.rows());
}
}
} // \namespace gtsam

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

@ -299,4 +299,39 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
return lastBacksubVariableCount;
}
/* ************************************************************************* */
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) {
// 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;
// Write into RgProd vector
internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->begin(), (*clique)->end());
}
}
/* ************************************************************************* */
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);
}
}

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

@ -126,6 +126,9 @@ 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);
};
}

4
gtsam/nonlinear/ISAM2.cpp
View File

@ -660,7 +660,7 @@ void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) {
}
/* ************************************************************************* */
VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, const VectorValues& x0) {
VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
}
@ -682,7 +682,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
}
/* ************************************************************************* */
void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, VectorValues& g) {
void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) {
// Zero-out gradient
g.setZero();

10
gtsam/nonlinear/ISAM2.h
View File

@ -293,9 +293,9 @@ protected:
mutable Permuted<VectorValues> delta_;
VectorValues deltaNewtonUnpermuted_;
mutable Permuted<VectorValues> deltaNewtonUnpermuted_;
VectorValues deltaGradSearchUnpermuted_;
mutable Permuted<VectorValues> deltaGradSearchUnpermuted_;
mutable Permuted<VectorValues> deltaNewton_;
VectorValues RgProdUnpermuted_;
mutable Permuted<VectorValues> RgProd_;
/** Indicates whether the current delta is up-to-date, only used
* internally - delta will always be updated if necessary when it is
@ -527,7 +527,7 @@ int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, const VectorValues& x0);
VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
@ -540,7 +540,7 @@ VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTr
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, VectorValues& g);
void gradientAtZero(const ISAM2& bayesTree, VectorValues& g);
} /// namespace gtsam