In progress
Richard Roberts
parent
047dda05d7
commit
4c2581f40e
|
|
@ -72,7 +72,7 @@
|
|||
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890." name="/" resourcePath="">
|
||||
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.1257341773" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
|
||||
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.1756820285" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
|
||||
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-timing}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.404194139" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
|
||||
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-timing}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.404194139" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="8" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
|
||||
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.879403713" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
|
||||
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.48676242" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
|
||||
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1326666213" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
|
||||
|
|
|
|||
2
.project
2
.project
|
|
@ -31,7 +31,7 @@
|
|||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.buildLocation</key>
|
||||
<value>${ProjDirPath}/build</value>
|
||||
<value>${workspace_loc:/gtsam/build-timing}</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
|
||||
|
|
|
|||
|
|
@ -135,9 +135,14 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
|
|||
const F& f, const VALUES& x0, const Ordering& ordering, const double f_error, const bool verbose) {
|
||||
|
||||
// Compute steepest descent and Newton's method points
|
||||
tic(0, "Steepest Descent");
|
||||
VectorValues dx_u = optimizeGradientSearch(Rd);
|
||||
toc(0, "Steepest Descent");
|
||||
tic(0, "optimizeGradientSearch");
|
||||
tic(0, "allocateVectorValues");
|
||||
VectorValues dx_u = *allocateVectorValues(Rd);
|
||||
toc(0, "allocateVectorValues");
|
||||
tic(1, "optimizeGradientSearchInPlace");
|
||||
optimizeGradientSearchInPlace(Rd, dx_u);
|
||||
toc(1, "optimizeGradientSearchInPlace");
|
||||
toc(0, "optimizeGradientSearch");
|
||||
tic(1, "optimize");
|
||||
VectorValues dx_n = optimize(Rd);
|
||||
toc(1, "optimize");
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
void ISAM2::Impl::AddVariables(
|
||||
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
||||
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch,
|
||||
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
||||
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
||||
|
||||
|
|
@ -37,6 +38,12 @@ void ISAM2::Impl::AddVariables(
|
|||
delta.container().append(dims);
|
||||
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
delta.permutation().resize(originalnVars + newTheta.size());
|
||||
deltaNewton.container().append(dims);
|
||||
deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaNewton.permutation().resize(originalnVars + newTheta.size());
|
||||
deltaGradSearch.container().append(dims);
|
||||
deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaGradSearch.permutation().resize(originalnVars + newTheta.size());
|
||||
{
|
||||
Index nextVar = originalnVars;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
|
||||
|
|
|
|||
|
|
@ -48,7 +48,8 @@ struct ISAM2::Impl {
|
|||
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
|
||||
* @param keyFormatter Formatter for printing nonlinear keys during debugging
|
||||
*/
|
||||
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
||||
static void AddVariables(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 = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -29,6 +29,14 @@ namespace gtsam {
|
|||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUE>
|
||||
VALUE ISAM2::calculateEstimate(Key key) const {
|
||||
const Index index = getOrdering()[key];
|
||||
const SubVector delta = getDelta()[index];
|
||||
return theta_.at<VALUE>(key).retract(delta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
template<class CLIQUE>
|
||||
|
|
|
|||
|
|
@ -593,14 +593,6 @@ Values ISAM2::calculateEstimate() const {
|
|||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUE>
|
||||
VALUE ISAM2::calculateEstimate(Key key) const {
|
||||
const Index index = getOrdering()[key];
|
||||
const SubVector delta = getDelta()[index];
|
||||
return theta_.at<VALUE>(key).retract(delta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values ISAM2::calculateBestEstimate() const {
|
||||
VectorValues delta(theta_.dims(ordering_));
|
||||
|
|
@ -617,11 +609,20 @@ const Permuted<VectorValues>& ISAM2::getDelta() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const ISAM2& isam) {
|
||||
tic(0, "allocateVectorValues");
|
||||
VectorValues delta = *allocateVectorValues(isam);
|
||||
internal::optimizeInPlace<ISAM2::Base>(isam.root(), delta);
|
||||
toc(0, "allocateVectorValues");
|
||||
optimizeInPlace(isam, delta);
|
||||
return delta;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
|
||||
tic(1, "optimizeInPlace");
|
||||
internal::optimizeInPlace<ISAM2::Base>(isam.root(), delta);
|
||||
toc(1, "optimizeInPlace");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimizeGradientSearch(const ISAM2& isam) {
|
||||
tic(0, "Allocate VectorValues");
|
||||
|
|
|
|||
|
|
@ -292,6 +292,11 @@ protected:
|
|||
*/
|
||||
mutable Permuted<VectorValues> delta_;
|
||||
|
||||
VectorValues deltaNewtonUnpermuted_;
|
||||
mutable Permuted<VectorValues> deltaNewtonUnpermuted_;
|
||||
VectorValues deltaGradSearchUnpermuted_;
|
||||
mutable Permuted<VectorValues> deltaGradSearchUnpermuted_;
|
||||
|
||||
/** Indicates whether the current delta is up-to-date, only used
|
||||
* internally - delta will always be updated if necessary when it is
|
||||
* requested with getDelta() or calculateEstimate().
|
||||
|
|
@ -459,6 +464,9 @@ private:
|
|||
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
|
||||
VectorValues optimize(const ISAM2& isam);
|
||||
|
||||
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
|
||||
void optimizeInPlace(const ISAM2& isam, VectorValues& delta);
|
||||
|
||||
/// Optimize the BayesTree, starting from the root.
|
||||
/// @param replaced Needs to contain
|
||||
/// all variables that are contained in the top of the Bayes tree that has been
|
||||
|
|
|
|||
|
|
@ -117,6 +117,8 @@ namespace gtsam {
|
|||
typedef boost::transform_iterator<
|
||||
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
|
||||
|
||||
typedef KeyValuePair value_type;
|
||||
|
||||
private:
|
||||
template<class ValueType>
|
||||
struct _KeyValuePair {
|
||||
|
|
@ -143,6 +145,7 @@ namespace gtsam {
|
|||
/** A key-value pair, with the value a specific derived Value type. */
|
||||
typedef _KeyValuePair<ValueType> KeyValuePair;
|
||||
typedef _ConstKeyValuePair<ValueType> ConstKeyValuePair;
|
||||
typedef KeyValuePair value_type;
|
||||
|
||||
typedef
|
||||
boost::transform_iterator<
|
||||
|
|
@ -208,6 +211,7 @@ namespace gtsam {
|
|||
public:
|
||||
/** A const key-value pair, with the value a specific derived Value type. */
|
||||
typedef _ConstKeyValuePair<ValueType> KeyValuePair;
|
||||
typedef KeyValuePair value_type;
|
||||
|
||||
typedef typename Filtered<ValueType>::const_const_iterator iterator;
|
||||
typedef typename Filtered<ValueType>::const_const_iterator const_iterator;
|
||||
|
|
|
|||
Loading…
Reference in New Issue