From 3ef9f6baae5e277263068705ca0e1e406424da19 Mon Sep 17 00:00:00 2001
From: Emiliano Borghi <emiliano.borghi@gmail.com>
Date: Wed, 8 Sep 2021 18:56:08 -0300
Subject: [PATCH] Compile and fix samples (#27)

* Add SBA and Eigen as dependencies
* Compile samples
---
 CMakeLists.txt         | 11 +++++++++--
 package.xml            |  2 ++
 samples/CMakeLists.txt | 22 ++++++++--------------
 samples/SpaSolver.cpp  |  6 +++---
 samples/SpaSolver.h    |  4 ++--
 samples/tutorial1.cpp  |  4 ++--
 samples/tutorial2.cpp  |  4 ++--
 7 files changed, 28 insertions(+), 25 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1d7838a..42c02d6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,11 +1,16 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(open_karto)
 
-find_package(catkin REQUIRED)
-
+find_package(catkin REQUIRED
+  COMPONENTS
+    sparse_bundle_adjustment
+)
+find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
 
 catkin_package(
+  CATKIN_DEPENDS
+    sparse_bundle_adjustment
   DEPENDS Boost
   INCLUDE_DIRS
     include
@@ -27,3 +32,5 @@ install(TARGETS karto
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
 )
+
+include(samples/CMakeLists.txt)
diff --git a/package.xml b/package.xml
index c53dab3..49dc8a8 100644
--- a/package.xml
+++ b/package.xml
@@ -13,7 +13,9 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>boost</build_depend>
+  <build_depend>sparse_bundle_adjustment</build_depend>
 
   <run_depend>boost</run_depend>
+  <run_depend>sparse_bundle_adjustment</run_depend>
 
 </package>
diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt
index a51b7ba..dc65cec 100644
--- a/samples/CMakeLists.txt
+++ b/samples/CMakeLists.txt
@@ -28,16 +28,14 @@ if(WIN32)
   endforeach(warning)
 endif()
 
-include_directories(../source)
-
 ############################################################################
 # tutorial 1
 add_executable(
-  Karto_Tutorial1 
-  tutorial1.cpp
+  Karto_Tutorial1
+  samples/tutorial1.cpp
 )
 
-target_link_libraries(Karto_Tutorial1 OpenKarto)
+target_link_libraries(Karto_Tutorial1 karto)
 
 if (UNIX)
   target_link_libraries(Karto_Tutorial1 "pthread")
@@ -46,20 +44,16 @@ endif()
 ############################################################################
 # tutorial 2
 
-include_directories(${CSPARSE_DIR}/CSparse/Include)
-include_directories(${SBA_DIR}/sba/include)
-include_directories(${EIGEN_DIR}/eigen)
+include_directories(${EIGEN3_INCLUDE_DIRS})
 
 add_executable(
   Karto_Tutorial2
-  tutorial2.cpp
-  SpaSolver.h
-  SpaSolver.cpp
+  samples/tutorial2.cpp
+  samples/SpaSolver.h
+  samples/SpaSolver.cpp
 )
 
-target_link_libraries(Karto_Tutorial2 OpenKarto)
-target_link_libraries(Karto_Tutorial2 spa2d)
-target_link_libraries(Karto_Tutorial2 csparse)
+target_link_libraries(Karto_Tutorial2 karto ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
 
 if(PNG_FOUND)
   target_link_libraries(Karto_Tutorial2 ${ZLIB_LIBRARIES})
diff --git a/samples/SpaSolver.cpp b/samples/SpaSolver.cpp
index c74bcdd..54ec72d 100644
--- a/samples/SpaSolver.cpp
+++ b/samples/SpaSolver.cpp
@@ -16,7 +16,7 @@
  */
 
 #include "SpaSolver.h"
-#include <OpenKarto/Karto.h>
+#include <open_karto/Karto.h>
 
 SpaSolver::SpaSolver()
 {
@@ -59,7 +59,7 @@ void SpaSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
 {
   karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
   Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
-  m_Spa.addNode2d(vector, pVertex->GetObject()->GetUniqueId());
+  m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
 }
 
 void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
@@ -80,5 +80,5 @@ void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
   m(1,2) = m(2,1) = precisionMatrix(1,2);
   m(2,2) = precisionMatrix(2,2);
 
-  m_Spa.addConstraint2d(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
+  m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
 }
diff --git a/samples/SpaSolver.h b/samples/SpaSolver.h
index 9fae93b..3ff84db 100644
--- a/samples/SpaSolver.h
+++ b/samples/SpaSolver.h
@@ -18,7 +18,7 @@
 #ifndef __SPASOLVER__
 #define __SPASOLVER__
 
-#include <OpenKarto/Mapper.h>
+#include <open_karto/Mapper.h>
 
 #ifndef EIGEN_USE_NEW_STDVECTOR
 #define EIGEN_USE_NEW_STDVECTOR
@@ -27,7 +27,7 @@
 #define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10)
 #include <Eigen/Eigen>
 
-#include "sba/spa2d.h"
+#include "sparse_bundle_adjustment/spa2d.h"
 
 typedef std::vector<karto::Matrix3> CovarianceVector;
 
diff --git a/samples/tutorial1.cpp b/samples/tutorial1.cpp
index 167b8a0..c4b0b2e 100644
--- a/samples/tutorial1.cpp
+++ b/samples/tutorial1.cpp
@@ -15,7 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <OpenKarto/Mapper.h>
+#include <open_karto/Mapper.h>
 
 /**
  * Sample code to demonstrate karto map creation
@@ -146,7 +146,7 @@ void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid)
   }
 }
 
-int main(int /*argc*/, char /***argv*/)
+int main(int /*argc*/, char **/*argv*/)
 {
   // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. 
   /////////////////////////////////////
diff --git a/samples/tutorial2.cpp b/samples/tutorial2.cpp
index 720c3cd..a5ea0ed 100644
--- a/samples/tutorial2.cpp
+++ b/samples/tutorial2.cpp
@@ -17,7 +17,7 @@
 
 #include "SpaSolver.h"
 
-#include <OpenKarto/Mapper.h>
+#include <open_karto/Mapper.h>
 
 /**
  * Sample code to demonstrate karto map creation
@@ -148,7 +148,7 @@ void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid)
   }
 }
 
-int main(int /*argc*/, char /***argv*/)
+int main(int /*argc*/, char **/*argv*/)
 {
   // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. 
   /////////////////////////////////////