diff --git a/CMakeLists.txt b/CMakeLists.txt
index f6ff9850b63391fd57619e02df4eb00702b0ec87..b9d5883f1a46d1d87e4f83bbea678c9dbc7d83b1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,6 +8,7 @@ endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
+
# The project name
PROJECT(laser)
set(PLUGIN_NAME wolf${PROJECT_NAME})
@@ -42,6 +43,22 @@ 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)
@@ -66,23 +83,6 @@ 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,13 +91,28 @@ 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)
@@ -118,10 +133,22 @@ include_directories("${PROJECT_BINARY_DIR}/conf")
#INCLUDES SECTION
# ============EXAMPLE==================
INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
+INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
+
+
#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
)
@@ -138,9 +165,6 @@ 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
@@ -148,14 +172,26 @@ SET(HDRS_PROCESSOR
SET(HDRS_SENSOR
include/laser/sensor/sensor_laser_2d.h
)
-SET(HDRS_STATE_BLOCK
- include/laser/state_bock/local_parametrization_polyline_extreme.h
+SET(HDRS_SOLVER
+ )
+SET(HDRS_DTASSC
)
#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
@@ -171,13 +207,13 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_laser_2d.cpp
)
-SET(SRCS_STATE_BLOCK
- src/state_block/local_parametrization_polyline_extreme.cpp
+SET(SRCS_DTASSC
+ )
+SET(SRCS_SOLVER
)
SET(SRCS_YAML
src/yaml/sensor_laser_2d_yaml.cpp
)
-
#OPTIONALS
if(csm_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
@@ -198,16 +234,44 @@ 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}
)
@@ -226,8 +290,7 @@ endif()
#Link the created libraries
#===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${laser_scan_utils_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES} ${laser_scan_utils_LIBRARY})
#Build tests
#===============EXAMPLE=========================
@@ -237,34 +300,46 @@ IF(BUILD_TESTS)
ENDIF(BUILD_TESTS)
#install library
+
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib
- ARCHIVE DESTINATION lib)
+ LIBRARY DESTINATION lib/iri-algorithms
+ ARCHIVE DESTINATION lib/iri-algorithms)
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_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
-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)
+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)
-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 87cf4b3f6b749b1e7f4300bebe998b694a45a0e7..960a529edac8bff00ce5165681644d3d26884562 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)
+ PATHS /usr/local/lib/iri-algorithms)
IF(wolflaser_LIBRARIES)
MESSAGE("Found laser lib: ${wolflaser_LIBRARIES}")
ELSE(wolflaser_LIBRARIES)
@@ -84,8 +84,3 @@ 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
new file mode 100644
index 0000000000000000000000000000000000000000..aa767438d338d6aebcde2340409c022fa2f302c4
--- /dev/null
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -0,0 +1,266 @@
+//--------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;
+ }
+}