diff --git a/.gitignore b/.gitignore
index 6c5bf11964b51ece53f21d2cc93e962a6c9e461b..f9a413586763392b487804acf4a3f830a91b60c0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -21,7 +21,7 @@ src/examples/map_polyline_example_write.yaml
/CMakeCache.txt
*.dat
.DS_Store
-
+src/examples
*.graffle
/Default/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6bed07c68bb32cca13c66823341eb583800f1e84..3039a1a0d63508e882539e13a97bfdd3bf1c298b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,7 +11,7 @@ SET(CMAKE_MACOSX_RPATH 1)
# The project name
PROJECT(gnss)
-
+set(PLUGIN_NAME wolf${PROJECT_NAME})
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
@@ -242,7 +242,7 @@ SET(SRCS_YAML
# ENDIF(Ceres_FOUND)
# create the shared library
-ADD_LIBRARY(${PROJECT_NAME}
+ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS_CAPTURE}
${SRCS_COMMON}
@@ -259,20 +259,20 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_WRAPPER}
${SRCS_YAML}
)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${laser_scan_utils_LIBRARY})
#Link the created libraries
#===============EXAMPLE=========================
# IF (Ceres_FOUND)
-# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
+# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
# ENDIF(Ceres_FOUND)
#Build tests
#===============EXAMPLE=========================
IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
+ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${GLOG_LIBRARY})
ENDIF (GLOG_FOUND)
IF (GLOG_FOUND)
IF(BUILD_TESTS)
@@ -284,12 +284,12 @@ ENDIF (GLOG_FOUND)
#install library
#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
+INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iri-algorithms
ARCHIVE DESTINATION lib/iri-algorithms)
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
+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)
@@ -321,24 +321,14 @@ INSTALL(FILES ${HDRS_YAML}
FILE(WRITE ${PROJECT_NAME}.found "")
INSTALL(FILES ${PROJECT_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
- DESTINATION "lib/cmake/wolf${PROJECT_NAME}")
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolf${PROJECT_NAME}Config.cmake"
- "${CMAKE_BINARY_DIR}/wolf${PROJECT_NAME}Config.cmake" @ONLY)
-# configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
-# "${CMAKE_BINARY_DIR}/Findwolf${PROJECT_NAME}.cmake" @ONLY)
-
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/internal)
-# INSTALL(FILES "${CMAKE_BINARY_DIR}/cmake_modules/Findwolf${PROJECT_NAME}.cmake"
-# DESTINATION "lib/cmake/${PROJECT_NAME}")
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolf${PROJECT_NAME}Config.cmake" DESTINATION "lib/cmake/wolf${PROJECT_NAME}")
+INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-export(PACKAGE wolf_${PROJECT_NAME})
+export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen)
diff --git a/cmake_modules/Findwolfgnss.cmake b/cmake_modules/Findwolfgnss.cmake
deleted file mode 100644
index d8283e3ead3531273ff8508f91b68b65b0dc6efa..0000000000000000000000000000000000000000
--- a/cmake_modules/Findwolfgnss.cmake
+++ /dev/null
@@ -1,36 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- example_INCLUDE_DIRS
- # NAMES wolf.found
- PATHS /usr/local/include/iri-algorithms/wolf/example)
-#change INCLUDE_DIRS to its parent directory
-# get_filename_component(example_INCLUDE_DIRS ${example_INCLUDE_DIRS} DIRECTORY)
-IF(example_INCLUDE_DIRS)
- MESSAGE("Found example include dirs: ${example_INCLUDE_DIRS}")
-ELSE
- MESSAGE("Couldn't find example include dirs")
-ENDIF
-
-FIND_LIBRARY(
- example_LIBRARY
- NAMES libexample.so
- PATHS /usr/lib /usr/local/lib /usr/local/lib/iri-algorithms)
-IF(example_LIBRARY)
- MESSAGE("Found example lib: ${example_LIBRARY}")
-ELSE
- MESSAGE("Couldn't find example lib")
-ENDIF
-IF (example_INCLUDE_DIRS AND example_LIBRARY)
- SET(example_FOUND TRUE)
-ENDIF (example_INCLUDE_DIRS AND example_LIBRARY)
-
-IF (example_FOUND)
- IF (NOT example_FIND_QUIETLY)
- MESSAGE(STATUS "Found example: ${example_LIBRARY}")
- ENDIF (NOT example_FIND_QUIETLY)
-ELSE (example_FOUND)
- IF (wolf_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find example")
- ENDIF (wolf_FIND_REQUIRED)
-ENDIF (example_FOUND)
-
diff --git a/cmake_modules/wolfgnssConfig.cmake b/cmake_modules/wolfgnssConfig.cmake
index 82bb7cf0a5523302eb3d6c7fd5064e382a6c59f5..f5637549db88d73cce0259e80f1306e4e1ba94b0 100644
--- a/cmake_modules/wolfgnssConfig.cmake
+++ b/cmake_modules/wolfgnssConfig.cmake
@@ -1,222 +1,65 @@
-# This file was copied and adapted from the ceres_solver project
-# http://ceres-solver.org/
+#edit the following line to add the librarie's header files
+FIND_PATH(
+ wolfgnss_INCLUDE_DIR
+ NAMES gnss.found
+ PATHS /usr/local/include/iri-algorithms/wolf/plugin_gnss)
+IF(wolfgnss_INCLUDE_DIR)
+ MESSAGE("Found gnss include dirs: ${wolfgnss_INCLUDE_DIR}")
+ELSE(wolfgnss_INCLUDE_DIR)
+ MESSAGE("Couldn't find gnss include dirs")
+ENDIF(wolfgnss_INCLUDE_DIR)
+
+FIND_LIBRARY(
+ wolfgnss_LIBRARY
+ NAMES libwolfgnss.so
+ PATHS /usr/local/lib/iri-algorithms)
+IF(wolfgnss_LIBRARY)
+ MESSAGE("Found gnss lib: ${wolfgnss_LIBRARY}")
+ELSE(wolfgnss_LIBRARY)
+ MESSAGE("Couldn't find wolf gnss lib")
+ENDIF(wolfgnss_LIBRARY)
+
+IF (wolfgnss_INCLUDE_DIR AND wolfgnss_LIBRARY)
+ SET(wolfgnss_FOUND TRUE)
+ ELSE(wolfgnss_INCLUDE_DIR AND wolfgnss_LIBRARY)
+ set(wolfgnss_FOUND FALSE)
+ENDIF (wolfgnss_INCLUDE_DIR AND wolfgnss_LIBRARY)
+
+IF (wolfgnss_FOUND)
+ IF (NOT wolfgnss_FIND_QUIETLY)
+ MESSAGE(STATUS "Found gnss: ${wolfgnss_LIBRARY}")
+ ENDIF (NOT wolfgnss_FIND_QUIETLY)
+ELSE (wolfgnss_FOUND)
+ IF (wolfgnss_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find wolf gnss")
+ ENDIF (wolfgnss_FIND_REQUIRED)
+ENDIF (wolfgnss_FOUND)
-# wolf - Windowed Localization Frames
-# Copyright 2016
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * Redistributions of source code must retain the above copyright notice,
-# this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-# this list of conditions and the following disclaimer in the documentation
-# and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-# used to endorse or promote products derived from this software without
-# specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Authors:
-#
-
-# Config file for wolf - Find wolf & dependencies.
-#
-# This file is used by CMake when find_package(wolf) is invoked and either
-# the directory containing this file either is present in CMAKE_MODULE_PATH
-# (if wolf was installed), or exists in the local CMake package registry if
-# the wolf build directory was exported.
-#
-# This module defines the following variables:
-#
-# wolf_FOUND / wolf_FOUND: True if wolf has been successfully
-# found. Both variables are set as although
-# FindPackage() only references wolf_FOUND
-# in Config mode, given the conventions for
-# <package>_FOUND when FindPackage() is
-# called in Module mode, users could
-# reasonably expect to use wolf_FOUND
-# instead.
-#
-# wolf_VERSION: Version of wolf found.
-#
-# wolf_INCLUDE_DIRS: Include directories for wolf and the
-# dependencies which appear in the wolf public
-# API and are thus required to use wolf.
-#
-# wolf_LIBRARIES: Libraries for wolf and all
-# dependencies against which wolf was
-# compiled. This will not include any optional
-# dependencies that were disabled when wolf was
-# compiled.
-#
-# The following variables are also defined for legacy compatibility
-# only. Any new code should not use them as they do not conform to
-# the standard CMake FindPackage naming conventions.
-#
-# wolf_INCLUDES = ${wolf_INCLUDE_DIRS}.
-
-# Called if we failed to find Ceres or any of its required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(wolf_report_not_found REASON_MSG)
- # FindPackage() only references Ceres_FOUND, and requires it to be
- # explicitly set FALSE to denote not found (not merely undefined).
- set(wolf_FOUND FALSE)
- set(wolf_FOUND FALSE)
- unset(wolf_INCLUDE_DIRS)
- unset(wolf_LIBRARIES)
+ set(wolfgnss_FOUND FALSE)
+ unset(wolfgnss_INCLUDE_DIR)
+ unset(wolfgnss_LIBRARIES)
# Reset the CMake module path to its state when this script was called.
set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
# FindPackage() use the camelcase library name, not uppercase.
- if (wolf_FIND_QUIETLY)
- message(STATUS "Failed to find wolf - " ${REASON_MSG} ${ARGN})
- else (wolf_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
+ if (wolfgnss_FIND_QUIETLY)
+ message(STATUS "Failed to find wolf gnss- " ${REASON_MSG} ${ARGN})
+ else (wolfgnss_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find wolf gnss - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
# that prevents generation, but continues configuration.
- message(SEND_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
+ message(SEND_ERROR "Failed to find wolf gnss - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(wolf_report_not_found)
-# Record the state of the CMake module path when this script was
-# called so that we can ensure that we leave it in the same state on
-# exit as it was on entry, but modify it locally.
-set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
-
-# Get the (current, i.e. installed) directory containing this file.
-get_filename_component(wolf_CURRENT_CONFIG_DIR
- "${CMAKE_CURRENT_LIST_FILE}" PATH)
-
-# Reset CMake module path to the installation directory of this
-# script, thus we will use the FindPackage() scripts shipped with
-# wolf to find wolf' dependencies, even if the user has equivalently
-# named FindPackage() scripts in their project.
-set(CMAKE_MODULE_PATH ${wolf_CURRENT_CONFIG_DIR})
-
-# Build the absolute root install directory as a relative path
-# (determined when wolf was configured & built) from the current
-# install directory for this this file. This allows for the install
-# tree to be relocated, after wolf was built, outside of CMake.
-get_filename_component(CURRENT_ROOT_INSTALL_DIR
- ${wolf_CURRENT_CONFIG_DIR}/../../../
- ABSOLUTE)
-if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
- wolf_report_not_found(
- "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
- "determined from relative path from wolfConfig.cmake install location: "
- "${wolf_CURRENT_CONFIG_DIR}, does not exist. Either the install "
- "directory was deleted, or the install tree was only partially relocated "
- "outside of CMake after wolf was built.")
-endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
-
+if(NOT wolfgnss_FOUND)
+ wolf_report_not_found("TROUBLE DUDE.")
+endif(NOT wolfgnss_FOUND)
# Set the include directories for wolf (itself).
-set(wolf_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include/iri-algorithms")
-
-# if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
-if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
- wolf_report_not_found(
- "wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
- "determined from relative path from wolfConfig.cmake install location: "
- "${wolf_CURRENT_CONFIG_DIR}, does not contain wolf headers. "
- "Either the install directory was deleted, or the install tree was only "
- "partially relocated outside of CMake after wolf was built.")
-# endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
-endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
-list(APPEND wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIR}/wolf)
-
-# Set the version.
-set(wolf_VERSION 0.0.1)
-
-# Eigen.
-
-# Flag set during configuration and build of wolf.
-set(wolf_EIGEN_VERSION @EIGEN_VERSION@)
-# Append the locations of Eigen when wolf was built to the search path hints.
-list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3)
-# Search quietly to control the timing of the error message if not found. The
-# search should be for an exact match, but for usability reasons do a soft
-# match and reject with an explanation below.
-
-find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET)
-
-# Flag set with currently found Eigen version.
-set(EIGEN_VERSION @EIGEN_VERSION@)
-if (EIGEN3_FOUND)
- if (NOT EIGEN_VERSION VERSION_EQUAL wolf_EIGEN_VERSION)
- # CMake's VERSION check in FIND_PACKAGE() will accept any version >= the
- # specified version. However, only version = is supported. Improve
- # usability by explaining why we don't accept non-exact version matching.
- wolf_report_not_found("Found Eigen dependency, but the version of Eigen "
- "found (${EIGEN_VERSION}) does not exactly match the version of Eigen "
- "wolf was compiled with (${wolf_EIGEN_VERSION}). This can cause subtle "
- "bugs by triggering violations of the One Definition Rule. See the "
- "Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule "
- "for more details")
- endif ()
- message(STATUS "Found required wolf dependency: "
- "Eigen version ${wolf_EIGEN_VERSION} in ${EIGEN3_INCLUDE_DIR}")
-else (EIGEN3_FOUND)
- wolf_report_not_found("Missing required wolf "
- "dependency: Eigen version ${wolf_EIGEN_VERSION}, please set "
- "EIGEN3_INCLUDE_DIR.")
-endif (EIGEN3_FOUND)
-list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
-
-# Import exported wolf targets, if they have not already been imported.
-if (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
- include(${wolf_CURRENT_CONFIG_DIR}/wolfTargets.cmake)
-endif (NOT TARGET wolf AND NOT wolf_BINARY_DIR)
-# Set the expected XX_LIBRARIES variable for FindPackage().
-set(wolf_LIBRARIES wolf)
-
-# Set legacy library variable for backwards compatibility.
-set(wolf_LIBRARY ${wolf_LIBRARIES})
-
-# Make user aware of any compile flags that will be added to their targets
-# which use wolf (i.e. flags exported in the wolf target). Only CMake
-# versions >= 2.8.12 support target_compile_options().
-if (TARGET ${wolf_LIBRARIES} AND
- NOT CMAKE_VERSION VERSION_LESS "2.8.12")
- get_target_property(wolf_INTERFACE_COMPILE_OPTIONS
- ${wolf_LIBRARIES} INTERFACE_COMPILE_OPTIONS)
-
- set(wolf_LOCATION "${CURRENT_ROOT_INSTALL_DIR}")
-
- # Check for -std=c++11 flags.
- if (wolf_INTERFACE_COMPILE_OPTIONS MATCHES ".*std=c\\+\\+11.*")
- message(STATUS "wolf version ${wolf_VERSION} detected here: "
- "${wolf_LOCATION} was built with C++11. wolf target will add "
- "C++11 flags to compile options for targets using it.")
- endif()
-endif()
-
-# Reset CMake module path to its state when this script was called.
-set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-# As we use wolf_REPORT_NOT_FOUND() to abort, if we reach this point we have
-# found wolf and all required dependencies.
-message(STATUS "Found wolf version: ${wolf_VERSION} installed in: ${CURRENT_ROOT_INSTALL_DIR}")
-
-# Set wolf_FOUND to be equivalent to wolf_FOUND, which is set to
-# TRUE by FindPackage() if this file is found and run, and after which
-# wolf_FOUND is not (explicitly, i.e. undefined does not count) set
-# to FALSE.
-set(wolf_FOUND TRUE)
+set(wolfgnss_FOUND TRUE)
\ No newline at end of file
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 9cf010e0eaf62fe4e74d4f0c241e088bc3e798aa..5e21843f4a88bbef865b8739deec5111cd12dcd5 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -59,7 +59,8 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr)
// EXTRACT AND ADD FEATURES
//WOLF_DEBUG( "PR ", getName()," - adding the feature...");
- FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssFix>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()));
+ FeatureBasePtr ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
+ // FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssFix>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()));
// EMPLACE CONSTRAINT
emplaceFactor(ftr_ptr);
@@ -110,14 +111,13 @@ void ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
FactorBasePtr new_fac_ptr = nullptr;
// 2D
if (getProblem()->getDim() == 2)
- new_fac_ptr = std::make_shared<FactorGnssFix2D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
+ new_fac_ptr = FactorBase::emplace<FactorGnssFix2D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
// 3D
else
- new_fac_ptr = std::make_shared<FactorGnssFix3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
+ new_fac_ptr = FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
// ADD CONSTRAINT --------------------
//WOLF_DEBUG("adding the factor...");
- ftr_ptr->addFactor(new_fac_ptr);
}
bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
@@ -170,7 +170,10 @@ void ProcessorGnssFix::configure(SensorBasePtr _sensor)
ProcessorBasePtr ProcessorGnssFix::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
{
- ProcessorGnssFixPtr prc_ptr = std::make_shared<ProcessorGnssFix>(std::static_pointer_cast<ProcessorParamsGnssFix>(_params), std::static_pointer_cast<SensorGnss>(sensor_ptr));
+ // ProcessorGnssFixPtr prc_ptr = std::make_shared<ProcessorGnssFix>(std::static_pointer_cast<ProcessorParamsGnssFix>(_params), std::static_pointer_cast<SensorGnss>(sensor_ptr));
+ ProcessorGnssFixPtr prc_ptr = std::static_pointer_cast<ProcessorGnssFix>(ProcessorBase::emplace<ProcessorParamsGnssFix>(sensor_ptr,
+ std::static_pointer_cast<ProcessorParamsGnssFix>(_params),
+ std::static_pointer_cast<SensorGnss>(sensor_ptr)));
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 9ee0f26f84dd90bedcfd88cfbb9cd4cc820174f3..1c61c896f94f9d906a51d57fefad51ad1d06d117 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -192,7 +192,6 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V
if (_extrinsics.size() == 3)
sen = std::make_shared<SensorGnss>(p_ptr, nullptr);
-
else
{
StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head<3>(), intrinsics_gnss_ptr->extrinsics_fixed_);