Skip to content
Snippets Groups Projects
Commit 42e26728 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

done

parent a99317af
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 40
− 115
View file @ 42e26728
......@@ -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)
......
cmake_modules/wolflaserConfig.cmake
+ 6
− 1
View file @ 42e26728
......@@ -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
src/solver_suitesparse/solver_manager.cpp deleted 100644 → 0
+ 0
− 266
View file @ a99317af
//--------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(&parameter_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;
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment