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 @@
catkin
boost
+ sparse_bundle_adjustment
boost
+ sparse_bundle_adjustment
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
+#include
SpaSolver::SpaSolver()
{
@@ -59,7 +59,7 @@ void SpaSolver::AddNode(karto::Vertex* 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* pEdge)
@@ -80,5 +80,5 @@ void SpaSolver::AddConstraint(karto::Edge* 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
+#include
#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
@@ -27,7 +27,7 @@
#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10)
#include
-#include "sba/spa2d.h"
+#include "sparse_bundle_adjustment/spa2d.h"
typedef std::vector 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 .
*/
-#include
+#include
/**
* 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
+#include
/**
* 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.
/////////////////////////////////////