In progress

.cproject

@@ -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">

.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>

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -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");
+  tic(0, "optimizeGradientSearch");
-  VectorValues dx_u = optimizeGradientSearch(Rd);
+  tic(0, "allocateVectorValues");
-  toc(0, "Steepest Descent");
+  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");

gtsam/nonlinear/ISAM2-impl.cpp

@@ -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) {

gtsam/nonlinear/ISAM2-impl.h

@@ -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);
 
   /**

gtsam/nonlinear/ISAM2-inl.h

@@ -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>

gtsam/nonlinear/ISAM2.cpp

@@ -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");

gtsam/nonlinear/ISAM2.h

@@ -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

gtsam/nonlinear/Values.h

@@ -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;