From d879491a58d092540f205e789ec7569445bc11f5 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 30 Nov 2016 14:25:01 -0500
Subject: [PATCH] Fixed #149

---
 cmake_modules/FindG2O.cmake                    | 16 +++++++++-------
 corelib/src/OptimizerG2O.cpp                   |  4 ----
 corelib/src/vertigo/g2o/edge_se2Switchable.cpp |  7 ++++---
 corelib/src/vertigo/g2o/edge_se3Switchable.cpp |  7 ++++---
 4 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/cmake_modules/FindG2O.cmake b/cmake_modules/FindG2O.cmake
index 4c5afd4a..4e23b573 100644
--- a/cmake_modules/FindG2O.cmake
+++ b/cmake_modules/FindG2O.cmake
@@ -18,8 +18,7 @@ FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
   PATHS "C:\\Program Files\\g2o\\include")
 
 # Macro to unify finding both the debug and release versions of the
-# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
-# macro.
+# libraries; this is adapted from the rtabmap config
 
 MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
 
@@ -31,11 +30,14 @@ MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
     NAMES "g2o_${MYLIBRARYNAME}"
 	PATHS "C:\\Program Files\\g2o\\lib")
   
-  IF(NOT ${MYLIBRARY}_DEBUG)
-    IF(MYLIBRARY)
-      SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
-    ENDIF(MYLIBRARY)
-  ENDIF( NOT ${MYLIBRARY}_DEBUG)
+  IF(${MYLIBRARY}_DEBUG AND ${MYLIBRARY})
+    SET(${MYLIBRARY}
+      debug ${${MYLIBRARY}_DEBUG}
+      optimized ${${MYLIBRARY}}
+    )
+  ELSEIF(${MYLIBRARY}_DEBUG)
+    SET(${MYLIBRARY} ${${MYLIBRARY}_DEBUG})
+  ENDIF()  
   
 ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
 
diff --git a/corelib/src/OptimizerG2O.cpp b/corelib/src/OptimizerG2O.cpp
index 28feb612..5b3ca552 100644
--- a/corelib/src/OptimizerG2O.cpp
+++ b/corelib/src/OptimizerG2O.cpp
@@ -523,10 +523,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				}
 			}
 		}
-		optimizer.clear();
-		g2o::Factory::destroy();
-		g2o::OptimizationAlgorithmFactory::destroy();
-		g2o::HyperGraphActionLibrary::destroy();
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
 	{
diff --git a/corelib/src/vertigo/g2o/edge_se2Switchable.cpp b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp
index 565ace93..35542c6f 100644
--- a/corelib/src/vertigo/g2o/edge_se2Switchable.cpp
+++ b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp
@@ -19,9 +19,10 @@ using namespace Eigen;
 EdgeSE2Switchable::EdgeSE2Switchable() : g2o::BaseMultiEdge<3, g2o::SE2>()
 {
   resize(3);
-  _jacobianOplus[0].resize(3,3);    
-  _jacobianOplus[1].resize(3,3);
-  _jacobianOplus[2].resize(3,1); 
+  _jacobianOplus.clear();
+  _jacobianOplus.push_back(JacobianType(0, 3, 3));
+  _jacobianOplus.push_back(JacobianType(0, 3, 3));
+  _jacobianOplus.push_back(JacobianType(0, 3, 1));
 
 }
 // ================================================
diff --git a/corelib/src/vertigo/g2o/edge_se3Switchable.cpp b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp
index f7e506a3..3aa4e6ee 100644
--- a/corelib/src/vertigo/g2o/edge_se3Switchable.cpp
+++ b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp
@@ -22,9 +22,10 @@ using namespace Eigen;
 EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d>()
 {
   resize(3);
-  _jacobianOplus[0].resize(6,6); 
-  _jacobianOplus[1].resize(6,6);
-  _jacobianOplus[2].resize(6,1);
+  _jacobianOplus.clear();
+  _jacobianOplus.push_back(JacobianType(0, 6, 6));
+  _jacobianOplus.push_back(JacobianType(0, 6, 6));
+  _jacobianOplus.push_back(JacobianType(0, 6, 1));
 
 }
 // ================================================