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

done

(cherry picked from commit 42e26728)
parent a99317af
No related branches found
No related tags found
1 merge request!27Resolve "Change destination of libxxx.so for WOLF and dependencies"
...@@ -8,7 +8,6 @@ endif(COMMAND cmake_policy) ...@@ -8,7 +8,6 @@ endif(COMMAND cmake_policy)
# MAC OSX RPATH # MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1) SET(CMAKE_MACOSX_RPATH 1)
# The project name # The project name
PROJECT(laser) PROJECT(laser)
set(PLUGIN_NAME wolf${PROJECT_NAME}) set(PLUGIN_NAME wolf${PROJECT_NAME})
...@@ -43,22 +42,6 @@ if(UNIX) ...@@ -43,22 +42,6 @@ if(UNIX)
endif(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) IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON) OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS) ENDIF(NOT BUILD_TESTS)
...@@ -83,6 +66,23 @@ if(BUILD_TESTS) ...@@ -83,6 +66,23 @@ if(BUILD_TESTS)
#include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
enable_testing() enable_testing()
endif() 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 # Does this has any other interest
# but for the examples ? # but for the examples ?
# yes, for the tests ! # yes, for the tests !
...@@ -91,28 +91,13 @@ IF(BUILD_EXAMPLES OR BUILD_TESTS) ...@@ -91,28 +91,13 @@ IF(BUILD_EXAMPLES OR BUILD_TESTS)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR}) set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
ENDIF(BUILD_EXAMPLES OR BUILD_TESTS) ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
#find dependencies. #find dependencies.
# ============EXAMPLE================== # ============EXAMPLE==================
FIND_PACKAGE(wolfcore REQUIRED) FIND_PACKAGE(wolfcore REQUIRED)
FIND_PACKAGE(laser_scan_utils REQUIRED) FIND_PACKAGE(laser_scan_utils REQUIRED)
FIND_PACKAGE(csm QUIET) 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 # Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal) SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
...@@ -133,22 +118,10 @@ include_directories("${PROJECT_BINARY_DIR}/conf") ...@@ -133,22 +118,10 @@ include_directories("${PROJECT_BINARY_DIR}/conf")
#INCLUDES SECTION #INCLUDES SECTION
# ============EXAMPLE================== # ============EXAMPLE==================
INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
#HEADERS #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 SET(HDRS_CAPTURE
include/laser/capture/capture_laser_2d.h include/laser/capture/capture_laser_2d.h
) )
...@@ -165,6 +138,9 @@ SET(HDRS_LANDMARK ...@@ -165,6 +138,9 @@ SET(HDRS_LANDMARK
include/laser/landmark/landmark_polyline_2d.h include/laser/landmark/landmark_polyline_2d.h
include/laser/landmark/landmark_match_polyline_2d.h include/laser/landmark/landmark_match_polyline_2d.h
) )
SET(HDRS_MATH
include/laser/math/laser_tools.h
)
SET(HDRS_PROCESSOR SET(HDRS_PROCESSOR
include/laser/processor/polyline_2d_utils.h include/laser/processor/polyline_2d_utils.h
include/laser/processor/processor_tracker_feature_polyline_2d.h include/laser/processor/processor_tracker_feature_polyline_2d.h
...@@ -172,26 +148,14 @@ SET(HDRS_PROCESSOR ...@@ -172,26 +148,14 @@ SET(HDRS_PROCESSOR
SET(HDRS_SENSOR SET(HDRS_SENSOR
include/laser/sensor/sensor_laser_2d.h include/laser/sensor/sensor_laser_2d.h
) )
SET(HDRS_SOLVER SET(HDRS_STATE_BLOCK
) include/laser/state_bock/local_parametrization_polyline_extreme.h
SET(HDRS_DTASSC
) )
#SOURCES #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 SET(SRCS_CAPTURE
src/capture/capture_laser_2d.cpp src/capture/capture_laser_2d.cpp
) )
SET(SRCS_FACTOR
)
SET(SRCS_FEATURE SET(SRCS_FEATURE
src/feature/feature_polyline_2d.cpp src/feature/feature_polyline_2d.cpp
src/feature/feature_match_polyline_2d.cpp src/feature/feature_match_polyline_2d.cpp
...@@ -207,13 +171,13 @@ SET(SRCS_PROCESSOR ...@@ -207,13 +171,13 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR SET(SRCS_SENSOR
src/sensor/sensor_laser_2d.cpp src/sensor/sensor_laser_2d.cpp
) )
SET(SRCS_DTASSC SET(SRCS_STATE_BLOCK
) src/state_block/local_parametrization_polyline_extreme.cpp
SET(SRCS_SOLVER
) )
SET(SRCS_YAML SET(SRCS_YAML
src/yaml/sensor_laser_2d_yaml.cpp src/yaml/sensor_laser_2d_yaml.cpp
) )
#OPTIONALS #OPTIONALS
if(csm_FOUND) if(csm_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
...@@ -234,44 +198,16 @@ if(csm_FOUND) ...@@ -234,44 +198,16 @@ if(csm_FOUND)
src/yaml/processor_odom_icp_yaml.cpp src/yaml/processor_odom_icp_yaml.cpp
) )
endif(csm_FOUND) 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 # create the shared library
ADD_LIBRARY(${PLUGIN_NAME} ADD_LIBRARY(${PLUGIN_NAME}
SHARED SHARED
${SRCS_CAPTURE} ${SRCS_CAPTURE}
${SRCS_COMMON}
${SRCS_DTASSC}
${SRCS_FACTOR}
${SRCS_FEATURE} ${SRCS_FEATURE}
${SRCS_LANDMARK} ${SRCS_LANDMARK}
${SRCS_MATH}
${SRCS_PROCESSOR} ${SRCS_PROCESSOR}
${SRCS_SENSOR} ${SRCS_SENSOR}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK} ${SRCS_STATE_BLOCK}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML} ${SRCS_YAML}
) )
...@@ -290,7 +226,8 @@ endif() ...@@ -290,7 +226,8 @@ endif()
#Link the created libraries #Link the created libraries
#===============EXAMPLE========================= #===============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 #Build tests
#===============EXAMPLE========================= #===============EXAMPLE=========================
...@@ -300,46 +237,34 @@ IF(BUILD_TESTS) ...@@ -300,46 +237,34 @@ IF(BUILD_TESTS)
ENDIF(BUILD_TESTS) ENDIF(BUILD_TESTS)
#install library #install library
#============================================================= #=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iri-algorithms LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib/iri-algorithms) ARCHIVE DESTINATION lib)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME}) install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers #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} INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_FACTOR} INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE} INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/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} INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
INSTALL(FILES ${HDRS_WRAPPER} INSTALL(FILES ${HDRS_MATH}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE} INSTALL(FILES ${HDRS_PROCESSOR}
# DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_SOLVER} INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
INSTALL(FILES ${HDRS_SERIALIZATION} INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/serialization) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
FILE(WRITE laser.found "")
INSTALL(FILES laser.found INSTALL(FILES laser.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
FILE(WRITE laser.found "")
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h" INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal) DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
......
...@@ -12,7 +12,7 @@ ENDIF(wolflaser_INCLUDE_DIRS) ...@@ -12,7 +12,7 @@ ENDIF(wolflaser_INCLUDE_DIRS)
FIND_LIBRARY( FIND_LIBRARY(
wolflaser_LIBRARIES wolflaser_LIBRARIES
NAMES libwolflaser.so NAMES libwolflaser.so
PATHS /usr/local/lib/iri-algorithms) PATHS /usr/local/lib)
IF(wolflaser_LIBRARIES) IF(wolflaser_LIBRARIES)
MESSAGE("Found laser lib: ${wolflaser_LIBRARIES}") MESSAGE("Found laser lib: ${wolflaser_LIBRARIES}")
ELSE(wolflaser_LIBRARIES) ELSE(wolflaser_LIBRARIES)
...@@ -84,3 +84,8 @@ if(NOT wolf_FOUND) ...@@ -84,3 +84,8 @@ if(NOT wolf_FOUND)
list(REVERSE wolflaser_LIBRARIES) list(REVERSE wolflaser_LIBRARIES)
endif() 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
//--------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% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment