v1

Thirdparty/DBoW2/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8)
+project(DBoW2)
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
+
+set(HDRS_DBOW2
+  DBoW2/BowVector.h
+  DBoW2/FORB.h 
+  DBoW2/FClass.h       
+  DBoW2/FeatureVector.h
+  DBoW2/ScoringObject.h   
+  DBoW2/TemplatedVocabulary.h)
+set(SRCS_DBOW2
+  DBoW2/BowVector.cpp
+  DBoW2/FORB.cpp      
+  DBoW2/FeatureVector.cpp
+  DBoW2/ScoringObject.cpp)
+
+set(HDRS_DUTILS
+  DUtils/Random.h
+  DUtils/Timestamp.h)
+set(SRCS_DUTILS
+  DUtils/Random.cpp
+  DUtils/Timestamp.cpp)
+
+find_package(OpenCV 3.0 QUIET)
+if(NOT OpenCV_FOUND)
+   find_package(OpenCV 2.4.3 QUIET)
+   if(NOT OpenCV_FOUND)
+      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+   endif()
+endif()
+
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS})
+target_link_libraries(DBoW2 ${OpenCV_LIBS})
+

Thirdparty/DBoW2/DBoW2/BowVector.cpp

@@ -0,0 +1,130 @@
+/**
+ * File: BowVector.cpp
+ * Date: March 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: bag of words vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+#include "BowVector.h"
+
+namespace DBoW2 {
+
+// --------------------------------------------------------------------------
+
+BowVector::BowVector(void)
+{
+}
+
+// --------------------------------------------------------------------------
+
+BowVector::~BowVector(void)
+{
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::addWeight(WordId id, WordValue v)
+{
+  BowVector::iterator vit = this->lower_bound(id);
+  
+  if(vit != this->end() && !(this->key_comp()(id, vit->first)))
+  {
+    vit->second += v;
+  }
+  else
+  {
+    this->insert(vit, BowVector::value_type(id, v));
+  }
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::addIfNotExist(WordId id, WordValue v)
+{
+  BowVector::iterator vit = this->lower_bound(id);
+  
+  if(vit == this->end() || (this->key_comp()(id, vit->first)))
+  {
+    this->insert(vit, BowVector::value_type(id, v));
+  }
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::normalize(LNorm norm_type)
+{
+  double norm = 0.0; 
+  BowVector::iterator it;
+
+  if(norm_type == DBoW2::L1)
+  {
+    for(it = begin(); it != end(); ++it)
+      norm += fabs(it->second);
+  }
+  else
+  {
+    for(it = begin(); it != end(); ++it)
+      norm += it->second * it->second;
+		norm = sqrt(norm);  
+  }
+
+  if(norm > 0.0)
+  {
+    for(it = begin(); it != end(); ++it)
+      it->second /= norm;
+  }
+}
+
+// --------------------------------------------------------------------------
+
+std::ostream& operator<< (std::ostream &out, const BowVector &v)
+{
+  BowVector::const_iterator vit;
+  std::vector<unsigned int>::const_iterator iit;
+  unsigned int i = 0; 
+  const unsigned int N = v.size();
+  for(vit = v.begin(); vit != v.end(); ++vit, ++i)
+  {
+    out << "<" << vit->first << ", " << vit->second << ">";
+    
+    if(i < N-1) out << ", ";
+  }
+  return out;
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::saveM(const std::string &filename, size_t W) const
+{
+  std::fstream f(filename.c_str(), std::ios::out);
+  
+  WordId last = 0;
+  BowVector::const_iterator bit;
+  for(bit = this->begin(); bit != this->end(); ++bit)
+  {
+    for(; last < bit->first; ++last)
+    {
+      f << "0 ";
+    }
+    f << bit->second << " ";
+    
+    last = bit->first + 1;
+  }
+  for(; last < (WordId)W; ++last)
+    f << "0 ";
+  
+  f.close();
+}
+
+// --------------------------------------------------------------------------
+
+} // namespace DBoW2
+

Thirdparty/DBoW2/DBoW2/BowVector.h

@@ -0,0 +1,109 @@
+/**
+ * File: BowVector.h
+ * Date: March 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: bag of words vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_BOW_VECTOR__
+#define __D_T_BOW_VECTOR__
+
+#include <iostream>
+#include <map>
+#include <vector>
+
+namespace DBoW2 {
+
+/// Id of words
+typedef unsigned int WordId;
+
+/// Value of a word
+typedef double WordValue;
+
+/// Id of nodes in the vocabulary treee
+typedef unsigned int NodeId;
+
+/// L-norms for normalization
+enum LNorm
+{
+  L1,
+  L2
+};
+
+/// Weighting type
+enum WeightingType
+{
+  TF_IDF,
+  TF,
+  IDF,
+  BINARY
+};
+
+/// Scoring type
+enum ScoringType
+{
+  L1_NORM,
+  L2_NORM,
+  CHI_SQUARE,
+  KL,
+  BHATTACHARYYA,
+  DOT_PRODUCT,
+};
+
+/// Vector of words to represent images
+class BowVector: 
+	public std::map<WordId, WordValue>
+{
+public:
+
+	/** 
+	 * Constructor
+	 */
+	BowVector(void);
+
+	/**
+	 * Destructor
+	 */
+	~BowVector(void);
+	
+	/**
+	 * Adds a value to a word value existing in the vector, or creates a new
+	 * word with the given value
+	 * @param id word id to look for
+	 * @param v value to create the word with, or to add to existing word
+	 */
+	void addWeight(WordId id, WordValue v);
+	
+	/**
+	 * Adds a word with a value to the vector only if this does not exist yet
+	 * @param id word id to look for
+	 * @param v value to give to the word if this does not exist
+	 */
+	void addIfNotExist(WordId id, WordValue v);
+
+	/**
+	 * L1-Normalizes the values in the vector 
+	 * @param norm_type norm used
+	 */
+	void normalize(LNorm norm_type);
+	
+	/**
+	 * Prints the content of the bow vector
+	 * @param out stream
+	 * @param v
+	 */
+	friend std::ostream& operator<<(std::ostream &out, const BowVector &v);
+	
+	/**
+	 * Saves the bow vector as a vector in a matlab file
+	 * @param filename
+	 * @param W number of words in the vocabulary
+	 */
+	void saveM(const std::string &filename, size_t W) const;
+};
+
+} // namespace DBoW2
+
+#endif

Thirdparty/DBoW2/DBoW2/FClass.h

@@ -0,0 +1,71 @@
+/**
+ * File: FClass.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: generic FClass to instantiate templated classes
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_FCLASS__
+#define __D_T_FCLASS__
+
+#include <opencv2/core/core.hpp>
+#include <vector>
+#include <string>
+
+namespace DBoW2 {
+
+/// Generic class to encapsulate functions to manage descriptors.
+/**
+ * This class must be inherited. Derived classes can be used as the
+ * parameter F when creating Templated structures
+ * (TemplatedVocabulary, TemplatedDatabase, ...)
+ */
+class FClass
+{
+  class TDescriptor;
+  typedef const TDescriptor *pDescriptor;
+  
+  /**
+   * Calculates the mean value of a set of descriptors
+   * @param descriptors
+   * @param mean mean descriptor
+   */
+  virtual void meanValue(const std::vector<pDescriptor> &descriptors, 
+    TDescriptor &mean) = 0;
+  
+  /**
+   * Calculates the distance between two descriptors
+   * @param a
+   * @param b
+   * @return distance
+   */
+  static double distance(const TDescriptor &a, const TDescriptor &b);
+  
+  /**
+   * Returns a string version of the descriptor
+   * @param a descriptor
+   * @return string version
+   */
+  static std::string toString(const TDescriptor &a);
+  
+  /**
+   * Returns a descriptor from a string
+   * @param a descriptor
+   * @param s string version
+   */
+  static void fromString(TDescriptor &a, const std::string &s);
+
+  /**
+   * Returns a mat with the descriptors in float format
+   * @param descriptors
+   * @param mat (out) NxL 32F matrix
+   */
+  static void toMat32F(const std::vector<TDescriptor> &descriptors, 
+    cv::Mat &mat);
+};
+
+} // namespace DBoW2
+
+#endif

Thirdparty/DBoW2/DBoW2/FORB.cpp

@@ -0,0 +1,193 @@
+/**
+ * File: FORB.cpp
+ * Date: June 2012
+ * Author: Dorian Galvez-Lopez
+ * Description: functions for ORB descriptors
+ * License: see the LICENSE.txt file
+ *
+ * Distance function has been modified 
+ *
+ */
+
+ 
+#include <vector>
+#include <string>
+#include <sstream>
+#include <stdint.h>
+
+#include "FORB.h"
+
+using namespace std;
+
+namespace DBoW2 {
+
+// --------------------------------------------------------------------------
+
+const int FORB::L=32;
+
+void FORB::meanValue(const std::vector<FORB::pDescriptor> &descriptors, 
+  FORB::TDescriptor &mean)
+{
+  if(descriptors.empty())
+  {
+    mean.release();
+    return;
+  }
+  else if(descriptors.size() == 1)
+  {
+    mean = descriptors[0]->clone();
+  }
+  else
+  {
+    vector<int> sum(FORB::L * 8, 0);
+    
+    for(size_t i = 0; i < descriptors.size(); ++i)
+    {
+      const cv::Mat &d = *descriptors[i];
+      const unsigned char *p = d.ptr<unsigned char>();
+      
+      for(int j = 0; j < d.cols; ++j, ++p)
+      {
+        if(*p & (1 << 7)) ++sum[ j*8     ];
+        if(*p & (1 << 6)) ++sum[ j*8 + 1 ];
+        if(*p & (1 << 5)) ++sum[ j*8 + 2 ];
+        if(*p & (1 << 4)) ++sum[ j*8 + 3 ];
+        if(*p & (1 << 3)) ++sum[ j*8 + 4 ];
+        if(*p & (1 << 2)) ++sum[ j*8 + 5 ];
+        if(*p & (1 << 1)) ++sum[ j*8 + 6 ];
+        if(*p & (1))      ++sum[ j*8 + 7 ];
+      }
+    }
+    
+    mean = cv::Mat::zeros(1, FORB::L, CV_8U);
+    unsigned char *p = mean.ptr<unsigned char>();
+    
+    const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2;
+    for(size_t i = 0; i < sum.size(); ++i)
+    {
+      if(sum[i] >= N2)
+      {
+        // set bit
+        *p |= 1 << (7 - (i % 8));
+      }
+      
+      if(i % 8 == 7) ++p;
+    }
+  }
+}
+
+// --------------------------------------------------------------------------
+  
+int FORB::distance(const FORB::TDescriptor &a,
+  const FORB::TDescriptor &b)
+{
+  // Bit set count operation from
+  // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
+
+  const int *pa = a.ptr<int32_t>();
+  const int *pb = b.ptr<int32_t>();
+
+  int dist=0;
+
+  for(int i=0; i<8; i++, pa++, pb++)
+  {
+      unsigned  int v = *pa ^ *pb;
+      v = v - ((v >> 1) & 0x55555555);
+      v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
+      dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
+  }
+
+  return dist;
+}
+
+// --------------------------------------------------------------------------
+  
+std::string FORB::toString(const FORB::TDescriptor &a)
+{
+  stringstream ss;
+  const unsigned char *p = a.ptr<unsigned char>();
+  
+  for(int i = 0; i < a.cols; ++i, ++p)
+  {
+    ss << (int)*p << " ";
+  }
+  
+  return ss.str();
+}
+
+// --------------------------------------------------------------------------
+  
+void FORB::fromString(FORB::TDescriptor &a, const std::string &s)
+{
+  a.create(1, FORB::L, CV_8U);
+  unsigned char *p = a.ptr<unsigned char>();
+  
+  stringstream ss(s);
+  for(int i = 0; i < FORB::L; ++i, ++p)
+  {
+    int n;
+    ss >> n;
+    
+    if(!ss.fail()) 
+      *p = (unsigned char)n;
+  }
+  
+}
+
+// --------------------------------------------------------------------------
+
+void FORB::toMat32F(const std::vector<TDescriptor> &descriptors, 
+  cv::Mat &mat)
+{
+  if(descriptors.empty())
+  {
+    mat.release();
+    return;
+  }
+  
+  const size_t N = descriptors.size();
+  
+  mat.create(N, FORB::L*8, CV_32F);
+  float *p = mat.ptr<float>();
+  
+  for(size_t i = 0; i < N; ++i)
+  {
+    const int C = descriptors[i].cols;
+    const unsigned char *desc = descriptors[i].ptr<unsigned char>();
+    
+    for(int j = 0; j < C; ++j, p += 8)
+    {
+      p[0] = (desc[j] & (1 << 7) ? 1 : 0);
+      p[1] = (desc[j] & (1 << 6) ? 1 : 0);
+      p[2] = (desc[j] & (1 << 5) ? 1 : 0);
+      p[3] = (desc[j] & (1 << 4) ? 1 : 0);
+      p[4] = (desc[j] & (1 << 3) ? 1 : 0);
+      p[5] = (desc[j] & (1 << 2) ? 1 : 0);
+      p[6] = (desc[j] & (1 << 1) ? 1 : 0);
+      p[7] = desc[j] & (1);
+    }
+  } 
+}
+
+// --------------------------------------------------------------------------
+
+void FORB::toMat8U(const std::vector<TDescriptor> &descriptors, 
+  cv::Mat &mat)
+{
+  mat.create(descriptors.size(), 32, CV_8U);
+  
+  unsigned char *p = mat.ptr<unsigned char>();
+  
+  for(size_t i = 0; i < descriptors.size(); ++i, p += 32)
+  {
+    const unsigned char *d = descriptors[i].ptr<unsigned char>();
+    std::copy(d, d+32, p);
+  }
+  
+}
+
+// --------------------------------------------------------------------------
+
+} // namespace DBoW2
+
+

Thirdparty/DBoW2/DBoW2/FORB.h

@@ -0,0 +1,79 @@
+/**
+ * File: FORB.h
+ * Date: June 2012
+ * Author: Dorian Galvez-Lopez
+ * Description: functions for ORB descriptors
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_F_ORB__
+#define __D_T_F_ORB__
+
+#include <opencv2/core/core.hpp>
+#include <vector>
+#include <string>
+
+#include "FClass.h"
+
+namespace DBoW2 {
+
+/// Functions to manipulate ORB descriptors
+class FORB: protected FClass
+{
+public:
+
+  /// Descriptor type
+  typedef cv::Mat TDescriptor; // CV_8U
+  /// Pointer to a single descriptor
+  typedef const TDescriptor *pDescriptor;
+  /// Descriptor length (in bytes)
+  static const int L;
+
+  /**
+   * Calculates the mean value of a set of descriptors
+   * @param descriptors
+   * @param mean mean descriptor
+   */
+  static void meanValue(const std::vector<pDescriptor> &descriptors,
+    TDescriptor &mean);
+
+  /**
+   * Calculates the distance between two descriptors
+   * @param a
+   * @param b
+   * @return distance
+   */
+  static int distance(const TDescriptor &a, const TDescriptor &b);
+
+  /**
+   * Returns a string version of the descriptor
+   * @param a descriptor
+   * @return string version
+   */
+  static std::string toString(const TDescriptor &a);
+
+  /**
+   * Returns a descriptor from a string
+   * @param a descriptor
+   * @param s string version
+   */
+  static void fromString(TDescriptor &a, const std::string &s);
+
+  /**
+   * Returns a mat with the descriptors in float format
+   * @param descriptors
+   * @param mat (out) NxL 32F matrix
+   */
+  static void toMat32F(const std::vector<TDescriptor> &descriptors,
+    cv::Mat &mat);
+
+  static void toMat8U(const std::vector<TDescriptor> &descriptors,
+    cv::Mat &mat);
+
+};
+
+} // namespace DBoW2
+
+#endif
+

Thirdparty/DBoW2/DBoW2/FeatureVector.cpp

@@ -0,0 +1,85 @@
+/**
+ * File: FeatureVector.cpp
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: feature vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include "FeatureVector.h"
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace DBoW2 {
+
+// ---------------------------------------------------------------------------
+
+FeatureVector::FeatureVector(void)
+{
+}
+
+// ---------------------------------------------------------------------------
+
+FeatureVector::~FeatureVector(void)
+{
+}
+
+// ---------------------------------------------------------------------------
+
+void FeatureVector::addFeature(NodeId id, unsigned int i_feature)
+{
+  FeatureVector::iterator vit = this->lower_bound(id);
+  
+  if(vit != this->end() && vit->first == id)
+  {
+    vit->second.push_back(i_feature);
+  }
+  else
+  {
+    vit = this->insert(vit, FeatureVector::value_type(id, 
+      std::vector<unsigned int>() ));
+    vit->second.push_back(i_feature);
+  }
+}
+
+// ---------------------------------------------------------------------------
+
+std::ostream& operator<<(std::ostream &out, 
+  const FeatureVector &v)
+{
+  if(!v.empty())
+  {
+    FeatureVector::const_iterator vit = v.begin();
+    
+    const std::vector<unsigned int>* f = &vit->second;
+
+    out << "<" << vit->first << ": [";
+    if(!f->empty()) out << (*f)[0];
+    for(unsigned int i = 1; i < f->size(); ++i)
+    {
+      out << ", " << (*f)[i];
+    }
+    out << "]>";
+    
+    for(++vit; vit != v.end(); ++vit)
+    {
+      f = &vit->second;
+      
+      out << ", <" << vit->first << ": [";
+      if(!f->empty()) out << (*f)[0];
+      for(unsigned int i = 1; i < f->size(); ++i)
+      {
+        out << ", " << (*f)[i];
+      }
+      out << "]>";
+    }
+  }
+  
+  return out;  
+}
+
+// ---------------------------------------------------------------------------
+
+} // namespace DBoW2

Thirdparty/DBoW2/DBoW2/FeatureVector.h

@@ -0,0 +1,56 @@
+/**
+ * File: FeatureVector.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: feature vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_FEATURE_VECTOR__
+#define __D_T_FEATURE_VECTOR__
+
+#include "BowVector.h"
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace DBoW2 {
+
+/// Vector of nodes with indexes of local features
+class FeatureVector: 
+  public std::map<NodeId, std::vector<unsigned int> >
+{
+public:
+
+  /**
+   * Constructor
+   */
+  FeatureVector(void);
+  
+  /**
+   * Destructor
+   */
+  ~FeatureVector(void);
+  
+  /**
+   * Adds a feature to an existing node, or adds a new node with an initial
+   * feature
+   * @param id node id to add or to modify
+   * @param i_feature index of feature to add to the given node
+   */
+  void addFeature(NodeId id, unsigned int i_feature);
+
+  /**
+   * Sends a string versions of the feature vector through the stream
+   * @param out stream
+   * @param v feature vector
+   */
+  friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v);
+    
+};
+
+} // namespace DBoW2
+
+#endif
+

Thirdparty/DBoW2/DBoW2/ScoringObject.cpp

@@ -0,0 +1,315 @@
+/**
+ * File: ScoringObject.cpp
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions to compute bow scores 
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <cfloat>
+#include "TemplatedVocabulary.h"
+#include "BowVector.h"
+
+using namespace DBoW2;
+
+// If you change the type of WordValue, make sure you change also the
+// epsilon value (this is needed by the KL method)
+const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double L1Scoring::score(const BowVector &v1, const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += fabs(vi - wi) - fabs(vi) - fabs(wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+  
+  // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) 
+  //		for all i | v_i != 0 and w_i != 0 
+  // (Nister, 2006)
+  // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}
+  score = -score/2.0;
+
+  return score; // [0..1]
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double L2Scoring::score(const BowVector &v1, const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += vi * wi;
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+  
+  // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) )
+	//		for all i | v_i != 0 and w_i != 0 )
+	// (Nister, 2006)
+	if(score >= 1) // rounding errors
+	  score = 1.0;
+	else
+    score = 1.0 - sqrt(1.0 - score); // [0..1]
+
+  return score;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) 
+  const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  // all the items are taken into account
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)
+      // we move the -4 out
+      if(vi + wi != 0.0) score += vi * wi / (vi + wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+    }
+  }
+    
+  // this takes the -4 into account
+  score = 2. * score; // [0..1]
+
+  return score;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double KLScoring::score(const BowVector &v1, const BowVector &v2) const
+{ 
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  // all the items or v are taken into account
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      if(vi != 0 && wi != 0) score += vi * log(vi/wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      score += vi * (log(vi) - LOG_EPS);
+      ++v1_it;
+    }
+    else
+    {
+      // move v2_it forward, do not add any score
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+  
+  // sum rest of items of v
+  for(; v1_it != v1_end; ++v1_it) 
+    if(v1_it->second != 0)
+      score += v1_it->second * (log(v1_it->second) - LOG_EPS);
+  
+  return score; // cannot be scaled
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double BhattacharyyaScoring::score(const BowVector &v1, 
+  const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += sqrt(vi * wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+
+  return score; // already scaled
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double DotProductScoring::score(const BowVector &v1, 
+  const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += vi * wi;
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+
+  return score; // cannot scale
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+

Thirdparty/DBoW2/DBoW2/ScoringObject.h

@@ -0,0 +1,96 @@
+/**
+ * File: ScoringObject.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions to compute bow scores 
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_SCORING_OBJECT__
+#define __D_T_SCORING_OBJECT__
+
+#include "BowVector.h"
+
+namespace DBoW2 {
+
+/// Base class of scoring functions
+class GeneralScoring
+{
+public:
+  /**
+   * Computes the score between two vectors. Vectors must be sorted and 
+   * normalized if necessary
+   * @param v (in/out)
+   * @param w (in/out)
+   * @return score
+   */
+  virtual double score(const BowVector &v, const BowVector &w) const = 0;
+
+  /**
+   * Returns whether a vector must be normalized before scoring according
+   * to the scoring scheme
+   * @param norm norm to use
+   * @return true iff must normalize
+   */
+  virtual bool mustNormalize(LNorm &norm) const = 0;
+
+  /// Log of epsilon
+	static const double LOG_EPS; 
+  // If you change the type of WordValue, make sure you change also the
+	// epsilon value (this is needed by the KL method)
+
+  virtual ~GeneralScoring() {} //!< Required for virtual base classes
+	
+};
+
+/** 
+ * Macro for defining Scoring classes
+ * @param NAME name of class
+ * @param MUSTNORMALIZE if vectors must be normalized to compute the score
+ * @param NORM type of norm to use when MUSTNORMALIZE
+ */
+#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \
+  NAME: public GeneralScoring \
+  { public: \
+    /** \
+     * Computes score between two vectors \
+     * @param v \
+     * @param w \
+     * @return score between v and w \
+     */ \
+    virtual double score(const BowVector &v, const BowVector &w) const; \
+    \
+    /** \
+     * Says if a vector must be normalized according to the scoring function \
+     * @param norm (out) if true, norm to use
+     * @return true iff vectors must be normalized \
+     */ \
+    virtual inline bool mustNormalize(LNorm &norm) const  \
+      { norm = NORM; return MUSTNORMALIZE; } \
+  }
+  
+/// L1 Scoring object
+class __SCORING_CLASS(L1Scoring, true, L1);
+
+/// L2 Scoring object
+class __SCORING_CLASS(L2Scoring, true, L2);
+
+/// Chi square Scoring object
+class __SCORING_CLASS(ChiSquareScoring, true, L1);
+
+/// KL divergence Scoring object
+class __SCORING_CLASS(KLScoring, true, L1);
+
+/// Bhattacharyya Scoring object
+class __SCORING_CLASS(BhattacharyyaScoring, true, L1);
+
+/// Dot product Scoring object
+class __SCORING_CLASS(DotProductScoring, false, L1);
+
+#undef __SCORING_CLASS
+  
+} // namespace DBoW2
+
+#endif
+

Thirdparty/DBoW2/DUtils/Random.cpp

@@ -0,0 +1,129 @@
+/*	
+ * File: Random.cpp
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: April 2010
+ * Description: manages pseudo-random numbers
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include "Random.h"
+#include "Timestamp.h"
+#include <cstdlib>
+using namespace std;
+
+bool DUtils::Random::m_already_seeded = false;
+
+void DUtils::Random::SeedRand(){
+	Timestamp time;
+	time.setToCurrentTime();
+	srand((unsigned)time.getFloatTime()); 
+}
+
+void DUtils::Random::SeedRandOnce()
+{
+  if(!m_already_seeded)
+  {
+    DUtils::Random::SeedRand();
+    m_already_seeded = true;
+  }
+}
+
+void DUtils::Random::SeedRand(int seed)
+{
+	srand(seed); 
+}
+
+void DUtils::Random::SeedRandOnce(int seed)
+{
+  if(!m_already_seeded)
+  {
+    DUtils::Random::SeedRand(seed);
+    m_already_seeded = true;
+  }
+}
+
+int DUtils::Random::RandomInt(int min, int max){
+	int d = max - min + 1;
+	return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max)
+{
+  if(min <= max)
+  {
+    m_min = min;
+    m_max = max;
+  }
+  else
+  {
+    m_min = max;
+    m_max = min;
+  }
+
+  createValues();
+}
+
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer
+  (const DUtils::Random::UnrepeatedRandomizer& rnd)
+{
+  *this = rnd;
+}
+
+// ---------------------------------------------------------------------------
+
+int DUtils::Random::UnrepeatedRandomizer::get()
+{
+  if(empty()) createValues();
+  
+  DUtils::Random::SeedRandOnce();
+  
+  int k = DUtils::Random::RandomInt(0, m_values.size()-1);
+  int ret = m_values[k];
+  m_values[k] = m_values.back();
+  m_values.pop_back();
+  
+  return ret;
+}
+
+// ---------------------------------------------------------------------------
+
+void DUtils::Random::UnrepeatedRandomizer::createValues()
+{
+  int n = m_max - m_min + 1;
+  
+  m_values.resize(n);
+  for(int i = 0; i < n; ++i) m_values[i] = m_min + i;
+}
+
+// ---------------------------------------------------------------------------
+
+void DUtils::Random::UnrepeatedRandomizer::reset()
+{
+  if((int)m_values.size() != m_max - m_min + 1) createValues();
+}
+
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer& 
+DUtils::Random::UnrepeatedRandomizer::operator=
+  (const DUtils::Random::UnrepeatedRandomizer& rnd)
+{
+  if(this != &rnd)
+  {
+    this->m_min = rnd.m_min;
+    this->m_max = rnd.m_max;
+    this->m_values = rnd.m_values;
+  }
+  return *this;
+}
+
+// ---------------------------------------------------------------------------
+
+

Thirdparty/DBoW2/DUtils/Random.h

@@ -0,0 +1,184 @@
+/*	
+ * File: Random.h
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: April 2010, November 2011
+ * Description: manages pseudo-random numbers
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#pragma once
+#ifndef __D_RANDOM__
+#define __D_RANDOM__
+
+#include <cstdlib>
+#include <vector>
+
+namespace DUtils {
+
+/// Functions to generate pseudo-random numbers
+class Random
+{
+public:
+  class UnrepeatedRandomizer;
+  
+public:
+	/**
+	 * Sets the random number seed to the current time
+	 */
+	static void SeedRand();
+	
+	/**
+	 * Sets the random number seed to the current time only the first
+	 * time this function is called
+	 */
+	static void SeedRandOnce();
+
+	/** 
+	 * Sets the given random number seed
+	 * @param seed
+	 */
+	static void SeedRand(int seed);
+
+	/** 
+	 * Sets the given random number seed only the first time this function 
+	 * is called
+	 * @param seed
+	 */
+	static void SeedRandOnce(int seed);
+
+	/**
+	 * Returns a random number in the range [0..1]
+	 * @return random T number in [0..1]
+	 */
+	template <class T>
+	static T RandomValue(){
+		return (T)rand()/(T)RAND_MAX;
+	}
+
+	/**
+	 * Returns a random number in the range [min..max]
+	 * @param min
+	 * @param max
+	 * @return random T number in [min..max]
+	 */
+	template <class T>
+	static T RandomValue(T min, T max){
+		return Random::RandomValue<T>() * (max - min) + min;
+	}
+
+	/**
+	 * Returns a random int in the range [min..max]
+	 * @param min
+	 * @param max
+	 * @return random int in [min..max]
+	 */
+	static int RandomInt(int min, int max);
+	
+	/** 
+	 * Returns a random number from a gaussian distribution
+	 * @param mean
+	 * @param sigma standard deviation
+	 */
+	template <class T>
+	static T RandomGaussianValue(T mean, T sigma)
+	{
+    // Box-Muller transformation
+    T x1, x2, w, y1;
+
+    do {
+      x1 = (T)2. * RandomValue<T>() - (T)1.;
+      x2 = (T)2. * RandomValue<T>() - (T)1.;
+      w = x1 * x1 + x2 * x2;
+    } while ( w >= (T)1. || w == (T)0. );
+
+    w = sqrt( ((T)-2.0 * log( w ) ) / w );
+    y1 = x1 * w;
+
+    return( mean + y1 * sigma );
+	}
+
+private:
+
+  /// If SeedRandOnce() or SeedRandOnce(int) have already been called
+  static bool m_already_seeded;
+  
+};
+
+// ---------------------------------------------------------------------------
+
+/// Provides pseudo-random numbers with no repetitions
+class Random::UnrepeatedRandomizer
+{
+public:
+
+  /** 
+   * Creates a randomizer that returns numbers in the range [min, max]
+   * @param min
+   * @param max
+   */
+  UnrepeatedRandomizer(int min, int max);
+  ~UnrepeatedRandomizer(){}
+  
+  /**
+   * Copies a randomizer
+   * @param rnd
+   */
+  UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd);
+  
+  /**
+   * Copies a randomizer
+   * @param rnd
+   */
+  UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd);
+  
+  /** 
+   * Returns a random number not given before. If all the possible values
+   * were already given, the process starts again
+   * @return unrepeated random number
+   */
+  int get();
+  
+  /**
+   * Returns whether all the possible values between min and max were
+   * already given. If get() is called when empty() is true, the behaviour
+   * is the same than after creating the randomizer
+   * @return true iff all the values were returned
+   */
+  inline bool empty() const { return m_values.empty(); }
+  
+  /**
+   * Returns the number of values still to be returned
+   * @return amount of values to return
+   */
+  inline unsigned int left() const { return m_values.size(); }
+  
+  /**
+   * Resets the randomizer as it were just created
+   */
+  void reset();
+  
+protected:
+
+  /** 
+   * Creates the vector with available values
+   */
+  void createValues();
+
+protected:
+
+  /// Min of range of values
+  int m_min;
+  /// Max of range of values
+  int m_max;
+
+  /// Available values
+  std::vector<int> m_values;
+
+};
+
+}
+
+#endif
+

Thirdparty/DBoW2/DUtils/Timestamp.cpp

@@ -0,0 +1,246 @@
+/*
+ * File: Timestamp.cpp
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ * 
+ * Note: in windows, this class has a 1ms resolution
+ *
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <cstdio>
+#include <cstdlib>
+#include <ctime>
+#include <cmath>
+#include <sstream>
+#include <iomanip>
+
+#ifdef _WIN32
+#ifndef WIN32
+#define WIN32
+#endif
+#endif
+
+#ifdef WIN32
+#include <sys/timeb.h>
+#define sprintf sprintf_s
+#else
+#include <sys/time.h>
+#endif
+
+#include "Timestamp.h"
+
+using namespace std;
+
+using namespace DUtils;
+
+Timestamp::Timestamp(Timestamp::tOptions option)
+{
+  if(option & CURRENT_TIME)
+    setToCurrentTime();
+  else if(option & ZERO)
+    setTime(0.);
+}
+
+Timestamp::~Timestamp(void)
+{
+}
+
+bool Timestamp::empty() const
+{
+  return m_secs == 0 && m_usecs == 0;
+}
+
+void Timestamp::setToCurrentTime(){
+	
+#ifdef WIN32
+	struct __timeb32 timebuffer;
+	_ftime32_s( &timebuffer ); // C4996
+	// Note: _ftime is deprecated; consider using _ftime_s instead
+	m_secs = timebuffer.time;
+	m_usecs = timebuffer.millitm * 1000;
+#else
+	struct timeval now;
+	gettimeofday(&now, NULL);
+	m_secs = now.tv_sec;
+	m_usecs = now.tv_usec;
+#endif
+
+}
+
+void Timestamp::setTime(const string &stime){
+	string::size_type p = stime.find('.');
+	if(p == string::npos){
+		m_secs = atol(stime.c_str());
+		m_usecs = 0;
+	}else{
+		m_secs = atol(stime.substr(0, p).c_str());
+		
+		string s_usecs = stime.substr(p+1, 6);
+		m_usecs = atol(stime.substr(p+1).c_str());
+		m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length()));
+	}
+}
+
+void Timestamp::setTime(double s)
+{
+  m_secs = (unsigned long)s;
+  m_usecs = (s - (double)m_secs) * 1e6;
+}
+
+double Timestamp::getFloatTime() const {
+	return double(m_secs) + double(m_usecs)/1000000.0;
+}
+
+string Timestamp::getStringTime() const {
+	char buf[32];
+	sprintf(buf, "%.6lf", this->getFloatTime());
+	return string(buf);
+}
+
+double Timestamp::operator- (const Timestamp &t) const {
+	return this->getFloatTime() - t.getFloatTime();
+}
+
+Timestamp& Timestamp::operator+= (double s)
+{
+  *this = *this + s;
+  return *this;
+}
+
+Timestamp& Timestamp::operator-= (double s)
+{
+  *this = *this - s;
+  return *this;
+}
+
+Timestamp Timestamp::operator+ (double s) const
+{
+	unsigned long secs = (long)floor(s);
+	unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+  return this->plus(secs, usecs);
+}
+
+Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const
+{
+  Timestamp t;
+
+	const unsigned long max = 1000000ul;
+
+	if(m_usecs + usecs >= max)
+		t.setTime(m_secs + secs + 1, m_usecs + usecs - max);
+	else
+		t.setTime(m_secs + secs, m_usecs + usecs);
+	
+	return t;
+}
+
+Timestamp Timestamp::operator- (double s) const
+{
+	unsigned long secs = (long)floor(s);
+	unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+	return this->minus(secs, usecs);
+}
+
+Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const
+{
+  Timestamp t;
+
+	const unsigned long max = 1000000ul;
+
+	if(m_usecs < usecs)
+		t.setTime(m_secs - secs - 1, max - (usecs - m_usecs));
+	else
+		t.setTime(m_secs - secs, m_usecs - usecs);
+	
+	return t;
+}
+
+bool Timestamp::operator> (const Timestamp &t) const
+{
+	if(m_secs > t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs > t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator>= (const Timestamp &t) const
+{
+	if(m_secs > t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator< (const Timestamp &t) const
+{
+	if(m_secs < t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs < t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator<= (const Timestamp &t) const
+{
+	if(m_secs < t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator== (const Timestamp &t) const
+{
+	return(m_secs == t.m_secs && m_usecs == t.m_usecs);
+}
+
+
+string Timestamp::Format(bool machine_friendly) const 
+{
+  struct tm tm_time;
+
+  time_t t = (time_t)getFloatTime();
+
+#ifdef WIN32
+  localtime_s(&tm_time, &t);
+#else
+  localtime_r(&t, &tm_time);
+#endif
+  
+  char buffer[128];
+  
+  if(machine_friendly)
+  {
+    strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time);
+  }
+  else
+  {
+    strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001
+  }
+  
+  return string(buffer);
+}
+
+string Timestamp::Format(double s) {
+	int days = int(s / (24. * 3600.0));
+	s -= days * (24. * 3600.0);
+	int hours = int(s / 3600.0);
+	s -= hours * 3600;
+	int minutes = int(s / 60.0);
+	s -= minutes * 60;
+	int seconds = int(s);
+	int ms = int((s - seconds)*1e6);
+
+	stringstream ss;
+	ss.fill('0');
+	bool b;
+	if((b = (days > 0))) ss << days << "d ";
+	if((b = (b || hours > 0))) ss << setw(2) << hours << ":";
+	if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":";
+	if(b) ss << setw(2);
+	ss << seconds;
+	if(!b) ss << "." << setw(6) << ms;
+
+	return ss.str();
+}
+
+

Thirdparty/DBoW2/DUtils/Timestamp.h

@@ -0,0 +1,204 @@
+/*
+ * File: Timestamp.h
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_TIMESTAMP__
+#define __D_TIMESTAMP__
+
+#include <iostream>
+using namespace std;
+
+namespace DUtils {
+
+/// Timestamp
+class Timestamp
+{
+public:
+
+  /// Options to initiate a timestamp
+  enum tOptions
+  {
+    NONE = 0,
+    CURRENT_TIME = 0x1,
+    ZERO = 0x2
+  };
+  
+public:
+  
+  /**
+   * Creates a timestamp 
+   * @param option option to set the initial time stamp
+   */
+	Timestamp(Timestamp::tOptions option = NONE);
+	
+	/**
+	 * Destructor
+	 */
+	virtual ~Timestamp(void);
+
+  /**
+   * Says if the timestamp is "empty": seconds and usecs are both 0, as 
+   * when initiated with the ZERO flag
+   * @return true iif secs == usecs == 0
+   */
+  bool empty() const;
+
+	/**
+	 * Sets this instance to the current time
+	 */
+	void setToCurrentTime();
+
+	/**
+	 * Sets the timestamp from seconds and microseconds
+	 * @param secs: seconds
+	 * @param usecs: microseconds
+	 */
+	inline void setTime(unsigned long secs, unsigned long usecs){
+		m_secs = secs;
+		m_usecs = usecs;
+	}
+	
+	/**
+	 * Returns the timestamp in seconds and microseconds
+	 * @param secs seconds
+	 * @param usecs microseconds
+	 */
+	inline void getTime(unsigned long &secs, unsigned long &usecs) const
+	{
+	  secs = m_secs;
+	  usecs = m_usecs;
+	}
+
+	/**
+	 * Sets the timestamp from a string with the time in seconds
+	 * @param stime: string such as "1235603336.036609"
+	 */
+	void setTime(const string &stime);
+	
+	/**
+	 * Sets the timestamp from a number of seconds from the epoch
+	 * @param s seconds from the epoch
+	 */
+	void setTime(double s);
+	
+	/**
+	 * Returns this timestamp as the number of seconds in (long) float format
+	 */
+	double getFloatTime() const;
+
+	/**
+	 * Returns this timestamp as the number of seconds in fixed length string format
+	 */
+	string getStringTime() const;
+
+	/**
+	 * Returns the difference in seconds between this timestamp (greater) and t (smaller)
+	 * If the order is swapped, a negative number is returned
+	 * @param t: timestamp to subtract from this timestamp
+	 * @return difference in seconds
+	 */
+	double operator- (const Timestamp &t) const;
+
+	/** 
+	 * Returns a copy of this timestamp + s seconds + us microseconds
+	 * @param s seconds
+	 * @param us microseconds
+	 */
+	Timestamp plus(unsigned long s, unsigned long us) const;
+
+  /**
+   * Returns a copy of this timestamp - s seconds - us microseconds
+   * @param s seconds
+   * @param us microseconds
+   */
+  Timestamp minus(unsigned long s, unsigned long us) const;
+
+  /**
+   * Adds s seconds to this timestamp and returns a reference to itself
+   * @param s seconds
+   * @return reference to this timestamp
+   */
+  Timestamp& operator+= (double s);
+  
+  /**
+   * Substracts s seconds to this timestamp and returns a reference to itself
+   * @param s seconds
+   * @return reference to this timestamp
+   */
+  Timestamp& operator-= (double s);
+
+	/**
+	 * Returns a copy of this timestamp + s seconds
+	 * @param s: seconds
+	 */
+	Timestamp operator+ (double s) const;
+
+	/**
+	 * Returns a copy of this timestamp - s seconds
+	 * @param s: seconds
+	 */
+	Timestamp operator- (double s) const;
+
+	/**
+	 * Returns whether this timestamp is at the future of t
+	 * @param t
+	 */
+	bool operator> (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the future of (or is the same as) t
+	 * @param t
+	 */
+	bool operator>= (const Timestamp &t) const;
+
+	/** 
+	 * Returns whether this timestamp and t represent the same instant
+	 * @param t
+	 */
+	bool operator== (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the past of t
+	 * @param t
+	 */
+	bool operator< (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the past of (or is the same as) t
+	 * @param t
+	 */
+	bool operator<= (const Timestamp &t) const;
+
+  /**
+   * Returns the timestamp in a human-readable string
+   * @param machine_friendly if true, the returned string is formatted
+   *   to yyyymmdd_hhmmss, without weekday or spaces
+   * @note This has not been tested under Windows
+   * @note The timestamp is truncated to seconds
+   */
+  string Format(bool machine_friendly = false) const;
+
+	/**
+	 * Returns a string version of the elapsed time in seconds, with the format
+	 * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us
+	 * @param s: elapsed seconds (given by getFloatTime) to format
+	 */
+	static string Format(double s);
+	
+
+protected:
+  /// Seconds
+	unsigned long m_secs;	// seconds
+	/// Microseconds
+	unsigned long m_usecs;	// microseconds
+};
+
+}
+
+#endif
+

Thirdparty/DBoW2/LICENSE.txt

@@ -0,0 +1,44 @@
+DBoW2: bag-of-words library for C++ with generic descriptors
+
+Copyright (c) 2015 Dorian Galvez-Lopez <http://doriangalvez.com> (Universidad de Zaragoza)
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+3. Neither the name of copyright holders nor the names of its
+   contributors may be used to endorse or promote products derived
+   from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS
+BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+
+If you use it in an academic work, please cite:
+
+  @ARTICLE{GalvezTRO12,
+    author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 
+    journal={IEEE Transactions on Robotics},
+    title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
+    year={2012},
+    month={October},
+    volume={28},
+    number={5},
+    pages={1188--1197},
+    doi={10.1109/TRO.2012.2197158},
+    ISSN={1552-3098}
+  }
+

Thirdparty/DBoW2/README.txt

@@ -0,0 +1,7 @@
+You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2).
+See the original DBoW2 library at: https://github.com/dorian3d/DBoW2
+All files included in this version are BSD, see LICENSE.txt
+
+We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils.
+See the original DLib library at: https://github.com/dorian3d/DLib
+All files included in this version are BSD, see LICENSE.txt

Thirdparty/g2o/CMakeLists.txt

@@ -0,0 +1,174 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+SET(CMAKE_LEGACY_CYGWIN_WIN32 0)
+
+PROJECT(g2o)
+
+SET(g2o_C_FLAGS)
+SET(g2o_CXX_FLAGS)
+
+# default built type
+IF(NOT CMAKE_BUILD_TYPE)
+  SET(CMAKE_BUILD_TYPE Release)
+ENDIF()
+
+MESSAGE(STATUS "BUILD TYPE:" ${CMAKE_BUILD_TYPE})
+
+SET (G2O_LIB_TYPE SHARED)
+
+# There seems to be an issue with MSVC8
+# see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=83
+if(MSVC90)
+  add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY=1)
+  message(STATUS "Disabling memory alignment for MSVC8")
+endif(MSVC90)
+
+# Set the output directory for the build executables and libraries
+IF(WIN32)
+  SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH "Target for the libraries")
+ELSE(WIN32)
+  SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH "Target for the libraries")
+ENDIF(WIN32)
+SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
+SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${g2o_RUNTIME_OUTPUT_DIRECTORY})
+
+# Set search directory for looking for our custom CMake scripts to
+# look for SuiteSparse, QGLViewer, and Eigen3.
+LIST(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules)
+
+# Detect OS and define macros appropriately
+IF(UNIX)
+  ADD_DEFINITIONS(-DUNIX)
+  MESSAGE(STATUS "Compiling on Unix")
+ENDIF(UNIX)
+
+# Eigen library parallelise itself, though, presumably due to performance issues
+# OPENMP is experimental. We experienced some slowdown with it
+FIND_PACKAGE(OpenMP)
+SET(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)")
+IF(OPENMP_FOUND AND G2O_USE_OPENMP)
+  SET (G2O_OPENMP 1)
+  SET(g2o_C_FLAGS "${g2o_C_FLAGS} ${OpenMP_C_FLAGS}")
+  SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}")
+  MESSAGE(STATUS "Compiling with OpenMP support")
+ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP)
+
+# Compiler specific options for gcc
+SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native") 
+SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native") 
+
+# activate warnings !!!
+SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W")
+SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -W")
+
+# specifying compiler flags
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}")
+SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}")
+
+# Find Eigen3
+SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE})
+FIND_PACKAGE(Eigen3 3.1.0 REQUIRED)
+IF(EIGEN3_FOUND)
+  SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3")
+ELSE(EIGEN3_FOUND)
+  SET(G2O_EIGEN3_INCLUDE "" CACHE PATH "Directory of Eigen3")
+ENDIF(EIGEN3_FOUND)
+
+# Generate config.h
+SET(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}")
+configure_file(config.h.in ${g2o_SOURCE_DIR}/config.h)
+
+# Set up the top-level include directories
+INCLUDE_DIRECTORIES(
+${g2o_SOURCE_DIR}/core
+${g2o_SOURCE_DIR}/types
+${g2o_SOURCE_DIR}/stuff 
+${G2O_EIGEN3_INCLUDE})
+
+# Include the subdirectories
+ADD_LIBRARY(g2o ${G2O_LIB_TYPE}
+#types
+g2o/types/types_sba.h
+g2o/types/types_six_dof_expmap.h
+g2o/types/types_sba.cpp
+g2o/types/types_six_dof_expmap.cpp
+g2o/types/types_seven_dof_expmap.cpp
+g2o/types/types_seven_dof_expmap.h
+g2o/types/se3quat.h
+g2o/types/se3_ops.h
+g2o/types/se3_ops.hpp
+#core
+g2o/core/base_edge.h
+g2o/core/base_binary_edge.h
+g2o/core/hyper_graph_action.cpp
+g2o/core/base_binary_edge.hpp
+g2o/core/hyper_graph_action.h
+g2o/core/base_multi_edge.h           
+g2o/core/hyper_graph.cpp
+g2o/core/base_multi_edge.hpp         
+g2o/core/hyper_graph.h
+g2o/core/base_unary_edge.h          
+g2o/core/linear_solver.h
+g2o/core/base_unary_edge.hpp         
+g2o/core/marginal_covariance_cholesky.cpp
+g2o/core/base_vertex.h               
+g2o/core/marginal_covariance_cholesky.h
+g2o/core/base_vertex.hpp             
+g2o/core/matrix_structure.cpp
+g2o/core/batch_stats.cpp             
+g2o/core/matrix_structure.h
+g2o/core/batch_stats.h               
+g2o/core/openmp_mutex.h
+g2o/core/block_solver.h              
+g2o/core/block_solver.hpp            
+g2o/core/parameter.cpp               
+g2o/core/parameter.h                 
+g2o/core/cache.cpp                   
+g2o/core/cache.h
+g2o/core/optimizable_graph.cpp       
+g2o/core/optimizable_graph.h         
+g2o/core/solver.cpp                  
+g2o/core/solver.h
+g2o/core/creators.h                 
+g2o/core/optimization_algorithm_factory.cpp
+g2o/core/estimate_propagator.cpp     
+g2o/core/optimization_algorithm_factory.h
+g2o/core/estimate_propagator.h       
+g2o/core/factory.cpp                 
+g2o/core/optimization_algorithm_property.h
+g2o/core/factory.h                   
+g2o/core/sparse_block_matrix.h
+g2o/core/sparse_optimizer.cpp  
+g2o/core/sparse_block_matrix.hpp
+g2o/core/sparse_optimizer.h
+g2o/core/hyper_dijkstra.cpp 
+g2o/core/hyper_dijkstra.h
+g2o/core/parameter_container.cpp     
+g2o/core/parameter_container.h
+g2o/core/optimization_algorithm.cpp 
+g2o/core/optimization_algorithm.h
+g2o/core/optimization_algorithm_with_hessian.cpp 
+g2o/core/optimization_algorithm_with_hessian.h
+g2o/core/optimization_algorithm_levenberg.cpp 
+g2o/core/optimization_algorithm_levenberg.h
+g2o/core/jacobian_workspace.cpp 
+g2o/core/jacobian_workspace.h
+g2o/core/robust_kernel.cpp 
+g2o/core/robust_kernel.h
+g2o/core/robust_kernel_factory.cpp
+g2o/core/robust_kernel_factory.h
+g2o/core/robust_kernel_impl.cpp 
+g2o/core/robust_kernel_impl.h
+#stuff
+g2o/stuff/string_tools.h
+g2o/stuff/color_macros.h 
+g2o/stuff/macros.h
+g2o/stuff/timeutil.cpp
+g2o/stuff/misc.h
+g2o/stuff/timeutil.h
+g2o/stuff/os_specific.c    
+g2o/stuff/os_specific.h
+g2o/stuff/string_tools.cpp
+g2o/stuff/property.cpp       
+g2o/stuff/property.h       
+)

Thirdparty/g2o/README.txt

@@ -0,0 +1,3 @@
+You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2).
+See the original g2o library at: https://github.com/RainerKuemmerle/g2o
+All files included in this g2o version are BSD, see license-bsd.txt

Thirdparty/g2o/cmake_modules/FindBLAS.cmake

@@ -0,0 +1,419 @@
+# Find BLAS library
+#
+# This module finds an installed library that implements the BLAS
+# linear-algebra interface (see http://www.netlib.org/blas/).
+# The list of libraries searched for is mainly taken
+# from the autoconf macro file, acx_blas.m4 (distributed at
+# http://ac-archive.sourceforge.net/ac-archive/acx_blas.html).
+#
+# This module sets the following variables:
+#  BLAS_FOUND - set to true if a library implementing the BLAS interface
+#    is found
+#  BLAS_INCLUDE_DIR - Directories containing the BLAS header files
+#  BLAS_DEFINITIONS - Compilation options to use BLAS
+#  BLAS_LINKER_FLAGS - Linker flags to use BLAS (excluding -l
+#    and -L).
+#  BLAS_LIBRARIES_DIR - Directories containing the BLAS libraries.
+#     May be null if BLAS_LIBRARIES contains libraries name using full path.
+#  BLAS_LIBRARIES - List of libraries to link against BLAS interface.
+#     May be null if the compiler supports auto-link (e.g. VC++).
+#  BLAS_USE_FILE - The name of the cmake module to include to compile
+#     applications or libraries using BLAS.
+#
+# This module was modified by CGAL team:
+# - find libraries for a C++ compiler, instead of Fortran
+# - added BLAS_INCLUDE_DIR, BLAS_DEFINITIONS and BLAS_LIBRARIES_DIR
+# - removed BLAS95_LIBRARIES
+
+include(CheckFunctionExists)
+
+
+# This macro checks for the existence of the combination of fortran libraries
+# given by _list.  If the combination is found, this macro checks (using the
+# check_function_exists macro) whether can link against that library
+# combination using the name of a routine given by _name using the linker
+# flags given by _flags.  If the combination of libraries is found and passes
+# the link test, LIBRARIES is set to the list of complete library paths that
+# have been found and DEFINITIONS to the required definitions.
+# Otherwise, LIBRARIES is set to FALSE.
+# N.B. _prefix is the prefix applied to the names of all cached variables that
+# are generated internally and marked advanced by this macro.
+macro(check_fortran_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _path)
+  #message("DEBUG: check_fortran_libraries(${_list} in ${_path})")
+
+  # Check for the existence of the libraries given by _list
+  set(_libraries_found TRUE)
+  set(_libraries_work FALSE)
+  set(${DEFINITIONS} "")
+  set(${LIBRARIES} "")
+  set(_combined_name)
+  foreach(_library ${_list})
+    set(_combined_name ${_combined_name}_${_library})
+
+    if(_libraries_found)
+      # search first in ${_path}
+      find_library(${_prefix}_${_library}_LIBRARY
+                  NAMES ${_library}
+                  PATHS ${_path} NO_DEFAULT_PATH
+                  )
+      # if not found, search in environment variables and system
+      if ( WIN32 )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS ENV LIB
+                    )
+      elseif ( APPLE )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
+                    )
+      else ()
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
+                    )
+      endif()
+      mark_as_advanced(${_prefix}_${_library}_LIBRARY)
+      set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
+      set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
+    endif(_libraries_found)
+  endforeach(_library ${_list})
+  if(_libraries_found)
+    set(_libraries_found ${${LIBRARIES}})
+  endif()
+
+  # Test this combination of libraries with the Fortran/f2c interface.
+  # We test the Fortran interface first as it is well standardized.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS}  "-D${_prefix}_USE_F2C")
+    set(${LIBRARIES}    ${_libraries_found})
+    # Some C++ linkers require the f2c library to link with Fortran libraries.
+    # I do not know which ones, thus I just add the f2c library if it is available.
+    find_package( F2C QUIET )
+    if ( F2C_FOUND )
+      set(${DEFINITIONS}  ${${DEFINITIONS}} ${F2C_DEFINITIONS})
+      set(${LIBRARIES}    ${${LIBRARIES}} ${F2C_LIBRARIES})
+    endif()
+    set(CMAKE_REQUIRED_DEFINITIONS  ${${DEFINITIONS}})
+    set(CMAKE_REQUIRED_LIBRARIES    ${_flags} ${${LIBRARIES}})
+    #message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    # Check if function exists with f2c calling convention (ie a trailing underscore)
+    check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(CMAKE_REQUIRED_DEFINITIONS} "")
+    set(CMAKE_REQUIRED_LIBRARIES    "")
+    mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # If not found, test this combination of libraries with a C interface.
+  # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   ${_libraries_found})
+    set(CMAKE_REQUIRED_DEFINITIONS "")
+    set(CMAKE_REQUIRED_LIBRARIES   ${_flags} ${${LIBRARIES}})
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
+    set(CMAKE_REQUIRED_LIBRARIES "")
+    mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # on failure
+  if(NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   FALSE)
+  endif()
+  #message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
+  #message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
+endmacro(check_fortran_libraries)
+
+
+#
+# main
+#
+
+# Is it already configured?
+if (BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
+
+  set(BLAS_FOUND TRUE)
+
+else()
+
+  # reset variables
+  set( BLAS_INCLUDE_DIR "" )
+  set( BLAS_DEFINITIONS "" )
+  set( BLAS_LINKER_FLAGS "" )
+  set( BLAS_LIBRARIES "" )
+  set( BLAS_LIBRARIES_DIR "" )
+
+    #
+    # If Unix, search for BLAS function in possible libraries
+    #
+
+    # BLAS in ATLAS library? (http://math-atlas.sourceforge.net/)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "cblas;f77blas;atlas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in PhiPACK libraries? (requires generic BLAS lib, too)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "sgemm;dgemm;blas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in Alpha CXML library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "cxml"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in Alpha DXML library? (now called CXML, see above)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "dxml"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in Sun Performance library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      "-xlic_lib=sunperf"
+      "sunperf;sunmath"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+      if(BLAS_LIBRARIES)
+        # Extra linker flag
+        set(BLAS_LINKER_FLAGS "-xlic_lib=sunperf")
+      endif()
+    endif()
+
+    # BLAS in SCSL library?  (SGI/Cray Scientific Library)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "scsl"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in SGIMATH library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "complib.sgimath"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in IBM ESSL library? (requires generic BLAS lib, too)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "essl;blas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in intel mkl 10 library? (em64t 64bit)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl_intel_lp64;mkl_intel_thread;mkl_core;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    ### windows version of intel mkl 10?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      SGEMM
+      ""
+      "mkl_c_dll;mkl_intel_thread_dll;mkl_core_dll;libguide40"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #older versions of intel mkl libs
+
+    # BLAS in intel mkl library? (shared)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in intel mkl library? (static, 32bit)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl_ia32;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in intel mkl library? (static, em64t 64bit)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl_em64t;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in acml library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "acml"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # Apple BLAS library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "Accelerate"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    if ( NOT BLAS_LIBRARIES )
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "vecLib"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif ( NOT BLAS_LIBRARIES )
+
+    # Generic BLAS library?
+    # This configuration *must* be the last try as this library is notably slow.
+    if ( NOT BLAS_LIBRARIES )
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "blas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+  if(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
+    set(BLAS_FOUND TRUE)
+  else()
+    set(BLAS_FOUND FALSE)
+  endif()
+
+  if(NOT BLAS_FIND_QUIETLY)
+    if(BLAS_FOUND)
+      message(STATUS "A library with BLAS API found.")
+    else(BLAS_FOUND)
+      if(BLAS_FIND_REQUIRED)
+        message(FATAL_ERROR "A required library with BLAS API not found. Please specify library location.")
+      else()
+        message(STATUS "A library with BLAS API not found. Please specify library location.")
+      endif()
+    endif(BLAS_FOUND)
+  endif(NOT BLAS_FIND_QUIETLY)
+
+  # Add variables to cache
+  set( BLAS_INCLUDE_DIR   "${BLAS_INCLUDE_DIR}"
+                          CACHE PATH "Directories containing the BLAS header files" FORCE )
+  set( BLAS_DEFINITIONS   "${BLAS_DEFINITIONS}"
+                          CACHE STRING "Compilation options to use BLAS" FORCE )
+  set( BLAS_LINKER_FLAGS  "${BLAS_LINKER_FLAGS}"
+                          CACHE STRING "Linker flags to use BLAS" FORCE )
+  set( BLAS_LIBRARIES     "${BLAS_LIBRARIES}"
+                          CACHE FILEPATH "BLAS libraries name" FORCE )
+  set( BLAS_LIBRARIES_DIR "${BLAS_LIBRARIES_DIR}"
+                          CACHE PATH "Directories containing the BLAS libraries" FORCE )
+
+  #message("DEBUG: BLAS_INCLUDE_DIR = ${BLAS_INCLUDE_DIR}")
+  #message("DEBUG: BLAS_DEFINITIONS = ${BLAS_DEFINITIONS}")
+  #message("DEBUG: BLAS_LINKER_FLAGS = ${BLAS_LINKER_FLAGS}")
+  #message("DEBUG: BLAS_LIBRARIES = ${BLAS_LIBRARIES}")
+  #message("DEBUG: BLAS_LIBRARIES_DIR = ${BLAS_LIBRARIES_DIR}")
+  #message("DEBUG: BLAS_FOUND = ${BLAS_FOUND}")
+
+endif(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)

Thirdparty/g2o/cmake_modules/FindEigen3.cmake

@@ -0,0 +1,87 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  # specific additional paths for some OS
+  if (WIN32)
+    set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
+  endif(WIN32)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${EIGEN_ADDITIONAL_SEARCH_PATHS}
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+

Thirdparty/g2o/cmake_modules/FindLAPACK.cmake

@@ -0,0 +1,273 @@
+# Find LAPACK library
+#
+# This module finds an installed library that implements the LAPACK
+# linear-algebra interface (see http://www.netlib.org/lapack/).
+# The approach follows mostly that taken for the autoconf macro file, acx_lapack.m4
+# (distributed at http://ac-archive.sourceforge.net/ac-archive/acx_lapack.html).
+#
+# This module sets the following variables:
+#  LAPACK_FOUND - set to true if a library implementing the LAPACK interface
+#    is found
+#  LAPACK_INCLUDE_DIR - Directories containing the LAPACK header files
+#  LAPACK_DEFINITIONS - Compilation options to use LAPACK
+#  LAPACK_LINKER_FLAGS - Linker flags to use LAPACK (excluding -l
+#    and -L).
+#  LAPACK_LIBRARIES_DIR - Directories containing the LAPACK libraries.
+#     May be null if LAPACK_LIBRARIES contains libraries name using full path.
+#  LAPACK_LIBRARIES - List of libraries to link against LAPACK interface.
+#     May be null if the compiler supports auto-link (e.g. VC++).
+#  LAPACK_USE_FILE - The name of the cmake module to include to compile
+#     applications or libraries using LAPACK.
+#
+# This module was modified by CGAL team:
+# - find libraries for a C++ compiler, instead of Fortran
+# - added LAPACK_INCLUDE_DIR, LAPACK_DEFINITIONS and LAPACK_LIBRARIES_DIR
+# - removed LAPACK95_LIBRARIES
+
+
+include(CheckFunctionExists)
+
+# This macro checks for the existence of the combination of fortran libraries
+# given by _list.  If the combination is found, this macro checks (using the
+# check_function_exists macro) whether can link against that library
+# combination using the name of a routine given by _name using the linker
+# flags given by _flags.  If the combination of libraries is found and passes
+# the link test, LIBRARIES is set to the list of complete library paths that
+# have been found and DEFINITIONS to the required definitions.
+# Otherwise, LIBRARIES is set to FALSE.
+# N.B. _prefix is the prefix applied to the names of all cached variables that
+# are generated internally and marked advanced by this macro.
+macro(check_lapack_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _blas _path)
+  #message("DEBUG: check_lapack_libraries(${_list} in ${_path} with ${_blas})")
+
+  # Check for the existence of the libraries given by _list
+  set(_libraries_found TRUE)
+  set(_libraries_work FALSE)
+  set(${DEFINITIONS} "")
+  set(${LIBRARIES} "")
+  set(_combined_name)
+  foreach(_library ${_list})
+    set(_combined_name ${_combined_name}_${_library})
+
+    if(_libraries_found)
+      # search first in ${_path}
+      find_library(${_prefix}_${_library}_LIBRARY
+                  NAMES ${_library}
+                  PATHS ${_path} NO_DEFAULT_PATH
+                  )
+      # if not found, search in environment variables and system
+      if ( WIN32 )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS ENV LIB
+                    )
+      elseif ( APPLE )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
+                    )
+      else ()
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
+                    )
+      endif()
+      mark_as_advanced(${_prefix}_${_library}_LIBRARY)
+      set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
+      set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
+    endif(_libraries_found)
+  endforeach(_library ${_list})
+  if(_libraries_found)
+    set(_libraries_found ${${LIBRARIES}})
+  endif()
+
+  # Test this combination of libraries with the Fortran/f2c interface.
+  # We test the Fortran interface first as it is well standardized.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS}  "-D${_prefix}_USE_F2C")
+    set(${LIBRARIES}    ${_libraries_found})
+    # Some C++ linkers require the f2c library to link with Fortran libraries.
+    # I do not know which ones, thus I just add the f2c library if it is available.
+    find_package( F2C QUIET )
+    if ( F2C_FOUND )
+      set(${DEFINITIONS}  ${${DEFINITIONS}} ${F2C_DEFINITIONS})
+      set(${LIBRARIES}    ${${LIBRARIES}} ${F2C_LIBRARIES})
+    endif()
+    set(CMAKE_REQUIRED_DEFINITIONS  ${${DEFINITIONS}})
+    set(CMAKE_REQUIRED_LIBRARIES    ${_flags} ${${LIBRARIES}} ${_blas})
+    #message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    # Check if function exists with f2c calling convention (ie a trailing underscore)
+    check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(CMAKE_REQUIRED_DEFINITIONS} "")
+    set(CMAKE_REQUIRED_LIBRARIES    "")
+    mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # If not found, test this combination of libraries with a C interface.
+  # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   ${_libraries_found})
+    set(CMAKE_REQUIRED_DEFINITIONS "")
+    set(CMAKE_REQUIRED_LIBRARIES   ${_flags} ${${LIBRARIES}} ${_blas})
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
+    set(CMAKE_REQUIRED_LIBRARIES "")
+    mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # on failure
+  if(NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   FALSE)
+  endif()
+  #message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
+  #message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
+endmacro(check_lapack_libraries)
+
+
+#
+# main
+#
+
+# LAPACK requires BLAS
+if(LAPACK_FIND_QUIETLY OR NOT LAPACK_FIND_REQUIRED)
+  find_package(BLAS)
+else()
+  find_package(BLAS REQUIRED)
+endif()
+
+if (NOT BLAS_FOUND)
+
+  message(STATUS "LAPACK requires BLAS.")
+  set(LAPACK_FOUND FALSE)
+
+# Is it already configured?
+elseif (LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
+
+  set(LAPACK_FOUND TRUE)
+
+else()
+
+  # reset variables
+  set( LAPACK_INCLUDE_DIR "" )
+  set( LAPACK_DEFINITIONS "" )
+  set( LAPACK_LINKER_FLAGS "" ) # unused (yet)
+  set( LAPACK_LIBRARIES "" )
+  set( LAPACK_LIBRARIES_DIR "" )
+
+    #
+    # If Unix, search for LAPACK function in possible libraries
+    #
+
+    #intel mkl lapack?
+    if(NOT LAPACK_LIBRARIES)
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "mkl_lapack"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+    #acml lapack?
+    if(NOT LAPACK_LIBRARIES)
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "acml"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+    # Apple LAPACK library?
+    if(NOT LAPACK_LIBRARIES)
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "Accelerate"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+    if ( NOT LAPACK_LIBRARIES )
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "vecLib"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif ( NOT LAPACK_LIBRARIES )
+
+    # Generic LAPACK library?
+    # This configuration *must* be the last try as this library is notably slow.
+    if ( NOT LAPACK_LIBRARIES )
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "lapack"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+  if(LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
+    set(LAPACK_FOUND TRUE)
+  else()
+    set(LAPACK_FOUND FALSE)
+  endif()
+
+  if(NOT LAPACK_FIND_QUIETLY)
+    if(LAPACK_FOUND)
+      message(STATUS "A library with LAPACK API found.")
+    else(LAPACK_FOUND)
+      if(LAPACK_FIND_REQUIRED)
+        message(FATAL_ERROR "A required library with LAPACK API not found. Please specify library location.")
+      else()
+        message(STATUS "A library with LAPACK API not found. Please specify library location.")
+      endif()
+    endif(LAPACK_FOUND)
+  endif(NOT LAPACK_FIND_QUIETLY)
+
+  # Add variables to cache
+  set( LAPACK_INCLUDE_DIR   "${LAPACK_INCLUDE_DIR}"
+                            CACHE PATH "Directories containing the LAPACK header files" FORCE )
+  set( LAPACK_DEFINITIONS   "${LAPACK_DEFINITIONS}"
+                            CACHE STRING "Compilation options to use LAPACK" FORCE )
+  set( LAPACK_LINKER_FLAGS  "${LAPACK_LINKER_FLAGS}"
+                            CACHE STRING "Linker flags to use LAPACK" FORCE )
+  set( LAPACK_LIBRARIES     "${LAPACK_LIBRARIES}"
+                            CACHE FILEPATH "LAPACK libraries name" FORCE )
+  set( LAPACK_LIBRARIES_DIR "${LAPACK_LIBRARIES_DIR}"
+                            CACHE PATH "Directories containing the LAPACK libraries" FORCE )
+
+  #message("DEBUG: LAPACK_INCLUDE_DIR = ${LAPACK_INCLUDE_DIR}")
+  #message("DEBUG: LAPACK_DEFINITIONS = ${LAPACK_DEFINITIONS}")
+  #message("DEBUG: LAPACK_LINKER_FLAGS = ${LAPACK_LINKER_FLAGS}")
+  #message("DEBUG: LAPACK_LIBRARIES = ${LAPACK_LIBRARIES}")
+  #message("DEBUG: LAPACK_LIBRARIES_DIR = ${LAPACK_LIBRARIES_DIR}")
+  #message("DEBUG: LAPACK_FOUND = ${LAPACK_FOUND}")
+
+endif(NOT BLAS_FOUND)

Thirdparty/g2o/config.h.in

@@ -0,0 +1,13 @@
+#ifndef G2O_CONFIG_H
+#define G2O_CONFIG_H
+
+#cmakedefine G2O_OPENMP 1
+#cmakedefine G2O_SHARED_LIBS 1
+
+// give a warning if Eigen defaults to row-major matrices.
+// We internally assume column-major matrices throughout the code.
+#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+#  error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)"
+#endif
+
+#endif

Thirdparty/g2o/g2o/core/base_binary_edge.h

@@ -0,0 +1,119 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_BINARY_EDGE_H
+#define G2O_BASE_BINARY_EDGE_H
+
+#include <iostream>
+#include <limits>
+
+#include "base_edge.h"
+#include "robust_kernel.h"
+#include "../../config.h"
+
+namespace g2o {
+
+  using namespace Eigen;
+
+  template <int D, typename E, typename VertexXi, typename VertexXj>
+  class BaseBinaryEdge : public BaseEdge<D, E>
+  {
+    public:
+
+      typedef VertexXi VertexXiType;
+      typedef VertexXj VertexXjType;
+
+      static const int Di = VertexXiType::Dimension;
+      static const int Dj = VertexXjType::Dimension;
+
+      static const int Dimension = BaseEdge<D, E>::Dimension;
+      typedef typename BaseEdge<D,E>::Measurement Measurement;
+      typedef typename Matrix<double, D, Di>::AlignedMapType JacobianXiOplusType;
+      typedef typename Matrix<double, D, Dj>::AlignedMapType JacobianXjOplusType;
+      typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
+      typedef typename BaseEdge<D,E>::InformationType InformationType;
+
+      typedef Eigen::Map<Matrix<double, Di, Dj>, Matrix<double, Di, Dj>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
+      typedef Eigen::Map<Matrix<double, Dj, Di>, Matrix<double, Dj, Di>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType;
+
+      BaseBinaryEdge() : BaseEdge<D,E>(),
+      _hessianRowMajor(false),
+      _hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps
+      _hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension),
+      _jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj)
+      {
+        _vertices.resize(2);
+      }
+
+      virtual OptimizableGraph::Vertex* createFrom();
+      virtual OptimizableGraph::Vertex* createTo();
+
+      virtual void resize(size_t size);
+
+      virtual bool allVerticesFixed() const;
+
+      virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
+
+      /**
+       * Linearizes the oplus operator in the vertex, and stores
+       * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
+       */
+      virtual void linearizeOplus();
+
+      //! returns the result of the linearization in the manifold space for the node xi
+      const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
+      //! returns the result of the linearization in the manifold space for the node xj
+      const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;}
+
+      virtual void constructQuadraticForm() ;
+
+      virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
+
+      using BaseEdge<D,E>::resize;
+      using BaseEdge<D,E>::computeError;
+
+    protected:
+      using BaseEdge<D,E>::_measurement;
+      using BaseEdge<D,E>::_information;
+      using BaseEdge<D,E>::_error;
+      using BaseEdge<D,E>::_vertices;
+      using BaseEdge<D,E>::_dimension;
+
+      bool _hessianRowMajor;
+      HessianBlockType _hessian;
+      HessianBlockTransposedType _hessianTransposed;
+      JacobianXiOplusType _jacobianOplusXi;
+      JacobianXjOplusType _jacobianOplusXj;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+#include "base_binary_edge.hpp"
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/base_binary_edge.hpp

@@ -0,0 +1,218 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createFrom(){
+  return new VertexXiType();
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createTo(){
+  return new VertexXjType();
+}
+
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::resize(size_t size)
+{
+  if (size != 2) {
+    std::cerr << "WARNING, attempting to resize binary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
+  }
+  BaseEdge<D, E>::resize(size);
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+bool BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::allVerticesFixed() const
+{
+  return (static_cast<const VertexXiType*> (_vertices[0])->fixed() &&
+          static_cast<const VertexXjType*> (_vertices[1])->fixed());
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::constructQuadraticForm()
+{
+  VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
+  VertexXjType* to   = static_cast<VertexXjType*>(_vertices[1]);
+
+  // get the Jacobian of the nodes in the manifold domain
+  const JacobianXiOplusType& A = jacobianOplusXi();
+  const JacobianXjOplusType& B = jacobianOplusXj();
+
+
+  bool fromNotFixed = !(from->fixed());
+  bool toNotFixed = !(to->fixed());
+
+  if (fromNotFixed || toNotFixed) {
+#ifdef G2O_OPENMP
+    from->lockQuadraticForm();
+    to->lockQuadraticForm();
+#endif
+    const InformationType& omega = _information;
+    Matrix<double, D, 1> omega_r = - omega * _error;
+    if (this->robustKernel() == 0) {
+      if (fromNotFixed) {
+        Matrix<double, VertexXiType::Dimension, D> AtO = A.transpose() * omega;
+        from->b().noalias() += A.transpose() * omega_r;
+        from->A().noalias() += AtO*A;
+        if (toNotFixed ) {
+          if (_hessianRowMajor) // we have to write to the block as transposed
+            _hessianTransposed.noalias() += B.transpose() * AtO.transpose();
+          else
+            _hessian.noalias() += AtO * B;
+        }
+      } 
+      if (toNotFixed) {
+        to->b().noalias() += B.transpose() * omega_r;
+        to->A().noalias() += B.transpose() * omega * B;
+      }
+    } else { // robust (weighted) error according to some kernel
+      double error = this->chi2();
+      Eigen::Vector3d rho;
+      this->robustKernel()->robustify(error, rho);
+      InformationType weightedOmega = this->robustInformation(rho);
+      //std::cout << PVAR(rho.transpose()) << std::endl;
+      //std::cout << PVAR(weightedOmega) << std::endl;
+
+      omega_r *= rho[1];
+      if (fromNotFixed) {
+        from->b().noalias() += A.transpose() * omega_r;
+        from->A().noalias() += A.transpose() * weightedOmega * A;
+        if (toNotFixed ) {
+          if (_hessianRowMajor) // we have to write to the block as transposed
+            _hessianTransposed.noalias() += B.transpose() * weightedOmega * A;
+          else
+            _hessian.noalias() += A.transpose() * weightedOmega * B;
+        }
+      } 
+      if (toNotFixed) {
+        to->b().noalias() += B.transpose() * omega_r;
+        to->A().noalias() += B.transpose() * weightedOmega * B;
+      }
+    }
+#ifdef G2O_OPENMP
+    to->unlockQuadraticForm();
+    from->unlockQuadraticForm();
+#endif
+  }
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
+{
+  new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di);
+  new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj);
+  linearizeOplus();
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus()
+{
+  VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
+  VertexXjType* vj = static_cast<VertexXjType*>(_vertices[1]);
+
+  bool iNotFixed = !(vi->fixed());
+  bool jNotFixed = !(vj->fixed());
+
+  if (!iNotFixed && !jNotFixed)
+    return;
+
+#ifdef G2O_OPENMP
+  vi->lockQuadraticForm();
+  vj->lockQuadraticForm();
+#endif
+
+  const double delta = 1e-9;
+  const double scalar = 1.0 / (2*delta);
+  ErrorVector errorBak;
+  ErrorVector errorBeforeNumeric = _error;
+
+  if (iNotFixed) {
+    //Xi - estimate the jacobian numerically
+    double add_vi[VertexXiType::Dimension];
+    std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
+    // add small step along the unit vector in each dimension
+    for (int d = 0; d < VertexXiType::Dimension; ++d) {
+      vi->push();
+      add_vi[d] = delta;
+      vi->oplus(add_vi);
+      computeError();
+      errorBak = _error;
+      vi->pop();
+      vi->push();
+      add_vi[d] = -delta;
+      vi->oplus(add_vi);
+      computeError();
+      errorBak -= _error;
+      vi->pop();
+      add_vi[d] = 0.0;
+
+      _jacobianOplusXi.col(d) = scalar * errorBak;
+    } // end dimension
+  }
+
+  if (jNotFixed) {
+    //Xj - estimate the jacobian numerically
+    double add_vj[VertexXjType::Dimension];
+    std::fill(add_vj, add_vj + VertexXjType::Dimension, 0.0);
+    // add small step along the unit vector in each dimension
+    for (int d = 0; d < VertexXjType::Dimension; ++d) {
+      vj->push();
+      add_vj[d] = delta;
+      vj->oplus(add_vj);
+      computeError();
+      errorBak = _error;
+      vj->pop();
+      vj->push();
+      add_vj[d] = -delta;
+      vj->oplus(add_vj);
+      computeError();
+      errorBak -= _error;
+      vj->pop();
+      add_vj[d] = 0.0;
+
+      _jacobianOplusXj.col(d) = scalar * errorBak;
+    }
+  } // end dimension
+
+  _error = errorBeforeNumeric;
+#ifdef G2O_OPENMP
+  vj->unlockQuadraticForm();
+  vi->unlockQuadraticForm();
+#endif
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
+{
+  (void) i; (void) j;
+  //assert(i == 0 && j == 1);
+  if (rowMajor) {
+    new (&_hessianTransposed) HessianBlockTransposedType(d, VertexXjType::Dimension, VertexXiType::Dimension);
+  } else {
+    new (&_hessian) HessianBlockType(d, VertexXiType::Dimension, VertexXjType::Dimension);
+  }
+  _hessianRowMajor = rowMajor;
+}

Thirdparty/g2o/g2o/core/base_edge.h

@@ -0,0 +1,110 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_EDGE_H
+#define G2O_BASE_EDGE_H
+
+#include <iostream>
+#include <limits>
+
+#include <Eigen/Core>
+
+#include "optimizable_graph.h"
+
+namespace g2o {
+
+  using namespace Eigen;
+
+  template <int D, typename E>
+  class BaseEdge : public OptimizableGraph::Edge
+  {
+    public:
+
+      static const int Dimension = D;
+      typedef E Measurement;
+      typedef Matrix<double, D, 1> ErrorVector;
+      typedef Matrix<double, D, D> InformationType;
+
+      BaseEdge() : OptimizableGraph::Edge()
+      {
+        _dimension = D;
+      }
+
+      virtual ~BaseEdge() {}
+
+      virtual double chi2() const 
+      {
+        return _error.dot(information()*_error);
+      }
+
+      virtual const double* errorData() const { return _error.data();}
+      virtual double* errorData() { return _error.data();}
+      const ErrorVector& error() const { return _error;}
+      ErrorVector& error() { return _error;}
+
+      //! information matrix of the constraint
+      const InformationType& information() const { return _information;}
+      InformationType& information() { return _information;}
+      void setInformation(const InformationType& information) { _information = information;}
+
+      virtual const double* informationData() const { return _information.data();}
+      virtual double* informationData() { return _information.data();}
+
+      //! accessor functions for the measurement represented by the edge
+      const Measurement& measurement() const { return _measurement;}
+      virtual void setMeasurement(const Measurement& m) { _measurement = m;}
+
+      virtual int rank() const {return _dimension;}
+
+      virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
+      {
+        std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl;
+      }
+
+    protected:
+
+      Measurement _measurement;
+      InformationType _information;
+      ErrorVector _error;
+
+      /**
+       * calculate the robust information matrix by updating the information matrix of the error
+       */
+      InformationType robustInformation(const Eigen::Vector3d& rho)
+      {
+        InformationType result = rho[1] * _information;
+        //ErrorVector weightedErrror = _information * _error;
+        //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());
+        return result;
+      }
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/base_multi_edge.h

@@ -0,0 +1,113 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_MULTI_EDGE_H
+#define G2O_BASE_MULTI_EDGE_H
+
+#include <iostream>
+#include <iomanip>
+#include <limits>
+
+#include <Eigen/StdVector>
+
+#include "base_edge.h"
+#include "robust_kernel.h"
+#include "../../config.h"
+
+namespace g2o {
+
+  using namespace Eigen;
+
+  /**
+   * \brief base class to represent an edge connecting an arbitrary number of nodes
+   *
+   * D - Dimension of the measurement
+   * E - type to represent the measurement
+   */
+  template <int D, typename E>
+  class BaseMultiEdge : public BaseEdge<D,E>
+  {
+    public:
+      /**
+       * \brief helper for mapping the Hessian memory of the upper triangular block
+       */
+      struct HessianHelper {
+        Eigen::Map<MatrixXd> matrix;     ///< the mapped memory
+        bool transposed;          ///< the block has to be transposed
+        HessianHelper() : matrix(0, 0, 0), transposed(false) {}
+      };
+
+    public:
+      static const int Dimension = BaseEdge<D,E>::Dimension;
+      typedef typename BaseEdge<D,E>::Measurement Measurement;
+      typedef MatrixXd::MapType JacobianType;
+      typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
+      typedef typename BaseEdge<D,E>::InformationType InformationType;
+      typedef Eigen::Map<MatrixXd, MatrixXd::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
+
+      BaseMultiEdge() : BaseEdge<D,E>()
+      {
+      }
+      
+      virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
+
+      /**
+       * Linearizes the oplus operator in the vertex, and stores
+       * the result in temporary variable vector _jacobianOplus
+       */
+      virtual void linearizeOplus();
+      
+      virtual void resize(size_t size);
+
+      virtual bool allVerticesFixed() const;
+
+      virtual void constructQuadraticForm() ;
+
+      virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
+
+      using BaseEdge<D,E>::computeError;
+
+    protected:
+      using BaseEdge<D,E>::_measurement;
+      using BaseEdge<D,E>::_information;
+      using BaseEdge<D,E>::_error;
+      using BaseEdge<D,E>::_vertices;
+      using BaseEdge<D,E>::_dimension;
+
+      std::vector<HessianHelper> _hessian;
+      std::vector<JacobianType, aligned_allocator<JacobianType> > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus)
+
+      void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+#include "base_multi_edge.hpp"
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/base_multi_edge.hpp

@@ -0,0 +1,222 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+namespace internal {
+  inline int computeUpperTriangleIndex(int i, int j)
+  {
+    int elemsUpToCol = ((j-1) * j) / 2;
+    return elemsUpToCol + i;
+  }
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::constructQuadraticForm()
+{
+  if (this->robustKernel()) {
+    double error = this->chi2();
+    Eigen::Vector3d rho;
+    this->robustKernel()->robustify(error, rho);
+    Matrix<double, D, 1> omega_r = - _information * _error;
+    omega_r *= rho[1];
+    computeQuadraticForm(this->robustInformation(rho), omega_r);
+  } else {
+    computeQuadraticForm(_information, - _information * _error);
+  }
+}
+
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
+{
+  for (size_t i = 0; i < _vertices.size(); ++i) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+    assert(v->dimension() >= 0);
+    new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension());
+  }
+  linearizeOplus();
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::linearizeOplus()
+{
+#ifdef G2O_OPENMP
+  for (size_t i = 0; i < _vertices.size(); ++i) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+    v->lockQuadraticForm();
+  }
+#endif
+
+  const double delta = 1e-9;
+  const double scalar = 1.0 / (2*delta);
+  ErrorVector errorBak;
+  ErrorVector errorBeforeNumeric = _error;
+
+  for (size_t i = 0; i < _vertices.size(); ++i) {
+    //Xi - estimate the jacobian numerically
+    OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+
+    if (vi->fixed())
+      continue;
+
+    const int vi_dim = vi->dimension();
+    assert(vi_dim >= 0);
+#ifdef _MSC_VER
+    double* add_vi = new double[vi_dim];
+#else
+    double add_vi[vi_dim];
+#endif
+    std::fill(add_vi, add_vi + vi_dim, 0.0);
+    assert(_dimension >= 0);
+    assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && "jacobian cache dimension does not match");
+      _jacobianOplus[i].resize(_dimension, vi_dim);
+    // add small step along the unit vector in each dimension
+    for (int d = 0; d < vi_dim; ++d) {
+      vi->push();
+      add_vi[d] = delta;
+      vi->oplus(add_vi);
+      computeError();
+      errorBak = _error;
+      vi->pop();
+      vi->push();
+      add_vi[d] = -delta;
+      vi->oplus(add_vi);
+      computeError();
+      errorBak -= _error;
+      vi->pop();
+      add_vi[d] = 0.0;
+
+      _jacobianOplus[i].col(d) = scalar * errorBak;
+    } // end dimension
+#ifdef _MSC_VER
+    delete[] add_vi;
+#endif
+  }
+  _error = errorBeforeNumeric;
+
+#ifdef G2O_OPENMP
+  for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+    v->unlockQuadraticForm();
+  }
+#endif
+
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
+{
+  int idx = internal::computeUpperTriangleIndex(i, j);
+  assert(idx < (int)_hessian.size());
+  OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(i));
+  OptimizableGraph::Vertex* vj = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(j));
+  assert(vi->dimension() >= 0);
+  assert(vj->dimension() >= 0);
+  HessianHelper& h = _hessian[idx];
+  if (rowMajor) {
+    if (h.matrix.data() != d || h.transposed != rowMajor)
+      new (&h.matrix) HessianBlockType(d, vj->dimension(), vi->dimension());
+  } else {
+    if (h.matrix.data() != d || h.transposed != rowMajor)
+      new (&h.matrix) HessianBlockType(d, vi->dimension(), vj->dimension());
+  }
+  h.transposed = rowMajor;
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::resize(size_t size)
+{
+  BaseEdge<D,E>::resize(size);
+  int n = (int)_vertices.size();
+  int maxIdx = (n * (n-1))/2;
+  assert(maxIdx >= 0);
+  _hessian.resize(maxIdx);
+  _jacobianOplus.resize(size, JacobianType(0,0,0));
+}
+
+template <int D, typename E>
+bool BaseMultiEdge<D, E>::allVerticesFixed() const
+{
+  for (size_t i = 0; i < _vertices.size(); ++i) {
+    if (!static_cast<const OptimizableGraph::Vertex*> (_vertices[i])->fixed()) {
+      return false;
+    }
+  }
+  return true;
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError)
+{
+  for (size_t i = 0; i < _vertices.size(); ++i) {
+    OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+    bool istatus = !(from->fixed());
+
+    if (istatus) {
+      const MatrixXd& A = _jacobianOplus[i];
+
+      MatrixXd AtO = A.transpose() * omega;
+      int fromDim = from->dimension();
+      assert(fromDim >= 0);
+      Eigen::Map<MatrixXd> fromMap(from->hessianData(), fromDim, fromDim);
+      Eigen::Map<VectorXd> fromB(from->bData(), fromDim);
+
+      // ii block in the hessian
+#ifdef G2O_OPENMP
+      from->lockQuadraticForm();
+#endif
+      fromMap.noalias() += AtO * A;
+      fromB.noalias() += A.transpose() * weightedError;
+
+      // compute the off-diagonal blocks ij for all j
+      for (size_t j = i+1; j < _vertices.size(); ++j) {
+        OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(_vertices[j]);
+#ifdef G2O_OPENMP
+        to->lockQuadraticForm();
+#endif
+        bool jstatus = !(to->fixed());
+        if (jstatus) {
+          const MatrixXd& B = _jacobianOplus[j];
+          int idx = internal::computeUpperTriangleIndex(i, j);
+          assert(idx < (int)_hessian.size());
+          HessianHelper& hhelper = _hessian[idx];
+          if (hhelper.transposed) { // we have to write to the block as transposed
+            hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
+          } else {
+            hhelper.matrix.noalias() += AtO * B;
+          }
+        }
+#ifdef G2O_OPENMP
+        to->unlockQuadraticForm();
+#endif
+      }
+
+#ifdef G2O_OPENMP
+      from->unlockQuadraticForm();
+#endif
+    }
+
+  }
+}

Thirdparty/g2o/g2o/core/base_unary_edge.h

@@ -0,0 +1,100 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_UNARY_EDGE_H
+#define G2O_BASE_UNARY_EDGE_H
+
+#include <iostream>
+#include <cassert>
+#include <limits>
+
+#include "base_edge.h"
+#include "robust_kernel.h"
+#include "../../config.h"
+
+namespace g2o {
+
+  using namespace Eigen;
+
+  template <int D, typename E, typename VertexXi>
+  class BaseUnaryEdge : public BaseEdge<D,E>
+  {
+    public:
+      static const int Dimension = BaseEdge<D, E>::Dimension;
+      typedef typename BaseEdge<D,E>::Measurement Measurement;
+      typedef VertexXi VertexXiType;
+      typedef typename Matrix<double, D, VertexXiType::Dimension>::AlignedMapType JacobianXiOplusType;
+      typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
+      typedef typename BaseEdge<D,E>::InformationType InformationType;
+
+      BaseUnaryEdge() : BaseEdge<D,E>(),
+        _jacobianOplusXi(0, D, VertexXiType::Dimension)
+      {
+        _vertices.resize(1);
+      }
+
+      virtual void resize(size_t size);
+
+      virtual bool allVerticesFixed() const;
+
+      virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
+
+      /**
+       * Linearizes the oplus operator in the vertex, and stores
+       * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
+       */
+      virtual void linearizeOplus();
+
+      //! returns the result of the linearization in the manifold space for the node xi
+      const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
+
+      virtual void constructQuadraticForm();
+
+      virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to);
+
+      virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");}
+
+      using BaseEdge<D,E>::resize;
+      using BaseEdge<D,E>::computeError;
+
+    protected:
+      using BaseEdge<D,E>::_measurement;
+      using BaseEdge<D,E>::_information;
+      using BaseEdge<D,E>::_error;
+      using BaseEdge<D,E>::_vertices;
+      using BaseEdge<D,E>::_dimension;
+
+      JacobianXiOplusType _jacobianOplusXi;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+#include "base_unary_edge.hpp"
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/base_unary_edge.hpp

@@ -0,0 +1,129 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::resize(size_t size)
+{
+  if (size != 1) {
+    std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
+  }
+  BaseEdge<D, E>::resize(size);
+}
+
+template <int D, typename E, typename VertexXiType>
+bool BaseUnaryEdge<D, E, VertexXiType>::allVerticesFixed() const
+{
+  return static_cast<const VertexXiType*> (_vertices[0])->fixed();
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::constructQuadraticForm()
+{
+  VertexXiType* from=static_cast<VertexXiType*>(_vertices[0]);
+
+  // chain rule to get the Jacobian of the nodes in the manifold domain
+  const JacobianXiOplusType& A = jacobianOplusXi();
+  const InformationType& omega = _information;
+
+  bool istatus = !from->fixed();
+  if (istatus) {
+#ifdef G2O_OPENMP
+    from->lockQuadraticForm();
+#endif
+    if (this->robustKernel()) {
+      double error = this->chi2();
+      Eigen::Vector3d rho;
+      this->robustKernel()->robustify(error, rho);
+      InformationType weightedOmega = this->robustInformation(rho);
+
+      from->b().noalias() -= rho[1] * A.transpose() * omega * _error;
+      from->A().noalias() += A.transpose() * weightedOmega * A;
+    } else {
+      from->b().noalias() -= A.transpose() * omega * _error;
+      from->A().noalias() += A.transpose() * omega * A;
+    }
+#ifdef G2O_OPENMP
+    from->unlockQuadraticForm();
+#endif
+  }
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
+{
+  new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension);
+  linearizeOplus();
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus()
+{
+  //Xi - estimate the jacobian numerically
+  VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
+
+  if (vi->fixed())
+    return;
+
+#ifdef G2O_OPENMP
+  vi->lockQuadraticForm();
+#endif
+
+  const double delta = 1e-9;
+  const double scalar = 1.0 / (2*delta);
+  ErrorVector error1;
+  ErrorVector errorBeforeNumeric = _error;
+
+  double add_vi[VertexXiType::Dimension];
+  std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
+  // add small step along the unit vector in each dimension
+  for (int d = 0; d < VertexXiType::Dimension; ++d) {
+    vi->push();
+    add_vi[d] = delta;
+    vi->oplus(add_vi);
+    computeError();
+    error1 = _error;
+    vi->pop();
+    vi->push();
+    add_vi[d] = -delta;
+    vi->oplus(add_vi);
+    computeError();
+    vi->pop();
+    add_vi[d] = 0.0;
+
+    _jacobianOplusXi.col(d) = scalar * (error1 - _error);
+  } // end dimension
+
+  _error = errorBeforeNumeric;
+#ifdef G2O_OPENMP
+  vi->unlockQuadraticForm();
+#endif
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
+{
+  std::cerr << __PRETTY_FUNCTION__ << " is not implemented, please give implementation in your derived class" << std::endl;
+}

Thirdparty/g2o/g2o/core/base_vertex.h

@@ -0,0 +1,120 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_VERTEX_H
+#define G2O_BASE_VERTEX_H
+
+#include "optimizable_graph.h"
+#include "creators.h"
+#include "../stuff/macros.h"
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+#include <Eigen/StdVector>
+#include <stack>
+
+namespace g2o {
+
+  using namespace Eigen;
+
+
+/**
+ * \brief Templatized BaseVertex
+ *
+ * Templatized BaseVertex
+ * D  : minimal dimension of the vertex, e.g., 3 for rotation in 3D
+ * T  : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
+ */
+  template <int D, typename T>
+  class BaseVertex : public OptimizableGraph::Vertex {
+    public:
+    typedef T EstimateType;
+    typedef std::stack<EstimateType, 
+                       std::vector<EstimateType,  Eigen::aligned_allocator<EstimateType> > >
+    BackupStackType;
+
+    static const int Dimension = D;           ///< dimension of the estimate (minimal) in the manifold space
+
+    typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned >  HessianBlockType;
+
+  public:
+    BaseVertex();
+
+    virtual const double& hessian(int i, int j) const { assert(i<D && j<D); return _hessian(i,j);}
+    virtual double& hessian(int i, int j)  { assert(i<D && j<D); return _hessian(i,j);}
+    virtual double hessianDeterminant() const {return _hessian.determinant();}
+    virtual double* hessianData() { return const_cast<double*>(_hessian.data());}
+
+    virtual void mapHessianMemory(double* d);
+
+    virtual int copyB(double* b_) const {
+      memcpy(b_, _b.data(), Dimension * sizeof(double));
+      return Dimension; 
+    }
+
+    virtual const double& b(int i) const { assert(i < D); return _b(i);}
+    virtual double& b(int i) { assert(i < D); return _b(i);}
+    virtual double* bData() { return _b.data();}
+
+    virtual void clearQuadraticForm();
+
+    //! updates the current vertex with the direct solution x += H_ii\b_ii
+    //! @returns the determinant of the inverted hessian
+    virtual double solveDirect(double lambda=0);
+
+    //! return right hand side b of the constructed linear system
+    Matrix<double, D, 1>& b() { return _b;}
+    const Matrix<double, D, 1>& b() const { return _b;}
+    //! return the hessian block associated with the vertex
+    HessianBlockType& A() { return _hessian;}
+    const HessianBlockType& A() const { return _hessian;}
+
+    virtual void push() { _backup.push(_estimate);}
+    virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
+    virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}
+    virtual int stackSize() const {return _backup.size();}
+
+    //! return the current estimate of the vertex
+    const EstimateType& estimate() const { return _estimate;}
+    //! set the estimate for the vertex also calls updateCache()
+    void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}
+
+  protected:
+    HessianBlockType _hessian;
+    Matrix<double, D, 1> _b;
+    EstimateType _estimate;
+    BackupStackType _backup;
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+#include "base_vertex.hpp"
+
+} // end namespace g2o
+
+
+#endif

Thirdparty/g2o/g2o/core/base_vertex.hpp

@@ -0,0 +1,55 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+template <int D, typename T>
+BaseVertex<D, T>::BaseVertex() :
+  OptimizableGraph::Vertex(),
+  _hessian(0, D, D)
+{
+  _dimension = D;
+}
+
+template <int D, typename T>
+double BaseVertex<D, T>::solveDirect(double lambda) {
+  Matrix <double, D, D> tempA=_hessian + Matrix <double, D, D>::Identity()*lambda;
+  double det=tempA.determinant();
+  if (g2o_isnan(det) || det < std::numeric_limits<double>::epsilon())
+    return det;
+  Matrix <double, D, 1> dx=tempA.llt().solve(_b);
+  oplus(&dx[0]);
+  return det;
+}
+
+template <int D, typename T>
+void BaseVertex<D, T>::clearQuadraticForm() {
+  _b.setZero();
+}
+
+template <int D, typename T>
+void BaseVertex<D, T>::mapHessianMemory(double* d)
+{
+  new (&_hessian) HessianBlockType(d, D, D);
+}

Thirdparty/g2o/g2o/core/batch_stats.cpp

@@ -0,0 +1,90 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "batch_stats.h"
+#include <cstring>
+
+namespace g2o {
+  using namespace std;
+
+  G2OBatchStatistics* G2OBatchStatistics::_globalStats=0;
+
+  #ifndef PTHING
+  #define PTHING(s) \
+    #s << "= " << (st.s) << "\t "
+  #endif
+
+  G2OBatchStatistics::G2OBatchStatistics(){
+    // zero all.
+    memset (this, 0, sizeof(G2OBatchStatistics));
+
+    // set the iteration to -1 to show that it isn't valid
+    iteration = -1;
+  }
+
+  std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st)
+  {
+    os << PTHING(iteration);
+
+    os << PTHING( numVertices ); // how many vertices are involved
+    os << PTHING( numEdges ); // hoe many edges
+    os << PTHING(  chi2 );  // total chi2
+    
+    /** timings **/
+    // nonlinear part
+    os << PTHING(  timeResiduals );  
+    os << PTHING(  timeLinearize );   // jacobians
+    os << PTHING(  timeQuadraticForm ); // construct the quadratic form in the graph
+    
+    // block_solver (constructs Ax=b, plus maybe schur);
+    os << PTHING(  timeSchurComplement ); // compute schur complement (0 if not done);
+    
+    // linear solver (computes Ax=b); );
+    os << PTHING(  timeSymbolicDecomposition ); // symbolic decomposition (0 if not done);
+    os << PTHING(  timeNumericDecomposition ); // numeric decomposition  (0 if not done);
+    os << PTHING(  timeLinearSolution );             // total time for solving Ax=b
+    os << PTHING(  iterationsLinearSolver );  // iterations of PCG
+    os << PTHING(  timeUpdate ); // oplus
+    os << PTHING(  timeIteration ); // total time );
+
+    os << PTHING( levenbergIterations );
+    os << PTHING( timeLinearSolver);
+
+    os << PTHING(hessianDimension);
+    os << PTHING(hessianPoseDimension);
+    os << PTHING(hessianLandmarkDimension);
+    os << PTHING(choleskyNNZ);
+    os << PTHING(timeMarginals);
+
+    return os;
+  };
+
+  void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b)
+  {
+    _globalStats = b;
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/batch_stats.h

@@ -0,0 +1,83 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BATCH_STATS_H_
+#define G2O_BATCH_STATS_H_
+
+#include <iostream>
+#include <vector>
+
+
+namespace g2o {
+
+  /**
+   * \brief statistics about the optimization
+   */
+  struct  G2OBatchStatistics {
+    G2OBatchStatistics();
+    int iteration;                    ///< which iteration
+    int numVertices;                  ///< how many vertices are involved
+    int numEdges;                     ///< how many edges
+    double chi2;                      ///< total chi2
+
+    /** timings **/
+    // nonlinear part
+    double timeResiduals;             ///< residuals
+    double timeLinearize;             ///< jacobians
+    double timeQuadraticForm;         ///< construct the quadratic form in the graph
+    int levenbergIterations;          ///< number of iterations performed by LM
+    // block_solver (constructs Ax=b, plus maybe schur)
+    double timeSchurComplement;      ///< compute schur complement (0 if not done)
+
+    // linear solver (computes Ax=b);
+    double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done)
+    double timeNumericDecomposition;  ///< numeric decomposition  (0 if not done)
+    double timeLinearSolution;        ///< total time for solving Ax=b (including detup for schur)
+    double timeLinearSolver;          ///< time for solving, excluding Schur setup
+    int    iterationsLinearSolver;    ///< iterations of PCG, (0 if not used, i.e., Cholesky)
+    double timeUpdate;                ///< time to apply the update
+    double timeIteration;             ///< total time;
+
+    double timeMarginals;             ///< computing the inverse elements (solve blocks) and thus the marginal covariances
+
+    // information about the Hessian matrix
+    size_t hessianDimension;          ///< rows / cols of the Hessian
+    size_t hessianPoseDimension;      ///< dimension of the pose matrix in Schur
+    size_t hessianLandmarkDimension;  ///< dimension of the landmark matrix in Schur
+    size_t choleskyNNZ;               ///< number of non-zeros in the cholesky factor
+
+    static G2OBatchStatistics* globalStats() {return _globalStats;}
+    static void setGlobalStats(G2OBatchStatistics* b);
+    protected:
+    static G2OBatchStatistics* _globalStats;
+  };
+
+   std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&);
+
+  typedef std::vector<G2OBatchStatistics> BatchStatisticsContainer;
+}
+
+#endif

Thirdparty/g2o/g2o/core/block_solver.h

@@ -0,0 +1,193 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BLOCK_SOLVER_H
+#define G2O_BLOCK_SOLVER_H
+#include <Eigen/Core>
+#include "solver.h"
+#include "linear_solver.h"
+#include "sparse_block_matrix.h"
+#include "sparse_block_matrix_diagonal.h"
+#include "openmp_mutex.h"
+#include "../../config.h"
+
+namespace g2o {
+  using namespace Eigen;
+
+  /**
+   * \brief traits to summarize the properties of the fixed size optimization problem
+   */
+  template <int _PoseDim, int _LandmarkDim>
+  struct BlockSolverTraits
+  {
+    static const int PoseDim = _PoseDim;
+    static const int LandmarkDim = _LandmarkDim;
+    typedef Matrix<double, PoseDim, PoseDim> PoseMatrixType;
+    typedef Matrix<double, LandmarkDim, LandmarkDim> LandmarkMatrixType;
+    typedef Matrix<double, PoseDim, LandmarkDim> PoseLandmarkMatrixType;
+    typedef Matrix<double, PoseDim, 1> PoseVectorType;
+    typedef Matrix<double, LandmarkDim, 1> LandmarkVectorType;
+
+    typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
+    typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
+    typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
+    typedef LinearSolver<PoseMatrixType> LinearSolverType;
+  };
+
+  /**
+   * \brief traits to summarize the properties of the dynamic size optimization problem
+   */
+  template <>
+  struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>
+  {
+    static const int PoseDim = Eigen::Dynamic;
+    static const int LandmarkDim = Eigen::Dynamic;
+    typedef MatrixXd PoseMatrixType;
+    typedef MatrixXd LandmarkMatrixType;
+    typedef MatrixXd PoseLandmarkMatrixType;
+    typedef VectorXd PoseVectorType;
+    typedef VectorXd LandmarkVectorType;
+
+    typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
+    typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
+    typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
+    typedef LinearSolver<PoseMatrixType> LinearSolverType;
+  };
+
+  /**
+   * \brief base for the block solvers with some basic function interfaces
+   */
+  class BlockSolverBase : public Solver
+  {
+    public:
+      virtual ~BlockSolverBase() {}
+      /**
+       * compute dest = H * src
+       */
+      virtual void multiplyHessian(double* dest, const double* src) const = 0;
+  };
+
+  /**
+   * \brief Implementation of a solver operating on the blocks of the Hessian
+   */
+  template <typename Traits>
+  class BlockSolver: public BlockSolverBase {
+    public:
+
+      static const int PoseDim = Traits::PoseDim;
+      static const int LandmarkDim = Traits::LandmarkDim;
+      typedef typename Traits::PoseMatrixType PoseMatrixType;
+      typedef typename Traits::LandmarkMatrixType LandmarkMatrixType; 
+      typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
+      typedef typename Traits::PoseVectorType PoseVectorType;
+      typedef typename Traits::LandmarkVectorType LandmarkVectorType;
+
+      typedef typename Traits::PoseHessianType PoseHessianType;
+      typedef typename Traits::LandmarkHessianType LandmarkHessianType;
+      typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
+      typedef typename Traits::LinearSolverType LinearSolverType;
+
+    public:
+
+      /**
+       * allocate a block solver ontop of the underlying linear solver.
+       * NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer
+       * in its destructor.
+       */
+      BlockSolver(LinearSolverType* linearSolver);
+      ~BlockSolver();
+
+      virtual bool init(SparseOptimizer* optmizer, bool online = false);
+      virtual bool buildStructure(bool zeroBlocks = false);
+      virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
+      virtual bool buildSystem();
+      virtual bool solve();
+      virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
+      virtual bool setLambda(double lambda, bool backup = false);
+      virtual void restoreDiagonal();
+      virtual bool supportsSchur() {return true;}
+      virtual bool schur() { return _doSchur;}
+      virtual void setSchur(bool s) { _doSchur = s;}
+
+      LinearSolver<PoseMatrixType>* linearSolver() const { return _linearSolver;}
+
+      virtual void setWriteDebug(bool writeDebug);
+      virtual bool writeDebug() const {return _linearSolver->writeDebug();}
+
+      virtual bool saveHessian(const std::string& fileName) const;
+
+      virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}
+
+    protected:
+      void resize(int* blockPoseIndices, int numPoseBlocks, 
+          int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);
+
+      void deallocate();
+
+      SparseBlockMatrix<PoseMatrixType>* _Hpp;
+      SparseBlockMatrix<LandmarkMatrixType>* _Hll;
+      SparseBlockMatrix<PoseLandmarkMatrixType>* _Hpl;
+
+      SparseBlockMatrix<PoseMatrixType>* _Hschur;
+      SparseBlockMatrixDiagonal<LandmarkMatrixType>* _DInvSchur;
+
+      SparseBlockMatrixCCS<PoseLandmarkMatrixType>* _HplCCS;
+      SparseBlockMatrixCCS<PoseMatrixType>* _HschurTransposedCCS;
+
+      LinearSolver<PoseMatrixType>* _linearSolver;
+
+      std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
+      std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;
+
+#    ifdef G2O_OPENMP
+      std::vector<OpenMPMutex> _coefficientsMutex;
+#    endif
+
+      bool _doSchur;
+
+      double* _coefficients;
+      double* _bschur;
+
+      int _numPoses, _numLandmarks;
+      int _sizePoses, _sizeLandmarks;
+  };
+
+
+  //variable size solver
+  typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > BlockSolverX;
+  // solver for BA/3D SLAM
+  typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;  
+  // solver fo BA with scale
+  typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;  
+  // 2Dof landmarks 3Dof poses
+  typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;
+
+} // end namespace
+
+#include "block_solver.hpp"
+
+
+#endif

Thirdparty/g2o/g2o/core/block_solver.hpp

@@ -0,0 +1,634 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "sparse_optimizer.h"
+#include <Eigen/LU>
+#include <fstream>
+#include <iomanip>
+
+#include "../stuff/timeutil.h"
+#include "../stuff/macros.h"
+#include "../stuff/misc.h"
+
+namespace g2o {
+
+using namespace std;
+using namespace Eigen;
+
+template <typename Traits>
+BlockSolver<Traits>::BlockSolver(LinearSolverType* linearSolver) :
+  BlockSolverBase(),
+  _linearSolver(linearSolver)
+{
+  // workspace
+  _Hpp=0;
+  _Hll=0;
+  _Hpl=0;
+  _HplCCS = 0;
+  _HschurTransposedCCS = 0;
+  _Hschur=0;
+  _DInvSchur=0;
+  _coefficients=0;
+  _bschur = 0;
+  _xSize=0;
+  _numPoses=0;
+  _numLandmarks=0;
+  _sizePoses=0;
+  _sizeLandmarks=0;
+  _doSchur=true;
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::resize(int* blockPoseIndices, int numPoseBlocks, 
+              int* blockLandmarkIndices, int numLandmarkBlocks,
+              int s)
+{
+  deallocate();
+
+  resizeVector(s);
+
+  if (_doSchur) {
+    // the following two are only used in schur
+    assert(_sizePoses > 0 && "allocating with wrong size");
+    _coefficients = new double [s];
+    _bschur = new double[_sizePoses];
+  }
+
+  _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
+  if (_doSchur) {
+    _Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
+    _Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);
+    _DInvSchur = new SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll->colBlockIndices());
+    _Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);
+    _HplCCS = new SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
+    _HschurTransposedCCS = new SparseBlockMatrixCCS<PoseMatrixType>(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
+#ifdef G2O_OPENMP
+    _coefficientsMutex.resize(numPoseBlocks);
+#endif
+  }
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::deallocate()
+{
+  if (_Hpp){
+    delete _Hpp;
+    _Hpp=0;
+  }
+  if (_Hll){
+    delete _Hll;
+    _Hll=0;
+  }
+  if (_Hpl){
+    delete _Hpl;
+    _Hpl = 0;
+  }
+  if (_Hschur){
+    delete _Hschur;
+    _Hschur=0;
+  }
+  if (_DInvSchur){
+    delete _DInvSchur;
+    _DInvSchur=0;
+  }
+  if (_coefficients) {
+    delete[] _coefficients;
+    _coefficients = 0;
+  }
+  if (_bschur) {
+    delete[] _bschur;
+    _bschur = 0;
+  }
+  if (_HplCCS) {
+    delete _HplCCS;
+    _HplCCS = 0;
+  }
+  if (_HschurTransposedCCS) {
+    delete _HschurTransposedCCS;
+    _HschurTransposedCCS = 0;
+  }
+}
+
+template <typename Traits>
+BlockSolver<Traits>::~BlockSolver()
+{
+  delete _linearSolver;
+  deallocate();
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
+{
+  assert(_optimizer);
+
+  size_t sparseDim = 0;
+  _numPoses=0;
+  _numLandmarks=0;
+  _sizePoses=0;
+  _sizeLandmarks=0;
+  int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
+  int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];
+
+  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
+    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
+    int dim = v->dimension();
+    if (! v->marginalized()){
+      v->setColInHessian(_sizePoses);
+      _sizePoses+=dim;
+      blockPoseIndices[_numPoses]=_sizePoses;
+      ++_numPoses;
+    } else {
+      v->setColInHessian(_sizeLandmarks);
+      _sizeLandmarks+=dim;
+      blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
+      ++_numLandmarks;
+    }
+    sparseDim += dim;
+  }
+  resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
+  delete[] blockLandmarkIndices;
+  delete[] blockPoseIndices;
+
+  // allocate the diagonal on Hpp and Hll
+  int poseIdx = 0;
+  int landmarkIdx = 0;
+  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
+    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
+    if (! v->marginalized()){
+      //assert(poseIdx == v->hessianIndex());
+      PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
+      if (zeroBlocks)
+        m->setZero();
+      v->mapHessianMemory(m->data());
+      ++poseIdx;
+    } else {
+      LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
+      if (zeroBlocks)
+        m->setZero();
+      v->mapHessianMemory(m->data());
+      ++landmarkIdx;
+    }
+  }
+  assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
+
+  // temporary structures for building the pattern of the Schur complement
+  SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
+  if (_doSchur) {
+    schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
+    schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
+  }
+
+  // here we assume that the landmark indices start after the pose ones
+  // create the structure in Hpp, Hll and in Hpl
+  for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
+    OptimizableGraph::Edge* e = *it;
+
+    for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
+      OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
+      int ind1 = v1->hessianIndex();
+      if (ind1 == -1)
+        continue;
+      int indexV1Bak = ind1;
+      for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
+        OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
+        int ind2 = v2->hessianIndex();
+        if (ind2 == -1)
+          continue;
+        ind1 = indexV1Bak;
+        bool transposedBlock = ind1 > ind2;
+        if (transposedBlock){ // make sure, we allocate the upper triangle block
+          swap(ind1, ind2);
+        }
+        if (! v1->marginalized() && !v2->marginalized()){
+          PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
+          if (zeroBlocks)
+            m->setZero();
+          e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
+          if (_Hschur) {// assume this is only needed in case we solve with the schur complement
+            schurMatrixLookup->addBlock(ind1, ind2);
+          }
+        } else if (v1->marginalized() && v2->marginalized()){
+          // RAINER hmm.... should we ever reach this here????
+          LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
+          if (zeroBlocks)
+            m->setZero();
+          e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
+        } else { 
+          if (v1->marginalized()){ 
+            PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
+            if (zeroBlocks)
+              m->setZero();
+            e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
+          } else {
+            PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
+            if (zeroBlocks)
+              m->setZero();
+            e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
+          }
+        }
+      }
+    }
+  }
+
+  if (! _doSchur)
+    return true;
+
+  _DInvSchur->diagonal().resize(landmarkIdx);
+  _Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
+
+  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
+    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
+    if (v->marginalized()){
+      const HyperGraph::EdgeSet& vedges=v->edges();
+      for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
+        for (size_t i=0; i<(*it1)->vertices().size(); ++i)
+        {
+          OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
+          if (v1->hessianIndex()==-1 || v1==v)
+            continue;
+          for  (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
+            for (size_t j=0; j<(*it2)->vertices().size(); ++j)
+            {
+              OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
+              if (v2->hessianIndex()==-1 || v2==v)
+                continue;
+              int i1=v1->hessianIndex();
+              int i2=v2->hessianIndex();
+              if (i1<=i2) {
+                schurMatrixLookup->addBlock(i1, i2);
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  _Hschur->takePatternFromHash(*schurMatrixLookup);
+  delete schurMatrixLookup;
+  _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);
+
+  return true;
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
+{
+  for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
+    int dim = v->dimension();
+    if (! v->marginalized()){
+      v->setColInHessian(_sizePoses);
+      _sizePoses+=dim;
+      _Hpp->rowBlockIndices().push_back(_sizePoses);
+      _Hpp->colBlockIndices().push_back(_sizePoses);
+      _Hpp->blockCols().push_back(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
+      ++_numPoses;
+      int ind = v->hessianIndex();
+      PoseMatrixType* m = _Hpp->block(ind, ind, true);
+      v->mapHessianMemory(m->data());
+    } else {
+      std::cerr << "updateStructure(): Schur not supported" << std::endl;
+      abort();
+    }
+  }
+  resizeVector(_sizePoses + _sizeLandmarks);
+
+  for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
+    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+
+    for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
+      OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
+      int ind1 = v1->hessianIndex();
+      int indexV1Bak = ind1;
+      if (ind1 == -1)
+        continue;
+      for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
+        OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
+        int ind2 = v2->hessianIndex();
+        if (ind2 == -1)
+          continue;
+        ind1 = indexV1Bak;
+        bool transposedBlock = ind1 > ind2;
+        if (transposedBlock) // make sure, we allocate the upper triangular block
+          swap(ind1, ind2);
+
+        if (! v1->marginalized() && !v2->marginalized()) {
+          PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
+          e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
+        } else { 
+          std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
+        }
+      }
+    }
+
+  }
+
+  return true;
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::solve(){
+  //cerr << __PRETTY_FUNCTION__ << endl;
+  if (! _doSchur){
+    double t=get_monotonic_time();
+    bool ok = _linearSolver->solve(*_Hpp, _x, _b);
+    G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+    if (globalStats) {
+      globalStats->timeLinearSolver = get_monotonic_time() - t;
+      globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
+    }
+    return ok;
+  }
+
+  // schur thing
+
+  // backup the coefficient matrix
+  double t=get_monotonic_time();
+
+  // _Hschur = _Hpp, but keeping the pattern of _Hschur
+  _Hschur->clear();
+  _Hpp->add(_Hschur);
+
+  //_DInvSchur->clear();
+  memset (_coefficients, 0, _sizePoses*sizeof(double));
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) schedule(dynamic, 10)
+# endif
+  for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
+    const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
+    assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
+
+    // calculate inverse block for the landmark
+    const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
+    assert (D && D->rows()==D->cols() && "Error in landmark matrix");
+    LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
+    Dinv = D->inverse();
+
+    LandmarkVectorType  db(D->rows());
+    for (int j=0; j<D->rows(); ++j) {
+      db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
+    }
+    db=Dinv*db;
+
+    assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
+    const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
+
+    for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
+        it_outer != landmarkColumn.end(); ++it_outer) {
+      int i1 = it_outer->row;
+
+      const PoseLandmarkMatrixType* Bi = it_outer->block;
+      assert(Bi);
+
+      PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
+      assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
+      typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
+#    ifdef G2O_OPENMP
+      ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
+#    endif
+      Bb.noalias() += (*Bi)*db;
+
+      assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
+      typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
+
+      typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
+      typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
+      for (; it_inner != landmarkColumn.end(); ++it_inner) {
+        int i2 = it_inner->row;
+        const PoseLandmarkMatrixType* Bj = it_inner->block;
+        assert(Bj); 
+        while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
+          ++targetColumnIt;
+        assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
+        PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
+        assert(Hi1i2);
+        (*Hi1i2).noalias() -= BDinv*Bj->transpose();
+      }
+    }
+  }
+  //cerr << "Solve [marginalize] = " <<  get_monotonic_time()-t << endl;
+
+  // _bschur = _b for calling solver, and not touching _b
+  memcpy(_bschur, _b, _sizePoses * sizeof(double));
+  for (int i=0; i<_sizePoses; ++i){
+    _bschur[i]-=_coefficients[i];
+  }
+
+  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+  if (globalStats){
+    globalStats->timeSchurComplement = get_monotonic_time() - t;
+  }
+
+  t=get_monotonic_time();
+  bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);
+  if (globalStats) {
+    globalStats->timeLinearSolver = get_monotonic_time() - t;
+    globalStats->hessianPoseDimension = _Hpp->cols();
+    globalStats->hessianLandmarkDimension = _Hll->cols();
+    globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
+  }
+  //cerr << "Solve [decompose and solve] = " <<  get_monotonic_time()-t << endl;
+
+  if (! solvedPoses)
+    return false;
+
+  // _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
+  // solution;
+  double* xp = _x;
+  double* cp = _coefficients;
+
+  double* xl=_x+_sizePoses;
+  double* cl=_coefficients + _sizePoses;
+  double* bl=_b+_sizePoses;
+
+  // cp = -xp
+  for (int i=0; i<_sizePoses; ++i)
+    cp[i]=-xp[i];
+
+  // cl = bl
+  memcpy(cl,bl,_sizeLandmarks*sizeof(double));
+
+  // cl = bl - Bt * xp
+  //Bt->multiply(cl, cp);
+  _HplCCS->rightMultiply(cl, cp);
+
+  // xl = Dinv * cl
+  memset(xl,0, _sizeLandmarks*sizeof(double));
+  _DInvSchur->multiply(xl,cl);
+  //_DInvSchur->rightMultiply(xl,cl);
+  //cerr << "Solve [landmark delta] = " <<  get_monotonic_time()-t << endl;
+
+  return true;
+}
+
+
+template <typename Traits>
+bool BlockSolver<Traits>::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
+{
+  double t = get_monotonic_time();
+  bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
+  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+  if (globalStats) {
+    globalStats->timeMarginals = get_monotonic_time() - t;
+  }
+  return ok;
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::buildSystem()
+{
+  // clear b vector
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
+# endif
+  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
+    OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
+    assert(v);
+    v->clearQuadraticForm();
+  }
+  _Hpp->clear();
+  if (_doSchur) {
+    _Hll->clear();
+    _Hpl->clear();
+  }
+
+  // resetting the terms for the pairwise constraints
+  // built up the current system by storing the Hessian blocks in the edges and vertices
+# ifndef G2O_OPENMP
+  // no threading, we do not need to copy the workspace
+  JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
+# else
+  // if running with threads need to produce copies of the workspace for each thread
+  JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
+# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
+# endif
+  for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
+    OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
+    e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
+    e->constructQuadraticForm();
+#  ifndef NDEBUG
+    for (size_t i = 0; i < e->vertices().size(); ++i) {
+      const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
+      if (! v->fixed()) {
+        bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
+        if (hasANan) {
+          cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << endl;
+          break;
+        }
+      }
+    }
+#  endif
+  }
+
+  // flush the current system in a sparse block matrix
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
+# endif
+  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
+    OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
+    int iBase = v->colInHessian();
+    if (v->marginalized())
+      iBase+=_sizePoses;
+    v->copyB(_b+iBase);
+  }
+
+  return 0;
+}
+
+
+template <typename Traits>
+bool BlockSolver<Traits>::setLambda(double lambda, bool backup)
+{
+  if (backup) {
+    _diagonalBackupPose.resize(_numPoses);
+    _diagonalBackupLandmark.resize(_numLandmarks);
+  }
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_numPoses > 100)
+# endif
+  for (int i = 0; i < _numPoses; ++i) {
+    PoseMatrixType *b=_Hpp->block(i,i);
+    if (backup)
+      _diagonalBackupPose[i] = b->diagonal();
+    b->diagonal().array() += lambda;
+  }
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_numLandmarks > 100)
+# endif
+  for (int i = 0; i < _numLandmarks; ++i) {
+    LandmarkMatrixType *b=_Hll->block(i,i);
+    if (backup)
+      _diagonalBackupLandmark[i] = b->diagonal();
+    b->diagonal().array() += lambda;
+  }
+  return true;
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::restoreDiagonal()
+{
+  assert((int) _diagonalBackupPose.size() == _numPoses && "Mismatch in dimensions");
+  assert((int) _diagonalBackupLandmark.size() == _numLandmarks && "Mismatch in dimensions");
+  for (int i = 0; i < _numPoses; ++i) {
+    PoseMatrixType *b=_Hpp->block(i,i);
+    b->diagonal() = _diagonalBackupPose[i];
+  }
+  for (int i = 0; i < _numLandmarks; ++i) {
+    LandmarkMatrixType *b=_Hll->block(i,i);
+    b->diagonal() = _diagonalBackupLandmark[i];
+  }
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::init(SparseOptimizer* optimizer, bool online)
+{
+  _optimizer = optimizer;
+  if (! online) {
+    if (_Hpp)
+      _Hpp->clear();
+    if (_Hpl)
+      _Hpl->clear();
+    if (_Hll)
+      _Hll->clear();
+  }
+  _linearSolver->init();
+  return true;
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::setWriteDebug(bool writeDebug)
+{
+  _linearSolver->setWriteDebug(writeDebug);
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::saveHessian(const std::string& fileName) const
+{
+  return _Hpp->writeOctave(fileName.c_str(), true);
+}
+
+} // end namespace

Thirdparty/g2o/g2o/core/cache.cpp

@@ -0,0 +1,183 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "cache.h"
+#include "optimizable_graph.h"
+#include "factory.h"
+
+#include <iostream>
+
+namespace g2o {
+  using namespace std;
+
+  Cache::CacheKey::CacheKey() :
+    _type(), _parameters()
+  {
+  }
+
+  Cache::CacheKey::CacheKey(const std::string& type_, const ParameterVector& parameters_) :
+    _type(type_), _parameters(parameters_)
+  {
+  }
+
+  Cache::Cache(CacheContainer* container_, const ParameterVector& parameters_) :
+    _updateNeeded(true), _parameters(parameters_), _container(container_)
+  {
+  }
+
+  bool Cache::CacheKey::operator<(const Cache::CacheKey& c) const{
+    if (_type < c._type)
+      return true;
+    return std::lexicographical_compare (_parameters.begin( ), _parameters.end( ),
+           c._parameters.begin( ), c._parameters.end( ) );
+  }
+
+
+  OptimizableGraph::Vertex* Cache::vertex() { 
+    if (container() ) 
+      return container()->vertex(); 
+    return 0; 
+  }
+
+  OptimizableGraph* Cache::graph() {
+    if (container())
+      return container()->graph();
+    return 0;
+  }
+
+  CacheContainer* Cache::container() {
+    return _container;
+  }
+
+  ParameterVector& Cache::parameters() {
+    return _parameters;
+  }
+  
+  Cache::CacheKey Cache::key() const {
+    Factory* factory=Factory::instance();
+    return CacheKey(factory->tag(this), _parameters);
+  };
+
+  
+  void Cache::update(){
+    if (! _updateNeeded)
+      return;
+    for(std::vector<Cache*>::iterator it=_parentCaches.begin(); it!=_parentCaches.end(); it++){
+      (*it)->update();
+    }
+    updateImpl();
+    _updateNeeded=false;
+  }
+
+  Cache* Cache::installDependency(const std::string& type_, const std::vector<int>& parameterIndices){
+    ParameterVector pv(parameterIndices.size());
+    for (size_t i=0; i<parameterIndices.size(); i++){
+      if (parameterIndices[i]<0 || parameterIndices[i] >=(int)_parameters.size())
+  return 0;
+      pv[i]=_parameters[ parameterIndices[i] ];
+    }
+    CacheKey k(type_, pv);
+    if (!container())
+      return 0;
+    Cache* c=container()->findCache(k);
+    if (!c) {
+      c = container()->createCache(k);
+    }
+    if (c)
+      _parentCaches.push_back(c);
+    return c;
+  }
+  
+  bool Cache::resolveDependancies(){
+    return true;
+  }
+
+  CacheContainer::CacheContainer(OptimizableGraph::Vertex* vertex_) {
+    _vertex = vertex_;
+  }
+
+  Cache* CacheContainer::findCache(const Cache::CacheKey& key) {
+    iterator it=find(key);
+    if (it==end())
+      return 0;
+    return it->second;
+  }
+  
+  Cache* CacheContainer::createCache(const Cache::CacheKey& key){
+    Factory* f = Factory::instance();
+    HyperGraph::HyperGraphElement* e = f->construct(key.type());
+    if (!e) {
+      cerr << __PRETTY_FUNCTION__ << endl;
+      cerr << "fatal error in creating cache of type " << key.type() << endl;
+      return 0;
+    }
+    Cache* c = dynamic_cast<Cache*>(e);
+    if (! c){
+      cerr << __PRETTY_FUNCTION__ << endl;
+      cerr << "fatal error in creating cache of type " << key.type() << endl;
+      return 0;
+    }
+    c->_container = this;
+    c->_parameters = key._parameters;
+    if (c->resolveDependancies()){
+      insert(make_pair(key,c));
+      c->update();
+      return c;
+    } 
+    return 0;
+  }
+  
+  OptimizableGraph::Vertex* CacheContainer::vertex() {
+    return _vertex;
+  }
+
+  OptimizableGraph* CacheContainer::graph(){
+    if (_vertex)
+      return _vertex->graph();
+    return 0;
+  }
+
+  void CacheContainer::update() {
+    for (iterator it=begin(); it!=end(); it++){
+      (it->second)->update();
+    }
+    _updateNeeded=false;
+  }
+
+  void CacheContainer::setUpdateNeeded(bool needUpdate) {
+    _updateNeeded=needUpdate;
+    for (iterator it=begin(); it!=end(); ++it){
+      (it->second)->_updateNeeded = needUpdate;
+    }
+  }
+
+  CacheContainer::~CacheContainer(){
+    for (iterator it=begin(); it!=end(); ++it){
+      delete (it->second);
+    }
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/cache.h

@@ -0,0 +1,140 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_CACHE_HH_
+#define G2O_CACHE_HH_
+
+#include <map>
+
+#include "optimizable_graph.h"
+
+namespace g2o {
+
+  class CacheContainer;
+  
+  class  Cache: public HyperGraph::HyperGraphElement
+  {
+    public:
+      friend class CacheContainer;
+      class  CacheKey
+      {
+        public:
+          friend class CacheContainer;
+          CacheKey();
+          CacheKey(const std::string& type_, const ParameterVector& parameters_);
+
+          bool operator<(const CacheKey& c) const;
+
+          const std::string& type() const { return _type;}
+          const ParameterVector& parameters() const { return _parameters;}
+
+        protected:
+          std::string _type;
+          ParameterVector _parameters;
+      };
+
+      Cache(CacheContainer* container_ = 0, const ParameterVector& parameters_ = ParameterVector());
+
+      CacheKey key() const;
+
+      OptimizableGraph::Vertex* vertex();
+      OptimizableGraph* graph();
+      CacheContainer* container();
+      ParameterVector& parameters();
+
+      void update();
+
+      virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_CACHE;}
+
+    protected:
+      //! redefine this to do the update
+      virtual void updateImpl() = 0;
+
+      /**
+       * this function installs and satisfies a cache
+       * @param type_: the typename of the dependency
+       * @param parameterIndices: a vector containing the indices if the parameters
+       * in _parameters that will be used to assemble the Key of the cache being created
+       * For example if I have a cache of type C2, having parameters "A, B, and C",
+       * and it depends on a cache of type C1 that depends on the parameters A and C, 
+       * the parameterIndices should contain "0, 2", since they are the positions in the
+       * parameter vector of C2 of the parameters needed to construct C1.
+       * @returns the newly created cache
+       */
+      Cache* installDependency(const std::string& type_, const std::vector<int>& parameterIndices);
+
+      /**
+       * Function to be called from a cache that has dependencies. It just invokes a
+       * sequence of installDependency().
+       * Although the caches returned are stored in the _parentCache vector,
+       * it is better that you redefine your own cache member variables, for better readability
+       */
+      virtual bool resolveDependancies();
+
+      bool _updateNeeded;
+      ParameterVector _parameters;
+      std::vector<Cache*> _parentCaches;
+      CacheContainer* _container;
+  };
+
+  class  CacheContainer: public std::map<Cache::CacheKey, Cache*>
+  {
+    public:
+      CacheContainer(OptimizableGraph::Vertex* vertex_);
+      virtual ~CacheContainer();
+      OptimizableGraph::Vertex* vertex();
+      OptimizableGraph* graph();
+      Cache* findCache(const Cache::CacheKey& key);
+      Cache* createCache(const Cache::CacheKey& key);
+      void setUpdateNeeded(bool needUpdate=true);
+      void update();
+    protected:
+      OptimizableGraph::Vertex* _vertex;
+      bool _updateNeeded;
+  };
+
+
+  template <typename CacheType>
+  void OptimizableGraph::Edge::resolveCache(CacheType*& cache, 
+      OptimizableGraph::Vertex* v, 
+      const std::string& type_, 
+      const ParameterVector& parameters_)
+  {
+    cache = 0;
+    CacheContainer* container= v->cacheContainer();
+    Cache::CacheKey key(type_, parameters_);
+    Cache* c = container->findCache(key);
+    if (!c) {
+      c = container->createCache(key);
+    }
+    if (c) {
+      cache = dynamic_cast<CacheType*>(c); 
+    }
+  }
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/creators.h

@@ -0,0 +1,75 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_CREATORS_H
+#define G2O_CREATORS_H
+
+#include "hyper_graph.h"
+
+#include <string>
+#include <typeinfo>
+
+namespace g2o
+{
+
+  /**
+   * \brief Abstract interface for allocating HyperGraphElement
+   */
+  class  AbstractHyperGraphElementCreator
+  {
+    public:
+      /**
+       * create a hyper graph element. Has to implemented in derived class.
+       */
+      virtual HyperGraph::HyperGraphElement* construct() = 0;
+      /**
+       * name of the class to be created. Has to implemented in derived class.
+       */
+      virtual const std::string& name() const = 0;
+
+      virtual ~AbstractHyperGraphElementCreator() { }
+  };
+
+  /**
+   * \brief templatized creator class which creates graph elements
+   */
+  template <typename T>
+  class HyperGraphElementCreator : public AbstractHyperGraphElementCreator
+  {
+    public:
+      HyperGraphElementCreator() : _name(typeid(T).name()) {}
+#if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC
+      __attribute__((force_align_arg_pointer))
+#endif
+      HyperGraph::HyperGraphElement* construct() { return new T;}
+      virtual const std::string& name() const { return _name;}
+    protected:
+      std::string _name;
+  };
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/eigen_types.h

@@ -0,0 +1,73 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_EIGEN_TYPES_H
+#define G2O_EIGEN_TYPES_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace g2o {
+
+  typedef Eigen::Matrix<int,2,1,Eigen::ColMajor>                                  Vector2I;
+  typedef Eigen::Matrix<int,3,1,Eigen::ColMajor>                                  Vector3I;
+  typedef Eigen::Matrix<int,4,1,Eigen::ColMajor>                                  Vector4I;
+  typedef Eigen::Matrix<int,Eigen::Dynamic,1,Eigen::ColMajor>                     VectorXI; 
+
+  typedef Eigen::Matrix<float,2,1,Eigen::ColMajor>                                Vector2F; 
+  typedef Eigen::Matrix<float,3,1,Eigen::ColMajor>                                Vector3F; 
+  typedef Eigen::Matrix<float,4,1,Eigen::ColMajor>                                Vector4F; 
+  typedef Eigen::Matrix<float,Eigen::Dynamic,1,Eigen::ColMajor>                   VectorXF; 
+
+  typedef Eigen::Matrix<double,2,1,Eigen::ColMajor>                               Vector2D;
+  typedef Eigen::Matrix<double,3,1,Eigen::ColMajor>                               Vector3D;
+  typedef Eigen::Matrix<double,4,1,Eigen::ColMajor>                               Vector4D;
+  typedef Eigen::Matrix<double,Eigen::Dynamic,1,Eigen::ColMajor>                  VectorXD;
+
+  typedef Eigen::Matrix<int,2,2,Eigen::ColMajor>                                  Matrix2I;
+  typedef Eigen::Matrix<int,3,3,Eigen::ColMajor>                                  Matrix3I;
+  typedef Eigen::Matrix<int,4,4,Eigen::ColMajor>                                  Matrix4I;
+  typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>        MatrixXI;
+
+  typedef Eigen::Matrix<float,2,2,Eigen::ColMajor>                                Matrix2F;
+  typedef Eigen::Matrix<float,3,3,Eigen::ColMajor>                                Matrix3F;
+  typedef Eigen::Matrix<float,4,4,Eigen::ColMajor>                                Matrix4F;
+  typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>      MatrixXF;
+
+  typedef Eigen::Matrix<double,2,2,Eigen::ColMajor>                               Matrix2D;
+  typedef Eigen::Matrix<double,3,3,Eigen::ColMajor>                               Matrix3D;
+  typedef Eigen::Matrix<double,4,4,Eigen::ColMajor>                               Matrix4D;
+  typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>     MatrixXD;
+
+  typedef Eigen::Transform<double,2,Eigen::Isometry,Eigen::ColMajor>              Isometry2D;
+  typedef Eigen::Transform<double,3,Eigen::Isometry,Eigen::ColMajor>              Isometry3D;
+
+  typedef Eigen::Transform<double,2,Eigen::Affine,Eigen::ColMajor>                Affine2D;
+  typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>                Affine3D;
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/estimate_propagator.cpp

@@ -0,0 +1,267 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "estimate_propagator.h"
+
+#include <queue>
+#include <vector>
+#include <cassert>
+#include <iostream>
+#include <algorithm>
+#include <fstream>
+
+//#define DEBUG_ESTIMATE_PROPAGATOR
+
+using namespace std;
+
+namespace g2o {
+
+# ifdef DEBUG_ESTIMATE_PROPAGATOR
+  struct FrontierLevelCmp {
+    bool operator()(EstimatePropagator::AdjacencyMapEntry* e1, EstimatePropagator::AdjacencyMapEntry* e2) const
+    {
+      return e1->frontierLevel() < e2->frontierLevel();
+    }
+  };
+# endif
+
+  EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry()
+  {
+    reset();
+  }
+
+  void EstimatePropagator::AdjacencyMapEntry::reset()
+  {
+    _child = 0;
+    _parent.clear();
+    _edge = 0;
+    _distance = numeric_limits<double>::max();
+    _frontierLevel = -1;
+    inQueue = false;
+  }
+
+  EstimatePropagator::EstimatePropagator(OptimizableGraph* g): _graph(g)
+  {
+    for (OptimizableGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it){
+      AdjacencyMapEntry entry;
+      entry._child = static_cast<OptimizableGraph::Vertex*>(it->second);
+      _adjacencyMap.insert(make_pair(entry.child(), entry));
+    }
+  }
+
+  void EstimatePropagator::reset()
+  {
+    for (OptimizableGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); ++it){
+      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+      AdjacencyMap::iterator at = _adjacencyMap.find(v);
+      assert(at != _adjacencyMap.end());
+      at->second.reset();
+    }
+    _visited.clear();
+  }
+
+  void EstimatePropagator::propagate(OptimizableGraph::Vertex* v, 
+      const EstimatePropagator::PropagateCost& cost, 
+       const EstimatePropagator::PropagateAction& action,
+       double maxDistance, 
+       double maxEdgeCost)
+  {
+    OptimizableGraph::VertexSet vset;
+    vset.insert(v);
+    propagate(vset, cost, action, maxDistance, maxEdgeCost);
+  }
+
+  void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset, 
+      const EstimatePropagator::PropagateCost& cost, 
+       const EstimatePropagator::PropagateAction& action,
+       double maxDistance, 
+       double maxEdgeCost)
+  {
+    reset();
+
+    PriorityQueue frontier;
+    for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
+      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
+      AdjacencyMap::iterator it = _adjacencyMap.find(v);
+      assert(it != _adjacencyMap.end());
+      it->second._distance = 0.;
+      it->second._parent.clear();
+      it->second._frontierLevel = 0;
+      frontier.push(&it->second);
+    }
+
+    while(! frontier.empty()){
+      AdjacencyMapEntry* entry = frontier.pop();
+      OptimizableGraph::Vertex* u = entry->child();
+      double uDistance = entry->distance();
+      //cerr << "uDistance " << uDistance << endl;
+
+      // initialize the vertex
+      if (entry->_frontierLevel > 0) {
+        action(entry->edge(), entry->parent(), u);
+      }
+
+      /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
+      OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
+      while (et != u->edges().end()){
+        OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
+        ++et;
+
+        int maxFrontier = -1;
+        OptimizableGraph::VertexSet initializedVertices;
+        for (size_t i = 0; i < edge->vertices().size(); ++i) {
+          OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
+          AdjacencyMap::iterator ot = _adjacencyMap.find(z);
+          if (ot->second._distance != numeric_limits<double>::max()) {
+            initializedVertices.insert(z);
+            maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
+          }
+        }
+        assert(maxFrontier >= 0);
+
+        for (size_t i = 0; i < edge->vertices().size(); ++i) {
+          OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
+          if (z == u)
+            continue;
+
+          size_t wasInitialized = initializedVertices.erase(z);
+
+          double edgeDistance = cost(edge, initializedVertices, z);
+          if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
+            double zDistance = uDistance + edgeDistance;
+            //cerr << z->id() << " " << zDistance << endl;
+
+            AdjacencyMap::iterator ot = _adjacencyMap.find(z);
+            assert(ot!=_adjacencyMap.end());
+
+            if (zDistance < ot->second.distance() && zDistance < maxDistance){
+              //if (ot->second.inQueue)
+                //cerr << "Updating" << endl;
+              ot->second._distance = zDistance;
+              ot->second._parent = initializedVertices;
+              ot->second._edge = edge;
+              ot->second._frontierLevel = maxFrontier + 1;
+              frontier.push(&ot->second);
+            }
+          }
+
+          if (wasInitialized > 0)
+            initializedVertices.insert(z);
+
+        }
+      }
+    }
+
+    // writing debug information like cost for reaching each vertex and the parent used to initialize
+#ifdef DEBUG_ESTIMATE_PROPAGATOR
+    cerr << "Writing cost.dat" << endl;
+    ofstream costStream("cost.dat");
+    for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
+      HyperGraph::Vertex* u = it->second.child();
+      costStream << "vertex " << u->id() << "  cost " << it->second._distance << endl;
+    }
+    cerr << "Writing init.dat" << endl;
+    ofstream initStream("init.dat");
+    vector<AdjacencyMapEntry*> frontierLevels;
+    for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
+      if (it->second._frontierLevel > 0)
+        frontierLevels.push_back(&it->second);
+    }
+    sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
+    for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
+      AdjacencyMapEntry* entry       = *it;
+      OptimizableGraph::Vertex* to   = entry->child();
+
+      initStream << "calling init level = " << entry->_frontierLevel << "\t (";
+      for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
+        initStream << " " << (*pit)->id();
+      }
+      initStream << " ) -> " << to->id() << endl;
+    }
+#endif
+
+  }
+
+  void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry)
+  {
+    assert(entry != NULL);
+    if (entry->inQueue) {
+      assert(entry->queueIt->second == entry);
+      erase(entry->queueIt);
+    }
+
+    entry->queueIt = insert(std::make_pair(entry->distance(), entry));
+    assert(entry->queueIt != end());
+    entry->inQueue = true;
+  }
+
+  EstimatePropagator::AdjacencyMapEntry* EstimatePropagator::PriorityQueue::pop()
+  {
+    assert(!empty());
+    iterator it = begin();
+    AdjacencyMapEntry* entry = it->second;
+    erase(it);
+
+    assert(entry != NULL);
+    entry->queueIt = end();
+    entry->inQueue = false;
+    return entry;
+  }
+
+  EstimatePropagatorCost::EstimatePropagatorCost (SparseOptimizer* graph) :
+    _graph(graph)
+  {
+  }
+
+  double EstimatePropagatorCost::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const
+  {
+    OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
+    OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
+    SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
+    if (it == _graph->activeEdges().end()) // it has to be an active edge
+      return std::numeric_limits<double>::max();
+    return e->initialEstimatePossible(from, to);
+  }
+
+  EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(SparseOptimizer* graph) :
+    EstimatePropagatorCost(graph)
+  {
+  }
+
+  double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const
+  {
+    OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
+    OptimizableGraph::Vertex* from = dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
+    OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
+    if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph
+      return std::numeric_limits<double>::max();
+    SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
+    if (it == _graph->activeEdges().end()) // it has to be an active edge
+      return std::numeric_limits<double>::max();
+    return e->initialEstimatePossible(from_, to);
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/estimate_propagator.h

@@ -0,0 +1,175 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ESTIMATE_PROPAGATOR_H
+#define G2O_ESTIMATE_PROPAGATOR_H
+
+#include "optimizable_graph.h"
+#include "sparse_optimizer.h"
+
+#include <map>
+#include <set>
+#include <limits>
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+namespace g2o {
+
+  /**
+   * \brief cost for traversing along active edges in the optimizer
+   *
+   * You may derive an own one, if necessary. The default is to return initialEstimatePossible(from, to) for the edge.
+   */
+  class  EstimatePropagatorCost {
+    public:
+      EstimatePropagatorCost (SparseOptimizer* graph);
+      virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const;
+      virtual const char* name() const { return "spanning tree";}
+    protected:
+      SparseOptimizer* _graph;
+  };
+
+  /**
+   * \brief cost for traversing only odometry edges.
+   *
+   * Initialize your graph along odometry edges. An odometry edge is assumed to connect vertices
+   * whose IDs only differs by one.
+   */
+  class  EstimatePropagatorCostOdometry : public EstimatePropagatorCost {
+    public:
+      EstimatePropagatorCostOdometry(SparseOptimizer* graph);
+      virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const;
+      virtual const char* name() const { return "odometry";}
+  };
+
+  /**
+   * \brief propagation of an initial guess
+   */
+  class  EstimatePropagator {
+    public:
+
+      /**
+       * \brief Applying the action for propagating.
+       *
+       * You may derive an own one, if necessary. The default is to call initialEstimate(from, to) for the edge.
+       */
+      struct PropagateAction {
+        virtual void operator()(OptimizableGraph::Edge* e, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) const
+        {
+          if (! to->fixed())
+            e->initialEstimate(from, to);
+        }
+      };
+
+      typedef EstimatePropagatorCost PropagateCost;
+
+      class AdjacencyMapEntry;
+
+      /**
+       * \brief priority queue for AdjacencyMapEntry
+       */
+      class PriorityQueue : public std::multimap<double, AdjacencyMapEntry*> {
+        public:
+          void push(AdjacencyMapEntry* entry);
+          AdjacencyMapEntry* pop();
+      };
+
+      /**
+       * \brief data structure for loopuk during Dijkstra
+       */
+      class AdjacencyMapEntry {
+        public:
+          friend class EstimatePropagator;
+          friend class PriorityQueue;
+          AdjacencyMapEntry();
+          void reset();
+          OptimizableGraph::Vertex* child() const {return _child;}
+          const OptimizableGraph::VertexSet& parent() const {return _parent;}
+          OptimizableGraph::Edge* edge() const {return _edge;}
+          double distance() const {return _distance;}
+          int frontierLevel() const { return _frontierLevel;}
+
+        protected:
+          OptimizableGraph::Vertex* _child;
+          OptimizableGraph::VertexSet _parent;
+          OptimizableGraph::Edge* _edge;
+          double _distance;
+          int _frontierLevel;
+        private: // for PriorityQueue
+          bool inQueue;
+          PriorityQueue::iterator queueIt;
+      };
+
+      /**
+       * \brief hash function for a vertex
+       */
+      class VertexIDHashFunction {
+        public:
+          size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}
+      };
+
+      typedef std::tr1::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> AdjacencyMap;
+
+    public:
+      EstimatePropagator(OptimizableGraph* g);
+      OptimizableGraph::VertexSet& visited() {return _visited; }
+      AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
+      OptimizableGraph* graph() {return _graph;} 
+
+      /**
+       * propagate an initial guess starting from v. The function computes a spanning tree
+       * whereas the cost for each edge is determined by calling cost() and the action applied to
+       * each vertex is action().
+       */
+      void propagate(OptimizableGraph::Vertex* v, 
+          const EstimatePropagator::PropagateCost& cost, 
+          const EstimatePropagator::PropagateAction& action = PropagateAction(),
+          double maxDistance=std::numeric_limits<double>::max(), 
+          double maxEdgeCost=std::numeric_limits<double>::max());
+
+      /**
+       * same as above but starting to propagate from a set of vertices instead of just a single one.
+       */
+      void propagate(OptimizableGraph::VertexSet& vset, 
+          const EstimatePropagator::PropagateCost& cost, 
+          const EstimatePropagator::PropagateAction& action = PropagateAction(),
+          double maxDistance=std::numeric_limits<double>::max(), 
+          double maxEdgeCost=std::numeric_limits<double>::max());
+
+    protected:
+      void reset();
+
+      AdjacencyMap _adjacencyMap;
+      OptimizableGraph::VertexSet _visited;
+      OptimizableGraph* _graph;
+  };
+
+}
+#endif

Thirdparty/g2o/g2o/core/factory.cpp

@@ -0,0 +1,217 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "factory.h"
+
+#include "creators.h"
+#include "parameter.h"
+#include "cache.h"
+#include "optimizable_graph.h"
+#include "../stuff/color_macros.h"
+
+#include <iostream>
+#include <typeinfo>
+#include <cassert>
+
+using namespace std;
+
+namespace g2o {
+
+Factory* Factory::factoryInstance = 0;
+
+Factory::Factory()
+{
+}
+
+Factory::~Factory()
+{
+# ifdef G2O_DEBUG_FACTORY
+  cerr << "# Factory destroying " << (void*)this << endl;
+# endif
+  for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {
+    delete it->second->creator;
+  }
+  _creator.clear();
+  _tagLookup.clear();
+}
+
+Factory* Factory::instance()
+{
+  if (factoryInstance == 0) {
+    factoryInstance = new Factory;
+#  ifdef G2O_DEBUG_FACTORY
+    cerr << "# Factory allocated " << (void*)factoryInstance << endl;
+#  endif
+  }
+
+  return factoryInstance;
+}
+
+void Factory::registerType(const std::string& tag, AbstractHyperGraphElementCreator* c)
+{
+  CreatorMap::const_iterator foundIt = _creator.find(tag);
+  if (foundIt != _creator.end()) {
+    cerr << "FACTORY WARNING: Overwriting Vertex tag " << tag << endl;
+    assert(0);
+  }
+  TagLookup::const_iterator tagIt = _tagLookup.find(c->name());
+  if (tagIt != _tagLookup.end()) {
+    cerr << "FACTORY WARNING: Registering same class for two tags " << c->name() << endl;
+    assert(0);
+  }
+
+  CreatorInformation* ci = new CreatorInformation();
+  ci->creator = c;
+
+#ifdef G2O_DEBUG_FACTORY
+  cerr << "# Factory " << (void*)this << " constructing type " << tag << " ";
+#endif
+  // construct an element once to figure out its type
+  HyperGraph::HyperGraphElement* element = c->construct();
+  ci->elementTypeBit = element->elementType();
+
+#ifdef G2O_DEBUG_FACTORY
+  cerr << "done." << endl;
+  cerr << "# Factory " << (void*)this << " registering " << tag;
+  cerr << " " << (void*) c << " ";
+  switch (element->elementType()) {
+    case HyperGraph::HGET_VERTEX:
+      cerr << " -> Vertex";
+      break;
+    case HyperGraph::HGET_EDGE:
+      cerr << " -> Edge";
+      break;
+    case HyperGraph::HGET_PARAMETER:
+      cerr << " -> Parameter";
+      break;
+    case HyperGraph::HGET_CACHE:
+      cerr << " -> Cache";
+      break;
+    case HyperGraph::HGET_DATA:
+      cerr << " -> Data";
+      break;
+    default:
+      assert(0 && "Unknown element type occured, fix elementTypes");
+      break;
+  }
+  cerr << endl;
+#endif
+
+  _creator[tag] = ci;
+  _tagLookup[c->name()] = tag;
+  delete element;
+}
+
+  void Factory::unregisterType(const std::string& tag)
+  {
+    // Look for the tag
+    CreatorMap::iterator tagPosition = _creator.find(tag);
+
+    if (tagPosition != _creator.end()) {
+
+      AbstractHyperGraphElementCreator* c = tagPosition->second->creator;
+
+      // If we found it, remove the creator from the tag lookup map
+      TagLookup::iterator classPosition = _tagLookup.find(c->name());
+      if (classPosition != _tagLookup.end())
+        {
+          _tagLookup.erase(classPosition);
+        }
+      _creator.erase(tagPosition);
+    }
+  }
+
+HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag) const
+{
+  CreatorMap::const_iterator foundIt = _creator.find(tag);
+  if (foundIt != _creator.end()) {
+    //cerr << "tag " << tag << " -> " << (void*) foundIt->second->creator << " " << foundIt->second->creator->name() << endl;
+    return foundIt->second->creator->construct();
+  }
+  return 0;
+}
+
+const std::string& Factory::tag(const HyperGraph::HyperGraphElement* e) const
+{
+  static std::string emptyStr("");
+  TagLookup::const_iterator foundIt = _tagLookup.find(typeid(*e).name());
+  if (foundIt != _tagLookup.end())
+    return foundIt->second;
+  return emptyStr;
+}
+
+void Factory::fillKnownTypes(std::vector<std::string>& types) const
+{
+  types.clear();
+  for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
+    types.push_back(it->first);
+}
+
+bool Factory::knowsTag(const std::string& tag, int* elementType) const
+{
+  CreatorMap::const_iterator foundIt = _creator.find(tag);
+  if (foundIt == _creator.end()) {
+    if (elementType)
+      *elementType = -1;
+    return false;
+  }
+  if (elementType)
+    *elementType = foundIt->second->elementTypeBit;
+  return true;
+}
+
+void Factory::destroy()
+{
+  delete factoryInstance;
+  factoryInstance = 0;
+}
+
+void Factory::printRegisteredTypes(std::ostream& os, bool comment) const
+{
+  if (comment)
+    os << "# ";
+  os << "types:" << endl;
+  for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
+    if (comment)
+      os << "#";
+    cerr << "\t" << it->first << endl;
+  }
+}
+
+HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const
+{
+  if (elemsToConstruct.none()) {
+    return construct(tag);
+  }
+  CreatorMap::const_iterator foundIt = _creator.find(tag);
+  if (foundIt != _creator.end() && foundIt->second->elementTypeBit >= 0 && elemsToConstruct.test(foundIt->second->elementTypeBit)) {
+    return foundIt->second->creator->construct();
+  }
+  return 0;
+}
+
+} // end namespace
+

Thirdparty/g2o/g2o/core/factory.h

@@ -0,0 +1,179 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_FACTORY_H
+#define G2O_FACTORY_H
+
+#include "../../config.h"
+#include "../stuff/misc.h"
+#include "hyper_graph.h"
+#include "creators.h"
+
+#include <string>
+#include <map>
+#include <iostream>
+
+// define to get some verbose output
+//#define G2O_DEBUG_FACTORY
+
+namespace g2o {
+
+  class AbstractHyperGraphElementCreator;
+  
+  /**
+   * \brief create vertices and edges based on TAGs in, for example, a file
+   */
+  class  Factory
+  {
+    public:
+
+      //! return the instance
+      static Factory* instance();
+
+      //! free the instance
+      static void destroy();
+
+      /**
+       * register a tag for a specific creator
+       */
+      void registerType(const std::string& tag, AbstractHyperGraphElementCreator* c);
+
+      /**
+       * unregister a tag for a specific creator
+       */
+      void unregisterType(const std::string& tag);
+
+      /**
+       * construct a graph element based on its tag
+       */
+      HyperGraph::HyperGraphElement* construct(const std::string& tag) const;
+
+      /**
+       * construct a graph element based on its tag, but only if it's type (a bitmask) matches. A bitmask without any
+       * bit set will construct any item. Otherwise a bit has to be set to allow construction of a graph element.
+       */
+      HyperGraph::HyperGraphElement* construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const;
+
+      /**
+       * return whether the factory knows this tag or not
+       */
+      bool knowsTag(const std::string& tag, int* elementType = 0) const;
+
+      //! return the TAG given a vertex
+      const std::string& tag(const HyperGraph::HyperGraphElement* v) const;
+
+      /**
+       * get a list of all known types
+       */
+      void fillKnownTypes(std::vector<std::string>& types) const;
+
+      /**
+       * print a list of the known registered types to the given stream
+       */
+      void printRegisteredTypes(std::ostream& os, bool comment = false) const;
+
+    protected:
+      class CreatorInformation
+      {
+        public:
+          AbstractHyperGraphElementCreator* creator;
+          int elementTypeBit;
+          CreatorInformation()
+          {
+            creator = 0;
+            elementTypeBit = -1;
+          }
+        
+          ~CreatorInformation()
+          {
+            std::cout << "Deleting " << (void*) creator << std::endl;
+            
+            delete creator;
+          }
+      };
+
+      typedef std::map<std::string, CreatorInformation*>               CreatorMap;
+      typedef std::map<std::string, std::string>                      TagLookup;
+      Factory();
+      ~Factory();
+
+      CreatorMap _creator;     ///< look-up map for the existing creators
+      TagLookup _tagLookup;    ///< reverse look-up, class name to tag
+
+    private:
+      static Factory* factoryInstance;
+  };
+
+  template<typename T>
+  class RegisterTypeProxy
+  {
+    public:
+      RegisterTypeProxy(const std::string& name) : _name(name)
+      {
+#ifdef G2O_DEBUG_FACTORY
+        std::cout << __FUNCTION__ << ": Registering " << _name << " of type " << typeid(T).name() << std::endl;
+#endif
+        Factory::instance()->registerType(_name, new HyperGraphElementCreator<T>());
+      }
+
+      ~RegisterTypeProxy()
+      {
+#ifdef G2O_DEBUG_FACTORY
+        std::cout << __FUNCTION__ << ": Unregistering " << _name << " of type " << typeid(T).name() << std::endl;
+#endif
+        Factory::instance()->unregisterType(_name);
+      }
+
+    private:
+      std::string _name;
+  };
+
+#if defined _MSC_VER && defined G2O_SHARED_LIBS
+#  define G2O_FACTORY_EXPORT __declspec(dllexport)
+#  define G2O_FACTORY_IMPORT __declspec(dllimport)
+#else
+#  define G2O_FACTORY_EXPORT
+#  define G2O_FACTORY_IMPORT
+#endif
+
+  // These macros are used to automate registering types and forcing linkage
+#define G2O_REGISTER_TYPE(name, classname) \
+    extern "C" void G2O_FACTORY_EXPORT g2o_type_##classname(void) {} \
+    static g2o::RegisterTypeProxy<classname> g_type_proxy_##classname(#name);
+
+#define G2O_USE_TYPE_BY_CLASS_NAME(classname) \
+    extern "C" void G2O_FACTORY_IMPORT g2o_type_##classname(void); \
+    static g2o::ForceLinker proxy_##classname(g2o_type_##classname);
+
+#define G2O_REGISTER_TYPE_GROUP(typeGroupName) \
+    extern "C" void G2O_FACTORY_EXPORT g2o_type_group_##typeGroupName(void) {}
+
+#define G2O_USE_TYPE_GROUP(typeGroupName) \
+    extern "C" void G2O_FACTORY_IMPORT g2o_type_group_##typeGroupName(void); \
+    static g2o::ForceLinker g2o_force_type_link_##typeGroupName(g2o_type_group_##typeGroupName);
+}
+
+#endif

Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp

@@ -0,0 +1,261 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <queue>
+#include <deque>
+#include <vector>
+#include <assert.h>
+#include <iostream>
+#include "hyper_dijkstra.h"
+#include "../stuff/macros.h"
+
+namespace g2o{
+
+  using namespace std;
+
+  double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e){
+    (void) v;
+    (void) vParent;
+    (void) e;
+    return std::numeric_limits<double>::max();
+  }
+
+  double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance){
+    if (distance==-1)
+      return perform (v,vParent,e);
+    return std::numeric_limits<double>::max();
+  }
+
+  HyperDijkstra::AdjacencyMapEntry::AdjacencyMapEntry(HyperGraph::Vertex* child_, HyperGraph::Vertex* parent_, 
+      HyperGraph::Edge* edge_, double distance_)
+  {
+    _child=child_;
+    _parent=parent_;
+    _edge=edge_;
+    _distance=distance_;
+  }
+
+  HyperDijkstra::HyperDijkstra(HyperGraph* g): _graph(g)
+  {
+    for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){
+      AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());
+      _adjacencyMap.insert(make_pair(entry.child(), entry));
+    }
+  }
+
+  void HyperDijkstra::reset()
+  {
+    for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){
+      AdjacencyMap::iterator at=_adjacencyMap.find(*it);
+      assert(at!=_adjacencyMap.end());
+      at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());
+    }
+    _visited.clear();
+  }
+
+
+  bool operator<(const HyperDijkstra::AdjacencyMapEntry& a, const HyperDijkstra::AdjacencyMapEntry& b)
+  {
+    return a.distance()>b.distance();
+  }
+
+
+  void HyperDijkstra::shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost, 
+      double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost)
+  {
+    reset();
+    std::priority_queue< AdjacencyMapEntry > frontier;
+    for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
+      HyperGraph::Vertex* v=*vit;
+      assert(v!=0);
+      AdjacencyMap::iterator it=_adjacencyMap.find(v);
+      if (it == _adjacencyMap.end()) {
+        cerr << __PRETTY_FUNCTION__ << "Vertex " << v->id() << " is not in the adjacency map" << endl;
+      }
+      assert(it!=_adjacencyMap.end());
+      it->second._distance=0.;
+      it->second._parent=0;
+      frontier.push(it->second);
+    }
+
+    while(! frontier.empty()){
+      AdjacencyMapEntry entry=frontier.top();
+      frontier.pop();
+      HyperGraph::Vertex* u=entry.child();
+      AdjacencyMap::iterator ut=_adjacencyMap.find(u);
+      if (ut == _adjacencyMap.end()) {
+        cerr << __PRETTY_FUNCTION__ << "Vertex " << u->id() << " is not in the adjacency map" << endl;
+      }
+      assert(ut!=_adjacencyMap.end());
+      double uDistance=ut->second.distance();
+
+      std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult;
+      HyperGraph::EdgeSet::iterator et=u->edges().begin();
+      while (et != u->edges().end()){
+        HyperGraph::Edge* edge=*et;
+        ++et;
+
+        if (directed && edge->vertex(0) != u)
+          continue;
+
+        for (size_t i = 0; i < edge->vertices().size(); ++i) {
+          HyperGraph::Vertex* z = edge->vertex(i);
+          if (z == u)
+            continue;
+
+          double edgeDistance=(*cost)(edge, u, z);
+          if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost)
+            continue;
+          double zDistance=uDistance+edgeDistance;
+          //cerr << z->id() << " " << zDistance << endl;
+
+          AdjacencyMap::iterator ot=_adjacencyMap.find(z);
+          assert(ot!=_adjacencyMap.end());
+
+          if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
+            ot->second._distance=zDistance;
+            ot->second._parent=u;
+            ot->second._edge=edge;
+            frontier.push(ot->second);
+          }
+        }
+      }
+    }
+  }
+
+  void HyperDijkstra::shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance, 
+      double comparisonConditioner, bool directed, double maxEdgeCost)
+  {
+    HyperGraph::VertexSet vset;
+    vset.insert(v);
+    shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost);
+  }
+
+  void HyperDijkstra::computeTree(AdjacencyMap& amap)
+  {
+    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
+      AdjacencyMapEntry& entry(it->second);
+      entry._children.clear();
+    }
+    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
+      AdjacencyMapEntry& entry(it->second);
+      HyperGraph::Vertex* parent=entry.parent();
+      if (!parent){
+        continue;
+      }
+      HyperGraph::Vertex* v=entry.child();
+      assert (v==it->first);
+
+      AdjacencyMap::iterator pt=amap.find(parent);
+      assert(pt!=amap.end());
+      pt->second._children.insert(v);
+    }
+  }
+
+
+  void HyperDijkstra::visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance)
+  {
+    
+    typedef std::deque<HyperGraph::Vertex*> Deque;
+    Deque q;
+    // scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them.
+    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
+      AdjacencyMapEntry& entry(it->second);
+      if (! entry.parent()) {
+        action->perform(it->first,0,0);
+        q.push_back(it->first);
+      }
+    }
+
+    //std::cerr << "q.size()" << q.size() << endl;
+    int count=0;
+    while (! q.empty()){
+      HyperGraph::Vertex* parent=q.front();
+      q.pop_front();
+      ++count;
+      AdjacencyMap::iterator parentIt=amap.find(parent);
+      if (parentIt==amap.end()) {
+        continue;
+      }
+      //cerr << "parent= " << parent << " parent id= " << parent->id() << "\t children id =";
+      HyperGraph::VertexSet& childs(parentIt->second.children());
+      for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){
+        HyperGraph::Vertex* child=*childsIt;
+        //cerr << child->id();
+        AdjacencyMap::iterator adjacencyIt=amap.find(child);
+        assert (adjacencyIt!=amap.end());
+        HyperGraph::Edge* edge=adjacencyIt->second.edge();  
+
+        assert(adjacencyIt->first==child);
+        assert(adjacencyIt->second.child()==child);
+        assert(adjacencyIt->second.parent()==parent);
+        if (! useDistance) {
+          action->perform(child, parent, edge);
+        } else {
+          action->perform(child, parent, edge, adjacencyIt->second.distance());
+        }
+        q.push_back(child);
+      }
+      //cerr << endl;
+    }
+
+  }
+
+  void HyperDijkstra::connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, 
+      HyperGraph::VertexSet& startingSet, 
+      HyperGraph* g, HyperGraph::Vertex* v,
+      HyperDijkstra::CostFunction* cost, double distance, 
+      double comparisonConditioner, double maxEdgeCost)
+  {
+    typedef std::queue<HyperGraph::Vertex*> VertexDeque;
+    visited.clear();
+    connected.clear();
+    VertexDeque frontier;
+    HyperDijkstra dv(g);
+    connected.insert(v);
+    frontier.push(v);
+    while (! frontier.empty()){
+      HyperGraph::Vertex* v0=frontier.front();
+      frontier.pop();
+      dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost);
+      for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){
+        visited.insert(*it);
+        if (startingSet.find(*it)==startingSet.end())
+          continue;
+        std::pair<HyperGraph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
+        if (insertOutcome.second){ // the node was not in the connectedSet;
+          frontier.push(dynamic_cast<HyperGraph::Vertex*>(*it));
+        }
+      }
+    }
+  }
+
+  double UniformCostFunction::operator () (HyperGraph::Edge* /*edge*/, HyperGraph::Vertex* /*from*/, HyperGraph::Vertex* /*to*/)
+  {
+    return 1.;
+  }
+
+};

Thirdparty/g2o/g2o/core/hyper_dijkstra.h

@@ -0,0 +1,112 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_AIS_GENERAL_DIJKSTRA_HH
+#define G2O_AIS_GENERAL_DIJKSTRA_HH
+
+#include <map>
+#include <set>
+#include <limits>
+
+#include "hyper_graph.h"
+
+namespace g2o{
+
+  struct  HyperDijkstra{
+    struct  CostFunction {
+      virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0;
+      virtual ~CostFunction() { }
+    };
+
+    struct  TreeAction {
+      virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e);
+      virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance);
+    };
+
+    
+    struct  AdjacencyMapEntry{
+      friend struct HyperDijkstra;
+      AdjacencyMapEntry(HyperGraph::Vertex* _child=0, 
+          HyperGraph::Vertex* _parent=0, 
+          HyperGraph::Edge* _edge=0, 
+          double _distance=std::numeric_limits<double>::max());
+      HyperGraph::Vertex* child() const {return _child;}
+      HyperGraph::Vertex* parent() const {return _parent;}
+      HyperGraph::Edge* edge() const {return _edge;}
+      double distance() const {return _distance;}
+      HyperGraph::VertexSet& children() {return _children;}
+      const HyperGraph::VertexSet& children() const {return _children;}
+      protected:
+      HyperGraph::Vertex* _child;
+      HyperGraph::Vertex* _parent;
+      HyperGraph::Edge* _edge;
+      double _distance;
+      HyperGraph::VertexSet _children;
+    };
+
+    typedef std::map<HyperGraph::Vertex*, AdjacencyMapEntry> AdjacencyMap;
+    HyperDijkstra(HyperGraph* g);
+    HyperGraph::VertexSet& visited() {return _visited; }
+    AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
+    HyperGraph* graph() {return _graph;} 
+
+    void shortestPaths(HyperGraph::Vertex* v, 
+           HyperDijkstra::CostFunction* cost, 
+           double maxDistance=std::numeric_limits< double >::max(), 
+           double comparisonConditioner=1e-3, 
+           bool directed=false,
+           double maxEdgeCost=std::numeric_limits< double >::max());
+
+    void shortestPaths(HyperGraph::VertexSet& vset, 
+           HyperDijkstra::CostFunction* cost, 
+           double maxDistance=std::numeric_limits< double >::max(), 
+           double comparisonConditioner=1e-3, 
+           bool directed=false,
+           double maxEdgeCost=std::numeric_limits< double >::max());
+
+
+    static void computeTree(AdjacencyMap& amap);
+    static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false);
+    static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, 
+           HyperGraph::VertexSet& startingSet, 
+           HyperGraph* g, HyperGraph::Vertex* v,
+           HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner,
+           double maxEdgeCost=std::numeric_limits< double >::max() );
+
+  protected:
+    void reset();
+
+    AdjacencyMap _adjacencyMap;
+    HyperGraph::VertexSet _visited;
+    HyperGraph* _graph;
+  };
+
+  struct  UniformCostFunction: public HyperDijkstra::CostFunction {
+    virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to);
+  };
+
+}
+#endif

Thirdparty/g2o/g2o/core/hyper_graph.cpp

@@ -0,0 +1,166 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "hyper_graph.h"
+
+#include <assert.h>
+#include <queue>
+
+namespace g2o {
+
+  HyperGraph::Vertex::Vertex(int id) : _id(id)
+  {
+  }
+
+  HyperGraph::Vertex::~Vertex()
+  {
+  }
+
+  HyperGraph::Edge::Edge(int id) : _id(id)
+  {
+  }
+
+  HyperGraph::Edge::~Edge()
+  {
+  }
+
+  void HyperGraph::Edge::resize(size_t size)
+  {
+    _vertices.resize(size, 0);
+  }
+
+  void HyperGraph::Edge::setId(int id)
+  {
+    _id = id;
+  }
+
+  HyperGraph::Vertex* HyperGraph::vertex(int id)
+  {
+    VertexIDMap::iterator it=_vertices.find(id);
+    if (it==_vertices.end())
+      return 0;
+    return it->second;
+  }
+
+  const HyperGraph::Vertex* HyperGraph::vertex(int id) const
+  {
+    VertexIDMap::const_iterator it=_vertices.find(id);
+    if (it==_vertices.end())
+      return 0;
+    return it->second;
+  }
+
+  bool HyperGraph::addVertex(Vertex* v)
+  {
+    Vertex* vn=vertex(v->id());
+    if (vn)
+      return false;
+    _vertices.insert( std::make_pair(v->id(),v) );
+    return true;
+  }
+
+  /**
+   * changes the id of a vertex already in the graph, and updates the bookkeeping
+   @ returns false if the vertex is not in the graph;
+  */
+  bool HyperGraph::changeId(Vertex* v, int newId){
+    Vertex* v2 = vertex(v->id());
+    if (v != v2)
+      return false;
+    _vertices.erase(v->id());
+    v->setId(newId);
+    _vertices.insert(std::make_pair(v->id(), v));
+    return true;
+  }
+
+  bool HyperGraph::addEdge(Edge* e)
+  {
+    std::pair<EdgeSet::iterator, bool> result = _edges.insert(e);
+    if (! result.second)
+      return false;
+    for (std::vector<Vertex*>::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+      Vertex* v = *it;
+      v->edges().insert(e);
+    }
+    return true;
+  }
+
+  bool HyperGraph::removeVertex(Vertex* v)
+  {
+    VertexIDMap::iterator it=_vertices.find(v->id());
+    if (it==_vertices.end())
+      return false;
+    assert(it->second==v);
+    //remove all edges which are entering or leaving v;
+    EdgeSet tmp(v->edges());
+    for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){
+      if (!removeEdge(*it)){
+        assert(0);
+      }
+    }
+    _vertices.erase(it);
+    delete v;
+    return true;
+  }
+
+  bool HyperGraph::removeEdge(Edge* e)
+  {
+    EdgeSet::iterator it = _edges.find(e);
+    if (it == _edges.end())
+      return false;
+    _edges.erase(it);
+
+    for (std::vector<Vertex*>::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
+      Vertex* v = *vit;
+      it = v->edges().find(e);
+      assert(it!=v->edges().end());
+      v->edges().erase(it);
+    }
+
+    delete e;
+    return true;
+  }
+
+  HyperGraph::HyperGraph()
+  {
+  }
+
+  void HyperGraph::clear()
+  {
+    for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it)
+      delete (it->second);
+    for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it)
+      delete (*it);
+    _vertices.clear();
+    _edges.clear();
+  }
+
+  HyperGraph::~HyperGraph()
+  {
+    clear();
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/hyper_graph.h

@@ -0,0 +1,220 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_AIS_HYPER_GRAPH_HH
+#define G2O_AIS_HYPER_GRAPH_HH
+
+#include <map>
+#include <set>
+#include <bitset>
+#include <cassert>
+#include <vector>
+#include <limits>
+#include <cstddef>
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+
+/** @addtogroup graph */
+//@{
+namespace g2o {
+
+  /**
+     Class that models a directed  Hyper-Graph. An hyper graph is a graph where an edge
+     can connect one or more nodes. Both Vertices and Edges of an hyoper graph
+     derive from the same class HyperGraphElement, thus one can implement generic algorithms
+     that operate transparently on edges or vertices (see HyperGraphAction).
+
+     The vertices are uniquely identified by an int id, while the edges are
+     identfied by their pointers. 
+   */
+  class  HyperGraph
+  {
+    public:
+
+      /**
+       * \brief enum of all the types we have in our graphs
+       */
+      enum  HyperGraphElementType {
+        HGET_VERTEX,
+        HGET_EDGE,
+        HGET_PARAMETER,
+        HGET_CACHE,
+        HGET_DATA,
+        HGET_NUM_ELEMS // keep as last elem
+      };
+
+      typedef std::bitset<HyperGraph::HGET_NUM_ELEMS> GraphElemBitset;
+
+      class  Vertex;
+      class  Edge;
+      
+      /**
+       * base hyper graph element, specialized in vertex and edge
+       */
+      struct  HyperGraphElement {
+        virtual ~HyperGraphElement() {}
+        /**
+         * returns the type of the graph element, see HyperGraphElementType
+         */
+        virtual HyperGraphElementType elementType() const = 0;
+      };
+
+      typedef std::set<Edge*>                           EdgeSet;
+      typedef std::set<Vertex*>                         VertexSet;
+
+      typedef std::unordered_map<int, Vertex*>     VertexIDMap;
+      typedef std::vector<Vertex*>                      VertexContainer;
+
+      //! abstract Vertex, your types must derive from that one
+      class  Vertex : public HyperGraphElement {
+        public:
+          //! creates a vertex having an ID specified by the argument
+          explicit Vertex(int id=-1);
+          virtual ~Vertex();
+          //! returns the id
+          int id() const {return _id;}
+	  virtual void setId( int newId) { _id=newId; }
+          //! returns the set of hyper-edges that are leaving/entering in this vertex
+          const EdgeSet& edges() const {return _edges;}
+          //! returns the set of hyper-edges that are leaving/entering in this vertex
+          EdgeSet& edges() {return _edges;}
+          virtual HyperGraphElementType elementType() const { return HGET_VERTEX;}
+        protected:
+          int _id;
+          EdgeSet _edges;
+      };
+
+      /** 
+       * Abstract Edge class. Your nice edge classes should inherit from that one.
+       * An hyper-edge has pointers to the vertices it connects and stores them in a vector.
+       */
+      class  Edge : public HyperGraphElement {
+        public:
+          //! creates and empty edge with no vertices
+          explicit Edge(int id = -1);
+          virtual ~Edge();
+
+          /**
+           * resizes the number of vertices connected by this edge
+           */
+          virtual void resize(size_t size);
+          /**
+            returns the vector of pointers to the vertices connected by the hyper-edge.
+            */
+          const VertexContainer& vertices() const { return _vertices;}
+          /**
+            returns the vector of pointers to the vertices connected by the hyper-edge.
+            */
+          VertexContainer& vertices() { return _vertices;}
+          /**
+            returns the pointer to the ith vertex connected to the hyper-edge.
+            */
+          const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
+          /**
+            returns the pointer to the ith vertex connected to the hyper-edge.
+            */
+          Vertex* vertex(size_t i) { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
+          /**
+            set the ith vertex on the hyper-edge to the pointer supplied
+            */
+          void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}
+
+          int id() const {return _id;}
+          void setId(int id);
+          virtual HyperGraphElementType elementType() const { return HGET_EDGE;}
+        protected:
+          VertexContainer _vertices;
+          int _id; ///< unique id
+      };
+
+    public:
+      //! constructs an empty hyper graph
+      HyperGraph();
+      //! destroys the hyper-graph and all the vertices of the graph
+      virtual ~HyperGraph();
+
+      //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
+      Vertex* vertex(int id);
+      //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
+      const Vertex* vertex(int id) const;
+
+      //! removes a vertex from the graph. Returns true on success (vertex was present)
+      virtual bool removeVertex(Vertex* v);
+      //! removes a vertex from the graph. Returns true on success (edge was present)
+      virtual bool removeEdge(Edge* e);
+      //! clears the graph and empties all structures.
+      virtual void clear();
+
+      //! @returns the map <i>id -> vertex</i> where the vertices are stored
+      const VertexIDMap& vertices() const {return _vertices;}
+      //! @returns the map <i>id -> vertex</i> where the vertices are stored
+      VertexIDMap& vertices() {return _vertices;}
+
+      //! @returns the set of edges of the hyper graph
+      const EdgeSet& edges() const {return _edges;}
+      //! @returns the set of edges of the hyper graph
+      EdgeSet& edges() {return _edges;}
+
+      /**
+       * adds a vertex to the graph. The id of the vertex should be set before
+       * invoking this function. the function fails if another vertex
+       * with the same id is already in the graph.
+       * returns true, on success, or false on failure.
+       */
+      virtual bool addVertex(Vertex* v);
+
+      /**
+       * Adds an edge  to the graph. If the edge is already in the graph, it
+       * does nothing and returns false. Otherwise it returns true.
+       */
+      virtual bool addEdge(Edge* e);
+
+      /**
+       * changes the id of a vertex already in the graph, and updates the bookkeeping
+       @ returns false if the vertex is not in the graph;
+       */
+      virtual bool changeId(Vertex* v, int newId);
+
+    protected:
+      VertexIDMap _vertices;
+      EdgeSet _edges;
+
+    private:
+      // Disable the copy constructor and assignment operator
+      HyperGraph(const HyperGraph&) { }
+      HyperGraph& operator= (const HyperGraph&) { return *this; }
+  };
+
+} // end namespace
+
+//@}
+
+#endif

Thirdparty/g2o/g2o/core/hyper_graph_action.cpp

@@ -0,0 +1,268 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "hyper_graph_action.h"
+#include "optimizable_graph.h"
+#include "../stuff/macros.h"
+
+
+#include <iostream>
+
+namespace g2o {
+  using namespace std;
+
+  HyperGraphActionLibrary* HyperGraphActionLibrary::actionLibInstance = 0;
+
+  HyperGraphAction::Parameters::~Parameters()
+  {
+  }
+
+  HyperGraphAction::ParametersIteration::ParametersIteration(int iter) :
+    HyperGraphAction::Parameters(),
+    iteration(iter)
+  {
+  }
+
+  HyperGraphAction::~HyperGraphAction()
+  {
+  }
+
+  HyperGraphAction* HyperGraphAction::operator()(const HyperGraph*, Parameters*)
+  {
+    return 0;
+  }
+
+  HyperGraphElementAction::Parameters::~Parameters()
+  {
+  }
+
+  HyperGraphElementAction::HyperGraphElementAction(const std::string& typeName_)
+  {
+    _typeName = typeName_;
+  }
+
+  void HyperGraphElementAction::setTypeName(const std::string& typeName_)
+  {
+    _typeName = typeName_;
+  }
+
+
+  HyperGraphElementAction* HyperGraphElementAction::operator()(HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )
+  {
+    return 0;
+  }
+  
+  HyperGraphElementAction* HyperGraphElementAction::operator()(const HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )
+  {
+    return 0;
+  }
+  
+  HyperGraphElementAction::~HyperGraphElementAction()
+  {
+  }
+
+  HyperGraphElementActionCollection::HyperGraphElementActionCollection(const std::string& name_)
+  {
+    _name = name_;
+  }
+
+  HyperGraphElementActionCollection::~HyperGraphElementActionCollection()
+  {
+    for (ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {
+      delete it->second;
+    }
+  }
+
+  HyperGraphElementAction* HyperGraphElementActionCollection::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)
+  {
+    ActionMap::iterator it=_actionMap.find(typeid(*element).name());
+    //cerr << typeid(*element).name() << endl;
+    if (it==_actionMap.end())
+      return 0;
+    HyperGraphElementAction* action=it->second;
+    return (*action)(element, params);
+  }
+
+  HyperGraphElementAction* HyperGraphElementActionCollection::operator()(const HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)
+  {
+    ActionMap::iterator it=_actionMap.find(typeid(*element).name());
+    if (it==_actionMap.end())
+      return 0;
+    HyperGraphElementAction* action=it->second;
+    return (*action)(element, params);
+  }
+
+  bool HyperGraphElementActionCollection::registerAction(HyperGraphElementAction* action)
+  {
+#  ifdef G2O_DEBUG_ACTIONLIB
+    cerr << __PRETTY_FUNCTION__ << " " << action->name() << " " << action->typeName() << endl;
+#  endif
+    if (action->name()!=name()){
+      cerr << __PRETTY_FUNCTION__  << ": invalid attempt to register an action in a collection with a different name " <<  name() << " " << action->name() << endl;
+    }
+    _actionMap.insert(make_pair ( action->typeName(), action) );
+    return true;
+  }
+
+  bool HyperGraphElementActionCollection::unregisterAction(HyperGraphElementAction* action)
+  {
+    for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {
+      if (it->second == action){
+        _actionMap.erase(it);
+        return true;
+      }
+    }
+    return false;
+  }
+  
+  HyperGraphActionLibrary::HyperGraphActionLibrary()
+  {
+  }
+
+  HyperGraphActionLibrary* HyperGraphActionLibrary::instance()
+  {
+    if (actionLibInstance == 0) {
+      actionLibInstance = new HyperGraphActionLibrary;
+    }
+    return actionLibInstance;
+  }
+
+  void HyperGraphActionLibrary::destroy()
+  {
+    delete actionLibInstance;
+    actionLibInstance = 0;
+  }
+
+  HyperGraphActionLibrary::~HyperGraphActionLibrary()
+  {
+    for (HyperGraphElementAction::ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {
+      delete it->second;
+    }
+  }
+  
+  HyperGraphElementAction* HyperGraphActionLibrary::actionByName(const std::string& name)
+  {
+    HyperGraphElementAction::ActionMap::iterator it=_actionMap.find(name);
+    if (it!=_actionMap.end())
+      return it->second;
+    return 0;
+  }
+
+  bool HyperGraphActionLibrary::registerAction(HyperGraphElementAction* action)
+  {
+    HyperGraphElementAction* oldAction = actionByName(action->name());
+    HyperGraphElementActionCollection* collection = 0;
+    if (oldAction) {
+      collection = dynamic_cast<HyperGraphElementActionCollection*>(oldAction);
+      if (! collection) {
+        cerr << __PRETTY_FUNCTION__ << ": fatal error, a collection is not at the first level in the library" << endl;
+        return 0;
+      }
+    }
+    if (! collection) {
+#ifdef G2O_DEBUG_ACTIONLIB
+      cerr << __PRETTY_FUNCTION__ << ": creating collection for \"" << action->name() << "\"" << endl;
+#endif
+      collection = new HyperGraphElementActionCollection(action->name());
+      _actionMap.insert(make_pair(action->name(), collection));
+    }
+    return collection->registerAction(action);
+  }
+  
+  bool HyperGraphActionLibrary::unregisterAction(HyperGraphElementAction* action)
+  {
+    list<HyperGraphElementActionCollection*> collectionDeleteList;
+
+    // Search all the collections and delete the registered actions; if a collection becomes empty, schedule it for deletion; note that we can't delete the collections as we go because this will screw up the state of the iterators
+    for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {
+      HyperGraphElementActionCollection* collection = dynamic_cast<HyperGraphElementActionCollection*> (it->second);
+      if (collection != 0) {
+        collection->unregisterAction(action);
+        if (collection->actionMap().size() == 0) {
+          collectionDeleteList.push_back(collection);
+        }
+      }
+    }
+
+    // Delete any empty action collections
+    for (list<HyperGraphElementActionCollection*>::iterator itc = collectionDeleteList.begin(); itc != collectionDeleteList.end(); ++itc) {
+      //cout << "Deleting collection " << (*itc)->name() << endl;
+      _actionMap.erase((*itc)->name());
+    }
+
+    return true;
+  }
+
+
+  WriteGnuplotAction::WriteGnuplotAction(const std::string& typeName_)
+    : HyperGraphElementAction(typeName_)
+  {
+    _name="writeGnuplot";
+  }
+
+  DrawAction::Parameters::Parameters(){
+  }
+
+  DrawAction::DrawAction(const std::string& typeName_) 
+    : HyperGraphElementAction(typeName_)
+  {
+    _name="draw";
+    _previousParams = (Parameters*)0x42;
+    refreshPropertyPtrs(0);
+  }
+
+  bool DrawAction::refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_){
+    if (_previousParams == params_)
+      return false;
+    DrawAction::Parameters* p=dynamic_cast<DrawAction::Parameters*>(params_);
+    if (! p){
+      _previousParams = 0;
+      _show = 0;
+      _showId = 0;
+    } else {
+      _previousParams = p;
+      _show = p->makeProperty<BoolProperty>(_typeName+"::SHOW", true);
+      _showId = p->makeProperty<BoolProperty>(_typeName+"::SHOW_ID", false);
+    }
+    return true;
+  }
+
+  void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* params, const std::string& typeName)
+  {
+    for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin(); 
+        it!=graph->vertices().end(); ++it){
+      if ( typeName.empty() || typeid(*it->second).name()==typeName){
+        (*action)(it->second, params);
+      }
+    }
+    for (HyperGraph::EdgeSet::iterator it=graph->edges().begin(); 
+        it!=graph->edges().end(); ++it){
+      if ( typeName.empty() || typeid(**it).name()==typeName)
+        (*action)(*it, params);
+    }
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/hyper_graph_action.h

@@ -0,0 +1,222 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_HYPER_GRAPH_ACTION_H
+#define G2O_HYPER_GRAPH_ACTION_H
+
+#include "hyper_graph.h"
+#include "../stuff/property.h"
+
+#include <typeinfo>
+#include <iosfwd>
+#include <set>
+#include <string>
+#include <iostream>
+
+
+// define to get verbose output
+//#define G2O_DEBUG_ACTIONLIB
+
+namespace g2o {
+
+  /**
+   * \brief Abstract action that operates on an entire graph
+   */
+  class  HyperGraphAction {
+    public:
+      class  Parameters {
+        public:
+          virtual ~Parameters();
+      };
+
+      class  ParametersIteration : public Parameters {
+        public:
+          explicit ParametersIteration(int iter);
+          int iteration;
+      };
+
+      virtual ~HyperGraphAction();
+
+      /**
+       * re-implement to carry out an action given the graph
+       */
+      virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0);
+  };
+
+  /**
+   * \brief Abstract action that operates on a graph entity
+   */
+  class  HyperGraphElementAction{
+    public:
+      struct  Parameters{
+        virtual ~Parameters();
+      };
+      typedef std::map<std::string, HyperGraphElementAction*> ActionMap;
+      //! an action should be instantiated with the typeid.name of the graph element 
+      //! on which it operates
+      HyperGraphElementAction(const std::string& typeName_="");
+
+      //! redefine this to do the action stuff. If successful, the action returns a pointer to itself
+      virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);
+
+      //! redefine this to do the action stuff. If successful, the action returns a pointer to itself
+      virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);
+
+      //! destroyed actions release the memory
+      virtual ~HyperGraphElementAction();
+
+      //! returns the typeid name of the action
+      const std::string& typeName() const { return _typeName;}
+
+      //! returns the name of an action, e.g "draw"
+      const std::string& name() const{ return _name;}
+
+      //! sets the type on which an action has to operate
+      void setTypeName(const std::string& typeName_);
+
+    protected:
+      std::string _typeName;
+      std::string _name;
+  };
+
+  /**
+   * \brief collection of actions
+   *
+   * collection of actions calls contains homogeneous actions operating on different types
+   * all collected actions have the same name and should have the same functionality
+   */
+  class  HyperGraphElementActionCollection: public HyperGraphElementAction{
+    public:
+      //! constructor. name_ is the name of the action e.g.draw).
+      HyperGraphElementActionCollection(const std::string& name_);
+      //! destructor: it deletes all actions in the pool.
+      virtual ~HyperGraphElementActionCollection();
+      //! calling functions, they return a pointer to the instance of action in actionMap
+      //! that was active on element
+      virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);
+      virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);
+      ActionMap& actionMap() {return _actionMap;}
+      //! inserts an action in the pool. The action should have the same name of the container.
+      //! returns false on failure (the container has a different name than the action);
+      bool registerAction(HyperGraphElementAction* action);
+      bool unregisterAction(HyperGraphElementAction* action);
+    protected:
+      ActionMap _actionMap;
+  };
+
+  /**
+   * \brief library of actions, indexed by the action name;
+   *
+   * library of actions, indexed by the action name;
+   * one can use ti to register a collection of actions
+   */
+  class  HyperGraphActionLibrary{
+    public:
+      //! return the single instance of the HyperGraphActionLibrary
+      static HyperGraphActionLibrary* instance();
+      //! free the instance
+      static void destroy();
+
+      // returns a pointer to a collection indexed by name
+      HyperGraphElementAction* actionByName(const std::string& name);
+      // registers a basic action in the pool. If necessary a container is created
+      bool registerAction(HyperGraphElementAction* action);
+      bool unregisterAction(HyperGraphElementAction* action);
+      
+      inline HyperGraphElementAction::ActionMap& actionMap() {return _actionMap;}
+    protected:
+      HyperGraphActionLibrary();
+      ~HyperGraphActionLibrary();
+      HyperGraphElementAction::ActionMap _actionMap;
+    private:
+      static HyperGraphActionLibrary* actionLibInstance;
+  };
+
+  /**
+   * apply an action to all the elements of the graph.
+   */
+  void  applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* parameters=0, const std::string& typeName="");
+
+  /**
+   * brief write into gnuplot
+   */
+  class  WriteGnuplotAction: public HyperGraphElementAction{
+    public:
+      struct  Parameters: public HyperGraphElementAction::Parameters{
+        std::ostream* os;
+      };
+      WriteGnuplotAction(const std::string& typeName_);
+  };
+
+  /**
+   * \brief draw actions
+   */
+
+  class  DrawAction : public HyperGraphElementAction{
+  public:
+    class  Parameters: public HyperGraphElementAction::Parameters,  public PropertyMap{
+    public:
+      Parameters();
+    };
+    DrawAction(const std::string& typeName_);
+  protected:
+    virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_);
+    Parameters* _previousParams;
+    BoolProperty* _show;
+    BoolProperty* _showId;
+  };
+
+  template<typename T> class RegisterActionProxy
+    {
+      public:
+      RegisterActionProxy()
+          {
+#ifdef G2O_DEBUG_ACTIONLIB
+            std::cout << __FUNCTION__ << ": Registering action of type " << typeid(T).name() << std::endl;
+#endif
+            _action = new T();
+            HyperGraphActionLibrary::instance()->registerAction(_action);
+          }
+      
+        ~RegisterActionProxy()
+          {
+#ifdef G2O_DEBUG_ACTIONLIB
+            std::cout << __FUNCTION__ << ": Unregistering action of type " << typeid(T).name() << std::endl;
+#endif
+            HyperGraphActionLibrary::instance()->unregisterAction(_action);
+            delete _action;
+          }
+
+    private:
+        HyperGraphElementAction* _action;
+  };
+
+#define G2O_REGISTER_ACTION(classname) \
+    extern "C" void g2o_action_##classname(void) {} \
+    static g2o::RegisterActionProxy<classname> g_action_proxy_##classname;
+};
+
+#endif

Thirdparty/g2o/g2o/core/jacobian_workspace.cpp

@@ -0,0 +1,89 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "jacobian_workspace.h"
+
+#include <cmath>
+
+#include "optimizable_graph.h"
+
+using namespace std;
+
+namespace g2o {
+
+JacobianWorkspace::JacobianWorkspace() :
+  _maxNumVertices(-1), _maxDimension(-1)
+{
+}
+
+JacobianWorkspace::~JacobianWorkspace()
+{
+}
+
+bool JacobianWorkspace::allocate()
+{
+  //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
+  if (_maxNumVertices <=0 || _maxDimension <= 0)
+    return false;
+  _workspace.resize(_maxNumVertices);
+  for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) {
+    it->resize(_maxDimension);
+    it->setZero();
+  }
+  return true;
+}
+
+void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_)
+{
+  const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(e_);
+  int errorDimension = e->dimension();
+  int numVertices = e->vertices().size();
+  int maxDimensionForEdge = -1;
+  for (int i = 0; i < numVertices; ++i) {
+    const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
+    assert(v && "Edge has no vertex assigned");
+    maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge);
+  }
+  _maxNumVertices = max(numVertices, _maxNumVertices);
+  _maxDimension = max(maxDimensionForEdge, _maxDimension);
+  //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
+}
+
+void JacobianWorkspace::updateSize(const OptimizableGraph& graph)
+{
+  for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) {
+    const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
+    updateSize(e);
+  }
+}
+
+void JacobianWorkspace::updateSize(int numVertices, int dimension)
+{
+  _maxNumVertices = max(numVertices, _maxNumVertices);
+  _maxDimension = max(dimension, _maxDimension);
+}
+
+} // end namespace

Thirdparty/g2o/g2o/core/jacobian_workspace.h

@@ -0,0 +1,96 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef JACOBIAN_WORKSPACE_H
+#define JACOBIAN_WORKSPACE_H
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+
+#include <vector>
+#include <cassert>
+
+#include "hyper_graph.h"
+
+namespace g2o {
+
+  struct OptimizableGraph;
+
+  /**
+   * \brief provide memory workspace for computing the Jacobians
+   *
+   * The workspace is used by an OptimizableGraph to provide temporary memory
+   * for computing the Jacobian of the error functions.
+   * Before calling linearizeOplus on an edge, the workspace needs to be allocated
+   * by calling allocate().
+   */
+  class  JacobianWorkspace
+  {
+    public:
+      typedef std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> >      WorkspaceVector;
+
+    public:
+      JacobianWorkspace();
+      ~JacobianWorkspace();
+
+      /**
+       * allocate the workspace
+       */
+      bool allocate();
+
+      /**
+       * update the maximum required workspace needed by taking into account this edge
+       */
+      void updateSize(const HyperGraph::Edge* e);
+
+      /**
+       * update the required workspace by looking at a full graph
+       */
+      void updateSize(const OptimizableGraph& graph);
+
+      /**
+       * manually update with the given parameters
+       */
+      void updateSize(int numVertices, int dimension);
+
+      /**
+       * return the workspace for a vertex in an edge
+       */
+      double* workspaceForVertex(int vertexIndex)
+      {
+        assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds");
+        return _workspace[vertexIndex].data();
+      }
+
+    protected:
+      WorkspaceVector _workspace;   ///< the memory pre-allocated for computing the Jacobians
+      int _maxNumVertices;          ///< the maximum number of vertices connected by a hyper-edge
+      int _maxDimension;            ///< the maximum dimension (number of elements) for a Jacobian
+  };
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/linear_solver.h

@@ -0,0 +1,109 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_LINEAR_SOLVER_H
+#define G2O_LINEAR_SOLVER_H
+#include "sparse_block_matrix.h"
+#include "sparse_block_matrix_ccs.h"
+
+namespace g2o {
+
+/**
+ * \brief basic solver for Ax = b
+ *
+ * basic solver for Ax = b which has to reimplemented for different linear algebra libraries.
+ * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit.
+ */
+template <typename MatrixType>
+class LinearSolver
+{
+  public:
+    LinearSolver() {};
+    virtual ~LinearSolver() {}
+
+    /**
+     * init for operating on matrices with a different non-zero pattern like before
+     */
+    virtual bool init() = 0;
+
+    /**
+     * Assumes that A is the same matrix for several calls.
+     * Among other assumptions, the non-zero pattern does not change!
+     * If the matrix changes call init() before.
+     * solve system Ax = b, x and b have to allocated beforehand!!
+     */
+    virtual bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b) = 0;
+
+    /**
+     * Inverts the diagonal blocks of A
+     * @returns false if not defined.
+     */
+    virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix<MatrixType>& A) { (void)blocks; (void) A; return false; }
+
+
+    /**
+     * Inverts the a block pattern of A in spinv
+     * @returns false if not defined.
+     */
+    virtual bool solvePattern(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices, const SparseBlockMatrix<MatrixType>& A){
+      (void) spinv;
+      (void) blockIndices;
+      (void) A;
+      return false;
+    }
+
+    //! write a debug dump of the system matrix if it is not PSD in solve
+    virtual bool writeDebug() const { return false;}
+    virtual void setWriteDebug(bool) {}
+};
+
+/**
+ * \brief Solver with faster iterating structure for the linear matrix
+ */
+template <typename MatrixType>
+class LinearSolverCCS : public LinearSolver<MatrixType>
+{
+  public:
+    LinearSolverCCS() : LinearSolver<MatrixType>(), _ccsMatrix(0) {}
+    ~LinearSolverCCS()
+    {
+      delete _ccsMatrix;
+    }
+
+  protected:
+    SparseBlockMatrixCCS<MatrixType>* _ccsMatrix;
+
+    void initMatrixStructure(const SparseBlockMatrix<MatrixType>& A)
+    {
+      delete _ccsMatrix;
+      _ccsMatrix = new SparseBlockMatrixCCS<MatrixType>(A.rowBlockIndices(), A.colBlockIndices());
+      A.fillSparseBlockMatrixCCS(*_ccsMatrix);
+    }
+};
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp

@@ -0,0 +1,222 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "marginal_covariance_cholesky.h"
+
+#include <algorithm>
+#include <cassert>
+using namespace std;
+
+namespace g2o {
+
+struct MatrixElem
+{
+  int r, c;
+  MatrixElem(int r_, int c_) : r(r_), c(c_) {}
+  bool operator<(const MatrixElem& other) const
+  {
+    return c > other.c || (c == other.c && r > other.r);
+  }
+};
+
+MarginalCovarianceCholesky::MarginalCovarianceCholesky() :
+  _n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0)
+{
+}
+
+MarginalCovarianceCholesky::~MarginalCovarianceCholesky()
+{
+}
+
+void MarginalCovarianceCholesky::setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv)
+{
+  _n = n;
+  _Ap = Lp;
+  _Ai = Li;
+  _Ax = Lx;
+  _perm = permInv;
+
+  // pre-compute reciprocal values of the diagonal of L
+  _diag.resize(n);
+  for (int r = 0; r < n; ++r) {
+    const int& sc = _Ap[r]; // L is lower triangular, thus the first elem in the column is the diagonal entry
+    assert(r == _Ai[sc] && "Error in CCS storage of L");
+    _diag[r] = 1.0 / _Ax[sc];
+  }
+}
+
+double MarginalCovarianceCholesky::computeEntry(int r, int c)
+{
+  assert(r <= c);
+  int idx = computeIndex(r, c);
+
+  LookupMap::const_iterator foundIt = _map.find(idx);
+  if (foundIt != _map.end()) {
+    return foundIt->second;
+  }
+
+  // compute the summation over column r
+  double s = 0.;
+  const int& sc = _Ap[r];
+  const int& ec = _Ap[r+1];
+  for (int j = sc+1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal
+    const int& rr = _Ai[j];
+    double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr);
+    s += val * _Ax[j];
+  }
+
+  double result;
+  if (r == c) {
+    const double& diagElem = _diag[r];
+    result = diagElem * (diagElem - s);
+  } else {
+    result = -s * _diag[r];
+  }
+  _map[idx] = result;
+  return result;
+}
+
+void MarginalCovarianceCholesky::computeCovariance(double** covBlocks, const std::vector<int>& blockIndices)
+{
+  _map.clear();
+  int base = 0;
+  vector<MatrixElem> elemsToCompute;
+  for (size_t i = 0; i < blockIndices.size(); ++i) {
+    int nbase = blockIndices[i];
+    int vdim = nbase - base;
+    for (int rr = 0; rr < vdim; ++rr)
+      for (int cc = rr; cc < vdim; ++cc) {
+        int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
+        int c = _perm ? _perm[cc + base] : cc + base;
+        if (r > c) // make sure it's still upper triangular after applying the permutation
+          swap(r, c);
+        elemsToCompute.push_back(MatrixElem(r, c));
+      }
+    base = nbase;
+  }
+
+  // sort the elems to reduce the recursive calls
+  sort(elemsToCompute.begin(), elemsToCompute.end());
+
+  // compute the inverse elements we need
+  for (size_t i = 0; i < elemsToCompute.size(); ++i) {
+    const MatrixElem& me = elemsToCompute[i];
+    computeEntry(me.r, me.c);
+  }
+
+  // set the marginal covariance for the vertices, by writing to the blocks memory
+  base = 0;
+  for (size_t i = 0; i < blockIndices.size(); ++i) {
+    int nbase = blockIndices[i];
+    int vdim = nbase - base;
+    double* cov = covBlocks[i];
+    for (int rr = 0; rr < vdim; ++rr)
+      for (int cc = rr; cc < vdim; ++cc) {
+        int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
+        int c = _perm ? _perm[cc + base] : cc + base;
+        if (r > c) // upper triangle
+          swap(r, c);
+        int idx = computeIndex(r, c);
+        LookupMap::const_iterator foundIt = _map.find(idx);
+        assert(foundIt != _map.end());
+        cov[rr*vdim + cc] = foundIt->second;
+        if (rr != cc)
+          cov[cc*vdim + rr] = foundIt->second;
+      }
+    base = nbase;
+  }
+}
+
+
+void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices)
+{
+  // allocate the sparse
+  spinv = SparseBlockMatrix<MatrixXd>(&rowBlockIndices[0], 
+              &rowBlockIndices[0], 
+              rowBlockIndices.size(),
+              rowBlockIndices.size(), true);
+  _map.clear();
+  vector<MatrixElem> elemsToCompute;
+  for (size_t i = 0; i < blockIndices.size(); ++i) {
+    int blockRow=blockIndices[i].first;    
+    int blockCol=blockIndices[i].second;
+    assert(blockRow>=0);
+    assert(blockRow < (int)rowBlockIndices.size());
+    assert(blockCol>=0);
+    assert(blockCol < (int)rowBlockIndices.size());
+
+    int rowBase=spinv.rowBaseOfBlock(blockRow);
+    int colBase=spinv.colBaseOfBlock(blockCol);
+    
+    MatrixXd *block=spinv.block(blockRow, blockCol, true);
+    assert(block);
+    for (int iRow=0; iRow<block->rows(); ++iRow)
+      for (int iCol=0; iCol<block->cols(); ++iCol){
+  int rr=rowBase+iRow;
+  int cc=colBase+iCol;
+        int r = _perm ? _perm[rr] : rr; // apply permutation
+        int c = _perm ? _perm[cc] : cc;
+        if (r > c)
+          swap(r, c);
+        elemsToCompute.push_back(MatrixElem(r, c));
+      }
+  }
+
+  // sort the elems to reduce the number of recursive calls
+  sort(elemsToCompute.begin(), elemsToCompute.end());
+
+  // compute the inverse elements we need
+  for (size_t i = 0; i < elemsToCompute.size(); ++i) {
+    const MatrixElem& me = elemsToCompute[i];
+    computeEntry(me.r, me.c);
+  }
+
+  // set the marginal covariance 
+  for (size_t i = 0; i < blockIndices.size(); ++i) {
+    int blockRow=blockIndices[i].first;    
+    int blockCol=blockIndices[i].second;
+    int rowBase=spinv.rowBaseOfBlock(blockRow);
+    int colBase=spinv.colBaseOfBlock(blockCol);
+    
+    MatrixXd *block=spinv.block(blockRow, blockCol);
+    assert(block);
+    for (int iRow=0; iRow<block->rows(); ++iRow)
+      for (int iCol=0; iCol<block->cols(); ++iCol){
+  int rr=rowBase+iRow;
+  int cc=colBase+iCol;
+        int r = _perm ? _perm[rr] : rr; // apply permutation
+        int c = _perm ? _perm[cc] : cc;
+        if (r > c)
+          swap(r, c);
+        int idx = computeIndex(r, c);
+        LookupMap::const_iterator foundIt = _map.find(idx);
+        assert(foundIt != _map.end());
+  (*block)(iRow, iCol) = foundIt->second;
+      }
+  }
+}
+
+} // end namespace

Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h

@@ -0,0 +1,103 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_MARGINAL_COVARIANCE_CHOLESKY_H
+#define G2O_MARGINAL_COVARIANCE_CHOLESKY_H
+
+#include "optimizable_graph.h"
+#include "sparse_block_matrix.h"
+
+#include <cassert>
+#include <vector>
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+
+namespace g2o {
+
+  /**
+   * \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor)
+   */
+  class  MarginalCovarianceCholesky {
+    protected:
+      /**
+       * hash struct for storing the matrix elements needed to compute the covariance
+       */
+      typedef std::tr1::unordered_map<int, double>     LookupMap;
+    
+    public:
+      MarginalCovarianceCholesky();
+      ~MarginalCovarianceCholesky();
+
+      /**
+       * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to
+       * be provided by the caller).
+       */
+      void computeCovariance(double** covBlocks, const std::vector<int>& blockIndices);
+
+
+      /**
+       * compute the marginal cov for the given block indices, write the result in spinv).
+       */
+      void computeCovariance(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices);
+
+
+      /**
+       * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in.
+       * permInv might be 0, will then not permute the entries.
+       *
+       * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers
+       * are owned by the caller, MarginalCovarianceCholesky does not free the pointers.
+       */
+      void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv);
+
+    protected:
+      // information about the cholesky factor (lower triangle)
+      int _n;           ///< L is an n X n matrix
+      int* _Ap;         ///< column pointer of the CCS storage
+      int* _Ai;         ///< row indices of the CCS storage
+      double* _Ax;      ///< values of the cholesky factor
+      int* _perm;       ///< permutation of the cholesky factor. Variable re-ordering for better fill-in
+
+      LookupMap _map;             ///< hash look up table for the already computed entries
+      std::vector<double> _diag;  ///< cache 1 / H_ii to avoid recalculations
+
+      //! compute the index used for hashing
+      int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;}
+      /**
+       * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular.
+       * May issue recursive calls to itself to compute the missing values.
+       */
+      double computeEntry(int r, int c);
+  };
+
+}
+
+#endif

Thirdparty/g2o/g2o/core/matrix_operations.h

@@ -0,0 +1,74 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_CORE_MATRIX_OPERATIONS_H
+#define G2O_CORE_MATRIX_OPERATIONS_H
+
+#include <Eigen/Core>
+
+namespace g2o {
+  namespace internal {
+
+    template<typename MatrixType>
+    inline void axpy(const MatrixType& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+    {
+      y.segment<MatrixType::RowsAtCompileTime>(yoff) += A * x.segment<MatrixType::ColsAtCompileTime>(xoff);
+    }
+
+    template<int t>
+    inline void axpy(const Eigen::Matrix<double, Eigen::Dynamic, t>& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+    {
+      y.segment(yoff, A.rows()) += A * x.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(xoff);
+    }
+
+    template<>
+    inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+    {
+      y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols());
+    }
+
+    template<typename MatrixType>
+    inline void atxpy(const MatrixType& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+    {
+      y.segment<MatrixType::ColsAtCompileTime>(yoff) += A.transpose() * x.segment<MatrixType::RowsAtCompileTime>(xoff);
+    }
+
+    template<int t>
+    inline void atxpy(const Eigen::Matrix<double, Eigen::Dynamic, t>& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+    {
+      y.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows());
+    }
+
+    template<>
+    inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+    {
+      y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows());
+    }
+
+  } // end namespace internal
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/matrix_structure.cpp

@@ -0,0 +1,125 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "matrix_structure.h"
+
+#include <string>
+#include <vector>
+#include <fstream>
+#include <algorithm>
+using namespace std;
+
+namespace g2o {
+
+struct ColSort
+{
+  bool operator()(const pair<int, int>& e1, const pair<int, int>& e2) const
+  {
+    return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first);
+  }
+};
+
+MatrixStructure::MatrixStructure() :
+  n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0)
+{
+}
+
+MatrixStructure::~MatrixStructure()
+{
+  free();
+}
+
+void MatrixStructure::alloc(int n_, int nz)
+{
+  if (n == 0) {
+    maxN = n = n_;
+    maxNz = nz;
+    Ap  = new int[maxN + 1];
+    Aii = new int[maxNz];
+  }
+  else {
+    n = n_;
+    if (maxNz < nz) {
+      maxNz = 2 * nz;
+      delete[] Aii;
+      Aii = new int[maxNz];
+    }
+    if (maxN < n) {
+      maxN = 2 * n;
+      delete[] Ap;
+      Ap = new int[maxN + 1];
+    }
+  }
+}
+
+void MatrixStructure::free()
+{
+  n = 0;
+  m = 0;
+  maxN = 0;
+  maxNz = 0;
+  delete[] Aii; Aii = 0;
+  delete[] Ap; Ap = 0;
+}
+
+bool MatrixStructure::write(const char* filename) const
+{
+  const int& cols = n;
+  const int& rows = m;
+
+  string name = filename;
+  std::string::size_type lastDot = name.find_last_of('.');
+  if (lastDot != std::string::npos) 
+    name = name.substr(0, lastDot);
+
+  vector<pair<int, int> > entries;
+  for (int i=0; i < cols; ++i) {
+    const int& rbeg = Ap[i];
+    const int& rend = Ap[i+1];
+    for (int j = rbeg; j < rend; ++j) {
+      entries.push_back(make_pair(Aii[j], i));
+      if (Aii[j] != i)
+        entries.push_back(make_pair(i, Aii[j]));
+    }
+  }
+
+  sort(entries.begin(), entries.end(), ColSort());
+
+  std::ofstream fout(filename);
+  fout << "# name: " << name << std::endl;
+  fout << "# type: sparse matrix" << std::endl;
+  fout << "# nnz: " << entries.size() << std::endl;
+  fout << "# rows: " << rows << std::endl;
+  fout << "# columns: " << cols << std::endl;
+  for (vector<pair<int, int> >::const_iterator it = entries.begin(); it != entries.end(); ++it) {
+    const pair<int, int>& entry = *it;
+    fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0
+  }
+
+  return fout.good();
+}
+
+} // end namespace

Thirdparty/g2o/g2o/core/matrix_structure.h

@@ -0,0 +1,69 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_MATRIX_STRUCTURE_H
+#define G2O_MATRIX_STRUCTURE_H
+
+
+namespace g2o {
+
+/**
+ * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix)
+ */
+class MatrixStructure
+{
+  public:
+    MatrixStructure();
+    ~MatrixStructure();
+    /**
+     * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will
+     * then reallocate the memory + additional space (double the required space).
+     */
+    void alloc(int n_, int nz);
+
+    void free();
+    
+    /**
+     * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix)
+     */
+    bool write(const char* filename) const;
+
+    int n;    ///< A is m-by-n.  n must be >= 0.
+    int m;    ///< A is m-by-n.  m must be >= 0.
+    int* Ap;  ///< column pointers for A, of size n+1
+    int* Aii; ///< row indices of A, of size nz = Ap [n]
+
+    //! max number of non-zeros blocks
+    int nzMax() const { return maxNz;}
+
+  protected:
+    int maxN;     ///< size of the allocated memory
+    int maxNz;    ///< size of the allocated memory
+};
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/openmp_mutex.h

@@ -0,0 +1,97 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPENMP_MUTEX
+#define G2O_OPENMP_MUTEX
+
+#include "../../config.h"
+
+#ifdef G2O_OPENMP
+#include <omp.h>
+#else
+#include <cassert>
+#endif
+
+namespace g2o {
+
+#ifdef G2O_OPENMP
+
+  /**
+   * \brief Mutex realized via OpenMP
+   */
+  class OpenMPMutex
+  {
+    public:
+      OpenMPMutex() { omp_init_lock(&_lock); }
+      ~OpenMPMutex() { omp_destroy_lock(&_lock); }
+      void lock() { omp_set_lock(&_lock); }
+      void unlock() { omp_unset_lock(&_lock); }
+    protected:
+      omp_lock_t _lock;
+  };
+
+#else
+
+  /*
+   * dummy which does nothing in case we don't have OpenMP support.
+   * In debug mode, the mutex allows to verify the correct lock and unlock behavior
+   */
+  class OpenMPMutex
+  {
+    public:
+#ifdef NDEBUG
+      OpenMPMutex() {}
+#else
+      OpenMPMutex() : _cnt(0) {}
+#endif
+      ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");}
+      void lock() { assert(++_cnt == 1 && "Locking already locked mutex");}
+      void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");}
+    protected:
+#ifndef NDEBUG
+      char _cnt;
+#endif
+  };
+
+#endif
+
+  /**
+   * \brief lock a mutex within a scope
+   */
+  class ScopedOpenMPMutex
+  {
+    public:
+      explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); }
+      ~ScopedOpenMPMutex() { _mutex->unlock(); }
+    private:
+      OpenMPMutex* const _mutex;
+      ScopedOpenMPMutex(const ScopedOpenMPMutex&);
+      void operator=(const ScopedOpenMPMutex&);
+  };
+
+}
+
+#endif

Thirdparty/g2o/g2o/core/optimizable_graph.cpp

@@ -0,0 +1,910 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimizable_graph.h"
+
+#include <cassert>
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <algorithm>
+
+#include <Eigen/Dense>
+
+#include "estimate_propagator.h"
+#include "factory.h"
+#include "optimization_algorithm_property.h"
+#include "hyper_graph_action.h"
+#include "cache.h"
+#include "robust_kernel.h"
+
+#include "../stuff/macros.h"
+#include "../stuff/color_macros.h"
+#include "../stuff/string_tools.h"
+#include "../stuff/misc.h"
+
+namespace g2o {
+
+  using namespace std;
+
+  OptimizableGraph::Data::Data(){
+    _next = 0;
+  }
+  
+  OptimizableGraph::Data::~Data(){
+    if (_next)
+      delete _next;
+  }
+  
+
+  OptimizableGraph::Vertex::Vertex() :
+    HyperGraph::Vertex(),
+    _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false),
+    _colInHessian(-1), _cacheContainer(0)
+  {
+  }
+
+  CacheContainer* OptimizableGraph::Vertex::cacheContainer(){
+    if (! _cacheContainer)
+      _cacheContainer = new CacheContainer(this);
+    return _cacheContainer;
+  }
+
+
+  void OptimizableGraph::Vertex::updateCache(){
+    if (_cacheContainer){
+      _cacheContainer->setUpdateNeeded();
+      _cacheContainer->update();
+    }
+  }
+
+  OptimizableGraph::Vertex::~Vertex()
+  {
+    if (_cacheContainer)
+      delete (_cacheContainer);
+    if (_userData)
+      delete _userData;
+  }
+  
+  OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const
+  {
+    return 0;
+  }
+
+  bool OptimizableGraph::Vertex::setEstimateData(const double* v)
+  {
+    bool ret = setEstimateDataImpl(v);
+    updateCache();
+    return ret;
+  }
+
+  bool OptimizableGraph::Vertex::getEstimateData(double *) const
+  {
+    return false;
+  }
+
+  int OptimizableGraph::Vertex::estimateDimension() const
+  {
+    return -1;
+  }
+
+  bool OptimizableGraph::Vertex::setMinimalEstimateData(const double* v)
+  {
+    bool ret = setMinimalEstimateDataImpl(v);
+    updateCache();
+    return ret;
+  }
+
+  bool OptimizableGraph::Vertex::getMinimalEstimateData(double *) const
+  {
+    return false;
+  }
+
+  int OptimizableGraph::Vertex::minimalEstimateDimension() const
+  {
+    return -1;
+  }
+
+
+  OptimizableGraph::Edge::Edge() :
+    HyperGraph::Edge(),
+    _dimension(-1), _level(0), _robustKernel(0)
+  {
+  }
+
+  OptimizableGraph::Edge::~Edge()
+  {
+    delete _robustKernel;
+  }
+
+  OptimizableGraph* OptimizableGraph::Edge::graph(){
+    if (! _vertices.size())
+      return 0;
+    OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)_vertices[0];
+    if (!v)
+      return 0;
+    return v->graph();
+  }
+  
+  const OptimizableGraph* OptimizableGraph::Edge::graph() const{
+    if (! _vertices.size())
+      return 0;
+    const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) _vertices[0];
+    if (!v)
+      return 0;
+    return v->graph();
+  }
+
+  bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){
+    if ((int)_parameters.size()<=argNum)
+      return false;
+    if (argNum<0)
+      return false;
+    *_parameters[argNum] = 0;
+    _parameterIds[argNum] = paramId;
+    return true;
+  }
+
+  bool OptimizableGraph::Edge::resolveParameters() {
+    if (!graph()) {
+      cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl;
+      return false;
+    }
+    
+    assert (_parameters.size() == _parameterIds.size());
+    //cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl;
+    for (size_t i=0; i<_parameters.size(); i++){
+      int index = _parameterIds[i];
+      *_parameters[i] = graph()->parameter(index);
+      if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){
+        cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << endl;
+      }
+      if (!*_parameters[i]) {
+        cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << endl;
+        return false;
+      }
+    }
+    return true;
+  }
+
+  void OptimizableGraph::Edge::setRobustKernel(RobustKernel* ptr)
+  {
+    if (_robustKernel)
+      delete _robustKernel;
+    _robustKernel = ptr;
+  }
+
+  bool OptimizableGraph::Edge::resolveCaches() {
+    return true;
+  }
+
+  bool OptimizableGraph::Edge::setMeasurementData(const double *)
+  {
+    return false;
+  }
+
+  bool OptimizableGraph::Edge::getMeasurementData(double *) const
+  {
+    return false;
+  }
+
+  int OptimizableGraph::Edge::measurementDimension() const
+  {
+    return -1;
+  }
+
+  bool OptimizableGraph::Edge::setMeasurementFromState(){
+    return false;
+  }
+
+
+  OptimizableGraph::Edge* OptimizableGraph::Edge::clone() const
+  {
+    // TODO
+    return 0;
+  }
+
+
+  OptimizableGraph::OptimizableGraph()
+  {
+    _nextEdgeId = 0; _edge_has_id = false;
+    _graphActions.resize(AT_NUM_ELEMENTS);
+  }
+
+  OptimizableGraph::~OptimizableGraph()
+  {
+    clear();
+    clearParameters();
+  }
+
+  bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData)
+  {
+    Vertex* inserted = vertex(v->id());
+    if (inserted) {
+      cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl;
+      assert(0 && "Vertex with this ID already contained in the graph");
+      return false;
+    }
+    OptimizableGraph::Vertex* ov=dynamic_cast<OptimizableGraph::Vertex*>(v);
+    assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex");
+    if (ov->_graph != 0 && ov->_graph != this) {
+      cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl;
+      assert(0 && "Vertex already registered with another graph");
+      return false;
+    }
+    if (userData)
+      ov->setUserData(userData);
+    ov->_graph=this;
+    return HyperGraph::addVertex(v);
+  }
+
+  bool OptimizableGraph::addEdge(HyperGraph::Edge* e_)
+  {
+    OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(e_);
+    assert(e && "Edge does not inherit from OptimizableGraph::Edge");
+    if (! e)
+      return false;
+    bool eresult = HyperGraph::addEdge(e);
+    if (! eresult)
+      return false;
+    e->_internalId = _nextEdgeId++;
+    if (! e->resolveParameters()){
+      cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl;
+      return false;
+    }
+    if (! e->resolveCaches()){
+      cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl;
+      return false;
+    } 
+    _jacobianWorkspace.updateSize(e);
+
+    return true;
+  }
+
+  int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;}
+
+double OptimizableGraph::chi2() const
+{
+  double chi = 0.0;
+  for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); ++it) {
+    const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
+    chi += e->chi2();
+  }
+  return chi;
+}
+
+void OptimizableGraph::push()
+{
+  for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
+    v->push();
+  }
+}
+
+void OptimizableGraph::pop()
+{
+  for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
+    OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
+    v->pop();
+  }
+}
+
+void OptimizableGraph::discardTop()
+{
+  for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
+    OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
+    v->discardTop();
+  }
+}
+
+void OptimizableGraph::push(HyperGraph::VertexSet& vset)
+{
+  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+    v->push();
+  }
+}
+
+void OptimizableGraph::pop(HyperGraph::VertexSet& vset)
+{
+  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+    v->pop();
+  }
+}
+
+void OptimizableGraph::discardTop(HyperGraph::VertexSet& vset)
+{
+  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+    v->discardTop();
+  }
+}
+
+  void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed)
+{
+  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+    v->setFixed(fixed);
+  }
+}
+
+
+bool OptimizableGraph::load(istream& is, bool createEdges)
+{
+  // scna for the paramers in the whole file
+  if (!_parameters.read(is,&_renamedTypesLookup))
+    return false;
+#ifndef NDEBUG
+  cerr << "Loaded " << _parameters.size() << " parameters" << endl;
+#endif
+  is.clear();
+  is.seekg(ios_base::beg);
+  set<string> warnedUnknownTypes;
+  stringstream currentLine;
+  string token;
+
+  Factory* factory = Factory::instance();
+  HyperGraph::GraphElemBitset elemBitset;
+  elemBitset[HyperGraph::HGET_PARAMETER] = 1;
+  elemBitset.flip();
+
+  Vertex* previousVertex = 0;
+  Data* previousData = 0;
+
+  while (1) {
+    int bytesRead = readLine(is, currentLine);
+    if (bytesRead == -1)
+      break;
+    currentLine >> token;
+    //cerr << "Token=" << token << endl;
+    if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
+      continue;
+
+    // handle commands encoded in the file
+    bool handledCommand = false;
+    
+    if (token == "FIX") {
+      handledCommand = true;
+      int id;
+      while (currentLine >> id) {
+        OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(vertex(id));
+        if (v) {
+#        ifndef NDEBUG
+          cerr << "Fixing vertex " << v->id() << endl;
+#        endif
+          v->setFixed(true);
+        } else {
+          cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl;
+        }
+      }
+    }
+
+    if (handledCommand)
+      continue;
+     
+    // do the mapping to an internal type if it matches
+    if (_renamedTypesLookup.size() > 0) {
+      map<string, string>::const_iterator foundIt = _renamedTypesLookup.find(token);
+      if (foundIt != _renamedTypesLookup.end()) {
+        token = foundIt->second;
+      }
+    }
+
+    if (! factory->knowsTag(token)) {
+      if (warnedUnknownTypes.count(token) != 1) {
+        warnedUnknownTypes.insert(token);
+        cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl;
+      }
+      continue;
+    }
+
+    HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
+    if (dynamic_cast<Vertex*>(element)) { // it's a vertex type
+      //cerr << "it is a vertex" << endl;
+      previousData = 0;
+      Vertex* v = static_cast<Vertex*>(element);
+      int id;
+      currentLine >> id;
+      bool r = v->read(currentLine);
+      if (! r)
+        cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl;
+      v->setId(id);
+      if (!addVertex(v)) {
+        cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl;
+        delete v;
+      } else {
+        previousVertex = v;
+      }
+    }
+    else if (dynamic_cast<Edge*>(element)) {
+      //cerr << "it is an edge" << endl;
+      previousData = 0;
+      Edge* e = static_cast<Edge*>(element);
+      int numV = e->vertices().size();
+      if (_edge_has_id){
+        int id;
+        currentLine >> id;
+        e->setId(id);
+      }
+      //cerr << PVAR(token) << " " << PVAR(numV) << endl;
+      if (numV == 2) { // it's a pairwise / binary edge type which we handle in a special way
+        int id1, id2;
+        currentLine >> id1 >> id2;
+        Vertex* from = vertex(id1);
+        Vertex* to = vertex(id2);
+        int doInit=0;
+        if ((!from || !to) ) {
+          if (! createEdges) {
+            cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token << " " << id1 << " <-> " << id2 << endl;
+            delete e;
+          } else {
+            if (! from) {
+              from=e->createFrom();
+              from->setId(id1);
+              addVertex(from);
+              doInit=2;
+            }
+            if (! to) {
+              to=e->createTo();
+              to->setId(id2);
+              addVertex(to);
+              doInit=1;
+            }
+          }
+        }
+        if (from && to) {
+          e->setVertex(0, from);
+          e->setVertex(1, to);
+          e->read(currentLine);
+          if (!addEdge(e)) {
+            cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token << " " << id1 << " <-> " << id2 << endl;
+            delete e;
+          } else {
+            switch (doInit){
+              case 1: 
+                {
+                  HyperGraph::VertexSet fromSet;
+                  fromSet.insert(from);
+                  e->initialEstimate(fromSet, to);
+                  break;
+                }
+              case 2:
+                {
+                  HyperGraph::VertexSet toSet;
+                  toSet.insert(to);
+                  e->initialEstimate(toSet, from);
+                  break;
+                }
+              default:;
+            }
+          }
+        }
+      }
+      else {
+        vector<int> ids;
+        ids.resize(numV);
+        for (int l = 0; l < numV; ++l)
+          currentLine >> ids[l];
+        bool vertsOkay = true;
+        for (int l = 0; l < numV; ++l) {
+          e->setVertex(l, vertex(ids[l]));
+          if (e->vertex(l) == 0) {
+            vertsOkay = false;
+            break;
+          }
+        }
+        if (! vertsOkay) {
+          cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token;
+          for (int l = 0; l < numV; ++l) {
+            if (l > 0)
+              cerr << " <->";
+            cerr << " " << ids[l];
+          }
+          delete e;
+        } else {
+          bool r = e->read(currentLine);
+          if (!r || !addEdge(e)) {
+            cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token; 
+            for (int l = 0; l < numV; ++l) {
+              if (l > 0)
+                cerr << " <->";
+              cerr << " " << ids[l];
+            }
+            delete e;
+          }
+        }
+      }
+    } else if (dynamic_cast<Data*>(element)) { // reading in the data packet for the vertex
+      //cerr << "read data packet " << token << " vertex " << previousVertex->id() << endl;
+      Data* d = static_cast<Data*>(element);
+      bool r = d->read(currentLine);
+      if (! r) {
+	cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for vertex " << previousVertex->id() << endl;
+	delete d;
+	previousData = 0;
+      } else if (previousData){
+	//cerr << "chaining" << endl;
+	previousData->setNext(d);
+        previousData = d;
+	//cerr << "done" << endl;
+      } else if (previousVertex){
+	//cerr << "embedding in vertex" << endl;
+	previousVertex->setUserData(d);
+	previousData = d;
+	previousVertex = 0;
+	//cerr << "done" << endl;
+      } else {
+        cerr << __PRETTY_FUNCTION__ << ": got data element, but no vertex available" << endl;
+        delete d;
+	previousData = 0;
+      }
+    }
+  } // while read line
+  
+  return true;
+}
+
+bool OptimizableGraph::load(const char* filename, bool createEdges)
+{
+  ifstream ifs(filename);
+  if (!ifs) {
+    cerr << __PRETTY_FUNCTION__ << " unable to open file " << filename << endl;
+    return false;
+  }
+  return load(ifs, createEdges);
+}
+
+bool OptimizableGraph::save(const char* filename, int level) const
+{
+  ofstream ofs(filename);
+  if (!ofs)
+    return false;
+  return save(ofs, level);
+}
+
+bool OptimizableGraph::save(ostream& os, int level) const
+{
+  if (! _parameters.write(os))
+    return false;
+  set<Vertex*, VertexIDCompare> verticesToSave;
+  for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+    if (e->level() == level) {
+      for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+        verticesToSave.insert(static_cast<OptimizableGraph::Vertex*>(*it));
+      }
+    }
+  }
+
+  for (set<Vertex*, VertexIDCompare>::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){
+    OptimizableGraph::Vertex* v = *it;
+    saveVertex(os, v);
+  }
+
+  EdgeContainer edgesToSave;
+  for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+    const OptimizableGraph::Edge* e = dynamic_cast<const OptimizableGraph::Edge*>(*it);
+    if (e->level() == level)
+      edgesToSave.push_back(const_cast<Edge*>(e));
+  }
+  sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare());
+
+  for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) {
+    OptimizableGraph::Edge* e = *it;
+    saveEdge(os, e);
+  }
+
+  return os.good();
+}
+
+
+bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::VertexSet& vset, int level)
+{
+  if (! _parameters.write(os))
+    return false;
+
+  for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){
+    OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
+    saveVertex(os, v);
+  }
+  for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+    OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
+    if (e->level() != level)
+      continue;
+
+    bool verticesInEdge = true;
+    for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+      if (vset.find(*it) == vset.end()) {
+        verticesInEdge = false;
+        break;
+      }
+    }
+    if (! verticesInEdge)
+      continue;
+
+    saveEdge(os, e);
+  }
+
+  return os.good();
+}
+
+bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset)
+{
+  if (!_parameters.write(os))
+    return false;
+  std::set<OptimizableGraph::Vertex*> vset;
+  for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
+    HyperGraph::Edge* e = *it;
+    for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+      vset.insert(v);
+    }
+  }
+
+  for (std::set<OptimizableGraph::Vertex*>::const_iterator it=vset.begin(); it!=vset.end(); ++it){
+    OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
+    saveVertex(os, v);
+  }
+
+  for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
+    OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
+    saveEdge(os, e);
+  }
+
+  return os.good();
+}
+  
+void OptimizableGraph::addGraph(OptimizableGraph* g){
+  for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){
+    OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second);
+    if (vertex(v->id()))
+      continue;
+    OptimizableGraph::Vertex* v2=v->clone();
+    v2->edges().clear();
+    v2->setHessianIndex(-1);
+    addVertex(v2);
+  }
+  for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){
+    OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it);
+    OptimizableGraph::Edge* en = e->clone();
+    en->resize(e->vertices().size());
+    int cnt = 0;
+    for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+      OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) vertex((*it)->id());
+      assert(v);
+      en->setVertex(cnt++, v);
+    }
+    addEdge(en);
+  }
+}
+
+int OptimizableGraph::maxDimension() const{
+  int maxDim=0;
+  for (HyperGraph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); ++it){
+    const OptimizableGraph::Vertex* v= static_cast< const OptimizableGraph::Vertex*>(it->second);
+    maxDim = (std::max)(maxDim, v->dimension());
+  }
+  return maxDim;
+}
+
+void OptimizableGraph::setRenamedTypesFromString(const std::string& types)
+{
+  Factory* factory = Factory::instance();
+  vector<string> typesMap = strSplit(types, ",");
+  for (size_t i = 0; i < typesMap.size(); ++i) {
+    vector<string> m = strSplit(typesMap[i], "=");
+    if (m.size() != 2) {
+      cerr << __PRETTY_FUNCTION__ << ": unable to extract type map from " << typesMap[i] << endl;
+      continue;
+    }
+    string typeInFile = trim(m[0]);
+    string loadedType = trim(m[1]);
+    if (! factory->knowsTag(loadedType)) {
+      cerr << __PRETTY_FUNCTION__ << ": unknown type " << loadedType << endl;
+      continue;
+    }
+
+    _renamedTypesLookup[typeInFile] = loadedType;
+  }
+
+  cerr << "# load look up table" << endl;
+  for (std::map<std::string, std::string>::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) {
+    cerr << "#\t" << it->first << " -> " << it->second << endl;
+  }
+}
+
+bool OptimizableGraph::isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims_) const
+{
+  std::set<int> auxDims;
+  if (vertDims_.size() == 0) {
+    auxDims = dimensions();
+  }
+  const set<int>& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;
+  bool suitableSolver = true;
+  if (vertDims.size() == 2) {
+    if (solverProperty.requiresMarginalize) {
+      suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1;
+    }
+    else {
+      suitableSolver = solverProperty.poseDim == -1;
+    }
+  } else if (vertDims.size() == 1) {
+    suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1;
+  } else {
+    suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize;
+  }
+  return suitableSolver;
+}
+
+std::set<int> OptimizableGraph::dimensions() const
+{
+  std::set<int> auxDims;
+  for (VertexIDMap::const_iterator it = vertices().begin(); it != vertices().end(); ++it) {
+    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
+    auxDims.insert(v->dimension());
+  }
+  return auxDims;
+}
+
+void OptimizableGraph::preIteration(int iter)
+{
+  HyperGraphActionSet& actions = _graphActions[AT_PREITERATION];
+  if (actions.size() > 0) {
+    HyperGraphAction::ParametersIteration params(iter);
+    for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
+      (*(*it))(this, &params);
+    }
+  }
+}
+
+void OptimizableGraph::postIteration(int iter)
+{
+  HyperGraphActionSet& actions = _graphActions[AT_POSTITERATION];
+  if (actions.size() > 0) {
+    HyperGraphAction::ParametersIteration params(iter);
+    for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
+      (*(*it))(this, &params);
+    }
+  }
+}
+
+bool OptimizableGraph::addPostIterationAction(HyperGraphAction* action)
+{
+  std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_POSTITERATION].insert(action);
+  return insertResult.second;
+}
+
+bool OptimizableGraph::addPreIterationAction(HyperGraphAction* action)
+{
+  std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_PREITERATION].insert(action);
+  return insertResult.second;
+}
+
+bool OptimizableGraph::removePreIterationAction(HyperGraphAction* action)
+{
+  return _graphActions[AT_PREITERATION].erase(action) > 0;
+}
+
+bool OptimizableGraph::removePostIterationAction(HyperGraphAction* action)
+{
+  return _graphActions[AT_POSTITERATION].erase(action) > 0;
+}
+
+bool OptimizableGraph::saveVertex(std::ostream& os, OptimizableGraph::Vertex* v) const
+{
+  Factory* factory = Factory::instance();
+  string tag = factory->tag(v);
+  if (tag.size() > 0) {
+    os << tag << " " << v->id() << " ";
+    v->write(os);
+    os << endl;
+    Data* d=v->userData();
+    while (d) { // write the data packet for the vertex
+      tag = factory->tag(d);
+      if (tag.size() > 0) {
+        os << tag << " ";
+        d->write(os);
+        os << endl;
+      }
+      d=d->next();
+    }
+    if (v->fixed()) {
+      os << "FIX " << v->id() << endl;
+    }
+    return os.good();
+  }
+  return false;
+}
+
+bool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) const
+{
+  Factory* factory = Factory::instance();
+  string tag = factory->tag(e);
+  if (tag.size() > 0) {
+    os << tag << " ";
+    if (_edge_has_id)
+      os << e->id() << " ";
+    for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+      os << v->id() << " ";
+    }
+    e->write(os);
+    os << endl;
+    return os.good();
+  }
+  return false;
+}
+
+void OptimizableGraph::clearParameters()
+{
+  _parameters.clear();
+}
+
+bool OptimizableGraph::verifyInformationMatrices(bool verbose) const
+{
+  bool allEdgeOk = true;
+  SelfAdjointEigenSolver<MatrixXd> eigenSolver;
+  for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+    Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension());
+    // test on symmetry
+    bool isSymmetric = information.transpose() == information;
+    bool okay = isSymmetric;
+    if (isSymmetric) {
+      // compute the eigenvalues
+      eigenSolver.compute(information, Eigen::EigenvaluesOnly);
+      bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;
+      okay = okay && isSPD;
+    }
+    allEdgeOk = allEdgeOk && okay;
+    if (! okay) {
+      if (verbose) {
+        if (! isSymmetric)
+          cerr << "Information Matrix for an edge is not symmetric:";
+        else
+          cerr << "Information Matrix for an edge is not SPD:";
+        for (size_t i = 0; i < e->vertices().size(); ++i)
+          cerr << " " << e->vertex(i)->id();
+        if (isSymmetric)
+          cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose();
+        cerr << endl;
+      }
+    }
+  }
+  return allEdgeOk;
+}
+
+bool OptimizableGraph::initMultiThreading()
+{
+# if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0)
+  Eigen::initParallel();
+# endif
+  return true;
+}
+
+} // end namespace
+

Thirdparty/g2o/g2o/core/optimizable_graph.h

@@ -0,0 +1,688 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_AIS_OPTIMIZABLE_GRAPH_HH_
+#define G2O_AIS_OPTIMIZABLE_GRAPH_HH_
+
+#include <set>
+#include <iostream>
+#include <list>
+#include <limits>
+#include <cmath>
+#include <typeinfo>
+
+#include "openmp_mutex.h"
+#include "hyper_graph.h"
+#include "parameter.h"
+#include "parameter_container.h"
+#include "jacobian_workspace.h"
+
+#include "../stuff/macros.h"
+
+namespace g2o {
+
+  class HyperGraphAction;
+  struct OptimizationAlgorithmProperty;
+  class Cache;
+  class CacheContainer;
+  class RobustKernel;
+
+  /**
+     @addtogroup g2o
+   */
+  /**
+     This is an abstract class that represents one optimization
+     problem.  It specializes the general graph to contain special
+     vertices and edges.  The vertices represent parameters that can
+     be optimized, while the edges represent constraints.  This class
+     also provides basic functionalities to handle the backup/restore
+     of portions of the vertices.
+   */
+  struct  OptimizableGraph : public HyperGraph {
+
+    enum ActionType {
+      AT_PREITERATION, AT_POSTITERATION,
+      AT_NUM_ELEMENTS, // keep as last element
+    };
+
+    typedef std::set<HyperGraphAction*>    HyperGraphActionSet;
+
+    // forward declarations
+    class  Vertex;
+    class  Edge;
+
+    /**
+     * \brief data packet for a vertex. Extend this class to store in the vertices
+     * the potential additional information you need (e.g. images, laser scans, ...).
+     */
+    class  Data : public HyperGraph::HyperGraphElement
+    {
+      friend struct OptimizableGraph;
+      public:
+        virtual ~Data();
+        Data();
+        //! read the data from a stream
+        virtual bool read(std::istream& is) = 0;
+        //! write the data to a stream
+        virtual bool write(std::ostream& os) const = 0;
+        virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;}
+        const Data* next() const {return _next;}
+        Data* next() {return _next;}
+        void setNext(Data* next_) { _next = next_; }
+      protected:
+        Data* _next; // linked list of multiple data;
+    };
+
+    /**
+     * \brief order vertices based on their ID
+     */
+    struct  VertexIDCompare {
+      bool operator() (const Vertex* v1, const Vertex* v2) const
+      {
+        return v1->id() < v2->id();
+      }
+    };
+
+    /**
+     * \brief order edges based on the internal ID, which is assigned to the edge in addEdge()
+     */
+    struct  EdgeIDCompare {
+      bool operator() (const Edge* e1, const Edge* e2) const
+      {
+        return e1->internalId() < e2->internalId();
+      }
+    };
+
+    //! vector container for vertices
+    typedef std::vector<OptimizableGraph::Vertex*>      VertexContainer;
+    //! vector container for edges
+    typedef std::vector<OptimizableGraph::Edge*>        EdgeContainer;
+
+    /**
+     * \brief A general case Vertex for optimization
+     */
+    class  Vertex : public HyperGraph::Vertex {
+      private:
+        friend struct OptimizableGraph;
+      public:
+        Vertex();
+
+        //! returns a deep copy of the current vertex
+        virtual Vertex* clone() const ;
+
+        //! the user data associated with this vertex
+        const Data* userData() const { return _userData; }
+        Data* userData() { return _userData; }
+
+        void setUserData(Data* obs) { _userData = obs;}
+	void addUserData(Data* obs) { 
+	  if (obs) {
+	    obs->setNext(_userData);
+	    _userData=obs;
+	  }
+	}
+	
+        virtual ~Vertex();
+
+        //! sets the node to the origin (used in the multilevel stuff)
+        void setToOrigin() { setToOriginImpl(); updateCache();}
+
+        //! get the element from the hessian matrix
+        virtual const double& hessian(int i, int j) const = 0;
+        virtual double& hessian(int i, int j) = 0;
+        virtual double hessianDeterminant() const = 0;
+        virtual double* hessianData() = 0;
+
+        /** maps the internal matrix to some external memory location */
+        virtual void mapHessianMemory(double* d) = 0;
+
+        /**
+         * copies the b vector in the array b_
+         * @return the number of elements copied
+         */
+        virtual int copyB(double* b_) const = 0;
+
+        //! get the b vector element
+        virtual const double& b(int i) const = 0;
+        virtual double& b(int i) = 0;
+        //! return a pointer to the b vector associated with this vertex
+        virtual double* bData() = 0;
+
+        /**
+         * set the b vector part of this vertex to zero
+         */
+        virtual void clearQuadraticForm() = 0;
+
+        /**
+         * updates the current vertex with the direct solution x += H_ii\b_ii
+         * @return the determinant of the inverted hessian
+         */
+        virtual double solveDirect(double lambda=0) = 0;
+
+        /**
+         * sets the initial estimate from an array of double
+         * Implement setEstimateDataImpl()
+         * @return true on success
+         */
+        bool setEstimateData(const double* estimate);
+
+        /**
+         * sets the initial estimate from an array of double
+         * Implement setEstimateDataImpl()
+         * @return true on success
+         */
+        bool setEstimateData(const std::vector<double>& estimate) { 
+#ifndef NDEBUG
+          int dim = estimateDimension();
+          assert((dim == -1) || (estimate.size() == std::size_t(dim)));
+#endif
+          return setEstimateData(&estimate[0]);
+        };
+
+        /**
+         * writes the estimater to an array of double
+         * @returns true on success
+         */
+        virtual bool getEstimateData(double* estimate) const;
+
+        /**
+         * writes the estimater to an array of double
+         * @returns true on success
+         */
+        virtual bool getEstimateData(std::vector<double>& estimate) const {
+          int dim = estimateDimension();
+          if (dim < 0)
+            return false;
+          estimate.resize(dim);
+          return getEstimateData(&estimate[0]);
+        };
+
+        /**
+         * returns the dimension of the extended representation used by get/setEstimate(double*)
+         * -1 if it is not supported
+         */
+        virtual int estimateDimension() const;
+
+        /**
+         * sets the initial estimate from an array of double.
+         * Implement setMinimalEstimateDataImpl()
+         * @return true on success
+         */
+        bool setMinimalEstimateData(const double* estimate);
+
+        /**
+         * sets the initial estimate from an array of double.
+         * Implement setMinimalEstimateDataImpl()
+         * @return true on success
+         */
+        bool setMinimalEstimateData(const std::vector<double>& estimate) {
+#ifndef NDEBUG
+          int dim = minimalEstimateDimension();
+          assert((dim == -1) || (estimate.size() == std::size_t(dim)));
+#endif
+          return setMinimalEstimateData(&estimate[0]);
+        };
+
+        /**
+         * writes the estimate to an array of double
+         * @returns true on success
+         */
+        virtual bool getMinimalEstimateData(double* estimate) const ;
+
+        /**
+         * writes the estimate to an array of double
+         * @returns true on success
+         */
+        virtual bool getMinimalEstimateData(std::vector<double>& estimate) const {
+          int dim = minimalEstimateDimension();
+          if (dim < 0)
+            return false;
+          estimate.resize(dim);
+          return getMinimalEstimateData(&estimate[0]);
+        };
+
+        /**
+         * returns the dimension of the extended representation used by get/setEstimate(double*)
+         * -1 if it is not supported
+         */
+        virtual int minimalEstimateDimension() const;
+
+        //! backup the position of the vertex to a stack
+        virtual void push() = 0;
+
+        //! restore the position of the vertex by retrieving the position from the stack
+        virtual void pop() = 0;
+
+        //! pop the last element from the stack, without restoring the current estimate
+        virtual void discardTop() = 0;
+
+        //! return the stack size
+        virtual int stackSize() const = 0;
+
+        /**
+         * Update the position of the node from the parameters in v.
+         * Depends on the implementation of oplusImpl in derived classes to actually carry
+         * out the update.
+         * Will also call updateCache() to update the caches of depending on the vertex.
+         */
+        void oplus(const double* v)
+        {
+          oplusImpl(v);
+          updateCache();
+        }
+
+        //! temporary index of this node in the parameter vector obtained from linearization
+        int hessianIndex() const { return _hessianIndex;}
+        int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex();}
+        //! set the temporary index of the vertex in the parameter blocks
+        void setHessianIndex(int ti) { _hessianIndex = ti;}
+        void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti);}
+
+        //! true => this node is fixed during the optimization
+        bool fixed() const {return _fixed;}
+        //! true => this node should be considered fixed during the optimization
+        void setFixed(bool fixed) { _fixed = fixed;}
+
+        //! true => this node is marginalized out during the optimization
+        bool marginalized() const {return _marginalized;}
+        //! true => this node should be marginalized out during the optimization
+        void setMarginalized(bool marginalized) { _marginalized = marginalized;}
+
+        //! dimension of the estimated state belonging to this node
+        int dimension() const { return _dimension;}
+
+        //! sets the id of the node in the graph be sure that the graph keeps consistent after changing the id
+        virtual void setId(int id) {_id = id;}
+
+        //! set the row of this vertex in the Hessian
+        void setColInHessian(int c) { _colInHessian = c;}
+        //! get the row of this vertex in the Hessian
+        int colInHessian() const {return _colInHessian;}
+
+        const OptimizableGraph* graph() const {return _graph;}
+
+        OptimizableGraph* graph() {return _graph;}
+
+        /**
+         * lock for the block of the hessian and the b vector associated with this vertex, to avoid
+         * race-conditions if multi-threaded.
+         */
+        void lockQuadraticForm() { _quadraticFormMutex.lock();}
+        /**
+         * unlock the block of the hessian and the b vector associated with this vertex
+         */
+        void unlockQuadraticForm() { _quadraticFormMutex.unlock();}
+
+        //! read the vertex from a stream, i.e., the internal state of the vertex
+        virtual bool read(std::istream& is) = 0;
+        //! write the vertex to a stream
+        virtual bool write(std::ostream& os) const = 0;
+
+        virtual void updateCache();
+
+        CacheContainer* cacheContainer();
+      protected:
+        OptimizableGraph* _graph;
+        Data* _userData;
+        int _hessianIndex;
+        bool _fixed;
+        bool _marginalized;
+        int _dimension;
+        int _colInHessian;
+        OpenMPMutex _quadraticFormMutex;
+
+        CacheContainer* _cacheContainer;
+
+        /**
+         * update the position of the node from the parameters in v.
+         * Implement in your class!
+         */
+        virtual void oplusImpl(const double* v) = 0;
+
+        //! sets the node to the origin (used in the multilevel stuff)
+        virtual void setToOriginImpl() = 0;
+
+        /**
+         * writes the estimater to an array of double
+         * @returns true on success
+         */
+        virtual bool setEstimateDataImpl(const double* ) { return false;}
+
+        /**
+         * sets the initial estimate from an array of double
+         * @return true on success
+         */
+        virtual bool setMinimalEstimateDataImpl(const double* ) { return false;}
+
+    };
+    
+    class  Edge: public HyperGraph::Edge {
+      private:
+        friend struct OptimizableGraph;
+      public:
+        Edge();
+        virtual ~Edge();
+        virtual Edge* clone() const;
+
+        // indicates if all vertices are fixed
+        virtual bool allVerticesFixed() const = 0;
+        
+        // computes the error of the edge and stores it in an internal structure
+        virtual void computeError() = 0;
+
+        //! sets the measurement from an array of double
+        //! @returns true on success
+        virtual bool setMeasurementData(const double* m);
+
+        //! writes the measurement to an array of double
+        //! @returns true on success
+        virtual bool getMeasurementData(double* m) const;
+
+        //! returns the dimension of the measurement in the extended representation which is used
+        //! by get/setMeasurement;
+        virtual int measurementDimension() const;
+
+        /**
+         * sets the estimate to have a zero error, based on the current value of the state variables
+         * returns false if not supported.
+         */
+        virtual bool setMeasurementFromState();
+
+        //! if NOT NULL, error of this edge will be robustifed with the kernel
+        RobustKernel* robustKernel() const { return _robustKernel;}
+        /**
+         * specify the robust kernel to be used in this edge
+         */
+        void setRobustKernel(RobustKernel* ptr);
+
+        //! returns the error vector cached after calling the computeError;
+        virtual const double* errorData() const = 0;
+        virtual double* errorData() = 0;
+
+        //! returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd>
+        virtual const double* informationData() const = 0;
+        virtual double* informationData() = 0;
+
+        //! computes the chi2 based on the cached error value, only valid after computeError has been called.
+        virtual double chi2() const = 0;
+
+        /**
+         * Linearizes the constraint in the edge.
+         * Makes side effect on the vertices of the graph by changing
+         * the parameter vector b and the hessian blocks ii and jj.
+         * The off diagoinal block is accesed via _hessian.
+         */
+        virtual void constructQuadraticForm() = 0;
+
+        /**
+         * maps the internal matrix to some external memory location,
+         * you need to provide the memory before calling constructQuadraticForm
+         * @param d the memory location to which we map
+         * @param i index of the vertex i
+         * @param j index of the vertex j (j > i, upper triangular fashion)
+         * @param rowMajor if true, will write in rowMajor order to the block. Since EIGEN is columnMajor by default, this results in writing the transposed
+         */
+        virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0;
+
+        /**
+         * Linearizes the constraint in the edge in the manifold space, and store
+         * the result in the given workspace
+         */
+        virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0;
+
+        /** set the estimate of the to vertex, based on the estimate of the from vertices in the edge. */
+        virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) = 0;
+
+        /**
+         * override in your class if it's possible to initialize the vertices in certain combinations.
+         * The return value may correspond to the cost for initiliaizng the vertex but should be positive if
+         * the initialization is possible and negative if not possible.
+         */
+        virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) from; (void) to; return -1.;}
+
+        //! returns the level of the edge
+        int level() const { return _level;}
+        //! sets the level of the edge
+        void setLevel(int l) { _level=l;}
+
+        //! returns the dimensions of the error function
+        int dimension() const { return _dimension;}
+
+        virtual Vertex* createFrom() {return 0;}
+        virtual Vertex* createTo()   {return 0;}
+
+        //! read the vertex from a stream, i.e., the internal state of the vertex
+        virtual bool read(std::istream& is) = 0;
+        //! write the vertex to a stream
+        virtual bool write(std::ostream& os) const = 0;
+
+        //! the internal ID of the edge
+        long long internalId() const { return _internalId;}
+
+        OptimizableGraph* graph();
+        const OptimizableGraph* graph() const;
+
+        bool setParameterId(int argNum, int paramId);
+        inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);}
+        inline size_t numParameters() const {return _parameters.size();}
+        inline void resizeParameters(size_t newSize) {
+          _parameters.resize(newSize, 0); 
+          _parameterIds.resize(newSize, -1);
+          _parameterTypes.resize(newSize, typeid(void*).name());
+        }
+      protected:
+        int _dimension;
+        int _level;
+        RobustKernel* _robustKernel;
+        long long _internalId;
+
+        std::vector<int> _cacheIds;
+
+        template <typename ParameterType>
+          bool installParameter(ParameterType*& p, size_t argNo, int paramId=-1){
+            if (argNo>=_parameters.size())
+              return false;
+            _parameterIds[argNo] = paramId;
+            _parameters[argNo] = (Parameter**)&p;
+            _parameterTypes[argNo] = typeid(ParameterType).name();
+            return true;
+          }
+
+        template <typename CacheType>
+          void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, 
+              const std::string& _type, 
+              const ParameterVector& parameters);
+
+        bool resolveParameters();
+        virtual bool resolveCaches();
+
+        std::vector<std::string> _parameterTypes;
+        std::vector<Parameter**> _parameters;
+        std::vector<int> _parameterIds;
+    };
+
+    //! returns the vertex number <i>id</i> appropriately casted
+    inline Vertex* vertex(int id) { return reinterpret_cast<Vertex*>(HyperGraph::vertex(id));}
+
+    //! returns the vertex number <i>id</i> appropriately casted
+    inline const Vertex* vertex (int id) const{ return reinterpret_cast<const Vertex*>(HyperGraph::vertex(id));}
+
+    //! empty constructor
+    OptimizableGraph();
+    virtual ~OptimizableGraph();
+
+    //! adds all edges and vertices of the graph <i>g</i> to this graph.
+    void addGraph(OptimizableGraph* g);
+ 
+    /**
+     * adds a new vertex. The new vertex is then "taken".
+     * @return false if a vertex with the same id as v is already in the graph, true otherwise.
+     */
+    virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);
+    virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);}
+
+    /**
+     * adds a new edge.
+     * The edge should point to the vertices that it is connecting (setFrom/setTo).
+     * @return false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise.
+     */
+    virtual bool addEdge(HyperGraph::Edge* e);
+
+    //! returns the chi2 of the current configuration
+    double chi2() const;
+
+    //! return the maximum dimension of all vertices in the graph
+    int maxDimension() const;
+
+    /**
+     * iterates over all vertices and returns a set of all the vertex dimensions in the graph
+     */
+    std::set<int> dimensions() const;
+
+    /**
+     * carry out n iterations
+     * @return the number of performed iterations
+     */
+    virtual int optimize(int iterations, bool online=false);
+
+    //! called at the beginning of an iteration (argument is the number of the iteration)
+    virtual void preIteration(int);
+    //! called at the end of an iteration (argument is the number of the iteration)
+    virtual void postIteration(int);
+
+    //! add an action to be executed before each iteration
+    bool addPreIterationAction(HyperGraphAction* action);
+    //! add an action to be executed after each iteration
+    bool addPostIterationAction(HyperGraphAction* action);
+
+    //! remove an action that should no longer be execured before each iteration
+    bool removePreIterationAction(HyperGraphAction* action);
+    //! remove an action that should no longer be execured after each iteration
+    bool removePostIterationAction(HyperGraphAction* action);
+
+    //! push the estimate of all variables onto a stack
+    virtual void push();
+    //! pop (restore) the estimate of all variables from the stack
+    virtual void pop();
+    //! discard the last backup of the estimate for all variables by removing it from the stack
+    virtual void discardTop();
+
+    //! load the graph from a stream. Uses the Factory singleton for creating the vertices and edges.
+    virtual bool load(std::istream& is, bool createEdges=true);
+    bool load(const char* filename, bool createEdges=true);
+    //! save the graph to a stream. Again uses the Factory system.
+    virtual bool save(std::ostream& os, int level = 0) const;
+    //! function provided for convenience, see save() above
+    bool save(const char* filename, int level = 0) const;
+
+    
+    //! save a subgraph to a stream. Again uses the Factory system.
+    bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0);
+
+    //! save a subgraph to a stream. Again uses the Factory system.
+    bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset);
+
+    //! push the estimate of a subset of the variables onto a stack
+    virtual void push(HyperGraph::VertexSet& vset);
+    //! pop (restore) the estimate a subset of the variables from the stack
+    virtual void pop(HyperGraph::VertexSet& vset);
+    //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate
+    virtual void discardTop(HyperGraph::VertexSet& vset);
+
+    //! fixes/releases a set of vertices
+    virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed);
+
+    /**
+     * set the renamed types lookup from a string, format is for example:
+     * VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP
+     * This will change the occurance of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP
+     */
+    void setRenamedTypesFromString(const std::string& types);
+
+    /**
+     * test whether a solver is suitable for optimizing this graph.
+     * @param solverProperty the solver property to evaluate.
+     * @param vertDims should equal to the set returned by dimensions() to avoid re-evaluating.
+     */
+    bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims = std::set<int>()) const;
+
+    /**
+     * remove the parameters of the graph
+     */
+    virtual void clearParameters();
+
+    bool addParameter(Parameter* p) {
+      return _parameters.addParameter(p);
+    }
+
+    Parameter* parameter(int id) {
+      return _parameters.getParameter(id);
+    }
+
+    /**
+     * verify that all the information of the edges are semi positive definite, i.e.,
+     * all Eigenvalues are >= 0.
+     * @param verbose output edges with not PSD information matrix on cerr
+     * @return true if all edges have PSD information matrix
+     */
+    bool verifyInformationMatrices(bool verbose = false) const;
+
+    // helper functions to save an individual vertex
+    bool saveVertex(std::ostream& os, Vertex* v) const;
+
+    // helper functions to save an individual edge
+    bool saveEdge(std::ostream& os, Edge* e) const;
+    //! the workspace for storing the Jacobians of the graph
+    JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace;}
+    const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace;}
+
+    /**
+     * Eigen starting from version 3.1 requires that we call an initialize
+     * function, if we perform calls to Eigen from several threads.
+     * Currently, this function calls Eigen::initParallel if g2o is compiled
+     * with OpenMP support and Eigen's version is at least 3.1
+     */
+    static bool initMultiThreading();
+
+  protected:
+    std::map<std::string, std::string> _renamedTypesLookup;
+    long long _nextEdgeId;
+    std::vector<HyperGraphActionSet> _graphActions;
+
+    // do not watch this. To be removed soon, or integrated in a nice way
+    bool _edge_has_id;
+
+    ParameterContainer _parameters;
+    JacobianWorkspace _jacobianWorkspace;
+  };
+  
+  /**
+    @}
+   */
+  
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/optimization_algorithm.cpp

@@ -0,0 +1,62 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm.h"
+
+using namespace std;
+
+namespace g2o {
+
+OptimizationAlgorithm::OptimizationAlgorithm() :
+  _optimizer(0)
+{
+}
+
+OptimizationAlgorithm::~OptimizationAlgorithm()
+{
+}
+
+void OptimizationAlgorithm::printProperties(std::ostream& os) const
+{
+  os << "------------- Algorithm Properties -------------"  << endl;
+  for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) {
+    BaseProperty* p = it->second;
+    os << it->first << "\t" << p->toString() << endl;
+  }
+  os << "------------------------------------------------" << endl;
+}
+
+bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString)
+{
+  return _properties.updateMapFromString(propString);
+}
+
+void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer)
+{
+  _optimizer = optimizer;
+}
+
+} // end namespace

Thirdparty/g2o/g2o/core/optimization_algorithm.h

@@ -0,0 +1,117 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_H
+#define G2O_OPTIMIZATION_ALGORITHM_H
+
+#include <vector>
+#include <utility>
+#include <iosfwd>
+
+#include "../stuff/property.h"
+
+#include "hyper_graph.h"
+#include "sparse_block_matrix.h"
+
+namespace g2o {
+
+  class SparseOptimizer;
+
+  /**
+   * \brief Generic interface for a non-linear solver operating on a graph
+   */
+  class  OptimizationAlgorithm
+  {
+    public:
+      enum  SolverResult {Terminate=2, OK=1, Fail=-1};
+      OptimizationAlgorithm();
+      virtual ~OptimizationAlgorithm();
+
+      /**
+       * initialize the solver, called once before the first call to solve()
+       */
+      virtual bool init(bool online = false) = 0;
+
+      /**
+       * Solve one iteration. The SparseOptimizer running on-top will call this
+       * for the given number of iterations.
+       * @param iteration indicates the current iteration
+       */
+      virtual SolverResult solve(int iteration, bool online = false) = 0;
+
+      /**
+       * computes the block diagonal elements of the pattern specified in the input
+       * and stores them in given SparseBlockMatrix.
+       * If your solver does not support computing the marginals, return false.
+       */
+      virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices) = 0;
+
+      /**
+       * update the structures for online processing
+       */
+      virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges) = 0;
+
+      /**
+       * called by the optimizer if verbose. re-implement, if you want to print something
+       */
+      virtual void printVerbose(std::ostream& os) const {(void) os;};
+
+    public:
+      //! return the optimizer operating on
+      const SparseOptimizer* optimizer() const { return _optimizer;}
+      SparseOptimizer* optimizer() { return _optimizer;}
+
+      /**
+       * specify on which optimizer the solver should work on
+       */
+      void setOptimizer(SparseOptimizer* optimizer);
+
+      //! return the properties of the solver
+      const PropertyMap& properties() const { return _properties;}
+
+      /**
+       * update the properties from a string, see PropertyMap::updateMapFromString()
+       */
+      bool updatePropertiesFromString(const std::string& propString);
+      
+      /**
+       * print the properties to a stream in a human readable fashion
+       */
+      void printProperties(std::ostream& os) const;
+
+    protected:
+      SparseOptimizer* _optimizer;   ///< the optimizer the solver is working on
+      PropertyMap _properties;       ///< the properties of your solver, use this to store the parameters of your solver
+
+    private:
+      // Disable the copy constructor and assignment operator
+      OptimizationAlgorithm(const OptimizationAlgorithm&) { }
+      OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; }
+  };
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp

@@ -0,0 +1,229 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_dogleg.h"
+
+#include <iostream>
+
+#include "../stuff/timeutil.h"
+
+#include "block_solver.h"
+#include "sparse_optimizer.h"
+#include "solver.h"
+#include "batch_stats.h"
+using namespace std;
+
+namespace g2o {
+
+  OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(BlockSolverBase* solver) :
+    OptimizationAlgorithmWithHessian(solver)
+  {
+    _userDeltaInit = _properties.makeProperty<Property<double> >("initialDelta", 1e4);
+    _maxTrialsAfterFailure = _properties.makeProperty<Property<int> >("maxTrialsAfterFailure", 100);
+    _initialLambda = _properties.makeProperty<Property<double> >("initialLambda", 1e-7);
+    _lamdbaFactor = _properties.makeProperty<Property<double> >("lambdaFactor", 10.);
+    _delta = _userDeltaInit->value();
+    _lastStep = STEP_UNDEFINED;
+    _wasPDInAllIterations = true;
+  }
+
+  OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg()
+  {
+  }
+
+  OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(int iteration, bool online)
+  {
+    assert(_optimizer && "_optimizer not set");
+    assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
+    assert(dynamic_cast<BlockSolverBase*>(_solver) && "underlying linear solver is not a block solver");
+
+    BlockSolverBase* blockSolver = static_cast<BlockSolverBase*>(_solver);
+
+    if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
+      bool ok = _solver->buildStructure();
+      if (! ok) {
+        cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
+        return OptimizationAlgorithm::Fail;
+      }
+
+      // init some members to the current size of the problem
+      _hsd.resize(_solver->vectorSize());
+      _hdl.resize(_solver->vectorSize());
+      _auxVector.resize(_solver->vectorSize());
+      _delta = _userDeltaInit->value();
+      _currentLambda = _initialLambda->value();
+      _wasPDInAllIterations = true;
+    }
+
+    double t=get_monotonic_time();
+    _optimizer->computeActiveErrors();
+    G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+    if (globalStats) {
+      globalStats->timeResiduals = get_monotonic_time()-t;
+      t=get_monotonic_time();
+    }
+
+    double currentChi = _optimizer->activeRobustChi2();
+
+    _solver->buildSystem();
+    if (globalStats) {
+      globalStats->timeQuadraticForm = get_monotonic_time()-t;
+    }
+
+    Eigen::VectorXd::ConstMapType b(_solver->b(), _solver->vectorSize());
+
+    // compute alpha
+    _auxVector.setZero();
+    blockSolver->multiplyHessian(_auxVector.data(), _solver->b());
+    double bNormSquared = b.squaredNorm();
+    double alpha = bNormSquared / _auxVector.dot(b);
+
+    _hsd = alpha * b;
+    double hsdNorm = _hsd.norm();
+    double hgnNorm = -1.;
+
+    bool solvedGaussNewton = false;
+    bool goodStep = false;
+    int& numTries = _lastNumTries;
+    numTries = 0;
+    do {
+      ++numTries;
+
+      if (! solvedGaussNewton) {
+        const double minLambda = 1e-12;
+        const double maxLambda = 1e3;
+        solvedGaussNewton = true;
+        // apply a damping factor to enforce positive definite Hessian, if the matrix appeared
+        // to be not positive definite in at least one iteration before.
+        // We apply a damping factor to obtain a PD matrix.
+        bool solverOk = false;
+        while(!solverOk) {
+          if (! _wasPDInAllIterations)
+            _solver->setLambda(_currentLambda, true);   // add _currentLambda to the diagonal
+          solverOk = _solver->solve();
+          if (! _wasPDInAllIterations)
+            _solver->restoreDiagonal();
+          _wasPDInAllIterations = _wasPDInAllIterations && solverOk;
+          if (! _wasPDInAllIterations) {
+            // simple strategy to control the damping factor
+            if (solverOk) {
+              _currentLambda = std::max(minLambda, _currentLambda / (0.5 * _lamdbaFactor->value()));
+            } else {
+              _currentLambda *= _lamdbaFactor->value();
+              if (_currentLambda > maxLambda) {
+                _currentLambda = maxLambda;
+                return Fail;
+              }
+            }
+          }
+        }
+        if (!solverOk) {
+          return Fail;
+        }
+        hgnNorm = Eigen::VectorXd::ConstMapType(_solver->x(), _solver->vectorSize()).norm();
+      }
+
+      Eigen::VectorXd::ConstMapType hgn(_solver->x(), _solver->vectorSize());
+      assert(hgnNorm >= 0. && "Norm of the GN step is not computed");
+
+      if (hgnNorm < _delta) {
+        _hdl = hgn;
+        _lastStep = STEP_GN;
+      }
+      else if (hsdNorm > _delta) {
+        _hdl = _delta / hsdNorm * _hsd;
+        _lastStep = STEP_SD;
+      } else {
+        _auxVector = hgn - _hsd;  // b - a
+        double c = _hsd.dot(_auxVector);
+        double bmaSquaredNorm = _auxVector.squaredNorm();
+        double beta;
+        if (c <= 0.)
+          beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm;
+        else {
+          double hsdSqrNorm = _hsd.squaredNorm();
+          beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm)));
+        }
+        assert(beta > 0. && beta < 1 && "Error while computing beta");
+        _hdl = _hsd + beta * (hgn - _hsd);
+        _lastStep = STEP_DL;
+        assert(_hdl.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region");
+      }
+
+      // compute the linear gain
+      _auxVector.setZero();
+      blockSolver->multiplyHessian(_auxVector.data(), _hdl.data());
+      double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl));
+
+      // apply the update and see what happens
+      _optimizer->push();
+      _optimizer->update(_hdl.data());
+      _optimizer->computeActiveErrors();
+      double newChi = _optimizer-> activeRobustChi2();
+      double nonLinearGain = currentChi - newChi;
+      if (fabs(linearGain) < 1e-12)
+        linearGain = 1e-12;
+      double rho = nonLinearGain / linearGain;
+      //cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " << PVAR(rho) << endl;
+      if (rho > 0) { // step is good and will be accepted
+        _optimizer->discardTop();
+        goodStep = true;
+      } else { // recover previous state
+        _optimizer->pop();
+      }
+
+      // update trust region based on the step quality
+      if (rho > 0.75)
+        _delta = std::max(_delta, 3. * _hdl.norm());
+      else if (rho < 0.25)
+        _delta *= 0.5;
+    } while (!goodStep && numTries < _maxTrialsAfterFailure->value());
+    if (numTries == _maxTrialsAfterFailure->value() || !goodStep)
+      return Terminate;
+    return OK;
+  }
+
+  void OptimizationAlgorithmDogleg::printVerbose(std::ostream& os) const
+  {
+    os
+      << "\t Delta= " << _delta
+      << "\t step= " << stepType2Str(_lastStep)
+      << "\t tries= " << _lastNumTries;
+    if (! _wasPDInAllIterations)
+      os << "\t lambda= " << _currentLambda;
+  }
+
+  const char* OptimizationAlgorithmDogleg::stepType2Str(int stepType)
+  {
+    switch (stepType) {
+      case STEP_SD: return "Descent";
+      case STEP_GN: return "GN";
+      case STEP_DL: return "Dogleg";
+      default: return "Undefined";
+    }
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h

@@ -0,0 +1,89 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
+#define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
+
+#include "optimization_algorithm_with_hessian.h"
+
+namespace g2o {
+
+  class BlockSolverBase;
+
+  /**
+   * \brief Implementation of Powell's Dogleg Algorithm
+   */
+  class  OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian
+  {
+    public:
+      /** \brief type of the step to take */
+      enum {
+        STEP_UNDEFINED,
+        STEP_SD, STEP_GN, STEP_DL
+      };
+
+    public:
+      /**
+       * construct the Dogleg algorithm, which will use the given Solver for solving the
+       * linearized system.
+       */
+      explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver);
+      virtual ~OptimizationAlgorithmDogleg();
+
+      virtual SolverResult solve(int iteration, bool online = false);
+
+      virtual void printVerbose(std::ostream& os) const;
+
+      //! return the type of the last step taken by the algorithm
+      int lastStep() const { return _lastStep;}
+      //! return the diameter of the trust region
+      double trustRegion() const { return _delta;}
+
+      //! convert the type into an integer
+      static const char* stepType2Str(int stepType);
+
+    protected:
+      // parameters
+      Property<int>* _maxTrialsAfterFailure;
+      Property<double>* _userDeltaInit;
+      // damping to enforce positive definite matrix
+      Property<double>* _initialLambda;
+      Property<double>* _lamdbaFactor;
+
+      Eigen::VectorXd _hsd;         ///< steepest decent step
+      Eigen::VectorXd _hdl;         ///< final dogleg step
+      Eigen::VectorXd _auxVector;   ///< auxilary vector used to perform multiplications or other stuff
+
+      double _currentLambda;        ///< the damping factor to force positive definite matrix
+      double _delta;                ///< trust region
+      int _lastStep;                ///< type of the step taken by the algorithm
+      bool _wasPDInAllIterations;   ///< the matrix we solve was positive definite in all iterations -> if not apply damping
+      int _lastNumTries;
+  };
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp

@@ -0,0 +1,137 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_factory.h"
+
+#include <iostream>
+#include <typeinfo>
+#include <cassert>
+
+using namespace std;
+
+namespace g2o {
+
+  AbstractOptimizationAlgorithmCreator::AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p) :
+    _property(p)
+  {
+  }
+
+  OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::factoryInstance = 0;
+
+  OptimizationAlgorithmFactory::OptimizationAlgorithmFactory()
+  {
+  }
+
+  OptimizationAlgorithmFactory::~OptimizationAlgorithmFactory()
+  {
+    for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it)
+      delete *it;
+  }
+
+  OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::instance()
+  {
+    if (factoryInstance == 0) {
+      factoryInstance = new OptimizationAlgorithmFactory;
+    }
+    return factoryInstance;
+  }
+
+  void OptimizationAlgorithmFactory::registerSolver(AbstractOptimizationAlgorithmCreator* c)
+  {
+    const string& name = c->property().name;
+    CreatorList::iterator foundIt = findSolver(name);
+    if (foundIt != _creator.end()) {
+      _creator.erase(foundIt);
+      cerr << "SOLVER FACTORY WARNING: Overwriting Solver creator " << name << endl;
+      assert(0);
+    }
+    _creator.push_back(c);
+  }
+
+  void OptimizationAlgorithmFactory::unregisterSolver(AbstractOptimizationAlgorithmCreator* c)
+  {
+    const string& name = c->property().name;
+    CreatorList::iterator foundIt = findSolver(name);
+    if (foundIt != _creator.end()) {
+      delete *foundIt;
+      _creator.erase(foundIt);
+    }
+  }
+
+  OptimizationAlgorithm* OptimizationAlgorithmFactory::construct(const std::string& name, OptimizationAlgorithmProperty& solverProperty) const
+  {
+    CreatorList::const_iterator foundIt = findSolver(name);
+    if (foundIt != _creator.end()) {
+      solverProperty = (*foundIt)->property();
+      return (*foundIt)->construct();
+    }
+    cerr << "SOLVER FACTORY WARNING: Unable to create solver " << name << endl;
+    return 0;
+  }
+
+  void OptimizationAlgorithmFactory::destroy()
+  {
+    delete factoryInstance;
+    factoryInstance = 0;
+  }
+
+  void OptimizationAlgorithmFactory::listSolvers(std::ostream& os) const
+  {
+    size_t solverNameColumnLength = 0;
+    for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
+      solverNameColumnLength = std::max(solverNameColumnLength, (*it)->property().name.size());
+    solverNameColumnLength += 4;
+
+    for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
+      const OptimizationAlgorithmProperty& sp = (*it)->property();
+      os << sp.name;
+      for (size_t i = sp.name.size(); i < solverNameColumnLength; ++i)
+        os << ' ';
+      os << sp.desc << endl;
+    }
+  }
+
+  OptimizationAlgorithmFactory::CreatorList::const_iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) const
+  {
+    for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
+      const OptimizationAlgorithmProperty& sp = (*it)->property();
+      if (sp.name == name)
+        return it;
+    }
+    return _creator.end();
+  }
+
+  OptimizationAlgorithmFactory::CreatorList::iterator OptimizationAlgorithmFactory::findSolver(const std::string& name)
+  {
+    for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) {
+      const OptimizationAlgorithmProperty& sp = (*it)->property();
+      if (sp.name == name)
+        return it;
+    }
+    return _creator.end();
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h

@@ -0,0 +1,167 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTMIZATION_ALGORITHM_PROPERTY_H
+#define G2O_OPTMIZATION_ALGORITHM_PROPERTY_H
+
+#include "../../config.h"
+#include "../stuff/misc.h"
+#include "optimization_algorithm_property.h"
+
+#include <list>
+#include <iostream>
+#include <typeinfo>
+
+
+// define to get some verbose output
+//#define G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
+
+namespace g2o {
+
+  // forward decl
+  class  OptimizationAlgorithm;
+  class  SparseOptimizer;
+
+  /**
+   * \brief base for allocating an optimization algorithm
+   *
+   * Allocating a solver for a given optimizer. The method construct() has to be
+   * implemented in your derived class to allocate the desired solver.
+   */
+  class  AbstractOptimizationAlgorithmCreator
+  {
+    public:
+      AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p);
+      virtual ~AbstractOptimizationAlgorithmCreator() { }
+      //! allocate a solver operating on optimizer, re-implement for your creator
+      virtual OptimizationAlgorithm* construct() = 0;
+      //! return the properties of the solver
+      const OptimizationAlgorithmProperty& property() const { return _property;}
+    protected:
+      OptimizationAlgorithmProperty _property;
+  };
+  
+  /**
+   * \brief create solvers based on their short name
+   *
+   * Factory to allocate solvers based on their short name.
+   * The Factory is implemented as a sigleton and the single
+   * instance can be accessed via the instance() function.
+   */
+  class  OptimizationAlgorithmFactory
+  {
+    public:
+      typedef std::list<AbstractOptimizationAlgorithmCreator*>      CreatorList;
+
+      //! return the instance
+      static OptimizationAlgorithmFactory* instance();
+
+      //! free the instance
+      static void destroy();
+
+      /**
+       * register a specific creator for allocating a solver
+       */
+      void registerSolver(AbstractOptimizationAlgorithmCreator* c);
+
+      /**
+       * unregister a specific creator for allocating a solver
+       */
+      void unregisterSolver(AbstractOptimizationAlgorithmCreator* c);
+      
+      /**
+       * construct a solver based on its name, e.g., var, fix3_2_cholmod
+       */
+      OptimizationAlgorithm* construct(const std::string& tag, OptimizationAlgorithmProperty& solverProperty) const;
+
+      //! list the known solvers into a stream
+      void listSolvers(std::ostream& os) const;
+
+      //! return the underlying list of creators
+      const CreatorList& creatorList() const { return _creator;}
+
+    protected:
+      OptimizationAlgorithmFactory();
+      ~OptimizationAlgorithmFactory();
+
+      CreatorList _creator;
+
+      CreatorList::const_iterator findSolver(const std::string& name) const;
+      CreatorList::iterator findSolver(const std::string& name);
+
+    private:
+      static OptimizationAlgorithmFactory* factoryInstance;
+  };
+
+  class RegisterOptimizationAlgorithmProxy
+  {
+    public:
+      RegisterOptimizationAlgorithmProxy(AbstractOptimizationAlgorithmCreator* c)
+      {
+        _creator = c;
+#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
+        std::cout << __FUNCTION__ << ": Registering " << _creator->property().name << " of type " << typeid(*_creator).name() << std::endl;
+#endif
+        OptimizationAlgorithmFactory::instance()->registerSolver(c);
+      }
+
+      ~RegisterOptimizationAlgorithmProxy()
+      {
+#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
+        std::cout << __FUNCTION__ << ": Unregistering " << _creator->property().name << std::endl;
+#endif
+        OptimizationAlgorithmFactory::instance()->unregisterSolver(_creator);
+      }
+    private:
+      AbstractOptimizationAlgorithmCreator* _creator;
+  };
+
+}
+
+#if defined _MSC_VER && defined G2O_SHARED_LIBS
+#  define G2O_OAF_EXPORT __declspec(dllexport)
+#  define G2O_OAF_IMPORT __declspec(dllimport)
+#else
+#  define G2O_OAF_EXPORT
+#  define G2O_OAF_IMPORT
+#endif
+
+#define G2O_REGISTER_OPTIMIZATION_LIBRARY(libraryname) \
+    extern "C" void G2O_OAF_EXPORT g2o_optimization_library_##libraryname(void) {}
+
+#define G2O_USE_OPTIMIZATION_LIBRARY(libraryname) \
+    extern "C" void G2O_OAF_IMPORT g2o_optimization_library_##libraryname(void); \
+    static g2o::ForceLinker g2o_force_optimization_algorithm_library_##libraryname(g2o_optimization_library_##libraryname);
+
+#define G2O_REGISTER_OPTIMIZATION_ALGORITHM(optimizername, instance) \
+    extern "C" void G2O_OAF_EXPORT g2o_optimization_algorithm_##optimizername(void) {} \
+    static g2o::RegisterOptimizationAlgorithmProxy g_optimization_algorithm_proxy_##optimizername(instance);
+
+#define G2O_USE_OPTIMIZATION_ALGORITHM(optimizername) \
+    extern "C" void G2O_OAF_IMPORT g2o_optimization_algorithm_##optimizername(void); \
+    static g2o::ForceLinker g2o_force_optimization_algorithm_link_##optimizername(g2o_optimization_algorithm_##optimizername);
+
+#endif

Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp

@@ -0,0 +1,101 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_gauss_newton.h"
+
+#include <iostream>
+
+#include "../stuff/timeutil.h"
+#include "../stuff/macros.h"
+
+#include "solver.h"
+#include "batch_stats.h"
+#include "sparse_optimizer.h"
+using namespace std;
+
+namespace g2o {
+
+  OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) :
+    OptimizationAlgorithmWithHessian(solver)
+  {
+  }
+
+  OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton()
+  {
+  }
+
+  OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online)
+  {
+    assert(_optimizer && "_optimizer not set");
+    assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
+    bool ok = true;
+    
+    //here so that correct component for max-mixtures can be computed before the build structure
+    double t=get_monotonic_time();
+    _optimizer->computeActiveErrors();
+    G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+    if (globalStats) {
+      globalStats->timeResiduals = get_monotonic_time()-t;
+    }
+    
+    if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
+      ok = _solver->buildStructure();
+      if (! ok) {
+        cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
+        return OptimizationAlgorithm::Fail;
+      }
+    }
+
+    t=get_monotonic_time();
+    _solver->buildSystem();
+    if (globalStats) {
+      globalStats->timeQuadraticForm = get_monotonic_time()-t;
+      t=get_monotonic_time();
+    }
+
+    ok = _solver->solve();
+    if (globalStats) {
+      globalStats->timeLinearSolution = get_monotonic_time()-t;
+      t=get_monotonic_time();
+    }
+
+    _optimizer->update(_solver->x());
+    if (globalStats) {
+      globalStats->timeUpdate = get_monotonic_time()-t;
+    }
+    if (ok)
+      return OK;
+    else
+      return Fail;
+  }
+
+  void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const
+  {
+    os
+      << "\t schur= " << _solver->schur();
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h

@@ -0,0 +1,54 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H
+#define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H
+
+#include "optimization_algorithm_with_hessian.h"
+
+namespace g2o {
+
+  /**
+   * \brief Implementation of the Gauss Newton Algorithm
+   */
+  class  OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian
+  {
+    public:
+      /**
+       * construct the Gauss Newton algorithm, which use the given Solver for solving the
+       * linearized system.
+       */
+      explicit OptimizationAlgorithmGaussNewton(Solver* solver);
+      virtual ~OptimizationAlgorithmGaussNewton();
+
+      virtual SolverResult solve(int iteration, bool online = false);
+
+      virtual void printVerbose(std::ostream& os) const;
+  };
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp

@@ -0,0 +1,209 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Modified Raul Mur Artal (2014)
+// - Stop criterium (solve function)
+
+#include "optimization_algorithm_levenberg.h"
+
+#include <iostream>
+
+#include "../stuff/timeutil.h"
+
+#include "sparse_optimizer.h"
+#include "solver.h"
+#include "batch_stats.h"
+using namespace std;
+
+namespace g2o {
+
+  OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Solver* solver) :
+    OptimizationAlgorithmWithHessian(solver)
+  {
+    _currentLambda = -1.;
+    _tau = 1e-5;
+    _goodStepUpperScale = 2./3.;
+    _goodStepLowerScale = 1./3.;
+    _userLambdaInit = _properties.makeProperty<Property<double> >("initialLambda", 0.);
+    _maxTrialsAfterFailure = _properties.makeProperty<Property<int> >("maxTrialsAfterFailure", 10);
+    _ni=2.;
+    _levenbergIterations = 0;
+    _nBad = 0;
+  }
+
+  OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg()
+  {
+  }
+
+  OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online)
+  {
+    assert(_optimizer && "_optimizer not set");
+    assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
+
+    if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
+      bool ok = _solver->buildStructure();
+      if (! ok) {
+        cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
+        return OptimizationAlgorithm::Fail;
+      }
+    }
+
+    double t=get_monotonic_time();
+    _optimizer->computeActiveErrors();
+    G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+    if (globalStats) {
+      globalStats->timeResiduals = get_monotonic_time()-t;
+      t=get_monotonic_time();
+    }
+
+    double currentChi = _optimizer->activeRobustChi2();
+    double tempChi=currentChi;
+
+    double iniChi = currentChi;
+
+    _solver->buildSystem();
+    if (globalStats) {
+      globalStats->timeQuadraticForm = get_monotonic_time()-t;
+    }
+
+    // core part of the Levenbarg algorithm
+    if (iteration == 0) {       
+      _currentLambda = computeLambdaInit();
+      _ni = 2;
+      _nBad = 0;
+    }
+
+    double rho=0;
+    int& qmax = _levenbergIterations;
+    qmax = 0;
+    do {
+      _optimizer->push();
+      if (globalStats) {
+        globalStats->levenbergIterations++;
+        t=get_monotonic_time();
+      }
+      // update the diagonal of the system matrix
+      _solver->setLambda(_currentLambda, true);
+      bool ok2 = _solver->solve();
+      if (globalStats) {
+        globalStats->timeLinearSolution+=get_monotonic_time()-t;
+        t=get_monotonic_time();
+      }
+      _optimizer->update(_solver->x());
+      if (globalStats) {
+        globalStats->timeUpdate = get_monotonic_time()-t;
+      }
+
+      // restore the diagonal
+      _solver->restoreDiagonal();
+
+      _optimizer->computeActiveErrors();
+      tempChi = _optimizer->activeRobustChi2();
+
+      if (! ok2)
+        tempChi=std::numeric_limits<double>::max();
+
+      rho = (currentChi-tempChi);
+      double scale = computeScale();
+      scale += 1e-3; // make sure it's non-zero :)
+      rho /=  scale;
+
+      if (rho>0 && g2o_isfinite(tempChi)){ // last step was good
+        double alpha = 1.-pow((2*rho-1),3);
+        // crop lambda between minimum and maximum factors
+        alpha = (std::min)(alpha, _goodStepUpperScale);
+        double scaleFactor = (std::max)(_goodStepLowerScale, alpha);
+        _currentLambda *= scaleFactor;
+        _ni = 2;
+        currentChi=tempChi;
+        _optimizer->discardTop();
+      } else {
+        _currentLambda*=_ni;
+        _ni*=2;
+        _optimizer->pop(); // restore the last state before trying to optimize
+      }
+      qmax++;
+    } while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate());
+
+    if (qmax == _maxTrialsAfterFailure->value() || rho==0)
+      return Terminate;
+
+    //Stop criterium (Raul)
+    if((iniChi-currentChi)*1e3<iniChi)
+        _nBad++;
+    else
+        _nBad=0;
+
+    if(_nBad>=3)
+        return Terminate;
+
+    return OK;
+  }
+
+  double OptimizationAlgorithmLevenberg::computeLambdaInit() const
+  {
+    if (_userLambdaInit->value() > 0)
+      return _userLambdaInit->value();
+    double maxDiagonal=0.;
+    for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) {
+      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k];
+      assert(v);
+      int dim = v->dimension();
+      for (int j = 0; j < dim; ++j){
+        maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal);
+      }
+    }
+    return _tau*maxDiagonal;
+  }
+
+  double OptimizationAlgorithmLevenberg::computeScale() const
+  {
+    double scale = 0.;
+    for (size_t j=0; j < _solver->vectorSize(); j++){
+      scale += _solver->x()[j] * (_currentLambda * _solver->x()[j] + _solver->b()[j]);
+    }
+    return scale;
+  }
+
+  void OptimizationAlgorithmLevenberg::setMaxTrialsAfterFailure(int max_trials)
+  {
+    _maxTrialsAfterFailure->setValue(max_trials);
+  }
+
+  void OptimizationAlgorithmLevenberg::setUserLambdaInit(double lambda)
+  {
+    _userLambdaInit->setValue(lambda);
+  }
+
+  void OptimizationAlgorithmLevenberg::printVerbose(std::ostream& os) const
+  {
+    os
+      << "\t schur= " << _solver->schur()
+      << "\t lambda= " << FIXED(_currentLambda)
+      << "\t levenbergIter= " << _levenbergIterations;
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h

@@ -0,0 +1,92 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SOLVER_LEVENBERG_H
+#define G2O_SOLVER_LEVENBERG_H
+
+#include "optimization_algorithm_with_hessian.h"
+
+namespace g2o {
+
+  /**
+   * \brief Implementation of the Levenberg Algorithm
+   */
+  class  OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian
+  {
+    public:
+      /**
+       * construct the Levenberg algorithm, which will use the given Solver for solving the
+       * linearized system.
+       */
+      explicit OptimizationAlgorithmLevenberg(Solver* solver);
+      virtual ~OptimizationAlgorithmLevenberg();
+
+      virtual SolverResult solve(int iteration, bool online = false);
+
+      virtual void printVerbose(std::ostream& os) const;
+
+      //! return the currently used damping factor
+      double currentLambda() const { return _currentLambda;}
+
+      //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt
+      void setMaxTrialsAfterFailure(int max_trials);
+
+      //! get the number of inner iterations for Levenberg-Marquardt
+      int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();}
+
+      //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda
+      double userLambdaInit() {return _userLambdaInit->value();}
+      //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value
+      void setUserLambdaInit(double lambda);
+
+      //! return the number of levenberg iterations performed in the last round
+      int levenbergIteration() { return _levenbergIterations;}
+
+    protected:
+      // Levenberg parameters
+      Property<int>* _maxTrialsAfterFailure;
+      Property<double>* _userLambdaInit;
+      double _currentLambda;
+      double _tau;
+      double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step
+      double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step
+      double _ni;
+      int _levenbergIterations;   ///< the numer of levenberg iterations performed to accept the last step
+      //RAUL
+      int _nBad;
+
+      /**
+       * helper for Levenberg, this function computes the initial damping factor, if the user did not
+       * specify an own value, see setUserLambdaInit()
+       */
+      double computeLambdaInit() const;
+      double computeScale() const;
+
+  };
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/optimization_algorithm_property.h

@@ -0,0 +1,57 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H
+#define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H
+
+#include <string>
+
+namespace g2o {
+
+/**
+ * \brief describe the properties of a solver
+ */
+struct  OptimizationAlgorithmProperty
+{
+  std::string name;           ///< name of the solver, e.g., var
+  std::string desc;           ///< short description of the solver
+  std::string type;           ///< type of solver, e.g., "CSparse Cholesky", "PCG"
+  bool requiresMarginalize;   ///< whether the solver requires marginalization of landmarks
+  int poseDim;                ///< dimension of the pose vertices (-1 if variable)
+  int landmarkDim;            ///< dimension of the landmar vertices (-1 if variable)
+  OptimizationAlgorithmProperty() :
+    name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1)
+  {
+  }
+  OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) :
+    name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_)
+  {
+  }
+};
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp

@@ -0,0 +1,101 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_with_hessian.h"
+
+#include "solver.h"
+#include "optimizable_graph.h"
+#include "sparse_optimizer.h"
+
+#include <iostream>
+using namespace std;
+
+namespace g2o {
+
+  OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) :
+    OptimizationAlgorithm(),
+    _solver(solver)
+  {
+    _writeDebug = _properties.makeProperty<Property<bool> >("writeDebug", true);
+  }
+
+  OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian()
+  {
+    delete _solver;
+  }
+
+  bool OptimizationAlgorithmWithHessian::init(bool online)
+  {
+    assert(_optimizer && "_optimizer not set");
+    assert(_solver && "Solver not set");
+    _solver->setWriteDebug(_writeDebug->value());
+    bool useSchur=false;
+    for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) {
+      OptimizableGraph::Vertex* v= *it;
+      if (v->marginalized()){
+        useSchur=true;
+        break;
+      }
+    }
+    if (useSchur){
+      if  (_solver->supportsSchur())
+        _solver->setSchur(true);
+    } else {
+      if  (_solver->supportsSchur())
+        _solver->setSchur(false);
+    }
+
+    bool initState = _solver->init(_optimizer, online);
+    return initState;
+  }
+
+  bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
+  {
+    return _solver ? _solver->computeMarginals(spinv, blockIndices) : false;
+  }
+
+  bool OptimizationAlgorithmWithHessian::buildLinearStructure()
+  {
+    return _solver ? _solver->buildStructure() : false;
+  }
+
+  void OptimizationAlgorithmWithHessian::updateLinearSystem()
+  {
+    if (_solver)
+      _solver->buildSystem();
+  }
+
+  bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
+  {
+    return _solver ? _solver->updateStructure(vset, edges) : false;
+  }
+
+  void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug)
+  {
+    _writeDebug->setValue(writeDebug);
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h

@@ -0,0 +1,72 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H
+#define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H
+
+#include "optimization_algorithm.h"
+
+namespace g2o {
+
+  class Solver;
+
+  /**
+   * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg
+   */
+  class  OptimizationAlgorithmWithHessian : public OptimizationAlgorithm
+  {
+    public:
+      explicit OptimizationAlgorithmWithHessian(Solver* solver);
+      virtual ~OptimizationAlgorithmWithHessian();
+
+      virtual bool init(bool online = false);
+
+      virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
+
+      virtual bool buildLinearStructure();
+
+      virtual void updateLinearSystem();
+
+      virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
+
+      //! return the underlying solver used to solve the linear system
+      Solver* solver() { return _solver;}
+
+      /**
+       * write debug output of the Hessian if system is not positive definite
+       */
+      virtual void setWriteDebug(bool writeDebug);
+      virtual bool writeDebug() const { return _writeDebug->value();}
+
+    protected:
+      Solver* _solver;
+      Property<bool>* _writeDebug;
+
+  };
+
+}// end namespace
+
+#endif

Thirdparty/g2o/g2o/core/parameter.cpp

@@ -0,0 +1,40 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "parameter.h"
+
+namespace g2o {
+
+  Parameter::Parameter() : _id(-1)
+  {
+  }
+
+  void Parameter::setId(int id_)
+  {
+    _id = id_;
+  }
+  
+} // end namespace

Thirdparty/g2o/g2o/core/parameter.h

@@ -0,0 +1,56 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_GRAPH_PARAMETER_HH_
+#define G2O_GRAPH_PARAMETER_HH_
+
+#include <iosfwd>
+
+#include "hyper_graph.h"
+
+namespace g2o {
+
+    class  Parameter : public HyperGraph::HyperGraphElement
+    {
+      public:
+        Parameter();
+        virtual ~Parameter() {};
+        //! read the data from a stream
+        virtual bool read(std::istream& is) = 0;
+        //! write the data to a stream
+        virtual bool write(std::ostream& os) const = 0;
+        int id() const {return _id;}
+        void setId(int id_);
+        virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;}
+      protected:
+        int _id;
+    };
+
+    typedef std::vector<Parameter*> ParameterVector;
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/parameter_container.cpp

@@ -0,0 +1,142 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "parameter_container.h"
+
+#include <iostream>
+
+#include "factory.h"
+#include "parameter.h"
+
+#include "../stuff/macros.h"
+#include "../stuff/color_macros.h"
+#include "../stuff/string_tools.h"
+
+namespace g2o {
+
+  using namespace std;
+
+  ParameterContainer::ParameterContainer(bool isMainStorage_) :
+    _isMainStorage(isMainStorage_)
+  {
+  }
+
+  void ParameterContainer::clear() {
+    if (!_isMainStorage)
+      return;
+    for (iterator it = begin(); it!=end(); it++){
+      delete it->second;
+    }
+    BaseClass::clear();
+  }
+
+  ParameterContainer::~ParameterContainer(){
+    clear();
+  }
+
+  bool ParameterContainer::addParameter(Parameter* p){
+    if (p->id()<0)
+      return false;
+    iterator it=find(p->id());
+    if (it!=end())
+      return false;
+    insert(make_pair(p->id(), p));
+    return true;
+  }
+
+  Parameter* ParameterContainer::getParameter(int id) {
+    iterator it=find(id);
+    if (it==end())
+      return 0;
+    return it->second;
+  }
+
+  Parameter* ParameterContainer::detachParameter(int id){
+    iterator it=find(id);
+    if (it==end())
+      return 0;
+    Parameter* p=it->second;
+    erase(it);
+    return p;
+  }
+  
+  bool ParameterContainer::write(std::ostream& os) const{
+    Factory* factory = Factory::instance();
+    for (const_iterator it=begin(); it!=end(); it++){
+      os << factory->tag(it->second) << " ";
+      os << it->second->id() << " ";
+      it->second->write(os);
+      os << endl;
+    }
+    return true;
+  }
+
+  bool ParameterContainer::read(std::istream& is, const std::map<std::string, std::string>* _renamedTypesLookup){
+    stringstream currentLine;
+    string token;
+
+    Factory* factory = Factory::instance();
+    HyperGraph::GraphElemBitset elemBitset;
+    elemBitset[HyperGraph::HGET_PARAMETER] = 1;
+    
+    while (1) {
+      int bytesRead = readLine(is, currentLine);
+      if (bytesRead == -1)
+        break;
+      currentLine >> token;
+      if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
+        continue;
+      if (_renamedTypesLookup && _renamedTypesLookup->size()>0){
+	map<string, string>::const_iterator foundIt = _renamedTypesLookup->find(token);
+	if (foundIt != _renamedTypesLookup->end()) {
+	  token = foundIt->second;
+	}
+      }
+
+      HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
+      if (! element) // not a parameter or otherwise unknown tag
+        continue;
+      assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param");
+
+      Parameter* p = static_cast<Parameter*>(element);
+      int pid;
+      currentLine >> pid;
+      p->setId(pid);
+      bool r = p->read(currentLine);
+      if (! r) {
+        cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl;
+        delete p;
+      } else {
+        if (! addParameter(p) ){
+          cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl;
+        }
+      }
+    } // while read line
+    
+    return true;
+  }
+  
+} // end namespace

Thirdparty/g2o/g2o/core/parameter_container.h

@@ -0,0 +1,74 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_
+#define G2O_GRAPH_PARAMETER_CONTAINER_HH_
+
+#include <iosfwd>
+#include <map>
+#include <string>
+
+namespace g2o {
+    
+    class Parameter;
+
+    /**
+     * \brief map id to parameters
+     */
+    class ParameterContainer : protected std::map<int, Parameter*> 
+    {
+    public:
+      typedef std::map<int, Parameter*> BaseClass;
+
+      /**
+       * create a container for the parameters.
+       * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor
+       */
+      ParameterContainer(bool isMainStorage_=true);
+      virtual ~ParameterContainer();
+      //! add parameter to the container
+      bool addParameter(Parameter* p);
+      //! return a parameter based on its ID
+      Parameter* getParameter(int id);
+      //! remove a parameter from the container, i.e., the user now owns the pointer
+      Parameter* detachParameter(int id);
+      //! read parameters from a stream
+      virtual bool read(std::istream& is, const std::map<std::string, std::string>* renamedMap =0);
+      //! write the data to a stream
+      virtual bool write(std::ostream& os) const;
+      bool isMainStorage() const {return _isMainStorage;}
+      void clear();
+
+      // stuff of the base class that should re-appear
+      using BaseClass::size;
+
+    protected:
+      bool _isMainStorage;
+    };
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/robust_kernel.cpp

@@ -0,0 +1,46 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "robust_kernel.h"
+
+namespace g2o {
+
+RobustKernel::RobustKernel() :
+  _delta(1.)
+{
+}
+
+RobustKernel::RobustKernel(double delta) :
+  _delta(delta)
+{
+}
+
+void RobustKernel::setDelta(double delta)
+{
+  _delta = delta;
+}
+
+} // end namespace g2o

Thirdparty/g2o/g2o/core/robust_kernel.h

@@ -0,0 +1,81 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ROBUST_KERNEL_H
+#define G2O_ROBUST_KERNEL_H
+
+#ifdef _MSC_VER
+#include <memory>
+#else
+#include <tr1/memory>
+#endif
+#include <Eigen/Core>
+
+
+namespace g2o {
+
+  /**
+   * \brief base for all robust cost functions
+   *
+   * Note in all the implementations for the other cost functions the e in the
+   * funtions corresponds to the sqaured errors, i.e., the robustification
+   * functions gets passed the squared error.
+   *
+   * e.g. the robustified least squares function is
+   *
+   * chi^2 = sum_{e} rho( e^T Omega e )
+   */
+  class  RobustKernel
+  {
+    public:
+      RobustKernel();
+      explicit RobustKernel(double delta);
+      virtual ~RobustKernel() {}
+      /**
+       * compute the scaling factor for a error:
+       * The error is e^T Omega e
+       * The output rho is
+       * rho[0]: The actual scaled error value
+       * rho[1]: First derivative of the scaling function
+       * rho[2]: Second derivative of the scaling function
+       */
+      virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0;
+
+      /**
+       * set the window size of the error. A squared error above delta^2 is considered
+       * as outlier in the data.
+       */
+      virtual void setDelta(double delta);
+      double delta() const { return _delta;}
+
+    protected:
+      double _delta;
+  };
+  typedef std::shared_ptr<RobustKernel> RobustKernelPtr;
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp

@@ -0,0 +1,111 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "robust_kernel_factory.h"
+#include "robust_kernel.h"
+
+#include <cassert>
+
+using namespace std;
+
+namespace g2o {
+
+RobustKernelFactory* RobustKernelFactory::factoryInstance = 0;
+
+RobustKernelFactory::RobustKernelFactory()
+{
+}
+
+RobustKernelFactory::~RobustKernelFactory()
+{
+  for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {
+    delete it->second;
+  }
+  _creator.clear();
+}
+
+RobustKernelFactory* RobustKernelFactory::instance()
+{
+  if (factoryInstance == 0) {
+    factoryInstance = new RobustKernelFactory;
+  }
+
+  return factoryInstance;
+}
+
+void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c)
+{
+  CreatorMap::const_iterator foundIt = _creator.find(tag);
+  if (foundIt != _creator.end()) {
+    cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl;
+    assert(0);
+  }
+
+  _creator[tag] = c;
+}
+
+void RobustKernelFactory::unregisterType(const std::string& tag)
+{
+  CreatorMap::iterator tagPosition = _creator.find(tag);
+  if (tagPosition != _creator.end()) {
+    AbstractRobustKernelCreator* c = tagPosition->second;
+    delete c;
+    _creator.erase(tagPosition);
+  }
+}
+
+RobustKernel* RobustKernelFactory::construct(const std::string& tag) const
+{
+  CreatorMap::const_iterator foundIt = _creator.find(tag);
+  if (foundIt != _creator.end()) {
+    return foundIt->second->construct();
+  }
+  return 0;
+}
+
+AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const
+{
+  CreatorMap::const_iterator foundIt = _creator.find(tag);
+  if (foundIt != _creator.end()) {
+    return foundIt->second;
+  }
+  return 0;
+}
+
+void RobustKernelFactory::fillKnownKernels(std::vector<std::string>& types) const
+{
+  types.clear();
+  for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
+    types.push_back(it->first);
+}
+
+void RobustKernelFactory::destroy()
+{
+  delete factoryInstance;
+  factoryInstance = 0;
+}
+
+} // end namespace

Thirdparty/g2o/g2o/core/robust_kernel_factory.h

@@ -0,0 +1,151 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ROBUST_KERNEL_FACTORY_H
+#define G2O_ROBUST_KERNEL_FACTORY_H
+
+#include "../stuff/misc.h"
+
+#include <string>
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace g2o {
+
+  class RobustKernel;
+
+    /**
+   * \brief Abstract interface for allocating a robust kernel
+   */
+  class  AbstractRobustKernelCreator
+  {
+    public:
+      /**
+       * create a hyper graph element. Has to implemented in derived class.
+       */
+      virtual RobustKernel* construct() = 0;
+      virtual ~AbstractRobustKernelCreator() { }
+  };
+
+  /**
+   * \brief templatized creator class which creates graph elements
+   */
+  template <typename T>
+  class RobustKernelCreator : public AbstractRobustKernelCreator
+  {
+    public:
+      RobustKernel* construct() { return new T;}
+  };
+
+  /**
+   * \brief create robust kernels based on their human readable name
+   */
+  class  RobustKernelFactory
+  {
+    public:
+
+      //! return the instance
+      static RobustKernelFactory* instance();
+
+      //! free the instance
+      static void destroy();
+
+      /**
+       * register a tag for a specific creator
+       */
+      void registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c);
+
+      /**
+       * unregister a tag for a specific creator
+       */
+      void unregisterType(const std::string& tag);
+
+      /**
+       * construct a robust kernel based on its tag
+       */
+      RobustKernel* construct(const std::string& tag) const;
+
+      /**
+       * return the creator for a specific tag
+       */
+      AbstractRobustKernelCreator* creator(const std::string& tag) const;
+
+      /**
+       * get a list of all known robust kernels
+       */
+      void fillKnownKernels(std::vector<std::string>& types) const;
+
+    protected:
+
+      typedef std::map<std::string, AbstractRobustKernelCreator*>              CreatorMap;
+      RobustKernelFactory();
+      ~RobustKernelFactory();
+
+      CreatorMap _creator;     ///< look-up map for the existing creators
+
+    private:
+      static RobustKernelFactory* factoryInstance;
+  };
+
+  template<typename T>
+  class RegisterRobustKernelProxy
+  {
+    public:
+      RegisterRobustKernelProxy(const std::string& name) : _name(name)
+      {
+        RobustKernelFactory::instance()->registerRobustKernel(_name, new RobustKernelCreator<T>());
+      }
+
+      ~RegisterRobustKernelProxy()
+      {
+        RobustKernelFactory::instance()->unregisterType(_name);
+      }
+
+    private:
+      std::string _name;
+  };
+
+#if defined _MSC_VER && defined G2O_SHARED_LIBS
+#  define G2O_ROBUST_KERNEL_FACTORY_EXPORT __declspec(dllexport)
+#  define G2O_ROBUST_KERNEL_FACTORY_IMPORT __declspec(dllimport)
+#else
+#  define G2O_ROBUST_KERNEL_FACTORY_EXPORT
+#  define G2O_ROBUST_KERNEL_FACTORY_IMPORT
+#endif
+
+  // These macros are used to automate registering of robust kernels and forcing linkage
+#define G2O_REGISTER_ROBUST_KERNEL(name, classname) \
+    extern "C" void G2O_ROBUST_KERNEL_FACTORY_EXPORT g2o_robust_kernel_##classname(void) {} \
+    static g2o::RegisterRobustKernelProxy<classname> g_robust_kernel_proxy_##classname(#name);
+
+#define G2O_USE_ROBUST_KERNEL(classname) \
+    extern "C" void G2O_ROBUST_KERNEL_FACTORY_IMPORT g2o_robust_kernel_##classname(void); \
+    static g2o::TypeFunctionProxy proxy_##classname(g2o_robust_kernel_##classname);
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp

@@ -0,0 +1,173 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "robust_kernel_impl.h"
+#include "robust_kernel_factory.h"
+
+#include <cmath>
+
+namespace g2o {
+
+RobustKernelScaleDelta::RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta) :
+  RobustKernel(delta),
+  _kernel(kernel)
+{
+}
+
+RobustKernelScaleDelta::RobustKernelScaleDelta(double delta) :
+  RobustKernel(delta)
+{
+}
+
+void RobustKernelScaleDelta::setKernel(const RobustKernelPtr& ptr)
+{
+  _kernel = ptr;
+}
+
+void RobustKernelScaleDelta::robustify(double error, Eigen::Vector3d& rho) const
+{
+  if (_kernel.get()) {
+    double dsqr = _delta * _delta;
+    double dsqrReci = 1. / dsqr;
+    _kernel->robustify(dsqrReci * error, rho);
+    rho[0] *= dsqr;
+    rho[2] *= dsqrReci;
+  } else { // no robustification
+    rho[0] = error;
+    rho[1] = 1.;
+    rho[2] = 0.;
+  }
+}
+
+void RobustKernelHuber::setDelta(double delta)
+{
+	dsqr = delta*delta;
+	_delta = delta;
+}
+
+
+void RobustKernelHuber::setDeltaSqr(const double &delta, const double &deltaSqr)
+{
+	dsqr = deltaSqr;
+	_delta = delta;
+}
+
+void RobustKernelHuber::robustify(double e, Eigen::Vector3d& rho) const
+{
+  //dsqr = _delta * _delta;
+  if (e <= dsqr) { // inlier
+    rho[0] = e;
+    rho[1] = 1.;
+    rho[2] = 0.;
+  } else { // outlier
+    double sqrte = sqrt(e); // absolut value of the error
+    rho[0] = 2*sqrte*_delta - dsqr; // rho(e)   = 2 * delta * e^(1/2) - delta^2
+    rho[1] = _delta / sqrte;        // rho'(e)  = delta / sqrt(e)
+    rho[2] = - 0.5 * rho[1] / e;    // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e
+  }
+}
+
+void RobustKernelTukey::setDeltaSqr(const double &deltaSqr, const double &inv)
+{
+ _deltaSqr = deltaSqr;
+ _invDeltaSqr = inv;
+ 
+}
+
+void RobustKernelTukey::robustify(double e, Eigen::Vector3d& rho) const
+{
+  if (e <= _deltaSqr) { // inlier
+    double factor = e*_invDeltaSqr;
+    double d = 1-factor;
+    double dd = d*d;
+    rho[0] = _deltaSqr*(1-dd*d);
+    rho[1] = 3*dd;
+    rho[2] = -6*_invDeltaSqr*d;
+  } else { // outlier
+    rho[0] = _deltaSqr; // rho(e)   = delta^2
+    rho[1] = 0.;
+    rho[2] = 0.;   
+  }
+}
+
+void RobustKernelPseudoHuber::robustify(double e2, Eigen::Vector3d& rho) const
+{
+  double dsqr = _delta * _delta;
+  double dsqrReci = 1. / dsqr;
+  double aux1 = dsqrReci * e2 + 1.0;
+  double aux2 = sqrt(aux1);
+  rho[0] = 2 * dsqr * (aux2 - 1);
+  rho[1] = 1. / aux2;
+  rho[2] = -0.5 * dsqrReci * rho[1] / aux1;
+}
+
+void RobustKernelCauchy::robustify(double e2, Eigen::Vector3d& rho) const
+{
+  double dsqr = _delta * _delta;
+  double dsqrReci = 1. / dsqr;
+  double aux = dsqrReci * e2 + 1.0;
+  rho[0] = dsqr * log(aux);
+  rho[1] = 1. / aux;
+  rho[2] = -dsqrReci * std::pow(rho[1], 2); 
+}
+
+void RobustKernelSaturated::robustify(double e2, Eigen::Vector3d& rho) const
+{
+  double dsqr = _delta * _delta;
+  if (e2 <= dsqr) { // inlier
+    rho[0] = e2;
+    rho[1] = 1.;
+    rho[2] = 0.;
+  } else { // outlier
+    rho[0] = dsqr;
+    rho[1] = 0.;
+    rho[2] = 0.;
+  }
+}
+
+//delta is used as $phi$
+void RobustKernelDCS::robustify(double e2, Eigen::Vector3d& rho) const
+{
+  const double& phi = _delta;
+  double scale = (2.0*phi)/(phi+e2);
+  if(scale>=1.0)
+    scale = 1.0;
+
+  rho[0] = scale*e2*scale;
+  rho[1] = (scale*scale);
+  rho[2] = 0;    
+}
+
+
+// register the kernel to their factory
+G2O_REGISTER_ROBUST_KERNEL(Huber, RobustKernelHuber)
+G2O_REGISTER_ROBUST_KERNEL(Tukey, RobustKernelTukey)
+G2O_REGISTER_ROBUST_KERNEL(PseudoHuber, RobustKernelPseudoHuber)
+G2O_REGISTER_ROBUST_KERNEL(Cauchy, RobustKernelCauchy)
+G2O_REGISTER_ROBUST_KERNEL(Saturated, RobustKernelSaturated)
+G2O_REGISTER_ROBUST_KERNEL(DCS, RobustKernelDCS)
+
+} // end namespace g2o

Thirdparty/g2o/g2o/core/robust_kernel_impl.h

@@ -0,0 +1,167 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ROBUST_KERNEL_IMPL_H
+#define G2O_ROBUST_KERNEL_IMPL_H
+
+#include "robust_kernel.h"
+
+namespace g2o {
+
+  /**
+   * \brief scale a robust kernel to another delta (window size)
+   *
+   * Scales a robust kernel to another window size. Useful, in case if
+   * one implements a kernel which only is designed for a fixed window
+   * size.
+   */
+  class  RobustKernelScaleDelta : public RobustKernel
+  {
+    public:
+      /**
+       * construct the scaled kernel ontop of another kernel which might be shared accross
+       * several scaled kernels
+       */
+      explicit RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta = 1.);
+      explicit RobustKernelScaleDelta(double delta = 1.);
+
+      //! return the underlying kernel
+      const RobustKernelPtr kernel() const { return _kernel;}
+      //! use another kernel for the underlying operation
+      void setKernel(const RobustKernelPtr& ptr);
+
+      void robustify(double error, Eigen::Vector3d& rho) const;
+
+    protected:
+      RobustKernelPtr _kernel;
+  };
+
+  /**
+   * \brief Huber Cost Function
+   *
+   * Loss function as described by Huber
+   * See http://en.wikipedia.org/wiki/Huber_loss_function
+   *
+   * If e^(1/2) < d
+   * rho(e) = e
+   *
+   * else
+   *
+   *               1/2    2
+   * rho(e) = 2 d e    - d
+   */
+  class  RobustKernelHuber : public RobustKernel
+  {
+    public:
+      virtual void setDelta(double delta);
+      virtual void setDeltaSqr(const double &delta, const double &deltaSqr);
+      virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+
+    private:
+      float dsqr;
+  };
+
+   /**
+   * \brief Tukey Cost Function
+   *
+   *
+   * If e^(1/2) < d
+   * rho(e) = delta2(1-(1-e/delta2)^3)
+   *
+   * else
+   *              
+   * rho(e) = delta2
+   */
+  class  RobustKernelTukey : public RobustKernel
+  {
+    public:
+
+      virtual void setDeltaSqr(const double &deltaSqr, const double &inv);
+      virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+    private:
+      float _deltaSqr;
+      float _invDeltaSqr;
+  };
+
+
+  /**
+   * \brief Pseudo Huber Cost Function
+   *
+   * The smooth pseudo huber cost function:
+   * See http://en.wikipedia.org/wiki/Huber_loss_function
+   *
+   *    2       e
+   * 2 d  (sqrt(-- + 1) - 1)
+   *             2
+   *            d
+   */
+  class  RobustKernelPseudoHuber : public RobustKernel
+  {
+    public:
+      virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+  };
+
+  /**
+   * \brief Cauchy cost function
+   *
+   *  2     e
+   * d  log(-- + 1)
+   *         2
+   *        d
+   */
+  class  RobustKernelCauchy : public RobustKernel
+  {
+    public:
+      virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+  };
+
+  /**
+   * \brief Saturated cost function.
+   *
+   * The error is at most delta^2
+   */
+  class  RobustKernelSaturated : public RobustKernel
+  {
+    public:
+      virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+  };
+
+  /**
+   * \brief Dynamic covariance scaling - DCS
+   *  
+   * See paper Robust Map Optimization from Agarwal et al.  ICRA 2013
+   *
+   * delta is used as $phi$
+   */
+  class  RobustKernelDCS : public RobustKernel
+  {
+    public:
+      virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+  };
+
+} // end namespace g2o
+
+#endif

Thirdparty/g2o/g2o/core/solver.cpp

@@ -0,0 +1,87 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "solver.h"
+
+#include <cstring>
+#include <algorithm>
+
+namespace g2o {
+
+Solver::Solver() :
+  _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0),
+  _isLevenberg(false), _additionalVectorSpace(0)
+{
+}
+
+Solver::~Solver()
+{
+  delete[] _x;
+  delete[] _b;
+}
+
+void Solver::resizeVector(size_t sx)
+{
+  size_t oldSize = _xSize;
+  _xSize = sx;
+  sx += _additionalVectorSpace; // allocate some additional space if requested
+  if (_maxXSize < sx) {
+    _maxXSize = 2*sx;
+    delete[] _x;
+    _x = new double[_maxXSize];
+#ifndef NDEBUG
+    memset(_x, 0, _maxXSize * sizeof(double));
+#endif
+    if (_b) { // backup the former b, might still be needed for online processing
+      memcpy(_x, _b, oldSize * sizeof(double));
+      delete[] _b;
+      _b = new double[_maxXSize];
+      std::swap(_b, _x);
+    } else {
+      _b = new double[_maxXSize];
+#ifndef NDEBUG
+      memset(_b, 0, _maxXSize * sizeof(double));
+#endif
+    }
+  }
+}
+
+void Solver::setOptimizer(SparseOptimizer* optimizer)
+{
+  _optimizer = optimizer;
+}
+
+void Solver::setLevenberg(bool levenberg)
+{
+  _isLevenberg = levenberg;
+}
+
+void Solver::setAdditionalVectorSpace(size_t s)
+{
+  _additionalVectorSpace = s;
+}
+
+} // end namespace

Thirdparty/g2o/g2o/core/solver.h

@@ -0,0 +1,151 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SOLVER_H
+#define G2O_SOLVER_H
+
+#include "hyper_graph.h"
+#include "batch_stats.h"
+#include "sparse_block_matrix.h"
+#include <cstddef>
+
+namespace g2o {
+
+
+  class SparseOptimizer;
+
+  /**
+   * \brief Generic interface for a sparse solver operating on a graph which solves one iteration of the linearized objective function
+   */
+  class  Solver
+  {
+    public:
+      Solver();
+      virtual ~Solver();
+
+    public:
+      /**
+       * initialize the solver, called once before the first iteration
+       */
+      virtual bool init(SparseOptimizer* optimizer, bool online = false) = 0;
+
+      /**
+       * build the structure of the system
+       */
+      virtual bool buildStructure(bool zeroBlocks = false) = 0;
+      /**
+       * update the structures for online processing
+       */
+      virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges) = 0;
+      /**
+       * build the current system
+       */
+      virtual bool buildSystem() = 0;
+
+      /**
+       * solve Ax = b
+       */
+      virtual bool solve() = 0;
+
+      /**
+       * computes the block diagonal elements of the pattern specified in the input
+       * and stores them in given SparseBlockMatrix
+       */
+      virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices) = 0;
+
+      /**
+       * update the system while performing Levenberg, i.e., modifying the diagonal
+       * components of A by doing += lambda along the main diagonal of the Matrix.
+       * Note that this function may be called with a positive and a negative lambda.
+       * The latter is used to undo a former modification.
+       * If backup is true, then the solver should store a backup of the diagonal, which
+       * can be restored by restoreDiagonal()
+       */
+      virtual bool setLambda(double lambda, bool backup = false) = 0;
+
+      /**
+       * restore a previosly made backup of the diagonal
+       */
+      virtual void restoreDiagonal() = 0;
+
+      //! return x, the solution vector
+      double* x() { return _x;}
+      const double* x() const { return _x;}
+      //! return b, the right hand side of the system
+      double* b() { return _b;}
+      const double* b() const { return _b;}
+
+      //! return the size of the solution vector (x) and b
+      size_t vectorSize() const { return _xSize;}
+
+      //! the optimizer (graph) on which the solver works
+      SparseOptimizer* optimizer() const { return _optimizer;}
+      void setOptimizer(SparseOptimizer* optimizer);
+
+      //! the system is Levenberg-Marquardt
+      bool levenberg() const { return _isLevenberg;}
+      void setLevenberg(bool levenberg);
+
+      /**
+       * does this solver support the Schur complement for solving a system consisting of poses and
+       * landmarks. Re-implemement in a derived solver, if your solver supports it.
+       */
+      virtual bool supportsSchur() {return false;}
+
+      //! should the solver perform the schur complement or not
+      virtual bool schur()=0;
+      virtual void setSchur(bool s)=0;
+
+      size_t additionalVectorSpace() const { return _additionalVectorSpace;}
+      void setAdditionalVectorSpace(size_t s);
+
+      /**
+       * write debug output of the Hessian if system is not positive definite
+       */
+      virtual void setWriteDebug(bool) = 0;
+      virtual bool writeDebug() const = 0;
+
+      //! write the hessian to disk using the specified file name
+      virtual bool saveHessian(const std::string& /*fileName*/) const = 0;
+
+    protected:
+      SparseOptimizer* _optimizer;
+      double* _x;
+      double* _b;
+      size_t _xSize, _maxXSize;
+      bool _isLevenberg; ///< the system we gonna solve is a Levenberg-Marquardt system
+      size_t _additionalVectorSpace;
+
+      void resizeVector(size_t sx);
+
+    private:
+      // Disable the copy constructor and assignment operator
+      Solver(const Solver&) { }
+      Solver& operator= (const Solver&) { return *this; }
+  };
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/core/sparse_block_matrix.h

@@ -0,0 +1,231 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SPARSE_BLOCK_MATRIX_
+#define G2O_SPARSE_BLOCK_MATRIX_
+
+#include <map>
+#include <vector>
+#include <fstream>
+#include <iostream>
+#include <iomanip>
+#include <cassert>
+#include <Eigen/Core>
+
+#include "sparse_block_matrix_ccs.h"
+#include "matrix_structure.h"
+#include "matrix_operations.h"
+#include "../../config.h"
+
+namespace g2o {
+  using namespace Eigen;
+/**
+ * \brief Sparse matrix which uses blocks
+ *
+ * Template class that specifies a sparse block matrix.  A block
+ * matrix is a sparse matrix made of dense blocks.  These blocks
+ * cannot have a random pattern, but follow a (variable) grid
+ * structure. This structure is specified by a partition of the rows
+ * and the columns of the matrix.  The blocks are represented by the
+ * Eigen::Matrix structure, thus they can be statically or dynamically
+ * allocated. For efficiency reasons it is convenient to allocate them
+ * statically, when possible. A static block matrix has all blocks of
+ * the same size, and the size of the block is specified by the
+ * template argument.  If this is not the case, and you have different
+ * block sizes than you have to use a dynamic-block matrix (default
+ * template argument).  
+ */
+template <class MatrixType = MatrixXd >
+class SparseBlockMatrix {
+
+  public:
+    //! this is the type of the elementary block, it is an Eigen::Matrix.
+    typedef MatrixType SparseMatrixBlock;
+
+    //! columns of the matrix
+    inline int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
+    //! rows of the matrix
+    inline int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
+
+    typedef std::map<int, SparseMatrixBlock*> IntBlockMap;
+
+    /**
+     * constructs a sparse block matrix having a specific layout
+     * @param rbi: array of int containing the row layout of the blocks. 
+     * the component i of the array should contain the index of the first row of the block i+1.
+     * @param rbi: array of int containing the column layout of the blocks. 
+     *  the component i of the array should contain the index of the first col of the block i+1.
+     * @param rb: number of row blocks
+     * @param cb: number of col blocks
+     * @param hasStorage: set it to true if the matrix "owns" the blocks, thus it deletes it on destruction.
+     * if false the matrix is only a "view" over an existing structure.
+     */
+    SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true);
+
+    SparseBlockMatrix();
+
+    ~SparseBlockMatrix();
+
+    
+    //! this zeroes all the blocks. If dealloc=true the blocks are removed from memory
+    void clear(bool dealloc=false) ;
+
+    //! returns the block at location r,c. if alloc=true he block is created if it does not exist
+    SparseMatrixBlock* block(int r, int c, bool alloc=false);
+    //! returns the block at location r,c
+    const SparseMatrixBlock* block(int r, int c) const;
+
+    //! how many rows does the block at block-row r has?
+    inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
+
+    //! how many cols does the block at block-col c has?
+    inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
+
+    //! where does the row at block-row r starts?
+    inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
+
+    //! where does the col at block-col r starts?
+    inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
+
+    //! number of non-zero elements
+    size_t nonZeros() const; 
+    //! number of allocated blocks
+    size_t nonZeroBlocks() const; 
+
+    //! deep copy of a sparse-block-matrix;
+    SparseBlockMatrix* clone() const ;
+
+    /**
+     * returns a view or a copy of the block matrix
+     * @param rmin: starting block row
+     * @param rmax: ending  block row
+     * @param cmin: starting block col
+     * @param cmax: ending  block col
+     * @param alloc: if true it makes a deep copy, if false it creates a view.
+     */
+    SparseBlockMatrix*  slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const;
+
+    //! transposes a block matrix, The transposed type should match the argument false on failure
+    template <class MatrixTransposedType>
+    bool transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const;
+
+    //! adds the current matrix to the destination
+    bool add(SparseBlockMatrix<MatrixType>*& dest) const ;
+
+    //! dest = (*this) *  M
+    template <class MatrixResultType, class MatrixFactorType>
+    bool multiply(SparseBlockMatrix<MatrixResultType> *& dest, const SparseBlockMatrix<MatrixFactorType>* M) const;
+
+    //! dest = (*this) *  src
+    void multiply(double*& dest, const double* src) const;
+
+    /**
+     * compute dest = (*this) *  src
+     * However, assuming that this is a symmetric matrix where only the upper triangle is stored
+     */
+    void multiplySymmetricUpperTriangle(double*& dest, const double* src) const;
+
+    //! dest = M * (*this)
+    void rightMultiply(double*& dest, const double* src) const;
+
+    //! *this *= a
+    void scale( double a);
+
+    /**
+     * writes in dest a block permutaton specified by pinv.
+     * @param pinv: array such that new_block[i] = old_block[pinv[i]]
+     */
+    bool symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool onlyUpper=false) const;
+
+    /**
+     * fill the CCS arrays of a matrix, arrays have to be allocated beforehand
+     */
+    int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const;
+
+    /**
+     * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes
+     * the values and assumes that column and row structures have already been written.
+     */
+    int fillCCS(double* Cx, bool upperTriangle = false) const;
+
+    //! exports the non zero blocks in the structure matrix ms
+    void fillBlockStructure(MatrixStructure& ms) const;
+
+    //! the block matrices per block-column
+    const std::vector<IntBlockMap>& blockCols() const { return _blockCols;}
+    std::vector<IntBlockMap>& blockCols() { return _blockCols;}
+
+    //! indices of the row blocks
+    const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
+    std::vector<int>& rowBlockIndices() { return _rowBlockIndices;}
+
+    //! indices of the column blocks
+    const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
+    std::vector<int>& colBlockIndices() { return _colBlockIndices;}
+
+    /**
+     * write the content of this matrix to a stream loadable by Octave
+     * @param upperTriangle does this matrix store only the upper triangular blocks
+     */
+    bool writeOctave(const char* filename, bool upperTriangle = true) const;
+
+    /**
+     * copy into CCS structure
+     * @return number of processed blocks, -1 on error
+     */
+    int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
+
+    /**
+     * copy as transposed into a CCS structure
+     * @return number of processed blocks, -1 on error
+     */
+    int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
+
+    /**
+     * take over the memory and matrix pattern from a hash matrix.
+     * The structure of the hash matrix will be cleared.
+     */
+    void takePatternFromHash(SparseBlockMatrixHashMap<MatrixType>& hashMatrix);
+
+  protected:
+    std::vector<int> _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
+    std::vector<int> _colBlockIndices; ///< vector of the indices of the blocks along the cols
+    //! array of maps of blocks. The index of the array represent a block column of the matrix
+    //! and the block column is stored as a map row_block -> matrix_block_ptr.
+    std::vector <IntBlockMap> _blockCols;
+    bool _hasStorage;
+};
+
+template < class  MatrixType >
+std::ostream& operator << (std::ostream&, const SparseBlockMatrix<MatrixType>& m);
+
+  typedef SparseBlockMatrix<MatrixXd> SparseBlockMatrixXd;   
+
+} //end namespace
+
+#include "sparse_block_matrix.hpp"
+
+#endif

Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp

@@ -0,0 +1,657 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+namespace g2o {
+  using namespace Eigen;
+
+  namespace {
+    struct TripletEntry
+    {
+      int r, c;
+      double x;
+      TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {}
+    };
+    struct TripletColSort
+    {
+      bool operator()(const TripletEntry& e1, const TripletEntry& e2) const
+      {
+        return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r);
+      }
+    };
+    /** Helper class to sort pair based on first elem */
+    template<class T1, class T2, class Pred = std::less<T1> >
+    struct CmpPairFirst {
+      bool operator()(const std::pair<T1,T2>& left, const std::pair<T1,T2>& right) {
+        return Pred()(left.first, right.first);
+      }
+    };
+  }
+
+  template <class MatrixType>
+  SparseBlockMatrix<MatrixType>::SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage):
+    _rowBlockIndices(rbi,rbi+rb),
+    _colBlockIndices(cbi,cbi+cb),
+    _blockCols(cb), _hasStorage(hasStorage) 
+  {
+  }
+
+  template <class MatrixType>
+  SparseBlockMatrix<MatrixType>::SparseBlockMatrix( ):
+    _blockCols(0), _hasStorage(true) 
+  {
+  }
+
+  template <class MatrixType>
+  void SparseBlockMatrix<MatrixType>::clear(bool dealloc) {
+#   ifdef G2O_OPENMP
+#   pragma omp parallel for default (shared) if (_blockCols.size() > 100)
+#   endif
+    for (int i=0; i < static_cast<int>(_blockCols.size()); ++i) {
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+        typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+        if (_hasStorage && dealloc)
+          delete b;
+        else
+          b->setZero();
+      }
+      if (_hasStorage && dealloc)
+        _blockCols[i].clear();
+    }
+  }
+
+  template <class MatrixType>
+  SparseBlockMatrix<MatrixType>::~SparseBlockMatrix(){
+    if (_hasStorage)
+      clear(true);
+  }
+
+  template <class MatrixType>
+  typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* SparseBlockMatrix<MatrixType>::block(int r, int c, bool alloc) {
+    typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator it =_blockCols[c].find(r);
+    typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* _block=0;
+    if (it==_blockCols[c].end()){
+      if (!_hasStorage && ! alloc )
+        return 0;
+      else {
+        int rb=rowsOfBlock(r);
+        int cb=colsOfBlock(c);
+        _block=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(rb,cb);
+        _block->setZero();
+        std::pair < typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator, bool> result
+          =_blockCols[c].insert(std::make_pair(r,_block)); (void) result;
+        assert (result.second);
+      }
+    } else {
+      _block=it->second;
+    }
+    return _block;
+  }
+
+  template <class MatrixType>
+  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* SparseBlockMatrix<MatrixType>::block(int r, int c) const {
+    typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it =_blockCols[c].find(r);
+    if (it==_blockCols[c].end())
+  return 0;
+    return it->second;
+  }
+
+
+  template <class MatrixType>
+  SparseBlockMatrix<MatrixType>* SparseBlockMatrix<MatrixType>::clone() const {
+    SparseBlockMatrix* ret= new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+        typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(*it->second);
+        ret->_blockCols[i].insert(std::make_pair(it->first, b));
+      }
+    }
+    ret->_hasStorage=true;
+    return ret;
+  }
+
+
+  template <class MatrixType>
+  template <class MatrixTransposedType>
+  bool SparseBlockMatrix<MatrixType>::transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const {
+    if (! dest){
+      dest=new SparseBlockMatrix<MatrixTransposedType>(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size());
+    } else {
+      if (! dest->_hasStorage)
+        return false;
+      if(_rowBlockIndices.size()!=dest->_colBlockIndices.size())
+        return false;
+      if (_colBlockIndices.size()!=dest->_rowBlockIndices.size())
+        return  false;
+      for (size_t i=0; i<_rowBlockIndices.size(); ++i){
+        if(_rowBlockIndices[i]!=dest->_colBlockIndices[i])
+          return false;
+      }
+      for (size_t i=0; i<_colBlockIndices.size(); ++i){
+        if(_colBlockIndices[i]!=dest->_rowBlockIndices[i])
+          return false;
+      }
+    }
+
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+        typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
+        typename SparseBlockMatrix<MatrixTransposedType>::SparseMatrixBlock* d=dest->block(i,it->first,true);
+        *d = s->transpose();
+      }
+    }
+    return true;
+  }
+
+  template <class MatrixType>
+  bool SparseBlockMatrix<MatrixType>::add(SparseBlockMatrix*& dest) const {
+    if (! dest){
+      dest=new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
+    } else {
+      if (! dest->_hasStorage)
+        return false;
+      if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size())
+        return false;
+      if (_colBlockIndices.size()!=dest->_colBlockIndices.size())
+        return  false;
+      for (size_t i=0; i<_rowBlockIndices.size(); ++i){
+        if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i])
+          return false;
+      }
+      for (size_t i=0; i<_colBlockIndices.size(); ++i){
+        if(_colBlockIndices[i]!=dest->_colBlockIndices[i])
+          return false;
+      }
+    }
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+        typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
+        typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* d=dest->block(it->first,i,true);
+        (*d)+=*s;
+      }
+    }
+    return true;
+  }
+
+  template <class MatrixType>
+  template < class MatrixResultType, class MatrixFactorType >
+  bool SparseBlockMatrix<MatrixType>::multiply(SparseBlockMatrix<MatrixResultType>*& dest, const SparseBlockMatrix<MatrixFactorType> * M) const {
+    // sanity check
+    if (_colBlockIndices.size()!=M->_rowBlockIndices.size())
+      return false;
+    for (size_t i=0; i<_colBlockIndices.size(); ++i){
+      if (_colBlockIndices[i]!=M->_rowBlockIndices[i])
+        return false;
+    }
+    if (! dest) {
+      dest=new SparseBlockMatrix<MatrixResultType>(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() );
+    }
+    if (! dest->_hasStorage)
+      return false;
+    for (size_t i=0; i<M->_blockCols.size(); ++i){
+      for (typename SparseBlockMatrix<MatrixFactorType>::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){
+        // look for a non-zero block in a row of column it
+        int colM=i;
+        const typename SparseBlockMatrix<MatrixFactorType>::SparseMatrixBlock *b=it->second;
+        typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator rbt=_blockCols[it->first].begin();
+        while(rbt!=_blockCols[it->first].end()){
+          //int colA=it->first;
+          int rowA=rbt->first;
+          typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock *a=rbt->second;
+          typename SparseBlockMatrix<MatrixResultType>::SparseMatrixBlock *c=dest->block(rowA,colM,true);
+          assert (c->rows()==a->rows());
+          assert (c->cols()==b->cols());
+          ++rbt;
+          (*c)+=(*a)*(*b);
+        }
+      }
+    }
+    return false;
+  }
+
+  template <class MatrixType>
+  void SparseBlockMatrix<MatrixType>::multiply(double*& dest, const double* src) const {
+    if (! dest){
+      dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
+      memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
+    }
+
+    // map the memory by Eigen
+    Eigen::Map<VectorXd> destVec(dest, rows());
+    const Eigen::Map<const VectorXd> srcVec(src, cols());
+
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      int srcOffset = i ? _colBlockIndices[i-1] : 0;
+
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+        const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+        int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;
+        // destVec += *a * srcVec (according to the sub-vector parts)
+        internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
+      }
+    }
+  }
+
+  template <class MatrixType>
+  void SparseBlockMatrix<MatrixType>::multiplySymmetricUpperTriangle(double*& dest, const double* src) const
+  {
+    if (! dest){
+      dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
+      memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
+    }
+
+    // map the memory by Eigen
+    Eigen::Map<VectorXd> destVec(dest, rows());
+    const Eigen::Map<const VectorXd> srcVec(src, cols());
+
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      int srcOffset = colBaseOfBlock(i);
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+        const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+        int destOffset = rowBaseOfBlock(it->first);
+        if (destOffset > srcOffset) // only upper triangle
+          break;
+        // destVec += *a * srcVec (according to the sub-vector parts)
+        internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
+        if (destOffset < srcOffset)
+          internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset);
+      }
+    }
+  }
+
+  template <class MatrixType>
+  void SparseBlockMatrix<MatrixType>::rightMultiply(double*& dest, const double* src) const {
+    int destSize=cols();
+
+    if (! dest){
+      dest=new double [ destSize ];
+      memset(dest,0, destSize*sizeof(double));
+    }
+
+    // map the memory by Eigen
+    Eigen::Map<VectorXd> destVec(dest, destSize);
+    Eigen::Map<const VectorXd> srcVec(src, rows());
+
+#   ifdef G2O_OPENMP
+#   pragma omp parallel for default (shared) schedule(dynamic, 10)
+#   endif
+    for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
+      int destOffset = colBaseOfBlock(i);
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); 
+          it!=_blockCols[i].end(); 
+          ++it){
+        const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+        int srcOffset = rowBaseOfBlock(it->first);
+        // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
+        internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+      }
+    }
+    
+  }
+
+  template <class MatrixType>
+  void SparseBlockMatrix<MatrixType>::scale(double a_) {
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+        typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+        *a *= a_;
+      }
+    }
+  }
+
+  template <class MatrixType>
+  SparseBlockMatrix<MatrixType>*  SparseBlockMatrix<MatrixType>::slice(int rmin, int rmax, int cmin, int cmax, bool alloc) const {
+    int m=rmax-rmin;
+    int n=cmax-cmin;
+    int rowIdx [m];
+    rowIdx[0] = rowsOfBlock(rmin);
+    for (int i=1; i<m; ++i){
+      rowIdx[i]=rowIdx[i-1]+rowsOfBlock(rmin+i);
+    }
+
+    int colIdx [n];
+    colIdx[0] = colsOfBlock(cmin);
+    for (int i=1; i<n; ++i){
+      colIdx[i]=colIdx[i-1]+colsOfBlock(cmin+i);
+    }
+    typename SparseBlockMatrix<MatrixType>::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);
+    for (int i=0; i<n; ++i){
+      int mc=cmin+i;
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){
+        if (it->first >= rmin && it->first < rmax){
+          typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock (* (it->second) ) : it->second;
+          s->_blockCols[i].insert(std::make_pair(it->first-rmin, b));
+        }
+      }
+    }
+    s->_hasStorage=alloc;
+    return s;
+  }
+
+  template <class MatrixType>
+  size_t SparseBlockMatrix<MatrixType>::nonZeroBlocks() const {
+    size_t count=0;
+    for (size_t i=0; i<_blockCols.size(); ++i)
+      count+=_blockCols[i].size();
+    return count;
+  }
+
+  template <class MatrixType>
+  size_t SparseBlockMatrix<MatrixType>::nonZeros() const{
+    if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) {
+      size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime;
+      return nnz;
+    } else {
+      size_t count=0;
+      for (size_t i=0; i<_blockCols.size(); ++i){
+        for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+          const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+          count += a->cols()*a->rows();
+        }
+      }
+      return count;
+    }
+  }
+
+  template <class MatrixType>
+  std::ostream& operator << (std::ostream& os, const SparseBlockMatrix<MatrixType>& m){
+    os << "RBI: " << m.rowBlockIndices().size();
+    for (size_t i=0; i<m.rowBlockIndices().size(); ++i)
+      os << " " << m.rowBlockIndices()[i];
+    os << std::endl;
+    os << "CBI: " << m.colBlockIndices().size();
+    for (size_t i=0; i<m.colBlockIndices().size(); ++i)
+      os << " " << m.colBlockIndices()[i];
+    os << std::endl;
+
+    for (size_t i=0; i<m.blockCols().size(); ++i){
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){
+        const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+        os << "BLOCK: " << it->first << " " << i << std::endl;
+        os << *b << std::endl;
+      }
+    }
+    return os;
+  }
+
+  template <class MatrixType>
+  bool SparseBlockMatrix<MatrixType>::symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool  upperTriangle) const{
+    // compute the permuted version of the new row/column layout
+    size_t n=_rowBlockIndices.size();
+    // computed the block sizes
+    int blockSizes[_rowBlockIndices.size()];
+    blockSizes[0]=_rowBlockIndices[0];
+    for (size_t i=1; i<n; ++i){
+      blockSizes[i]=_rowBlockIndices[i]-_rowBlockIndices[i-1];
+    }
+    // permute them
+    int pBlockIndices[_rowBlockIndices.size()];
+    for (size_t i=0; i<n; ++i){
+      pBlockIndices[pinv[i]]=blockSizes[i];
+    }
+    for (size_t i=1; i<n; ++i){
+      pBlockIndices[i]+=pBlockIndices[i-1];
+    }
+    // allocate C, or check the structure;
+    if (! dest){
+      dest=new SparseBlockMatrix(pBlockIndices, pBlockIndices, n, n);
+    } else {
+      if (dest->_rowBlockIndices.size()!=n)
+        return false;
+      if (dest->_colBlockIndices.size()!=n)
+        return false;
+      for (size_t i=0; i<n; ++i){
+        if (dest->_rowBlockIndices[i]!=pBlockIndices[i])
+          return false;
+        if (dest->_colBlockIndices[i]!=pBlockIndices[i])
+          return false;
+      }
+      dest->clear();
+    }
+    // now ready to permute the columns
+    for (size_t i=0; i<n; ++i){
+      //cerr << PVAR(i) <<  " ";
+      int pi=pinv[i];
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); 
+          it!=_blockCols[i].end(); ++it){
+        int pj=pinv[it->first];
+
+        const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
+
+        typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=0;
+        if (! upperTriangle || pj<=pi) {
+          b=dest->block(pj,pi,true);
+          assert(b->cols()==s->cols());
+          assert(b->rows()==s->rows());
+          *b=*s;
+        } else {
+          b=dest->block(pi,pj,true);
+          assert(b);
+          assert(b->rows()==s->cols());
+          assert(b->cols()==s->rows());
+          *b=s->transpose();
+        }
+      }
+      //cerr << endl;
+      // within each row, 
+    }
+    return true;
+    
+  }
+
+  template <class MatrixType>
+  int SparseBlockMatrix<MatrixType>::fillCCS(double* Cx, bool upperTriangle) const
+  {
+    assert(Cx && "Target destination is NULL");
+    double* CxStart = Cx;
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      int cstart=i ? _colBlockIndices[i-1] : 0;
+      int csize=colsOfBlock(i);
+      for (int c=0; c<csize; ++c) {
+        for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+          const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+          int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
+
+          int elemsToCopy = b->rows();
+          if (upperTriangle && rstart == cstart)
+            elemsToCopy = c + 1;
+          memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
+          Cx += elemsToCopy;
+
+        }
+      }
+    }
+    return Cx - CxStart;
+  }
+
+  template <class MatrixType>
+  int SparseBlockMatrix<MatrixType>::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const
+  {
+    assert(Cp && Ci && Cx && "Target destination is NULL");
+    int nz=0;
+    for (size_t i=0; i<_blockCols.size(); ++i){
+      int cstart=i ? _colBlockIndices[i-1] : 0;
+      int csize=colsOfBlock(i);
+      for (int c=0; c<csize; ++c) {
+        *Cp=nz;
+        for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+          const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+          int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
+
+          int elemsToCopy = b->rows();
+          if (upperTriangle && rstart == cstart)
+            elemsToCopy = c + 1;
+          for (int r=0; r<elemsToCopy; ++r){
+            *Cx++ = (*b)(r,c);
+            *Ci++ = rstart++;
+            ++nz;
+          }
+        }
+        ++Cp;
+      }
+    }
+    *Cp=nz;
+    return nz;
+  }
+
+  template <class MatrixType>
+  void SparseBlockMatrix<MatrixType>::fillBlockStructure(MatrixStructure& ms) const
+  {
+    int n     = _colBlockIndices.size();
+    int nzMax = (int)nonZeroBlocks();
+
+    ms.alloc(n, nzMax);
+    ms.m = _rowBlockIndices.size();
+
+    int nz = 0;
+    int* Cp = ms.Ap;
+    int* Ci = ms.Aii;
+    for (int i = 0; i < static_cast<int>(_blockCols.size()); ++i){
+      *Cp = nz;
+      const int& c = i;
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+        const int& r = it->first;
+        if (r <= c) {
+          *Ci++ = r;
+          ++nz;
+        }
+      }
+      Cp++;
+    }
+    *Cp=nz;
+    assert(nz <= nzMax);
+  }
+
+  template <class MatrixType>
+  bool SparseBlockMatrix<MatrixType>::writeOctave(const char* filename, bool upperTriangle) const
+  {
+    std::string name = filename;
+    std::string::size_type lastDot = name.find_last_of('.');
+    if (lastDot != std::string::npos) 
+      name = name.substr(0, lastDot);
+
+    std::vector<TripletEntry> entries;
+    for (size_t i = 0; i<_blockCols.size(); ++i){
+      const int& c = i;
+      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+        const int& r = it->first;
+        const MatrixType& m = *(it->second);
+        for (int cc = 0; cc < m.cols(); ++cc)
+          for (int rr = 0; rr < m.rows(); ++rr) {
+            int aux_r = rowBaseOfBlock(r) + rr;
+            int aux_c = colBaseOfBlock(c) + cc;
+            entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc)));
+            if (upperTriangle && r != c) {
+              entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc)));
+            }
+          }
+      }
+    }
+
+    int nz = entries.size();
+    std::sort(entries.begin(), entries.end(), TripletColSort());
+
+    std::ofstream fout(filename);
+    fout << "# name: " << name << std::endl;
+    fout << "# type: sparse matrix" << std::endl;
+    fout << "# nnz: " << nz << std::endl;
+    fout << "# rows: " << rows() << std::endl;
+    fout << "# columns: " << cols() << std::endl;
+    fout << std::setprecision(9) << std::fixed << std::endl;
+
+    for (std::vector<TripletEntry>::const_iterator it = entries.begin(); it != entries.end(); ++it) {
+      const TripletEntry& entry = *it;
+      fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl;
+    }
+    return fout.good();
+  }
+
+  template <class MatrixType>
+  int SparseBlockMatrix<MatrixType>::fillSparseBlockMatrixCCS(SparseBlockMatrixCCS<MatrixType>& blockCCS) const
+  {
+    blockCCS.blockCols().resize(blockCols().size());
+    int numblocks = 0;
+    for (size_t i = 0; i < blockCols().size(); ++i) {
+      const IntBlockMap& row = blockCols()[i];
+      typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[i];
+      dest.clear();
+      dest.reserve(row.size());
+      for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
+        dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(it->first, it->second));
+        ++numblocks;
+      }
+    }
+    return numblocks;
+  }
+
+  template <class MatrixType>
+  int SparseBlockMatrix<MatrixType>::fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS<MatrixType>& blockCCS) const
+  {
+    blockCCS.blockCols().clear();
+    blockCCS.blockCols().resize(_rowBlockIndices.size());
+    int numblocks = 0;
+    for (size_t i = 0; i < blockCols().size(); ++i) {
+      const IntBlockMap& row = blockCols()[i];
+      for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
+        typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[it->first];
+        dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(i, it->second));
+        ++numblocks;
+      }
+    }
+    return numblocks;
+  }
+
+  template <class MatrixType>
+  void SparseBlockMatrix<MatrixType>::takePatternFromHash(SparseBlockMatrixHashMap<MatrixType>& hashMatrix)
+  {
+    // sort the sparse columns and add them to the map structures by
+    // exploiting that we are inserting a sorted structure
+    typedef std::pair<int, MatrixType*> SparseColumnPair;
+    typedef typename SparseBlockMatrixHashMap<MatrixType>::SparseColumn HashSparseColumn;
+    for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) {
+      // prepare a temporary vector for sorting
+      HashSparseColumn& column = hashMatrix.blockCols()[i];
+      if (column.size() == 0)
+        continue;
+      std::vector<SparseColumnPair> sparseRowSorted; // temporary structure
+      sparseRowSorted.reserve(column.size());
+      for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it)
+        sparseRowSorted.push_back(*it);
+      std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst<int, MatrixType*>());
+      // try to free some memory early
+      HashSparseColumn aux;
+      swap(aux, column);
+      // now insert sorted vector to the std::map structure
+      IntBlockMap& destColumnMap = blockCols()[i];
+      destColumnMap.insert(sparseRowSorted[0]);
+      for (size_t j = 1; j < sparseRowSorted.size(); ++j) {
+        typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator hint = destColumnMap.end();
+        --hint; // cppreference says the element goes after the hint (until C++11)
+        destColumnMap.insert(hint, sparseRowSorted[j]);
+      }
+    }
+  }
+
+}// end namespace

Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h

@@ -0,0 +1,282 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SPARSE_BLOCK_MATRIX_CCS_H
+#define G2O_SPARSE_BLOCK_MATRIX_CCS_H
+
+#include <vector>
+#include <cassert>
+#include <Eigen/Core>
+
+#include "../../config.h"
+#include "matrix_operations.h"
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+namespace g2o {
+
+  /**
+   * \brief Sparse matrix which uses blocks
+   *
+   * This class is used as a const view on a SparseBlockMatrix
+   * which allows a faster iteration over the elements of the
+   * matrix.
+   */
+  template <class MatrixType>
+  class SparseBlockMatrixCCS
+  {
+    public:
+      //! this is the type of the elementary block, it is an Eigen::Matrix.
+      typedef MatrixType SparseMatrixBlock;
+
+      //! columns of the matrix
+      int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
+      //! rows of the matrix
+      int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
+
+      /**
+       * \brief A block within a column
+       */
+      struct RowBlock
+      {
+        int row;              ///< row of the block
+        MatrixType* block;    ///< matrix pointer for the block
+        RowBlock() : row(-1), block(0) {}
+        RowBlock(int r, MatrixType* b) : row(r), block(b) {}
+        bool operator<(const RowBlock& other) const { return row < other.row;}
+      };
+      typedef std::vector<RowBlock>      SparseColumn;
+
+      SparseBlockMatrixCCS(const std::vector<int>& rowIndices, const std::vector<int>& colIndices) :
+        _rowBlockIndices(rowIndices), _colBlockIndices(colIndices)
+      {}
+
+      //! how many rows does the block at block-row r has?
+      int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
+
+      //! how many cols does the block at block-col c has?
+      int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
+
+      //! where does the row at block-row r start?
+      int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
+
+      //! where does the col at block-col r start?
+      int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
+
+      //! the block matrices per block-column
+      const std::vector<SparseColumn>& blockCols() const { return _blockCols;}
+      std::vector<SparseColumn>& blockCols() { return _blockCols;}
+
+      //! indices of the row blocks
+      const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
+
+      //! indices of the column blocks
+      const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
+
+      void rightMultiply(double*& dest, const double* src) const
+      {
+        int destSize=cols();
+
+        if (! dest){
+          dest=new double [ destSize ];
+          memset(dest,0, destSize*sizeof(double));
+        }
+
+        // map the memory by Eigen
+        Eigen::Map<Eigen::VectorXd> destVec(dest, destSize);
+        Eigen::Map<const Eigen::VectorXd> srcVec(src, rows());
+
+#      ifdef G2O_OPENMP
+#      pragma omp parallel for default (shared) schedule(dynamic, 10)
+#      endif
+        for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
+          int destOffset = colBaseOfBlock(i);
+          for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+            const SparseMatrixBlock* a = it->block;
+            int srcOffset = rowBaseOfBlock(it->row);
+            // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
+            internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+          }
+        }
+      }
+
+      /**
+       * sort the blocks in each column
+       */
+      void sortColumns()
+      {
+        for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
+          std::sort(_blockCols[i].begin(), _blockCols[i].end());
+        }
+      }
+
+      /**
+       * fill the CCS arrays of a matrix, arrays have to be allocated beforehand
+       */
+      int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const
+      {
+        assert(Cp && Ci && Cx && "Target destination is NULL");
+        int nz=0;
+        for (size_t i=0; i<_blockCols.size(); ++i){
+          int cstart=i ? _colBlockIndices[i-1] : 0;
+          int csize=colsOfBlock(i);
+          for (int c=0; c<csize; ++c) {
+            *Cp=nz;
+            for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+              const SparseMatrixBlock* b=it->block;
+              int rstart=it->row ? _rowBlockIndices[it->row-1] : 0;
+
+              int elemsToCopy = b->rows();
+              if (upperTriangle && rstart == cstart)
+                elemsToCopy = c + 1;
+              for (int r=0; r<elemsToCopy; ++r){
+                *Cx++ = (*b)(r,c);
+                *Ci++ = rstart++;
+                ++nz;
+              }
+            }
+            ++Cp;
+          }
+        }
+        *Cp=nz;
+        return nz;
+      }
+
+      /**
+       * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes
+       * the values and assumes that column and row structures have already been written.
+       */
+      int fillCCS(double* Cx, bool upperTriangle = false) const
+      {
+        assert(Cx && "Target destination is NULL");
+        double* CxStart = Cx;
+        int cstart = 0;
+        for (size_t i=0; i<_blockCols.size(); ++i){
+          int csize = _colBlockIndices[i] - cstart;
+          for (int c=0; c<csize; ++c) {
+            for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+              const SparseMatrixBlock* b = it->block;
+              int rstart = it->row ? _rowBlockIndices[it->row-1] : 0;
+
+              int elemsToCopy = b->rows();
+              if (upperTriangle && rstart == cstart)
+                elemsToCopy = c + 1;
+              memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
+              Cx += elemsToCopy;
+
+            }
+          }
+          cstart = _colBlockIndices[i];
+        }
+        return Cx - CxStart;
+      }
+
+    protected:
+      const std::vector<int>& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
+      const std::vector<int>& _colBlockIndices; ///< vector of the indices of the blocks along the cols
+      std::vector<SparseColumn> _blockCols;     ///< the matrices stored in CCS order
+  };
+
+
+
+  /**
+   * \brief Sparse matrix which uses blocks based on hash structures
+   *
+   * This class is used to construct the pattern of a sparse block matrix 
+   */
+  template <class MatrixType>
+  class SparseBlockMatrixHashMap
+  {
+    public:
+      //! this is the type of the elementary block, it is an Eigen::Matrix.
+      typedef MatrixType SparseMatrixBlock;
+
+      //! columns of the matrix
+      int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
+      //! rows of the matrix
+      int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
+
+      typedef std::unordered_map<int, MatrixType*> SparseColumn;
+
+      SparseBlockMatrixHashMap(const std::vector<int>& rowIndices, const std::vector<int>& colIndices) :
+        _rowBlockIndices(rowIndices), _colBlockIndices(colIndices)
+      {}
+
+      //! how many rows does the block at block-row r has?
+      int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
+
+      //! how many cols does the block at block-col c has?
+      int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
+
+      //! where does the row at block-row r start?
+      int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
+
+      //! where does the col at block-col r start?
+      int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
+
+      //! the block matrices per block-column
+      const std::vector<SparseColumn>& blockCols() const { return _blockCols;}
+      std::vector<SparseColumn>& blockCols() { return _blockCols;}
+
+      //! indices of the row blocks
+      const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
+
+      //! indices of the column blocks
+      const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
+
+      /**
+       * add a block to the pattern, return a pointer to the added block
+       */
+      MatrixType* addBlock(int r, int c, bool zeroBlock = false)
+      {
+        assert(c <(int)_blockCols.size() && "accessing column which is not available");
+        SparseColumn& sparseColumn = _blockCols[c];
+        typename SparseColumn::iterator foundIt = sparseColumn.find(r);
+        if (foundIt == sparseColumn.end()) {
+          int rb = rowsOfBlock(r);
+          int cb = colsOfBlock(c);
+          MatrixType* m = new MatrixType(rb, cb);
+          if (zeroBlock)
+            m->setZero();
+          sparseColumn[r] = m;
+          return m;
+        }
+        return foundIt->second;
+      }
+
+    protected:
+      const std::vector<int>& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
+      const std::vector<int>& _colBlockIndices; ///< vector of the indices of the blocks along the cols
+      std::vector<SparseColumn> _blockCols;     ///< the matrices stored in CCS order
+  };
+
+} //end namespace
+
+#endif

Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h

@@ -0,0 +1,108 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
+#define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+
+#include "../../config.h"
+#include "matrix_operations.h"
+
+namespace g2o {
+
+  /**
+   * \brief Sparse matrix which uses blocks on the diagonal
+   *
+   * This class is used as a const view on a SparseBlockMatrix
+   * which allows a faster iteration over the elements of the
+   * matrix.
+   */
+  template <class MatrixType>
+  class SparseBlockMatrixDiagonal
+  {
+    public:
+      //! this is the type of the elementary block, it is an Eigen::Matrix.
+      typedef MatrixType SparseMatrixBlock;
+
+      //! columns of the matrix
+      int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
+      //! rows of the matrix
+      int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
+
+      typedef std::vector<MatrixType, Eigen::aligned_allocator<MatrixType> >      DiagonalVector;
+
+      SparseBlockMatrixDiagonal(const std::vector<int>& blockIndices) :
+        _blockIndices(blockIndices)
+      {}
+
+      //! how many rows/cols does the block at block-row / block-column r has?
+      inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; }
+
+      //! where does the row /col at block-row / block-column r starts?
+      inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; }
+
+      //! the block matrices per block-column
+      const DiagonalVector& diagonal() const { return _diagonal;}
+      DiagonalVector& diagonal() { return _diagonal;}
+
+      //! indices of the row blocks
+      const std::vector<int>& blockIndices() const { return _blockIndices;}
+
+      void multiply(double*& dest, const double* src) const
+      {
+        int destSize=cols();
+        if (! dest) {
+          dest=new double[destSize];
+          memset(dest,0, destSize*sizeof(double));
+        }
+
+        // map the memory by Eigen
+        Eigen::Map<Eigen::VectorXd> destVec(dest, destSize);
+        Eigen::Map<const Eigen::VectorXd> srcVec(src, rows());
+
+#      ifdef G2O_OPENMP
+#      pragma omp parallel for default (shared) schedule(dynamic, 10)
+#      endif
+        for (int i=0; i < static_cast<int>(_diagonal.size()); ++i){
+          int destOffset = baseOfBlock(i);
+          int srcOffset = destOffset;
+          const SparseMatrixBlock& A = _diagonal[i];
+          // destVec += *A.transpose() * srcVec (according to the sub-vector parts)
+          internal::axpy(A, srcVec, srcOffset, destVec, destOffset);
+        }
+      }
+
+    protected:
+      const std::vector<int>& _blockIndices; ///< vector of the indices of the blocks along the diagonal
+      DiagonalVector _diagonal;
+  };
+
+} //end namespace
+
+#endif

Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp

@@ -0,0 +1,107 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "sparse_block_matrix.h"
+#include <iostream>
+
+using namespace std;
+using namespace g2o;
+using namespace Eigen;
+
+typedef SparseBlockMatrix< MatrixXd >
+SparseBlockMatrixX;
+
+std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) {
+  for (int i=0; i<m.rows(); ++i){
+    for (int j=0; j<m.cols(); ++j)
+      cerr << m(i,j) << " ";
+    cerr << endl;
+  }
+  return os;
+}
+
+int main (int argc, char** argv){
+  int rcol[] = {3,6,8,12};
+  int ccol[] = {2,4,13};
+  cerr << "creation" << endl;
+  SparseBlockMatrixX* M=new SparseBlockMatrixX(rcol, ccol, 4,3);
+
+  cerr << "block access" << endl;
+
+  SparseBlockMatrixX::SparseMatrixBlock* b=M->block(0,0, true);
+  cerr << b->rows() << " " << b->cols() << endl;
+  for (int i=0; i<b->rows(); ++i)
+    for (int j=0; j<b->cols(); ++j){
+      (*b)(i,j)=i*b->cols()+j;
+    }
+
+
+  cerr << "block access 2" << endl;
+  b=M->block(0,2, true);
+  cerr << b->rows() << " " << b->cols() << endl;
+  for (int i=0; i<b->rows(); ++i)
+    for (int j=0; j<b->cols(); ++j){
+      (*b)(i,j)=i*b->cols()+j;
+    }
+
+  b=M->block(3,2, true);
+  cerr << b->rows() << " " << b->cols() << endl;
+  for (int i=0; i<b->rows(); ++i)
+    for (int j=0; j<b->cols(); ++j){
+      (*b)(i,j)=i*b->cols()+j;
+    }
+
+  cerr << *M << endl;
+
+  cerr << "SUM" << endl;
+
+  SparseBlockMatrixX* Ms=0;
+  M->add(Ms);
+  M->add(Ms);
+  cerr << *Ms;
+  
+  SparseBlockMatrixX* Mt=0;
+  M->transpose(Mt);
+  cerr << *Mt << endl;
+
+  SparseBlockMatrixX* Mp=0;
+  M->multiply(Mp, Mt);
+  cerr << *Mp << endl;
+  
+  int iperm[]={3,2,1,0};
+  SparseBlockMatrixX* PMp=0;
+
+  Mp->symmPermutation(PMp,iperm, false);
+  cerr << *PMp << endl;
+
+  PMp->clear(true);
+  Mp->block(3,0)->fill(0.);
+  Mp->symmPermutation(PMp,iperm, true);
+  cerr << *PMp << endl;
+  
+  
+  
+}

Thirdparty/g2o/g2o/core/sparse_optimizer.cpp

@@ -0,0 +1,615 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "sparse_optimizer.h"
+
+#include <iostream>
+#include <iomanip>
+#include <algorithm>
+#include <iterator>
+#include <cassert>
+#include <algorithm>
+
+#include "estimate_propagator.h"
+#include "optimization_algorithm.h"
+#include "batch_stats.h"
+#include "hyper_graph_action.h"
+#include "robust_kernel.h"
+#include "../stuff/timeutil.h"
+#include "../stuff/macros.h"
+#include "../stuff/misc.h"
+#include "../../config.h"
+
+namespace g2o{
+  using namespace std;
+
+
+  SparseOptimizer::SparseOptimizer() :
+    _forceStopFlag(0), _verbose(false), _algorithm(0), _computeBatchStatistics(false)
+  {
+    _graphActions.resize(AT_NUM_ELEMENTS);
+  }
+
+  SparseOptimizer::~SparseOptimizer(){
+    delete _algorithm;
+    G2OBatchStatistics::setGlobalStats(0);
+  }
+
+  void SparseOptimizer::computeActiveErrors()
+  {
+    // call the callbacks in case there is something registered
+    HyperGraphActionSet& actions = _graphActions[AT_COMPUTEACTIVERROR];
+    if (actions.size() > 0) {
+      for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it)
+        (*(*it))(this);
+    }
+
+#   ifdef G2O_OPENMP
+#   pragma omp parallel for default (shared) if (_activeEdges.size() > 50)
+#   endif
+    for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
+      OptimizableGraph::Edge* e = _activeEdges[k];
+      e->computeError();
+    }
+
+#  ifndef NDEBUG
+    for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
+      OptimizableGraph::Edge* e = _activeEdges[k];
+      bool hasNan = arrayHasNaN(e->errorData(), e->dimension());
+      if (hasNan) {
+        cerr << "computeActiveErrors(): found NaN in error for edge " << e << endl;
+      }
+    }
+#  endif
+
+  }
+
+  double SparseOptimizer::activeChi2( ) const
+  {
+    double chi = 0.0;
+    for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
+      const OptimizableGraph::Edge* e = *it;
+      chi += e->chi2();
+    }
+    return chi;
+  }
+
+  double SparseOptimizer::activeRobustChi2() const
+  {
+    Eigen::Vector3d rho;
+    double chi = 0.0;
+    for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
+      const OptimizableGraph::Edge* e = *it;
+      if (e->robustKernel()) {
+        e->robustKernel()->robustify(e->chi2(), rho);
+        chi += rho[0];
+      }
+      else
+        chi += e->chi2();
+    }
+    return chi;
+  }
+
+  OptimizableGraph::Vertex* SparseOptimizer::findGauge(){
+    if (vertices().empty())
+      return 0;
+
+    int maxDim=0;
+    for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+      OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second); 
+      maxDim=std::max(maxDim,v->dimension());
+    }
+    
+    OptimizableGraph::Vertex* rut=0;
+    for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+      OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
+      if (v->dimension()==maxDim){
+        rut=v;
+        break;
+      }
+    }
+    return rut;
+  }
+
+  bool SparseOptimizer::gaugeFreedom()
+  {
+    if (vertices().empty())
+      return false;
+
+    int maxDim=0;
+    for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+      OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second); 
+      maxDim = std::max(maxDim,v->dimension());
+    }
+
+    for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+      OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
+      if (v->dimension() == maxDim) {
+        // test for fixed vertex
+        if (v->fixed()) {
+          return false;
+        }
+        // test for full dimension prior
+        for (HyperGraph::EdgeSet::const_iterator eit = v->edges().begin(); eit != v->edges().end(); ++eit) {
+          OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*eit);
+          if (e->vertices().size() == 1 && e->dimension() == maxDim)
+            return false;
+        }
+      }
+    }
+    return true;
+  }
+
+  bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){
+    if (! vlist.size()){
+      _ivMap.clear();
+      return false;
+    }
+
+    _ivMap.resize(vlist.size());
+    size_t i = 0;
+    for (int k=0; k<2; k++)
+      for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){
+      OptimizableGraph::Vertex* v = *it;
+      if (! v->fixed()){
+        if (static_cast<int>(v->marginalized()) == k){
+          v->setHessianIndex(i);
+          _ivMap[i]=v;
+          i++;
+        }
+      }
+      else {
+        v->setHessianIndex(-1);
+      }
+    }
+    _ivMap.resize(i);
+    return true;
+  }
+
+  void SparseOptimizer::clearIndexMapping(){
+    for (size_t i=0; i<_ivMap.size(); ++i){
+      _ivMap[i]->setHessianIndex(-1);
+      _ivMap[i]=0;
+    }
+  }
+
+  bool SparseOptimizer::initializeOptimization(int level){
+    HyperGraph::VertexSet vset;
+    for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it)
+      vset.insert(it->second);
+    return initializeOptimization(vset,level);
+  }
+
+  bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
+    if (edges().size() == 0) {
+      cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
+      return false;
+    }
+    bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
+    assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
+    clearIndexMapping();
+    _activeVertices.clear();
+    _activeVertices.reserve(vset.size());
+    _activeEdges.clear();
+    set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
+    for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
+      OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
+      const OptimizableGraph::EdgeSet& vEdges=v->edges();
+      // count if there are edges in that level. If not remove from the pool
+      int levelEdges=0;
+      for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
+        OptimizableGraph::Edge* e=reinterpret_cast<OptimizableGraph::Edge*>(*it);
+        if (level < 0 || e->level() == level) {
+
+          bool allVerticesOK = true;
+          for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
+            if (vset.find(*vit) == vset.end()) {
+              allVerticesOK = false;
+              break;
+            }
+          }
+          if (allVerticesOK && !e->allVerticesFixed()) {
+            auxEdgeSet.insert(e);
+            levelEdges++;
+          }
+
+        }
+      }
+      if (levelEdges){
+        _activeVertices.push_back(v);
+
+        // test for NANs in the current estimate if we are debugging
+#      ifndef NDEBUG
+        int estimateDim = v->estimateDimension();
+        if (estimateDim > 0) {
+          Eigen::VectorXd estimateData(estimateDim);
+          if (v->getEstimateData(estimateData.data()) == true) {
+            int k;
+            bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
+            if (hasNan)
+              cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
+          }
+        }
+#      endif
+
+      }
+    }
+
+    _activeEdges.reserve(auxEdgeSet.size());
+    for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
+      _activeEdges.push_back(*it);
+
+    sortVectorContainers();
+    return buildIndexMapping(_activeVertices);
+  }
+
+  bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset){
+    bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
+    assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
+    clearIndexMapping();
+    _activeVertices.clear();
+    _activeEdges.clear();
+    _activeEdges.reserve(eset.size());
+    set<Vertex*> auxVertexSet; // temporary structure to avoid duplicates
+    for (HyperGraph::EdgeSet::iterator it=eset.begin(); it!=eset.end(); ++it){
+      OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it);
+      for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
+        auxVertexSet.insert(static_cast<OptimizableGraph::Vertex*>(*vit));
+      }
+      _activeEdges.push_back(reinterpret_cast<OptimizableGraph::Edge*>(*it));
+    }
+
+    _activeVertices.reserve(auxVertexSet.size());
+    for (set<Vertex*>::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it)
+      _activeVertices.push_back(*it);
+
+    sortVectorContainers();
+    return buildIndexMapping(_activeVertices);
+  }
+
+  void SparseOptimizer::setToOrigin(){
+    for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) {
+      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
+      v->setToOrigin();
+    }
+  }
+
+  void SparseOptimizer::computeInitialGuess()
+  {
+    EstimatePropagator::PropagateCost costFunction(this);
+    computeInitialGuess(costFunction);
+  }
+
+  void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction)
+  {
+    OptimizableGraph::VertexSet emptySet;
+    std::set<Vertex*> backupVertices;
+    HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization
+    for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
+      OptimizableGraph::Edge* e = *it;
+      for (size_t i = 0; i < e->vertices().size(); ++i) {
+        OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertex(i));
+        if (v->fixed())
+          fixedVertices.insert(v);
+        else { // check for having a prior which is able to fully initialize a vertex
+          for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) {
+            OptimizableGraph::Edge* vedge = static_cast<OptimizableGraph::Edge*>(*vedgeIt);
+            if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) {
+              //cerr << "Initialize with prior for " << v->id() << endl;
+              vedge->initialEstimate(emptySet, v);
+              fixedVertices.insert(v);
+            }
+          }
+        }
+        if (v->hessianIndex() == -1) {
+          std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v);
+          if (foundIt == backupVertices.end()) {
+            v->push();
+            backupVertices.insert(v);
+          }
+        }
+      }
+    }
+
+    EstimatePropagator estimatePropagator(this);
+    estimatePropagator.propagate(fixedVertices, costFunction);
+
+    // restoring the vertices that should not be initialized
+    for (std::set<Vertex*>::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) {
+      Vertex* v = *it;
+      v->pop();
+    }
+    if (verbose()) {
+      computeActiveErrors();
+      cerr << "iteration= -1\t chi2= " << activeChi2()
+          << "\t time= 0.0"
+          << "\t cumTime= 0.0"
+          << "\t (using initial guess from " << costFunction.name() << ")" << endl;
+    }
+  }
+
+  int SparseOptimizer::optimize(int iterations, bool online)
+  {
+    if (_ivMap.size() == 0) {
+      cerr << __PRETTY_FUNCTION__ << ": 0 vertices to optimize, maybe forgot to call initializeOptimization()" << endl;
+      return -1;
+    }
+
+    int cjIterations=0;
+    double cumTime=0;
+    bool ok=true;
+
+    ok = _algorithm->init(online);
+    if (! ok) {
+      cerr << __PRETTY_FUNCTION__ << " Error while initializing" << endl;
+      return -1;
+    }
+
+    _batchStatistics.clear();
+    if (_computeBatchStatistics)
+      _batchStatistics.resize(iterations);
+    
+    OptimizationAlgorithm::SolverResult result = OptimizationAlgorithm::OK;
+    for (int i=0; i<iterations && ! terminate() && ok; i++){
+      preIteration(i);
+
+      if (_computeBatchStatistics) {
+        G2OBatchStatistics& cstat = _batchStatistics[i];
+        G2OBatchStatistics::setGlobalStats(&cstat);
+        cstat.iteration = i;
+        cstat.numEdges =  _activeEdges.size();
+        cstat.numVertices = _activeVertices.size();
+      }
+      
+      double ts = get_monotonic_time();
+      result = _algorithm->solve(i, online);
+      ok = ( result == OptimizationAlgorithm::OK );
+
+      bool errorComputed = false;
+      if (_computeBatchStatistics) {
+        computeActiveErrors();
+        errorComputed = true;
+        _batchStatistics[i].chi2 = activeRobustChi2();
+        _batchStatistics[i].timeIteration = get_monotonic_time()-ts;
+      }
+
+      if (verbose()){
+        double dts = get_monotonic_time()-ts;
+        cumTime += dts;
+        if (! errorComputed)
+          computeActiveErrors();
+        cerr << "iteration= " << i
+          << "\t chi2= " << FIXED(activeRobustChi2())
+          << "\t time= " << dts
+          << "\t cumTime= " << cumTime
+          << "\t edges= " << _activeEdges.size();
+        _algorithm->printVerbose(cerr);
+        cerr << endl;
+      }
+      ++cjIterations; 
+      postIteration(i);
+    }
+    if (result == OptimizationAlgorithm::Fail) {
+      return 0;
+    }
+    return cjIterations;
+  }
+
+
+  void SparseOptimizer::update(const double* update)
+  {
+    // update the graph by calling oplus on the vertices
+    for (size_t i=0; i < _ivMap.size(); ++i) {
+      OptimizableGraph::Vertex* v= _ivMap[i];
+#ifndef NDEBUG
+      bool hasNan = arrayHasNaN(update, v->dimension());
+      if (hasNan)
+        cerr << __PRETTY_FUNCTION__ << ": Update contains a nan for vertex " << v->id() << endl;
+#endif
+      v->oplus(update);
+      update += v->dimension();
+    }
+  }
+
+  void SparseOptimizer::setComputeBatchStatistics(bool computeBatchStatistics)
+  {
+    if ((_computeBatchStatistics == true) && (computeBatchStatistics == false)) {
+      G2OBatchStatistics::setGlobalStats(0);
+      _batchStatistics.clear();
+    }
+    _computeBatchStatistics = computeBatchStatistics;
+  }
+
+  bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)
+  {
+    std::vector<HyperGraph::Vertex*> newVertices;
+    newVertices.reserve(vset.size());
+    _activeVertices.reserve(_activeVertices.size() + vset.size());
+    _activeEdges.reserve(_activeEdges.size() + eset.size());
+    for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
+      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+      if (!e->allVerticesFixed()) _activeEdges.push_back(e);
+    }
+    
+    // update the index mapping
+    size_t next = _ivMap.size();
+    for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
+      OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(*it);
+      if (! v->fixed()){
+        if (! v->marginalized()){
+          v->setHessianIndex(next);
+          _ivMap.push_back(v);
+          newVertices.push_back(v);
+          _activeVertices.push_back(v);
+          next++;
+        } 
+        else // not supported right now
+          abort();
+      }
+      else {
+        v->setHessianIndex(-1);
+      }
+    }
+
+    //if (newVertices.size() != vset.size())
+    //cerr << __PRETTY_FUNCTION__ << ": something went wrong " << PVAR(vset.size()) << " " << PVAR(newVertices.size()) << endl;
+    return _algorithm->updateStructure(newVertices, eset);
+  }
+
+  void SparseOptimizer::sortVectorContainers()
+  {
+    // sort vector structures to get deterministic ordering based on IDs
+    sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare());
+    sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare());
+  }
+
+  void SparseOptimizer::clear() {
+    _ivMap.clear();
+    _activeVertices.clear();
+    _activeEdges.clear();
+    OptimizableGraph::clear();
+  }
+
+  SparseOptimizer::VertexContainer::const_iterator SparseOptimizer::findActiveVertex(const OptimizableGraph::Vertex* v) const
+  {
+    VertexContainer::const_iterator lower = lower_bound(_activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare());
+    if (lower == _activeVertices.end())
+      return _activeVertices.end();
+    if ((*lower) == v)
+      return lower;
+    return _activeVertices.end();
+  }
+
+  SparseOptimizer::EdgeContainer::const_iterator SparseOptimizer::findActiveEdge(const OptimizableGraph::Edge* e) const
+  {
+    EdgeContainer::const_iterator lower = lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare());
+    if (lower == _activeEdges.end())
+      return _activeEdges.end();
+    if ((*lower) == e)
+      return lower;
+    return _activeEdges.end();
+  }
+
+  void SparseOptimizer::push(SparseOptimizer::VertexContainer& vlist)
+  {
+    for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
+      (*it)->push();
+  }
+
+  void SparseOptimizer::pop(SparseOptimizer::VertexContainer& vlist)
+  {
+    for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
+      (*it)->pop();
+  }
+
+  void SparseOptimizer::push(HyperGraph::VertexSet& vlist)
+  {
+    for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) {
+      OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
+      if (v)
+	v->push();
+      else 
+	cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl;
+    }
+  }
+
+  void SparseOptimizer::pop(HyperGraph::VertexSet& vlist)
+  {
+    for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it){
+      OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*> (*it);
+      if (v)
+	v->pop();
+      else 
+	cerr << __FUNCTION__ << ": FATAL POP SET" << endl;
+    }
+  }
+
+  void SparseOptimizer::discardTop(SparseOptimizer::VertexContainer& vlist)
+  {
+    for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
+      (*it)->discardTop();
+  }
+
+  void SparseOptimizer::setVerbose(bool verbose)
+  {
+    _verbose = verbose;
+  }
+
+  void SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
+  {
+    if (_algorithm) // reset the optimizer for the formerly used solver
+      _algorithm->setOptimizer(0);
+    _algorithm = algorithm;
+    if (_algorithm)
+      _algorithm->setOptimizer(this);
+  }
+
+  bool SparseOptimizer::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices){
+    return _algorithm->computeMarginals(spinv, blockIndices);
+  }
+
+  void SparseOptimizer::setForceStopFlag(bool* flag)
+  {
+    _forceStopFlag=flag;
+  }
+
+  bool SparseOptimizer::removeVertex(HyperGraph::Vertex* v)
+  {
+    OptimizableGraph::Vertex* vv = static_cast<OptimizableGraph::Vertex*>(v);
+    if (vv->hessianIndex() >= 0) {
+      clearIndexMapping();
+      _ivMap.clear();
+    }
+    return HyperGraph::removeVertex(v);
+  }
+
+  bool SparseOptimizer::addComputeErrorAction(HyperGraphAction* action)
+  {
+    std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_COMPUTEACTIVERROR].insert(action);
+    return insertResult.second;
+  }
+
+  bool SparseOptimizer::removeComputeErrorAction(HyperGraphAction* action)
+  {
+    return _graphActions[AT_COMPUTEACTIVERROR].erase(action) > 0;
+  }
+
+  void SparseOptimizer::push()
+  {
+    push(_activeVertices);
+  }
+
+  void SparseOptimizer::pop()
+  {
+    pop(_activeVertices);
+  }
+
+  void SparseOptimizer::discardTop()
+  {
+    discardTop(_activeVertices);
+  }
+
+} // end namespace

Thirdparty/g2o/g2o/core/sparse_optimizer.h

@@ -0,0 +1,312 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_GRAPH_OPTIMIZER_CHOL_H_
+#define G2O_GRAPH_OPTIMIZER_CHOL_H_
+
+#include "../stuff/macros.h"
+
+#include "optimizable_graph.h"
+#include "sparse_block_matrix.h"
+#include "batch_stats.h"
+
+#include <map>
+
+namespace g2o {
+
+  // forward declaration
+  class ActivePathCostFunction;
+  class OptimizationAlgorithm;
+  class EstimatePropagatorCost;
+
+  class  SparseOptimizer : public OptimizableGraph {
+
+    public:
+    enum {
+      AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS,
+      AT_NUM_ELEMENTS, // keep as last element
+    };
+
+    friend class ActivePathCostFunction;
+
+    // Attention: _solver & _statistics is own by SparseOptimizer and will be
+    // deleted in its destructor.
+    SparseOptimizer();
+    virtual ~SparseOptimizer();
+
+    // new interface for the optimizer
+    // the old functions will be dropped
+    /**
+     * Initializes the structures for optimizing a portion of the graph specified by a subset of edges.
+     * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the 
+     * schur complement or to set as fixed during the optimization.
+     * @param eset: the subgraph to be optimized.
+     * @returns false if somethings goes wrong
+     */
+    virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);
+
+    /**
+     * Initializes the structures for optimizing a portion of the graph specified by a subset of vertices.
+     * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the 
+     * schur complement or to set as fixed during the optimization.
+     * @param vset: the subgraph to be optimized.
+     * @param level: is the level (in multilevel optimization)
+     * @returns false if somethings goes wrong
+     */
+    virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0);
+
+    /**
+     * Initializes the structures for optimizing the whole graph.
+     * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the 
+     * schur complement or to set as fixed during the optimization.
+     * @param level: is the level (in multilevel optimization)
+     * @returns false if somethings goes wrong
+     */
+    virtual bool initializeOptimization(int level=0);
+
+    /**
+     * HACK updating the internal structures for online processing
+     */
+    virtual bool updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset);
+  
+    /**
+     * Propagates an initial guess from the vertex specified as origin.
+     * It should be called after initializeOptimization(...), as it relies on the _activeVertices/_edges structures.
+     * It constructs a set of trees starting from the nodes in the graph which are fixed and eligible for preforming optimization.
+     * The trees are constructed by utlizing a cost-function specified.
+     * @param costFunction: the cost function used
+     * @patam maxDistance: the distance where to stop the search
+     */
+    virtual void computeInitialGuess();
+
+    /**
+     * Same as above but using a specific propagator
+     */
+    virtual void computeInitialGuess(EstimatePropagatorCost& propagator);
+
+    /**
+     * sets all vertices to their origin.
+     */
+    virtual void setToOrigin();
+
+
+    /**
+     * starts one optimization run given the current configuration of the graph, 
+     * and the current settings stored in the class instance.
+     * It can be called only after initializeOptimization
+     */
+    int optimize(int iterations, bool online = false);
+
+    /**
+     * computes the blocks of the inverse of the specified pattern.
+     * the pattern is given via pairs <row, col> of the blocks in the hessian
+     * @param blockIndices: the pattern
+     * @param spinv: the sparse block matrix with the result
+     * @returns false if the operation is not supported by the solver
+     */
+    bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
+
+    /**
+     * computes the inverse of the specified vertex.
+     * @param vertex: the vertex whose state is to be marginalised
+     * @param spinv: the sparse block matrix with the result
+     * @returns false if the operation is not supported by the solver
+     */
+    bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const Vertex* vertex) {
+      if (vertex->hessianIndex() < 0) {
+          return false;
+      }
+      std::vector<std::pair<int, int> > index;
+      index.push_back(std::pair<int, int>(vertex->hessianIndex(), vertex->hessianIndex()));
+      return computeMarginals(spinv, index);
+    }
+
+    /**
+     * computes the inverse of the set specified vertices, assembled into a single covariance matrix.
+     * @param vertex: the pattern
+     * @param spinv: the sparse block matrix with the result
+     * @returns false if the operation is not supported by the solver
+     */
+    bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const VertexContainer& vertices) {
+      std::vector<std::pair<int, int> > indices;
+      for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
+        indices.push_back(std::pair<int, int>((*it)->hessianIndex(),(*it)->hessianIndex()));
+      }
+      return computeMarginals(spinv, indices);
+    }
+
+    //! finds a gauge in the graph to remove the undefined dof.
+    // The gauge should be fixed() and then the optimization can work (if no additional dof are in
+    // the system. The default implementation returns a node with maximum dimension.
+    virtual Vertex* findGauge();
+
+    bool gaugeFreedom();
+
+    /**returns the cached chi2 of the active portion of the graph*/
+    double activeChi2() const;
+    /**
+     * returns the cached chi2 of the active portion of the graph.
+     * In contrast to activeChi2() this functions considers the weighting
+     * of the error according to the robustification of the error functions.
+     */
+    double activeRobustChi2() const;
+
+    //! verbose information during optimization
+    bool verbose()  const {return _verbose;}
+    void setVerbose(bool verbose);
+
+    /**
+     * sets a variable checked at every iteration to force a user stop. The iteration exits when the variable is true;
+     */
+    void setForceStopFlag(bool* flag);
+    bool* forceStopFlag() const { return _forceStopFlag;};
+
+    //! if external stop flag is given, return its state. False otherwise
+    bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; }
+
+    //! the index mapping of the vertices
+    const VertexContainer& indexMapping() const {return _ivMap;}
+    //! the vertices active in the current optimization
+    const VertexContainer& activeVertices() const { return _activeVertices;}
+    //! the edges active in the current optimization
+    const EdgeContainer& activeEdges() const { return _activeEdges;}
+
+    /**
+     * Remove a vertex. If the vertex is contained in the currently active set
+     * of vertices, then the internal temporary structures are cleaned, e.g., the index
+     * mapping is erased. In case you need the index mapping for manipulating the
+     * graph, you have to store it in your own copy.
+     */
+    virtual bool removeVertex(HyperGraph::Vertex* v);
+
+    /**
+     * search for an edge in _activeVertices and return the iterator pointing to it
+     * getActiveVertices().end() if not found
+     */
+    VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const;
+    /**
+     * search for an edge in _activeEdges and return the iterator pointing to it
+     * getActiveEdges().end() if not found
+     */
+    EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const;
+
+    //! the solver used by the optimizer
+    const OptimizationAlgorithm* algorithm() const { return _algorithm;}
+    OptimizationAlgorithm* solver() { return _algorithm;}
+    void setAlgorithm(OptimizationAlgorithm* algorithm);
+
+    //! push the estimate of a subset of the variables onto a stack
+    void push(SparseOptimizer::VertexContainer& vlist);
+    //! push the estimate of a subset of the variables onto a stack
+    void push(HyperGraph::VertexSet& vlist);
+    //! push all the active vertices onto a stack
+    void push();
+    //! pop (restore) the estimate a subset of the variables from the stack
+    void pop(SparseOptimizer::VertexContainer& vlist);
+    //! pop (restore) the estimate a subset of the variables from the stack
+    void pop(HyperGraph::VertexSet& vlist);
+    //! pop (restore) the estimate of the active vertices from the stack
+    void pop();
+
+    //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate
+    void discardTop(SparseOptimizer::VertexContainer& vlist);
+    //! same as above, but for the active vertices
+    void discardTop();
+    using OptimizableGraph::discardTop;
+
+    /**
+     * clears the graph, and polishes some intermediate structures
+     * Note that this only removes nodes / edges. Parameters can be removed
+     * with clearParameters().
+     */
+    virtual void clear();
+
+    /**
+     * computes the error vectors of all edges in the activeSet, and caches them
+     */
+    void computeActiveErrors();
+
+    /**
+     * Linearizes the system by computing the Jacobians for the nodes
+     * and edges in the graph
+     */
+    G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem())
+    {
+      // nothing needed, linearization is now done inside the solver
+    }
+
+    /**
+     * update the estimate of the active vertices 
+     * @param update: the double vector containing the stacked
+     * elements of the increments on the vertices.
+     */
+    void update(const double* update);
+
+    /**
+       returns the set of batch statistics about the optimisation
+    */
+    const BatchStatisticsContainer& batchStatistics() const { return _batchStatistics;}
+    /**
+       returns the set of batch statistics about the optimisation
+    */
+    BatchStatisticsContainer& batchStatistics() { return _batchStatistics;}
+    
+    void setComputeBatchStatistics(bool computeBatchStatistics);
+    
+    bool computeBatchStatistics() const { return _computeBatchStatistics;}
+
+    /**** callbacks ****/
+    //! add an action to be executed before the error vectors are computed
+    bool addComputeErrorAction(HyperGraphAction* action);
+    //! remove an action that should no longer be execured before computing the error vectors
+    bool removeComputeErrorAction(HyperGraphAction* action);
+
+    
+
+    protected:
+    bool* _forceStopFlag;
+    bool _verbose;
+
+    VertexContainer _ivMap;
+    VertexContainer _activeVertices;   ///< sorted according to VertexIDCompare
+    EdgeContainer _activeEdges;        ///< sorted according to EdgeIDCompare
+
+    void sortVectorContainers();
+ 
+    OptimizationAlgorithm* _algorithm;
+
+    /**
+     * builds the mapping of the active vertices to the (block) row / column in the Hessian
+     */
+    bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist);
+    void clearIndexMapping();
+
+    BatchStatisticsContainer _batchStatistics;   ///< global statistics of the optimizer, e.g., timing, num-non-zeros
+    bool _computeBatchStatistics;
+  };
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/solvers/linear_solver_dense.h

@@ -0,0 +1,125 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// Copyright (C) 2012 R. Kümmerle
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_LINEAR_SOLVER_DENSE_H
+#define G2O_LINEAR_SOLVER_DENSE_H
+
+#include "../core/linear_solver.h"
+#include "../core/batch_stats.h"
+
+#include <vector>
+#include <utility>
+#include<Eigen/Core>
+#include<Eigen/Cholesky>
+
+
+namespace g2o {
+
+  /**
+   * \brief linear solver using dense cholesky decomposition
+   */
+  template <typename MatrixType>
+  class LinearSolverDense : public LinearSolver<MatrixType>
+  {
+    public:
+      LinearSolverDense() :
+        LinearSolver<MatrixType>(),
+        _reset(true)
+      {
+      }
+
+      virtual ~LinearSolverDense()
+      {
+      }
+
+      virtual bool init()
+      {
+        _reset = true;
+        return true;
+      }
+
+      bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b)
+      {
+        int n = A.cols();
+        int m = A.cols();
+
+        Eigen::MatrixXd& H = _H;
+        if (H.cols() != n) {
+          H.resize(n, m);
+          _reset = true;
+        }
+        if (_reset) {
+          _reset = false;
+          H.setZero();
+        }
+
+        // copy the sparse block matrix into a dense matrix
+        int c_idx = 0;
+        for (size_t i = 0; i < A.blockCols().size(); ++i) {
+          int c_size = A.colsOfBlock(i);
+          assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices");
+
+          const typename SparseBlockMatrix<MatrixType>::IntBlockMap& col = A.blockCols()[i];
+          if (col.size() > 0) {
+            typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it;
+            for (it = col.begin(); it != col.end(); ++it) {
+              int r_idx = A.rowBaseOfBlock(it->first);
+              // only the upper triangular block is processed
+              if (it->first <= (int)i) {
+                int r_size = A.rowsOfBlock(it->first);
+                H.block(r_idx, c_idx, r_size, c_size) = *(it->second);
+                if (r_idx != c_idx) // write the lower triangular block
+                  H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose();
+              }
+            }
+          }
+
+          c_idx += c_size;
+        }
+
+        // solving via Cholesky decomposition
+        Eigen::VectorXd::MapType xvec(x, m);
+        Eigen::VectorXd::ConstMapType bvec(b, n);
+        _cholesky.compute(H);
+        if (_cholesky.isPositive()) {
+          xvec = _cholesky.solve(bvec);
+          return true;
+        }
+        return false;
+      }
+
+    protected:
+      bool _reset;
+      Eigen::MatrixXd _H;
+      Eigen::LDLT<Eigen::MatrixXd> _cholesky;
+
+  };
+
+
+}// end namespace
+
+#endif

Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h

@@ -0,0 +1,237 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_LINEAR_SOLVER_EIGEN_H
+#define G2O_LINEAR_SOLVER_EIGEN_H
+
+#include <Eigen/Sparse>
+#include <Eigen/SparseCholesky>
+
+#include "../core/linear_solver.h"
+#include "../core/batch_stats.h"
+#include "../stuff/timeutil.h"
+
+#include "../core/eigen_types.h"
+
+#include <iostream>
+#include <vector>
+
+namespace g2o {
+
+/**
+ * \brief linear solver which uses the sparse Cholesky solver from Eigen
+ *
+ * Has no dependencies except Eigen. Hence, should compile almost everywhere
+ * without to much issues. Performance should be similar to CSparse, I guess.
+ */
+template <typename MatrixType>
+class LinearSolverEigen: public LinearSolver<MatrixType>
+{
+  public:
+    typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseMatrix;
+    typedef Eigen::Triplet<double> Triplet;
+    typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> PermutationMatrix;
+    /**
+     * \brief Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering
+     */
+    class CholeskyDecomposition : public Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>
+    {
+      public:
+        CholeskyDecomposition() : Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>() {}
+        using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered;
+
+        void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation)
+        {
+          m_Pinv = permutation;
+          m_P = permutation.inverse();
+          int size = a.cols();
+          SparseMatrix ap(size, size);
+          ap.selfadjointView<Eigen::Upper>() = a.selfadjointView<UpLo>().twistedBy(m_P);
+          analyzePattern_preordered(ap, true);
+        }
+    };
+
+  public:
+    LinearSolverEigen() :
+      LinearSolver<MatrixType>(),
+      _init(true), _blockOrdering(false), _writeDebug(false)
+    {
+    }
+
+    virtual ~LinearSolverEigen()
+    {
+    }
+
+    virtual bool init()
+    {
+      _init = true;
+      return true;
+    }
+
+    bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b)
+    {
+      if (_init)
+        _sparseMatrix.resize(A.rows(), A.cols());
+      fillSparseMatrix(A, !_init);
+      if (_init) // compute the symbolic composition once
+        computeSymbolicDecomposition(A);
+      _init = false;
+
+      double t=get_monotonic_time();
+      _cholesky.factorize(_sparseMatrix);
+      if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite
+        if (_writeDebug) {
+          std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
+          A.writeOctave("debug.txt");
+        }
+        return false;
+      }
+
+      // Solving the system
+      VectorXD::MapType xx(x, _sparseMatrix.cols());
+      VectorXD::ConstMapType bb(b, _sparseMatrix.cols());
+      xx = _cholesky.solve(bb);
+      G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+      if (globalStats) {
+        globalStats->timeNumericDecomposition = get_monotonic_time() - t;
+        globalStats->choleskyNNZ = _cholesky.matrixL().nestedExpression().nonZeros() + _sparseMatrix.cols(); // the elements of D
+      }
+
+      return true;
+    }
+
+    //! do the AMD ordering on the blocks or on the scalar matrix
+    bool blockOrdering() const { return _blockOrdering;}
+    void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering;}
+
+    //! write a debug dump of the system matrix if it is not SPD in solve
+    virtual bool writeDebug() const { return _writeDebug;}
+    virtual void setWriteDebug(bool b) { _writeDebug = b;}
+
+  protected:
+    bool _init;
+    bool _blockOrdering;
+    bool _writeDebug;
+    SparseMatrix _sparseMatrix;
+    CholeskyDecomposition _cholesky;
+
+    /**
+     * compute the symbolic decompostion of the matrix only once.
+     * Since A has the same pattern in all the iterations, we only
+     * compute the fill-in reducing ordering once and re-use for all
+     * the following iterations.
+     */
+    void computeSymbolicDecomposition(const SparseBlockMatrix<MatrixType>& A)
+    {
+      double t=get_monotonic_time();
+      if (! _blockOrdering) {
+        _cholesky.analyzePattern(_sparseMatrix);
+      } else {
+        // block ordering with the Eigen Interface
+        // This is really ugly currently, as it calls internal functions from Eigen
+        // and modifies the SparseMatrix class
+        Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic> blockP;
+        {
+          // prepare a block structure matrix for calling AMD
+          std::vector<Triplet> triplets;
+          for (size_t c = 0; c < A.blockCols().size(); ++c){
+            const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
+            for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
+              const int& r = it->first;
+              if (r > static_cast<int>(c)) // only upper triangle
+                break;
+              triplets.push_back(Triplet(r, c, 0.));
+            }
+          }
+
+          // call the AMD ordering on the block matrix.
+          // Relies on Eigen's internal stuff, probably bad idea
+          SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size());
+          auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end());
+          typename CholeskyDecomposition::CholMatrixType C;
+          C = auxBlockMatrix.selfadjointView<Eigen::Upper>();
+          Eigen::internal::minimum_degree_ordering(C, blockP);
+        }
+
+        int rows = A.rows();
+        assert(rows == A.cols() && "Matrix A is not square");
+
+        // Adapt the block permutation to the scalar matrix
+        PermutationMatrix scalarP;
+        scalarP.resize(rows);
+        int scalarIdx = 0;
+        for (int i = 0; i < blockP.size(); ++i) {
+          const int& p = blockP.indices()(i);
+          int base  = A.colBaseOfBlock(p);
+          int nCols = A.colsOfBlock(p);
+          for (int j = 0; j < nCols; ++j)
+            scalarP.indices()(scalarIdx++) = base++;
+        }
+        assert(scalarIdx == rows && "did not completely fill the permutation matrix");
+        // analyze with the scalar permutation
+        _cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP);
+
+      }
+      G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+      if (globalStats)
+        globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;
+    }
+
+    void fillSparseMatrix(const SparseBlockMatrix<MatrixType>& A, bool onlyValues)
+    {
+      if (onlyValues) {
+        A.fillCCS(_sparseMatrix.valuePtr(), true);
+      } else {
+
+        // create from triplet structure
+        std::vector<Triplet> triplets;
+        triplets.reserve(A.nonZeros());
+        for (size_t c = 0; c < A.blockCols().size(); ++c) {
+          int colBaseOfBlock = A.colBaseOfBlock(c);
+          const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
+          for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
+            int rowBaseOfBlock = A.rowBaseOfBlock(it->first);
+            const MatrixType& m = *(it->second);
+            for (int cc = 0; cc < m.cols(); ++cc) {
+              int aux_c = colBaseOfBlock + cc;
+              for (int rr = 0; rr < m.rows(); ++rr) {
+                int aux_r = rowBaseOfBlock + rr;
+                if (aux_r > aux_c)
+                  break;
+                triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc)));
+              }
+            }
+          }
+        }
+        _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end());
+
+      }
+    }
+};
+
+} // end namespace
+
+#endif

Thirdparty/g2o/g2o/stuff/color_macros.h

@@ -0,0 +1,65 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_COLOR_MACROS_H
+#define G2O_COLOR_MACROS_H
+
+// font attributes
+#define FT_BOLD      "\033[1m"
+#define FT_UNDERLINE "\033[4m"
+
+//background color
+#define BG_BLACK     "\033[40m"
+#define BG_RED       "\033[41m"
+#define BG_GREEN     "\033[42m"
+#define BG_YELLOW    "\033[43m"
+#define BG_LIGHTBLUE "\033[44m"
+#define BG_MAGENTA   "\033[45m"
+#define BG_BLUE      "\033[46m"
+#define BG_WHITE     "\033[47m"
+
+// font color
+#define CL_BLACK(s)     "\033[30m" << s << "\033[0m"
+#define CL_RED(s)       "\033[31m" << s << "\033[0m"
+#define CL_GREEN(s)     "\033[32m" << s << "\033[0m"
+#define CL_YELLOW(s)    "\033[33m" << s << "\033[0m"
+#define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m"
+#define CL_MAGENTA(s)   "\033[35m" << s << "\033[0m"
+#define CL_BLUE(s)      "\033[36m" << s << "\033[0m"
+#define CL_WHITE(s)     "\033[37m" << s << "\033[0m"
+
+#define FG_BLACK     "\033[30m"
+#define FG_RED       "\033[31m"
+#define FG_GREEN     "\033[32m"
+#define FG_YELLOW    "\033[33m"
+#define FG_LIGHTBLUE "\033[34m"
+#define FG_MAGENTA   "\033[35m"
+#define FG_BLUE      "\033[36m"
+#define FG_WHITE     "\033[37m"
+
+#define FG_NORM      "\033[0m"
+
+#endif

Thirdparty/g2o/g2o/stuff/macros.h

@@ -0,0 +1,134 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_MACROS_H
+#define G2O_MACROS_H
+
+#ifndef DEG2RAD
+#define DEG2RAD(x) ((x) * 0.01745329251994329575)
+#endif
+
+#ifndef RAD2DEG
+#define RAD2DEG(x) ((x) * 57.29577951308232087721)
+#endif
+
+// GCC the one and only
+#if defined(__GNUC__)
+#  define G2O_ATTRIBUTE_CONSTRUCTOR(func) \
+     static void func(void)__attribute__ ((constructor)); \
+     static void func(void)
+
+#  define G2O_ATTRIBUTE_UNUSED __attribute__((unused))
+#  define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2)))
+#  define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3)))
+#  define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning))
+#  define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated))
+
+#ifdef ANDROID
+# define g2o_isnan(x)     isnan(x)
+# define g2o_isinf(x)     isinf(x)
+# define g2o_isfinite(x)  isfinite(x)
+#else
+# define g2o_isnan(x)     std::isnan(x)
+# define g2o_isinf(x)     std::isinf(x)
+# define g2o_isfinite(x)  std::isfinite(x)
+#endif
+
+// MSVC on Windows
+#elif defined _MSC_VER
+#  define __PRETTY_FUNCTION__ __FUNCTION__
+
+/**
+Modified by Mark Pupilli from:
+
+	"Initializer/finalizer sample for MSVC and GCC.
+    2010 Joe Lowe. Released into the public domain."
+
+	"For MSVC, places a ptr to the function in the user initializer section (.CRT$XCU), basically the same thing the compiler does for the constructor calls for static C++ objects. For GCC, uses a constructor attribute."
+
+	(As posted on Stack OVerflow)
+*/
+#  define G2O_ATTRIBUTE_CONSTRUCTOR(f) \
+     __pragma(section(".CRT$XCU",read)) \
+     static void __cdecl f(void); \
+     __declspec(allocate(".CRT$XCU")) void (__cdecl*f##_)(void) = f; \
+     static void __cdecl f(void)
+
+#  define G2O_ATTRIBUTE_UNUSED
+#  define G2O_ATTRIBUTE_FORMAT12
+#  define G2O_ATTRIBUTE_FORMAT23
+#  define G2O_ATTRIBUTE_WARNING(func) func
+#  define G2O_ATTRIBUTE_DEPRECATED(func) func
+
+#include <float.h>
+
+# define g2o_isnan(x)    _isnan(x)
+# define g2o_isinf(x)    (_finite(x) == 0)
+# define g2o_isfinite(x) (_finite(x) != 0)
+
+// unknown compiler
+#else
+#  ifndef __PRETTY_FUNCTION__
+#    define __PRETTY_FUNCTION__ ""
+#  endif
+#  define G2O_ATTRIBUTE_CONSTRUCTOR(func) func
+#  define G2O_ATTRIBUTE_UNUSED
+#  define G2O_ATTRIBUTE_FORMAT12
+#  define G2O_ATTRIBUTE_FORMAT23
+#  define G2O_ATTRIBUTE_WARNING(func) func
+#  define G2O_ATTRIBUTE_DEPRECATED(func) func
+
+#include <math.h>
+#define g2o_isnan(x)    isnan(x)
+#define g2o_isinf(x)    isinf(x)
+#define g2o_isfinite(x) isfinite(x)
+
+#endif
+
+// some macros that are only useful for c++
+#ifdef __cplusplus
+
+#define G2O_FSKIP_LINE(f) \
+   {char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);}
+
+#ifndef PVAR
+  #define PVAR(s) \
+    #s << " = " << (s) << std::flush
+#endif
+
+#ifndef PVARA
+#define PVARA(s) \
+  #s << " = " << RAD2DEG(s) << "deg" << std::flush
+#endif
+
+#ifndef FIXED
+#define FIXED(s) \
+  std::fixed << s << std::resetiosflags(std::ios_base::fixed)
+#endif
+
+#endif // __cplusplus
+
+#endif

Thirdparty/g2o/g2o/stuff/misc.h

@@ -0,0 +1,206 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_STUFF_MISC_H
+#define G2O_STUFF_MISC_H
+
+#include "macros.h"
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/** @addtogroup utils **/
+// @{
+
+/** \file misc.h
+ * \brief some general case utility functions
+ *
+ *  This file specifies some general case utility functions
+ **/
+
+namespace g2o {
+
+/**
+ * return the square value
+ */
+template <typename T>
+inline T square(T x)
+{
+  return x*x;
+}
+
+/**
+ * return the hypot of x and y
+ */
+template <typename T>
+inline T hypot(T x, T y)
+{
+  return (T) (sqrt(x*x + y*y));
+}
+
+/**
+ * return the squared hypot of x and y
+ */
+template <typename T>
+inline T hypot_sqr(T x, T y)
+{
+  return x*x + y*y;
+}
+
+/**
+ * convert from degree to radian
+ */
+inline double deg2rad(double degree)
+{
+  return degree * 0.01745329251994329576;
+}
+
+/**
+ * convert from radian to degree
+ */
+inline double rad2deg(double rad)
+{
+  return rad * 57.29577951308232087721;
+}
+
+/**
+ * normalize the angle
+ */
+inline double normalize_theta(double theta)
+{
+  if (theta >= -M_PI && theta < M_PI)
+    return theta;
+  
+  double multiplier = floor(theta / (2*M_PI));
+  theta = theta - multiplier*2*M_PI;
+  if (theta >= M_PI)
+    theta -= 2*M_PI;
+  if (theta < -M_PI)
+    theta += 2*M_PI;
+
+  return theta;
+}
+
+/**
+ * inverse of an angle, i.e., +180 degree
+ */
+inline double inverse_theta(double th)
+{
+  return normalize_theta(th + M_PI);
+}
+
+/**
+ * average two angles
+ */
+inline double average_angle(double theta1, double theta2)
+{
+  double x, y;
+
+  x = cos(theta1) + cos(theta2);
+  y = sin(theta1) + sin(theta2);
+  if(x == 0 && y == 0)
+    return 0;
+  else
+    return std::atan2(y, x);
+}
+
+/**
+ * sign function.
+ * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0
+ */
+template <typename T>
+inline int sign(T x)
+{
+  if (x > 0)
+    return 1;
+  else if (x < 0)
+    return -1;
+  else
+    return 0;
+}
+
+/**
+ * clamp x to the interval [l, u]
+ */
+template <typename T>
+inline T clamp(T l, T x, T u) 
+{
+  if (x < l)
+    return l;
+  if (x > u)
+    return u;
+  return x;
+}
+
+/**
+ * wrap x to be in the interval [l, u]
+ */
+template <typename T>
+inline T wrap(T l, T x, T u) 
+{
+  T intervalWidth = u - l;
+  while (x < l)
+    x += intervalWidth;
+  while (x > u)
+    x -= intervalWidth;
+  return x;
+}
+
+/**
+ * tests whether there is a NaN in the array
+ */
+inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0)
+{
+  for (int i = 0; i < size; ++i)
+    if (g2o_isnan(array[i])) {
+      if (nanIndex)
+        *nanIndex = i;
+      return true;
+    }
+  return false;
+}
+
+/**
+ * The following two functions are used to force linkage with static libraries.
+ */
+extern "C"
+{
+    typedef void (* ForceLinkFunction) (void);
+}
+
+struct ForceLinker
+{
+    ForceLinker(ForceLinkFunction function) { (function)(); }
+};
+
+
+} // end namespace
+
+// @}
+
+#endif

Thirdparty/g2o/g2o/stuff/os_specific.c

@@ -0,0 +1,64 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "os_specific.h"
+
+#ifdef WINDOWS
+
+int vasprintf(char** strp, const char* fmt, va_list ap)
+{
+  int n;
+  int size = 100;
+  char* p;
+  char* np;
+
+  if ((p = (char*)malloc(size * sizeof(char))) == NULL)
+    return -1;
+
+  while (1) {
+#ifdef _MSC_VER
+    n = vsnprintf_s(p, size, size - 1, fmt, ap);
+#else
+    n = vsnprintf(p, size, fmt, ap);
+#endif
+    if (n > -1 && n < size) {
+      *strp = p;
+      return n;
+    }
+    if (n > -1)
+      size = n+1;
+    else
+      size *= 2;
+    if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) {
+      free(p);
+      return -1;
+    } else
+      p = np;
+  }
+}
+
+
+#endif

Thirdparty/g2o/g2o/stuff/os_specific.h

@@ -0,0 +1,56 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OS_SPECIFIC_HH_
+#define G2O_OS_SPECIFIC_HH_
+
+#ifdef WINDOWS
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#ifndef _WINDOWS
+#include <sys/time.h>
+#endif
+#define drand48() ((double) rand()/(double)RAND_MAX)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int vasprintf(char** strp, const char* fmt, va_list ap);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+#ifdef UNIX
+#include <sys/time.h>
+// nothing to do on real operating systems
+#endif
+
+#endif