diff --git a/CMakeLists.txt b/CMakeLists.txt
index b9d5883f1a46d1d87e4f83bbea678c9dbc7d83b1..f6ff9850b63391fd57619e02df4eb00702b0ec87 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,7 +8,6 @@ endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
-
# The project name
PROJECT(laser)
set(PLUGIN_NAME wolf${PROJECT_NAME})
@@ -43,22 +42,6 @@ if(UNIX)
endif(UNIX)
-MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
-IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
- set(_WOLF_DEBUG true)
-ENDIF()
-
-option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-
IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
@@ -83,6 +66,23 @@ if(BUILD_TESTS)
#include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
enable_testing()
endif()
+
+MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+
+#CMAKE modules
+
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+MESSAGE(STATUS ${CMAKE_MODULE_PATH})
+
+# Some wolf compilation options
+
+IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
+ set(_WOLF_DEBUG true)
+ENDIF()
+
+option(_WOLF_TRACE "Enable wolf tracing macro" ON)
+
# Does this has any other interest
# but for the examples ?
# yes, for the tests !
@@ -91,28 +91,13 @@ IF(BUILD_EXAMPLES OR BUILD_TESTS)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
+
#find dependencies.
# ============EXAMPLE==================
FIND_PACKAGE(wolfcore REQUIRED)
-
FIND_PACKAGE(laser_scan_utils REQUIRED)
-
FIND_PACKAGE(csm QUIET)
-#FIND_PATH(
-# Suitesparse_INCLUDE_DIRS
-# NAMES SuiteSparse_config.h
-# PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-#MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-#
-#IF(Suitesparse_INCLUDE_DIRS)
-# SET(Suitesparse_FOUND TRUE)
-# MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-#ELSE (Suitesparse_INCLUDE_DIRS)
-# SET(Suitesparse_FOUND FALSE)
-# MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-#ENDIF (Suitesparse_INCLUDE_DIRS)
-
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -133,22 +118,10 @@ include_directories("${PROJECT_BINARY_DIR}/conf")
#INCLUDES SECTION
# ============EXAMPLE==================
INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(BEFORE "include")
INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-
-
+INCLUDE_DIRECTORIES(BEFORE "include")
#HEADERS
-SET(HDRS_COMMON
- )
-SET(HDRS_MATH
- include/laser/math/laser_tools.h
- )
-SET(HDRS_UTILS
- )
-SET(HDRS_STATE_BLOCK
- include/laser/state_block/local_parametrization_polyline_extreme.h
- )
SET(HDRS_CAPTURE
include/laser/capture/capture_laser_2d.h
)
@@ -165,6 +138,9 @@ SET(HDRS_LANDMARK
include/laser/landmark/landmark_polyline_2d.h
include/laser/landmark/landmark_match_polyline_2d.h
)
+SET(HDRS_MATH
+ include/laser/math/laser_tools.h
+ )
SET(HDRS_PROCESSOR
include/laser/processor/polyline_2d_utils.h
include/laser/processor/processor_tracker_feature_polyline_2d.h
@@ -172,26 +148,14 @@ SET(HDRS_PROCESSOR
SET(HDRS_SENSOR
include/laser/sensor/sensor_laser_2d.h
)
-SET(HDRS_SOLVER
- )
-SET(HDRS_DTASSC
+SET(HDRS_STATE_BLOCK
+ include/laser/state_bock/local_parametrization_polyline_extreme.h
)
#SOURCES
-SET(SRCS_COMMON
- )
-SET(SRCS_MATH
- )
-SET(SRCS_UTILS
- )
-SET(SRCS_STATE_BLOCK
- src/state_block/local_parametrization_polyline_extreme.cpp
- )
SET(SRCS_CAPTURE
src/capture/capture_laser_2d.cpp
)
-SET(SRCS_FACTOR
- )
SET(SRCS_FEATURE
src/feature/feature_polyline_2d.cpp
src/feature/feature_match_polyline_2d.cpp
@@ -207,13 +171,13 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_laser_2d.cpp
)
-SET(SRCS_DTASSC
- )
-SET(SRCS_SOLVER
+SET(SRCS_STATE_BLOCK
+ src/state_block/local_parametrization_polyline_extreme.cpp
)
SET(SRCS_YAML
src/yaml/sensor_laser_2d_yaml.cpp
)
+
#OPTIONALS
if(csm_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
@@ -234,44 +198,16 @@ if(csm_FOUND)
src/yaml/processor_odom_icp_yaml.cpp
)
endif(csm_FOUND)
-#optional HDRS and SRCS
-# ==================EXAMPLE===============
-# IF (Ceres_FOUND)
-# SET(HDRS_WRAPPER
-# include/base/solver_suitesparse/sparse_utils.h
-# include/base/solver/solver_manager.h
-# include/base/ceres_wrapper/ceres_manager.h
-# include/base/ceres_wrapper/cost_function_wrapper.h
-# include/base/ceres_wrapper/create_numeric_diff_cost_function.h
-# include/base/ceres_wrapper/local_parametrization_wrapper.h
-# )
-# SET(SRCS_WRAPPER
-# src/solver/solver_manager.cpp
-# src/ceres_wrapper/ceres_manager.cpp
-# src/ceres_wrapper/local_parametrization_wrapper.cpp
-# )
-# ELSE(Ceres_FOUND)
-# SET(HDRS_WRAPPER)
-# SET(SRCS_WRAPPER)
-# ENDIF(Ceres_FOUND)
-
# create the shared library
ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS_CAPTURE}
- ${SRCS_COMMON}
- ${SRCS_DTASSC}
- ${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_LANDMARK}
- ${SRCS_MATH}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
- ${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
- ${SRCS_UTILS}
- ${SRCS_WRAPPER}
${SRCS_YAML}
)
@@ -290,7 +226,8 @@ endif()
#Link the created libraries
#===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES} ${laser_scan_utils_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${laser_scan_utils_LIBRARY})
#Build tests
#===============EXAMPLE=========================
@@ -300,46 +237,34 @@ IF(BUILD_TESTS)
ENDIF(BUILD_TESTS)
#install library
-
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
-INSTALL(FILES ${HDRS_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/serialization)
-INSTALL(FILES ${HDRS_YAML}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
+INSTALL(FILES ${HDRS_MATH}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
+INSTALL(FILES ${HDRS_PROCESSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+INSTALL(FILES ${HDRS_SENSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
+FILE(WRITE laser.found "")
INSTALL(FILES laser.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
-FILE(WRITE laser.found "")
-
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
diff --git a/cmake_modules/wolflaserConfig.cmake b/cmake_modules/wolflaserConfig.cmake
index 960a529edac8bff00ce5165681644d3d26884562..87cf4b3f6b749b1e7f4300bebe998b694a45a0e7 100644
--- a/cmake_modules/wolflaserConfig.cmake
+++ b/cmake_modules/wolflaserConfig.cmake
@@ -12,7 +12,7 @@ ENDIF(wolflaser_INCLUDE_DIRS)
FIND_LIBRARY(
wolflaser_LIBRARIES
NAMES libwolflaser.so
- PATHS /usr/local/lib/iri-algorithms)
+ PATHS /usr/local/lib)
IF(wolflaser_LIBRARIES)
MESSAGE("Found laser lib: ${wolflaser_LIBRARIES}")
ELSE(wolflaser_LIBRARIES)
@@ -84,3 +84,8 @@ if(NOT wolf_FOUND)
list(REVERSE wolflaser_LIBRARIES)
endif()
+
+# provide both INCLUDE_DIR and INCLUDE_DIRS
+SET(wolflaser_INCLUDE_DIR ${wolflaser_INCLUDE_DIRS})
+# provide both LIBRARY and LIBRARIES
+SET(wolflaser_LIBRARY ${wolflaser_LIBRARIES})
\ No newline at end of file
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
deleted file mode 100644
index aa767438d338d6aebcde2340409c022fa2f302c4..0000000000000000000000000000000000000000
--- a/src/solver_suitesparse/solver_manager.cpp
+++ /dev/null
@@ -1,266 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "core/ceres_wrapper/ceres_manager.h"
-
-SolverManager::SolverManager()
-{
-
-}
-
-SolverManager::~SolverManager()
-{
- removeAllStateUnits();
-}
-
-void SolverManager::solve()
-{
-
-}
-
-//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
-//{
-//}
-
-void SolverManager::update(const WolfProblemPtr _problem_ptr)
-{
- // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
- if (_problem_ptr->isReallocated())
- {
- // todo: reallocate x
- }
- else
- {
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
- {
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
-
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
- }
- //std::cout << "state units updated!" << std::endl;
-
- // REMOVE STATE UNITS
- while (!_problem_ptr->getRemovedStateList().empty())
- {
- // TODO: remove state unit
- //_problem_ptr->getRemovedStateList().pop_front();
- }
- //std::cout << "state units removed!" << std::endl;
-
- // ADD CONSTRAINTS
- FactorBasePtrList fac_list;
- _problem_ptr->getTrajectory()->getFactorList(fac_list);
- //std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
- for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
- if ((*fac_it)->getPendingStatus() == ADD_PENDING)
- addFactor(*fac_it);
-
- //std::cout << "factors updated!" << std::endl;
- }
-}
-
-void SolverManager::addFactor(FactorBasePtr _corr_ptr)
-{
- //TODO MatrixXd J; Vector e;
- // getResidualsAndJacobian(_corr_ptr, J, e);
- // solverQR->addFactor(_corr_ptr, J, e);
-
-// factor_map_[_corr_ptr->id()] = blockIdx;
- _corr_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeFactor(const unsigned int& _corr_idx)
-{
- // TODO
-}
-
-void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
-{
- //std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- // TODO
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_1d:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1d*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_VECTOR:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3d:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- if (_st_ptr->isFixed())
- updateStateUnitStatus(_st_ptr);
-
- _st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeAllStateUnits()
-{
- std::vector<double*> parameter_blocks;
-
- ceres_problem_->GetParameterBlocks(¶meter_blocks);
-
- for (unsigned int i = 0; i< parameter_blocks.size(); i++)
- ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
-}
-
-void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
-{
- // TODO
-
-// if (!_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
-// else if (_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
-// else
-// printf("\nERROR: Update state unit status with unknown status");
-//
-// _st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
-{
- //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
- //_corrPtr->print();
-
- switch (_corrPtr->getFactorType())
- {
- case FAC_GPS_FIX_2d:
- {
- FactorGPS2d* specific_ptr = (FactorGPS2d*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorGPS2d,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2d_COMPLEX_ANGLE:
- {
- FactorOdom2dComplexAngle* specific_ptr = (FactorOdom2dComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2dComplexAngle,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2d:
- {
- FactorOdom2d* specific_ptr = (FactorOdom2d*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2d,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_CORNER_2d:
- {
- FactorCorner2d* specific_ptr = (FactorCorner2d*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorCorner2d,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_IMU:
- {
- FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorIMU,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
-}