From 0b7fbd39e2fe3f7241820ee781bcac2d81c3aaca Mon Sep 17 00:00:00 2001
From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Mon, 16 Feb 2015 14:44:38 +0000
Subject: [PATCH] ceres branch moved to trunk
---
CMakeLists.txt | 101 --
Findwolf.cmake | 26 -
README.txt | 6 -
Wolf_ideas.txt | 96 --
src/CMakeLists.txt | 106 --
src/capture_base.cpp | 128 ---
src/capture_base.h | 106 --
src/capture_gps_fix.cpp | 53 -
src/capture_gps_fix.h | 32 -
src/capture_laser_2D.cpp | 22 -
src/capture_laser_2D.h | 43 -
src/capture_odom_2D.cpp | 97 --
src/capture_odom_2D.h | 32 -
src/capture_relative.cpp | 23 -
src/capture_relative.h | 24 -
src/ceres_wrapper/ceres_manager.cpp | 177 ---
src/ceres_wrapper/ceres_manager.h | 54 -
.../complex_angle_parametrization.cpp | 45 -
.../complex_angle_parametrization.h | 24 -
src/correspondence_base.cpp | 35 -
src/correspondence_base.h | 78 --
src/correspondence_gps_2D.h | 39 -
src/correspondence_odom_2D_complex_angle.h | 45 -
src/correspondence_odom_2D_theta.h | 45 -
src/correspondence_sparse.h | 140 ---
src/examples/CMakeLists.txt | 43 -
src/examples/test_ceres_manager.cpp | 818 -------------
src/examples/test_ceres_manager_tree.cpp | 374 ------
src/examples/test_ceres_odom_batch.cpp | 450 --------
src/examples/test_ceres_odom_iterative.cpp | 459 --------
src/examples/test_ceres_odom_plot.sce | 75 --
src/examples/test_ceres_wrapper_jet.cpp | 218 ----
.../test_ceres_wrapper_jet_2_sensors.cpp | 419 -------
.../test_ceres_wrapper_jet_ind_block.cpp | 214 ----
src/examples/test_ceres_wrapper_numeric.cpp | 220 ----
.../test_ceres_wrapper_state_units.cpp | 1021 -----------------
src/examples/test_node_linked.cpp | 147 ---
src/examples/test_wolf_tree.cpp | 68 --
src/feature_base.cpp | 87 --
src/feature_base.h | 80 --
src/feature_gps_fix.cpp | 30 -
src/feature_gps_fix.h | 50 -
src/feature_odom_2D.cpp | 29 -
src/feature_odom_2D.h | 51 -
src/frame_base.cpp | 161 ---
src/frame_base.h | 109 --
src/landmark_base.cpp | 65 --
src/landmark_base.h | 72 --
src/lineFit.sce | 111 --
src/map_base.cpp | 17 -
src/map_base.h | 57 -
src/node_base.cpp | 17 -
src/node_base.h | 92 --
src/node_linked.h | 416 -------
src/node_terminus.cpp | 12 -
src/node_terminus.h | 26 -
src/sensor_base.cpp | 33 -
src/sensor_base.h | 31 -
src/sensor_gps_fix.cpp | 17 -
src/sensor_gps_fix.h | 35 -
src/sensor_laser_2D.cpp | 33 -
src/sensor_laser_2D.h | 67 --
src/sensor_odom_2D.cpp | 22 -
src/sensor_odom_2D.h | 43 -
src/state_base.cpp | 30 -
src/state_base.h | 81 --
src/state_complex_angle.cpp | 38 -
src/state_complex_angle.h | 65 --
src/state_point.h | 98 --
src/time_stamp.cpp | 110 --
src/time_stamp.h | 142 ---
src/trajectory_base.cpp | 27 -
src/trajectory_base.h | 73 --
src/wolf.h | 293 -----
74 files changed, 8823 deletions(-)
delete mode 100644 CMakeLists.txt
delete mode 100644 Findwolf.cmake
delete mode 100644 README.txt
delete mode 100644 Wolf_ideas.txt
delete mode 100644 src/CMakeLists.txt
delete mode 100644 src/capture_base.cpp
delete mode 100644 src/capture_base.h
delete mode 100644 src/capture_gps_fix.cpp
delete mode 100644 src/capture_gps_fix.h
delete mode 100644 src/capture_laser_2D.cpp
delete mode 100644 src/capture_laser_2D.h
delete mode 100644 src/capture_odom_2D.cpp
delete mode 100644 src/capture_odom_2D.h
delete mode 100644 src/capture_relative.cpp
delete mode 100644 src/capture_relative.h
delete mode 100644 src/ceres_wrapper/ceres_manager.cpp
delete mode 100644 src/ceres_wrapper/ceres_manager.h
delete mode 100644 src/ceres_wrapper/complex_angle_parametrization.cpp
delete mode 100644 src/ceres_wrapper/complex_angle_parametrization.h
delete mode 100644 src/correspondence_base.cpp
delete mode 100644 src/correspondence_base.h
delete mode 100644 src/correspondence_gps_2D.h
delete mode 100644 src/correspondence_odom_2D_complex_angle.h
delete mode 100644 src/correspondence_odom_2D_theta.h
delete mode 100644 src/correspondence_sparse.h
delete mode 100644 src/examples/CMakeLists.txt
delete mode 100644 src/examples/test_ceres_manager.cpp
delete mode 100644 src/examples/test_ceres_manager_tree.cpp
delete mode 100644 src/examples/test_ceres_odom_batch.cpp
delete mode 100644 src/examples/test_ceres_odom_iterative.cpp
delete mode 100644 src/examples/test_ceres_odom_plot.sce
delete mode 100644 src/examples/test_ceres_wrapper_jet.cpp
delete mode 100644 src/examples/test_ceres_wrapper_jet_2_sensors.cpp
delete mode 100644 src/examples/test_ceres_wrapper_jet_ind_block.cpp
delete mode 100644 src/examples/test_ceres_wrapper_numeric.cpp
delete mode 100644 src/examples/test_ceres_wrapper_state_units.cpp
delete mode 100644 src/examples/test_node_linked.cpp
delete mode 100644 src/examples/test_wolf_tree.cpp
delete mode 100644 src/feature_base.cpp
delete mode 100644 src/feature_base.h
delete mode 100644 src/feature_gps_fix.cpp
delete mode 100644 src/feature_gps_fix.h
delete mode 100644 src/feature_odom_2D.cpp
delete mode 100644 src/feature_odom_2D.h
delete mode 100644 src/frame_base.cpp
delete mode 100644 src/frame_base.h
delete mode 100644 src/landmark_base.cpp
delete mode 100644 src/landmark_base.h
delete mode 100644 src/lineFit.sce
delete mode 100644 src/map_base.cpp
delete mode 100644 src/map_base.h
delete mode 100644 src/node_base.cpp
delete mode 100644 src/node_base.h
delete mode 100644 src/node_linked.h
delete mode 100644 src/node_terminus.cpp
delete mode 100644 src/node_terminus.h
delete mode 100644 src/sensor_base.cpp
delete mode 100644 src/sensor_base.h
delete mode 100644 src/sensor_gps_fix.cpp
delete mode 100644 src/sensor_gps_fix.h
delete mode 100644 src/sensor_laser_2D.cpp
delete mode 100644 src/sensor_laser_2D.h
delete mode 100644 src/sensor_odom_2D.cpp
delete mode 100644 src/sensor_odom_2D.h
delete mode 100644 src/state_base.cpp
delete mode 100644 src/state_base.h
delete mode 100644 src/state_complex_angle.cpp
delete mode 100644 src/state_complex_angle.h
delete mode 100644 src/state_point.h
delete mode 100644 src/time_stamp.cpp
delete mode 100644 src/time_stamp.h
delete mode 100644 src/trajectory_base.cpp
delete mode 100644 src/trajectory_base.h
delete mode 100644 src/wolf.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
deleted file mode 100644
index 02d0bde31..000000000
--- a/CMakeLists.txt
+++ /dev/null
@@ -1,101 +0,0 @@
-
-# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
-
-if(COMMAND cmake_policy)
- cmake_policy(SET CMP0005 NEW)
- cmake_policy(SET CMP0003 NEW)
-
-endif(COMMAND cmake_policy)
-
-# The project name and the type of project
-PROJECT(wolf)
-
-SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
-SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-SET(CMAKE_INSTALL_PREFIX /usr/local)
-
-IF (NOT CMAKE_BUILD_TYPE)
- SET(CMAKE_BUILD_TYPE "DEBUG")
-ENDIF (NOT CMAKE_BUILD_TYPE)
-message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
-
-#Set Flags
-SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
-SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
-
-#Set compiler according C++11 support
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-else()
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-endif()
-
-
-ADD_SUBDIRECTORY(src)
-
-FIND_PACKAGE(Doxygen)
-
-FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
-IF (IRI_DOC_DIR)
- ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/iri_doc/doxygen.conf)
-ELSE (IRI_DOC_DIR)
- ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
-ENDIF (IRI_DOC_DIR)
-
-ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
-
-IF (UNIX)
- ADD_CUSTOM_COMMAND(
- COMMENT "distribution clean"
- COMMAND make ARGS clean
- COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
-
- TARGET distclean
- )
-ELSE(UNIX)
- ADD_CUSTOM_COMMAND(
- COMMENT "distclean only implemented in unix"
- TARGET distclean
- )
-ENDIF(UNIX)
-
-ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
-
-IF (UNIX)
- ADD_CUSTOM_COMMAND(
- COMMENT "uninstall package"
- COMMAND xargs ARGS rm < install_manifest.txt
-
- TARGET uninstall
- )
-ELSE(UNIX)
- ADD_CUSTOM_COMMAND(
- COMMENT "uninstall only implemented in unix"
- TARGET uninstall
- )
-ENDIF(UNIX)
-
-IF (UNIX)
- SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
- SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
- SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
- SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
- SET(CPACK_GENERATOR "DEB")
- SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu")
- SET(CPACK_SET_DESTDIR "ON") # Necessary because of the absolute install paths
- INCLUDE(CPack)
-ELSE(UNIX)
- ADD_CUSTOM_COMMAND(
- COMMENT "packaging only implemented in unix"
- TARGET uninstall
- )
-ENDIF(UNIX)
-
diff --git a/Findwolf.cmake b/Findwolf.cmake
deleted file mode 100644
index e54db6d71..000000000
--- a/Findwolf.cmake
+++ /dev/null
@@ -1,26 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolf_INCLUDE_DIR
- NAMES wolf.h
- PATHS /usr/local/include/iri-algorithms
-)
-
-FIND_LIBRARY(
- wolf_LIBRARY
- NAMES wolf
- PATHS /usr/lib /usr/local/lib /usr/local/lib/iri-algorithms)
-
-IF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
- SET(wolf_FOUND TRUE)
-ENDIF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
-
-IF (wolf_FOUND)
- IF (NOT wolf_FIND_QUIETLY)
- MESSAGE(STATUS "Found wolf: ${wolf_LIBRARY}")
- ENDIF (NOT wolf_FIND_QUIETLY)
-ELSE (wolf_FOUND)
- IF (wolf_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find wolf")
- ENDIF (wolf_FIND_REQUIRED)
-ENDIF (wolf_FOUND)
-
diff --git a/README.txt b/README.txt
deleted file mode 100644
index 1cf90bdb6..000000000
--- a/README.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-Wolf means Windowed Localization Frames !
-
-Tips:
-1. Use with ECLIPSE: If you use eclipse, cd to wolf/, then > source src/make_eclipse_project.bash, before importing the project.
-
-
diff --git a/Wolf_ideas.txt b/Wolf_ideas.txt
deleted file mode 100644
index 045cea3fb..000000000
--- a/Wolf_ideas.txt
+++ /dev/null
@@ -1,96 +0,0 @@
-This file serves as a guideline for future developments.
-
-As the ideas here get implemented, we advise you add a /////// OK tag indicating the work is done and working.
-
-
-1. Getters ///////////// OK
-============
-source return
- vvv >>>
- shptr ptr obj map
-shptr & .get() * /
-ptr / . * /
-obj / / & /
-map / / / & state: norm; error: const
-
-
-2. Classes
-===============
-
-IDEA: Allow Vehicles, Frames, Captures, Features to live out of the node for enhanced reusability.
-
-In the WOLF nodes, we can have like this:
-
-class FrameBase
-{
-State* state_ptr;
-}
-
-class NodeConstrainer
-{
-virtual computeError()
-}
-
-class NodeFrameBase : public FrameBase, public NodeConstrainer
-{
-computeError() <-- in case we want to overload
-}
-
-
-3. Precomputing constant values ////////////// OK
-===============================
-
-PinHole::precomputeConstants() // call in in constructor /////////// OK
-{
- intrinsic_matrix_ = bla bla bla;
- inverse_intrinsic_matrix_ = bla bla bla
-}
-
-Capture::precomputeGlobalPose() // call from Capture::precomputeConstants(), see below //////////// OK
-{
- global_pose_ = bla bla bla
- inverse_global_pose_ = bla bla bla
-}
-
-Capture::precomputeConstants() ////////////////// OK
-{
- precomputeGlobalPose();
- for (trans_sensor_iter = bla bla)
- {
- trans_sensor_iter->precomputeConstants();
- }
-}
-
-TransSensor::precomputeConstants() = 0; /////////////// OK
-
-TransPinHole::precomputeConstants() /////////////// OK
-{
- capture_own->globalPose()
- capture_other->inverseGlobalPose()
- computePoseRelative()
- sensor_own->inverseIntrinsicMatrix()
- sensor_other->intrinsicMatrix()
- fundamental_ = bla bla
-}
-
-NodeConstrainer::precomputeConstants() ///////////////// OK
-{
- for (iter = bla bla bla)
- {
- *iter->precomputeConstants();
- }
-}
-
-TOP::computeError() //////////////////// OK
-{
- precomputeConstants();
- for (iter = bla bla bla)
- {
- *iter->compputeError();
- }
-}
-
-CorrespondenceBase::precomputeConstants() //////////////// OK not overloaded.
-{
- // Leave it blank or simply do not overload it. Nobody will call it anyway.
-}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index d37f3fd95..000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,106 +0,0 @@
-
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#find dependencies.
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: ceres_wrapper will be built.")
-ENDIF(Ceres_FOUND)
-
-#include directories
-INCLUDE_DIRECTORIES(.)
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-#headers
-SET(HDRS
- capture_base.h
- capture_relative.h
- capture_gps_fix.h
- capture_laser_2D.h
- capture_odom_2D.h
- correspondence_base.h
- correspondence_sparse.h
- correspondence_gps_2D.h
- correspondence_odom_2D_theta.h
- correspondence_odom_2D_complex_angle.h
- feature_base.h
- feature_gps_fix.h
- feature_odom_2D.h
- frame_base.h
- landmark_base.h
- map_base.h
- node_base.h
- node_terminus.h
- node_linked.h
- sensor_base.h
- sensor_laser_2D.h
- sensor_odom_2D.h
- sensor_gps_fix.h
- state_base.h
- state_point.h
- state_complex_angle.h
- time_stamp.h
- trajectory_base.h
- wolf.h)
-
-#sources
-SET(SRCS
- capture_base.cpp
- capture_relative.cpp
- capture_gps_fix.cpp
- capture_laser_2D.cpp
- capture_odom_2D.cpp
- correspondence_base.cpp
- feature_base.cpp
- feature_gps_fix.cpp
- feature_odom_2D.cpp
- frame_base.cpp
- landmark_base.cpp
- map_base.cpp
- node_base.cpp
- node_terminus.cpp
- sensor_base.cpp
- sensor_laser_2D.cpp
- sensor_odom_2D.cpp
- sensor_gps_fix.cpp
- state_base.cpp
- state_complex_angle.cpp
- time_stamp.cpp
- trajectory_base.cpp)
-
-#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(SRCS ${SRCS} ceres_wrapper/complex_angle_parametrization.cpp)
- SET(HDRS ${HDRS} ceres_wrapper/complex_angle_parametrization.h)
- SET(SRCS ${SRCS} ceres_wrapper/ceres_manager.cpp)
- SET(HDRS ${HDRS} ceres_wrapper/ceres_manager.h)
-ENDIF(Ceres_FOUND)
-
-# create the shared library
-ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
-
-#Link the created library with ceres
-IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
-
-
-#install library
-INSTALL(TARGETS ${PROJECT_NAME}
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
-
-#install headers
-INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms)
-
-INSTALL(FILES ../Findwolf.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
-
-#Build examples & tests
-MESSAGE("Building examples and tests.")
-ADD_SUBDIRECTORY(examples)
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
deleted file mode 100644
index 3de588932..000000000
--- a/src/capture_base.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-#include "capture_base.h"
-
-CaptureBase::CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
- NodeLinked(MID, "CAPTURE"),
- time_stamp_(_ts),
- sensor_ptr_(_sensor_ptr)
-{
- //
-}
-
-CaptureBase::CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
- NodeLinked(MID, "CAPTURE"),
- time_stamp_(_ts),
- sensor_ptr_(_sensor_ptr),
- data_(_data)
-{
- //
-}
-
-CaptureBase::CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- NodeLinked(MID, "CAPTURE"),
- time_stamp_(_ts),
- sensor_ptr_(_sensor_ptr),
- data_(_data),
- data_covariance_(_data_covariance)
-{
- //
-}
-
-CaptureBase::~CaptureBase()
-{
- //std::cout << "Destroying capture...\n";
-}
-
-void CaptureBase::linkToFrame(const FrameBaseShPtr& _frm_ptr)
-{
- linkToUpperNode(_frm_ptr.get());
-}
-
-// TODO: Why the linker throws an error when this function is inline...
-void CaptureBase::addFeature(FeatureBaseShPtr & _ft_ptr)
-{
- addDownNode(_ft_ptr);
-}
-
-const FrameBasePtr CaptureBase::getFramePtr() const
-{
- return upperNodePtr();
-}
-
-FeatureBaseList* CaptureBase::getFeatureListPtr()
-{
- return getDownNodeListPtr();
-}
-
-void CaptureBase::getCorrespondenceList(CorrespondenceBaseList & _corr_list)
-{
- std::list<FeatureBaseShPtr>::iterator f_it;
- std::list<CorrespondenceBaseShPtr>* c_list_ptr;
- std::list<CorrespondenceBaseShPtr>::iterator c_it;
-
- for( f_it = down_node_list_.begin(); f_it != down_node_list_.end(); ++f_it)
- {
- c_list_ptr = (f_it->get())->getCorrespondenceListPtr();
- for( c_it = c_list_ptr->begin(); c_it != c_list_ptr->end(); ++c_it)
- {
- _corr_list.push_back(*c_it);
- }
- }
-}
-
-TimeStamp CaptureBase::getTimeStamp() const
-{
- return time_stamp_;
-}
-
-SensorBasePtr CaptureBase::getSensorPtr() const
-{
- return sensor_ptr_;
-}
-
-SensorType CaptureBase::getSensorType() const
-{
- return sensor_ptr_->getSensorType();
-}
-
-void CaptureBase::setTimeStamp(const TimeStamp & _ts)
-{
- time_stamp_ = _ts;
-}
-
-void CaptureBase::setTimeStampToNow()
-{
- time_stamp_.setToNow();
-}
-
-void CaptureBase::setData(unsigned int _size, const WolfScalar *_data)
-{
- data_.resize(_size);
- for (unsigned int ii=0; ii<_size; ii++) data_(ii) = *(&_data[ii]);
-}
-
-void CaptureBase::setData(const Eigen::VectorXs& _data)
-{
- data_=_data;
-}
-
-void CaptureBase::setDataCovariance(const Eigen::MatrixXs& _data_cov)
-{
- data_covariance_ = _data_cov;
-}
-
-void CaptureBase::processCapture()
-{
- std::cout << "CaptureBase::processCapture()... processing capture" << std::endl;
-}
-
-void CaptureBase::printSelf(unsigned int _ntabs, std::ostream & _ost) const
-{
- NodeLinked::printSelf(_ntabs, _ost);
- //printTabs(_ntabs);
- //_ost << "\tSensor pose : ( " << sensor_ptr_->pose().x().transpose() << " )" << std::endl;
- //printNTabs(_ntabs);
- //_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
-}
-
-
-
diff --git a/src/capture_base.h b/src/capture_base.h
deleted file mode 100644
index 62c7760a9..000000000
--- a/src/capture_base.h
+++ /dev/null
@@ -1,106 +0,0 @@
-
-#ifndef CAPTURE_BASE_H_
-#define CAPTURE_BASE_H_
-
-// Forward declarations for node templates
-class FrameBase;
-class FeatureBase;
-
-//std includes
-//
-
-//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_linked.h"
-#include "frame_base.h"
-#include "feature_base.h"
-#include "sensor_base.h"
-
-//class CaptureBase
-class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
-{
- protected:
- TimeStamp time_stamp_; ///< Time stamp
- SensorBasePtr sensor_ptr_; ///< Pointer to sensor
- Eigen::VectorXs data_; ///< raw data
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture
- Eigen::Vector3s sensor_pose_global_; ///< Sensor pose in world frame: composition of the frame pose and the sensor pose. TODO: use state units
- Eigen::Vector3s inverse_sensor_pose_; ///< World pose in the sensor frame: inverse of the global_pose_. TODO: use state units
-
- public:
- CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);//TODO: to be removed ??
-
- CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
-
- CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
-
- virtual ~CaptureBase();
-
- /** \brief Set link to Frame
- *
- * Set link to Frame
- *
- **/
- void linkToFrame(const FrameBaseShPtr& _frm_ptr);
-
- /** \brief Adds a Feature to the down node list
- *
- * Adds a Feature to the down node list
- *
- **/
- void addFeature(FeatureBaseShPtr & _ft_ptr);
-
- /** \brief Gets up_node_ptr_
- *
- * Gets up_node_ptr_, which is a pointer to the Frame owning of this Capture
- *
- **/
- const FrameBasePtr getFramePtr() const;
-
- /** \brief Gets a pointer to feature list
- *
- * Gets a pointer to feature list
- *
- **/
- FeatureBaseList* getFeatureListPtr();
-
- /** \brief Fills the provided list with all correspondences related to this capture
- *
- * Fills the provided list with all correspondences related to this capture
- *
- *
- **/
- //const CorrespondenceBaseList getCorrespondenceList() const;
- void getCorrespondenceList(CorrespondenceBaseList & _corr_list);//TODO: Should be const
-
- TimeStamp getTimeStamp() const;
-
- SensorBasePtr getSensorPtr() const;
-
- SensorType getSensorType() const;
-
- void setTimeStamp(const TimeStamp & _ts);
-
- void setTimeStampToNow();
-
- void setData(unsigned int _size, const WolfScalar *_data);
-
- void setData(const Eigen::VectorXs& _data);
-
- void setDataCovariance(const Eigen::MatrixXs& _data_cov);
-
- virtual void processCapture();// = 0;
-
- virtual Eigen::VectorXs computePrior() const = 0;
-
- /** \brief Generic interface to find correspondences
- *
- * Generic interface to find correspondences between its features and a map (static/slam) or a previous feature
- *
- **/
- virtual void findCorrespondences() = 0;
-
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
-};
-#endif
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
deleted file mode 100644
index 4ce7a1a02..000000000
--- a/src/capture_gps_fix.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-#include "capture_gps_fix.h"
-
-CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
- CaptureBase(_ts, _sensor_ptr)
-{
- //
-}
-
-CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
- CaptureBase(_ts, _sensor_ptr, _data)
-{
- //
-}
-
-CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase(_ts, _sensor_ptr, _data, _data_covariance)
-{
- //
-}
-
-CaptureGPSFix::~CaptureGPSFix()
-{
- //std::cout << "Destroying GPS fix capture...\n";
-}
-
-void CaptureGPSFix::processCapture()
-{
- FeatureBaseShPtr new_feature = FeatureBaseShPtr(new FeatureGPSFix(CaptureBasePtr(this),data_,data_covariance_));
- addFeature(new_feature);
-}
-
-Eigen::VectorXs CaptureGPSFix::computePrior() const
-{
- return data_;
-}
-
-void CaptureGPSFix::findCorrespondences()
-{
- CorrespondenceBaseShPtr gps_correspondence(new CorrespondenceGPS2D(getFeatureListPtr()->front().get(), getFramePtr()->getPPtr()));
- getFeatureListPtr()->front()->addCorrespondence(gps_correspondence);
-}
-
-//void CaptureGPSFix::printSelf(unsigned int _ntabs, std::ostream & _ost) const
-//{
-// NodeLinked::printSelf(_ntabs, _ost);
-// //printTabs(_ntabs);
-// //_ost << "\tSensor pose : ( " << sensor_ptr_->pose().x().transpose() << " )" << std::endl;
-// //printNTabs(_ntabs);
-// //_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
-//}
-
-
-
diff --git a/src/capture_gps_fix.h b/src/capture_gps_fix.h
deleted file mode 100644
index 3b4b5de5e..000000000
--- a/src/capture_gps_fix.h
+++ /dev/null
@@ -1,32 +0,0 @@
-
-#ifndef CAPTURE_GPS_FIX_H_
-#define CAPTURE_GPS_FIX_H_
-
-//std includes
-//
-
-//Wolf includes
-#include "capture_base.h"
-#include "feature_gps_fix.h"
-
-//class CaptureGPSFix
-class CaptureGPSFix : public CaptureBase
-{
- public:
- CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);
-
- CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
-
- CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
-
- virtual ~CaptureGPSFix();
-
- virtual void processCapture();
-
- virtual Eigen::VectorXs computePrior() const;
-
- virtual void findCorrespondences();
-
- //virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
-};
-#endif
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
deleted file mode 100644
index 1e8691e40..000000000
--- a/src/capture_laser_2D.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "capture_laser_2D.h"
-
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, const SensorLaser2DPtr & _sensor_ptr, const Eigen::VectorXs& _ranges):
- CaptureBase(_ts, _sensor_ptr, _ranges)
-{
- //
-}
-
-CaptureLaser2D::~CaptureLaser2D()
-{
- //
-}
-
-void CaptureLaser2D::processCapture()
-{
- extractCorners();
-}
-
-void CaptureLaser2D::extractCorners()
-{
-
-}
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
deleted file mode 100644
index 342b67212..000000000
--- a/src/capture_laser_2D.h
+++ /dev/null
@@ -1,43 +0,0 @@
-
-#ifndef CAPTURE_LASER_2D_H_
-#define CAPTURE_LASER_2D_H_
-
-//wolf includes
-#include "capture_base.h"
-#include "sensor_laser_2D.h"
-
-//wolf forward declarations
-//#include "feature_corner_2D.h"
-
-class CaptureLaser2D : public CaptureBase
-{
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- **/
- CaptureLaser2D(const TimeStamp & _ts, const SensorLaser2DPtr & _sensor_ptr, const Eigen::VectorXs& _ranges);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~CaptureLaser2D();
-
- /** \brief Calls the necessary pipeline from raw scan to features.
- *
- * Calls the necessary pipeline from raw scan to features.
- *
- **/
- virtual void processCapture();
-
- /** \brief Extract corners and push-back to Feature down list
- *
- * Extract corners and push-back to Feature down list .
- *
- **/
- virtual void extractCorners();
-};
-#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
deleted file mode 100644
index 9a2450bce..000000000
--- a/src/capture_odom_2D.cpp
+++ /dev/null
@@ -1,97 +0,0 @@
-#include "capture_odom_2D.h"
-
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
- CaptureRelative(_ts, _sensor_ptr)
-{
- //
-}
-
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
- CaptureRelative(_ts, _sensor_ptr, _data)
-{
- //
-}
-
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureRelative(_ts, _sensor_ptr, _data, _data_covariance)
-{
- //
-}
-
-CaptureOdom2D::~CaptureOdom2D()
-{
- //std::cout << "Destroying GPS fix capture...\n";
-}
-
-inline void CaptureOdom2D::processCapture()
-{
- FeatureBaseShPtr new_feature(new FeatureOdom2D(CaptureBasePtr(this),data_,data_covariance_));
- addFeature(new_feature);
-}
-
-Eigen::VectorXs CaptureOdom2D::computePrior() const
-{
- assert(up_node_ptr_ != nullptr && "This Capture is not linked to any frame");
-
- FrameBasePtr _previous_frame = getFramePtr()->getPreviousFrame();
-
- if (_previous_frame->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
- {
- Eigen::VectorXs pose_predicted(4);
- Eigen::Map<Eigen::VectorXs> previous_pose(_previous_frame->getPPtr()->getPtr(), 4);
-
- double new_pose_predicted_2 = previous_pose(2) * cos(data_(1)) - previous_pose(3) * sin(data_(1));
- double new_pose_predicted_3 = previous_pose(2) * sin(data_(1)) + previous_pose(3) * cos(data_(1));
- pose_predicted(0) = previous_pose(0) + data_(0) * new_pose_predicted_2;
- pose_predicted(1) = previous_pose(1) + data_(0) * new_pose_predicted_3;
- pose_predicted(2) = new_pose_predicted_2;
- pose_predicted(3) = new_pose_predicted_3;
-
- return pose_predicted;
- }
- else
- {
- Eigen::VectorXs pose_predicted(3);
- Eigen::Map<Eigen::VectorXs> previous_pose(_previous_frame->getPPtr()->getPtr(), 3);
-
- pose_predicted(0) = previous_pose(0) + data_(0) * cos(previous_pose(2) + (data_(1)));
- pose_predicted(1) = previous_pose(1) + data_(0) * sin(previous_pose(2) + (data_(1)));
- pose_predicted(2) = previous_pose(2) + data_(1);
-
- return pose_predicted;
- }
-}
-
-void CaptureOdom2D::findCorrespondences()
-{
- if (getFramePtr()->getOPtr()->getStateType() == ST_THETA)
- {
- CorrespondenceBaseShPtr odom_correspondence(new CorrespondenceOdom2DTheta(getFeatureListPtr()->front().get(),
- getFramePtr()->getPreviousFrame()->getPPtr(),
- getFramePtr()->getPreviousFrame()->getOPtr(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr()));
- getFeatureListPtr()->front()->addCorrespondence(odom_correspondence);
- }
- else
- {
- CorrespondenceBaseShPtr odom_correspondence(new CorrespondenceOdom2DComplexAngle(getFeatureListPtr()->front().get(),
- getFramePtr()->getPreviousFrame()->getPPtr(),
- getFramePtr()->getPreviousFrame()->getOPtr(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr()));
- getFeatureListPtr()->front()->addCorrespondence(odom_correspondence);
- }
-}
-
-//void CaptureOdom2D::printSelf(unsigned int _ntabs, std::ostream & _ost) const
-//{
-// NodeLinked::printSelf(_ntabs, _ost);
-// //printTabs(_ntabs);
-// //_ost << "\tSensor pose : ( " << sensor_ptr_->pose().x().transpose() << " )" << std::endl;
-// //printNTabs(_ntabs);
-// //_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
-//}
-
-
-
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
deleted file mode 100644
index b3d3c4b9d..000000000
--- a/src/capture_odom_2D.h
+++ /dev/null
@@ -1,32 +0,0 @@
-
-#ifndef CAPTURE_ODOM_2D_H_
-#define CAPTURE_ODOM_2D_H_
-
-//std includes
-//
-
-//Wolf includes
-#include "capture_relative.h"
-#include "feature_odom_2D.h"
-
-//class CaptureGPSFix
-class CaptureOdom2D : public CaptureRelative
-{
- public:
- CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);
-
- CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
-
- CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
-
- virtual ~CaptureOdom2D();
-
- virtual void processCapture();
-
- virtual Eigen::VectorXs computePrior() const;
-
- virtual void findCorrespondences();
-
- //virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
-};
-#endif
diff --git a/src/capture_relative.cpp b/src/capture_relative.cpp
deleted file mode 100644
index ea41a4103..000000000
--- a/src/capture_relative.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-#include "capture_relative.h"
-
-CaptureRelative::CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
- CaptureBase(_ts, _sensor_ptr)
-{
- //
-}
-
-CaptureRelative::CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
- CaptureBase(_ts, _sensor_ptr, _data)
-{
- //
-}
-
-CaptureRelative::CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase(_ts, _sensor_ptr, _data, _data_covariance)
-{
- //
-}
-CaptureRelative::~CaptureRelative()
-{
- //
-}
diff --git a/src/capture_relative.h b/src/capture_relative.h
deleted file mode 100644
index 7f6fed652..000000000
--- a/src/capture_relative.h
+++ /dev/null
@@ -1,24 +0,0 @@
-
-#ifndef CAPTURE_RELATIVE_H_
-#define CAPTURE_RELATIVE_H_
-
-//std includes
-//
-
-//Wolf includes
-#include "wolf.h"
-#include "capture_base.h"
-
-//class CaptureBase
-class CaptureRelative : public CaptureBase
-{
- public:
- CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);
-
- CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
-
- CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
-
- virtual ~CaptureRelative();
-};
-#endif
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
deleted file mode 100644
index 4ab15a89e..000000000
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ /dev/null
@@ -1,177 +0,0 @@
-#include "ceres_manager.h"
-
-CeresManager::CeresManager(ceres::Problem* _ceres_problem) :
- ceres_problem_(_ceres_problem)
-{
-}
-
-CeresManager::~CeresManager()
-{
-// std::vector<double*> state_units;
-// ceres_problem_->GetParameterBlocks(&state_units);
-//
-// for (uint i = 0; i< state_units.size(); i++)
-// removeStateUnit(state_units.at(i));
-//
-// std::cout << "all state units removed! \n";
-// std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n";
-// std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << "\n";
-}
-
-ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_options)
-{
- // create summary
- ceres::Solver::Summary ceres_summary_;
-
- // run Ceres Solver
- ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
-
- //display results
- return ceres_summary_;
-}
-
-void CeresManager::addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences)
-{
- //std::cout << _new_correspondences.size() << " new correspondences\n";
- while (!_new_correspondences.empty())
- {
- addCorrespondence(_new_correspondences.front());
- _new_correspondences.pop_front();
- }
-}
-
-void CeresManager::removeCorrespondences()
-{
- for (uint i = 0; i<correspondence_list_.size(); i++)
- {
- ceres_problem_->RemoveResidualBlock(correspondence_list_.at(i).first);
- }
- correspondence_list_.clear();
-}
-
-void CeresManager::addCorrespondence(const CorrespondenceBasePtr& _corr_ptr)
-{
- ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
- correspondence_list_.push_back(std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>(blockIdx,_corr_ptr));
-}
-
-void CeresManager::addStateUnits(std::list<StateBasePtr>& _new_state_units)
-{
- while (!_new_state_units.empty())
- {
- addStateUnit(_new_state_units.front());
- _new_state_units.pop_front();
- }
-}
-
-void CeresManager::removeStateUnit(WolfScalar* _st_ptr)
-{
- ceres_problem_->RemoveParameterBlock(_st_ptr);
-}
-
-void CeresManager::addStateUnit(const StateBasePtr& _st_ptr)
-{
- //std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
-// case PARAM_QUATERNION:
-// {
-// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
-// ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
-// ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
-// break;
-// }
- case ST_POINT_1D:
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_2D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_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->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
-}
-
-ceres::CostFunction* CeresManager::createCostFunction(const CorrespondenceBasePtr& _corrPtr)
-{
- switch (_corrPtr->getCorrespondenceType())
- {
- case CORR_GPS_FIX_2D:
- {
- CorrespondenceGPS2D* specific_ptr = (CorrespondenceGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<CorrespondenceGPS2D,
- specific_ptr->measurementSize,
- 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 CORR_ODOM_2D_COMPLEX_ANGLE:
- {
- CorrespondenceOdom2DComplexAngle* specific_ptr = (CorrespondenceOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DComplexAngle,
- specific_ptr->measurementSize,
- 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 CORR_ODOM_2D_THETA:
- {
- CorrespondenceOdom2DTheta* specific_ptr = (CorrespondenceOdom2DTheta*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DTheta,
- specific_ptr->measurementSize,
- 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 correspondence type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
-}
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
deleted file mode 100644
index ab74a15de..000000000
--- a/src/ceres_wrapper/ceres_manager.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef CERES_MANAGER_H_
-#define CERES_MANAGER_H_
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//wof includes
-#include "wolf.h"
-#include "state_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "correspondence_sparse.h"
-#include "correspondence_gps_2D.h"
-#include "correspondence_odom_2D_theta.h"
-#include "correspondence_odom_2D_complex_angle.h"
-
-// ceres wrapper includes
-#include "ceres_wrapper/complex_angle_parametrization.h"
-
-/** \brief Ceres manager for WOLF
- *
- */
-
-class CeresManager
-{
- protected:
- std::vector<std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>> correspondence_list_;
- ceres::Problem* ceres_problem_;
-
- public:
- CeresManager(ceres::Problem* _ceres_problem);
-
- ~CeresManager();
-
- ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
-
- void addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences);
-
- void removeCorrespondences();
-
- void addCorrespondence(const CorrespondenceBasePtr& _corr_ptr);
-
- void addStateUnits(std::list<StateBasePtr>& _new_state_units);
-
- void removeStateUnit(WolfScalar* _st_ptr);
-
- void addStateUnit(const StateBasePtr& _st_ptr);
-
- ceres::CostFunction* createCostFunction(const CorrespondenceBasePtr& _corrPtr);
-};
-
-#endif
diff --git a/src/ceres_wrapper/complex_angle_parametrization.cpp b/src/ceres_wrapper/complex_angle_parametrization.cpp
deleted file mode 100644
index 90584657f..000000000
--- a/src/ceres_wrapper/complex_angle_parametrization.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "complex_angle_parametrization.h"
-
-//ComplexAngleParameterization::ComplexAngleParameterization()
-//{
-// //
-//}
-ComplexAngleParameterization::~ComplexAngleParameterization()
-{
- //
-}
-
-bool ComplexAngleParameterization::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
-{
- x_plus_delta_raw[0] = x_raw[0] * cos(delta_raw[0]) - x_raw[1] * sin(delta_raw[0]);
- x_plus_delta_raw[1] = x_raw[1] * cos(delta_raw[0]) + x_raw[0] * sin(delta_raw[0]);
-
- //normalize
- //double norm = sqrt(x_plus_delta_raw[0] * x_plus_delta_raw[0] + x_plus_delta_raw[1] * x_plus_delta_raw[1]);
- //std::cout << "(before normalization) norm = " << norm << std::endl;
- //x_plus_delta_raw[0] /= norm;
- //x_plus_delta_raw[1] /= norm;
-
- return true;
-}
-
-bool ComplexAngleParameterization::ComputeJacobian(const double* x, double* jacobian) const
-{
- jacobian[0] = -x[1];
- jacobian[1] = x[0];
- return true;
-}
-
-int ComplexAngleParameterization::GlobalSize() const
-{
- return 2;
-}
-
-int ComplexAngleParameterization::LocalSize() const
-{
- return 1;
-}
-
-
-
-
diff --git a/src/ceres_wrapper/complex_angle_parametrization.h b/src/ceres_wrapper/complex_angle_parametrization.h
deleted file mode 100644
index bb8030098..000000000
--- a/src/ceres_wrapper/complex_angle_parametrization.h
+++ /dev/null
@@ -1,24 +0,0 @@
-
-#ifndef COMPLEX_ANGLE_PARAMETRIZATION_H_
-#define COMPLEX_ANGLE_PARAMETRIZATION_H_
-
-//Ceres includes
-#include "ceres/ceres.h"
-
-class ComplexAngleParameterization : public ceres::LocalParameterization
-{
- public:
-
- //ComplexAngleParameterization();
-
- virtual ~ComplexAngleParameterization();
-
- virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const;
-
- virtual bool ComputeJacobian(const double* x, double* jacobian) const;
-
- virtual int GlobalSize() const;
-
- virtual int LocalSize() const;
-};
-#endif
diff --git a/src/correspondence_base.cpp b/src/correspondence_base.cpp
deleted file mode 100644
index 87345a436..000000000
--- a/src/correspondence_base.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "correspondence_base.h"
-
-CorrespondenceBase::CorrespondenceBase(const FeatureBasePtr& _ftr_ptr, CorrespondenceType _tp) :
- NodeLinked(BOTTOM, "CORRESPONDENCE", _ftr_ptr),
- type_(_tp),
- measurement_ptr_(_ftr_ptr->getMeasurementPtr()),
- measurement_covariance_ptr_(_ftr_ptr->getMeasurementCovariancePtr())
-{
- //
-}
-
-CorrespondenceBase::~CorrespondenceBase()
-{
- //
-}
-
-CorrespondenceType CorrespondenceBase::getCorrespondenceType() const
-{
- return type_;
-}
-
-const Eigen::VectorXs * CorrespondenceBase::getMeasurementPtr()
-{
- return upperNodePtr()->getMeasurementPtr();
-}
-
-FeatureBasePtr CorrespondenceBase::getFeaturePtr() const
-{
- return upperNodePtr();
-}
-
-CaptureBasePtr CorrespondenceBase::getCapturePtr() const
-{
- return upperNodePtr()->upperNodePtr();
-}
diff --git a/src/correspondence_base.h b/src/correspondence_base.h
deleted file mode 100644
index b6eea58c7..000000000
--- a/src/correspondence_base.h
+++ /dev/null
@@ -1,78 +0,0 @@
-
-#ifndef CORRESPONDENCE_BASE_H_
-#define CORRESPONDENCE_BASE_H_
-
-// Forward declarations for node templates
-class FeatureBase;
-class NodeTerminus;
-
-//std includes
-//
-
-//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_linked.h"
-#include "feature_base.h"
-#include "node_terminus.h"
-
-//class CorrespondenceBase
-class CorrespondenceBase : public NodeLinked<FeatureBase,NodeTerminus>
-{
- protected:
- CorrespondenceType type_; //type of correspondence (types defined at wolf.h)
- Eigen::VectorXs * measurement_ptr_; // TBD: pointer, map or copy of the feature measurement?
- Eigen::MatrixXs * measurement_covariance_ptr_; // TBD: pointer, map or copy of the feature measurement covariance?
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- **/
- CorrespondenceBase(const FeatureBasePtr& _ftr_ptr, CorrespondenceType _tp);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~CorrespondenceBase();
-
- /** \brief Returns the correspondence type
- *
- * Returns the correspondence type
- *
- **/
- CorrespondenceType getCorrespondenceType() const;
-
- /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the correspondence
- *
- * Returns a vector of scalar pointers to the first element of all state blocks involved in the correspondence.
- *
- **/
- virtual const std::vector<WolfScalar*> getStateBlockPtrVector() = 0;
-
- /** \brief Returns a pointer to the feature measurement
- *
- * Returns a pointer to the feature measurement
- *
- **/
- const Eigen::VectorXs * getMeasurementPtr();
-
- /** \brief Returns a pointer to its capture
- *
- * Returns a pointer to its capture
- *
- **/
- FeatureBasePtr getFeaturePtr() const;
-
- /** \brief Returns a pointer to its capture
- *
- * Returns a pointer to its capture
- *
- **/
- CaptureBasePtr getCapturePtr() const;
-
-};
-#endif
diff --git a/src/correspondence_gps_2D.h b/src/correspondence_gps_2D.h
deleted file mode 100644
index 4e3a195f7..000000000
--- a/src/correspondence_gps_2D.h
+++ /dev/null
@@ -1,39 +0,0 @@
-
-#ifndef CORRESPONDENCE_GPS_2D_H_
-#define CORRESPONDENCE_GPS_2D_H_
-
-//Wolf includes
-#include "wolf.h"
-#include "correspondence_sparse.h"
-
-class CorrespondenceGPS2D: public CorrespondenceSparse<2,2>
-{
- public:
- static const unsigned int N_BLOCKS = 1;
-
- CorrespondenceGPS2D(const FeatureBasePtr& _ftr_ptr, WolfScalar* _statePtr) :
- CorrespondenceSparse<2,2>(_ftr_ptr,CORR_GPS_FIX_2D, _statePtr)
- {
- //
- }
-
- CorrespondenceGPS2D(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _statePtr):
- CorrespondenceSparse<2,2>(_ftr_ptr,CORR_GPS_FIX_2D, _statePtr->getPtr())
- {
- //
- }
-
- virtual ~CorrespondenceGPS2D()
- {
- //
- }
-
- template <typename T>
- bool operator()(const T* const _x, T* _residuals) const
- {
- _residuals[0] = (T((*measurement_ptr_)(0)) - _x[0]) / T((*measurement_covariance_ptr_)(0,0));
- _residuals[1] = (T((*measurement_ptr_)(1)) - _x[1]) / T((*measurement_covariance_ptr_)(1,1));
- return true;
- }
-};
-#endif
diff --git a/src/correspondence_odom_2D_complex_angle.h b/src/correspondence_odom_2D_complex_angle.h
deleted file mode 100644
index a1bb15dab..000000000
--- a/src/correspondence_odom_2D_complex_angle.h
+++ /dev/null
@@ -1,45 +0,0 @@
-
-#ifndef CORRESPONDENCE_ODOM_2D_COMPLEX_ANGLE_H_
-#define CORRESPONDENCE_ODOM_2D_COMPLEX_ANGLE_H_
-
-//Wolf includes
-#include "wolf.h"
-#include "correspondence_sparse.h"
-
-class CorrespondenceOdom2DComplexAngle: public CorrespondenceSparse<2,2,2,2,2>
-{
- public:
- static const unsigned int N_BLOCKS = 2;
-
- CorrespondenceOdom2DComplexAngle(const FeatureBasePtr& _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- CorrespondenceSparse<2,2,2,2,2>(_ftr_ptr,CORR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
- {
- //
- }
-
- CorrespondenceOdom2DComplexAngle(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
- CorrespondenceSparse<2,2,2,2,2>(_ftr_ptr,CORR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
- {
- //
- }
-
- virtual ~CorrespondenceOdom2DComplexAngle()
- {
- //
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
- T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
-
- // Residuals
- _residuals[0] = (expected_range - T((*measurement_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0));
- _residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1));
-
- return true;
- }
-};
-#endif
diff --git a/src/correspondence_odom_2D_theta.h b/src/correspondence_odom_2D_theta.h
deleted file mode 100644
index 957abb584..000000000
--- a/src/correspondence_odom_2D_theta.h
+++ /dev/null
@@ -1,45 +0,0 @@
-
-#ifndef CORRESPONDENCE_ODOM_2D_THETA_H_
-#define CORRESPONDENCE_ODOM_2D_THETA_H_
-
-//Wolf includes
-#include "wolf.h"
-#include "correspondence_sparse.h"
-
-class CorrespondenceOdom2DTheta: public CorrespondenceSparse<2,2,1,2,1>
-{
- public:
- static const unsigned int N_BLOCKS = 2;
-
- CorrespondenceOdom2DTheta(const FeatureBasePtr& _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_ftr_ptr,CORR_ODOM_2D_THETA, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
- {
- //
- }
-
- CorrespondenceOdom2DTheta(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_ftr_ptr,CORR_ODOM_2D_THETA, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
- {
- //
- }
-
- virtual ~CorrespondenceOdom2DTheta()
- {
- //
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
- T expected_rotation = _o2[0]-_o1[0];
-
- // Residuals
- _residuals[0] = (expected_range - T((*measurement_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0));
- _residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1));
-
- return true;
- }
-};
-#endif
diff --git a/src/correspondence_sparse.h b/src/correspondence_sparse.h
deleted file mode 100644
index a4351fe59..000000000
--- a/src/correspondence_sparse.h
+++ /dev/null
@@ -1,140 +0,0 @@
-
-#ifndef CORRESPONDENCE_SPARSE_H_
-#define CORRESPONDENCE_SPARSE_H_
-
-//Wolf includes
-#include "wolf.h"
-#include "correspondence_base.h"
-
-//TODO:
-// - public static const may be are not necessary, since sizes are already kept in CorrespondenceBase::state_block_sizes_vector_
-// JVN: Yes, they are necessary for the ceres cost function constructor. Maybe, the state_block_sizes_vector_ is not necessary (it can be useful in filtering...)
-// - measurement_ptr can be set from FeatureBase::measurement_, once this correspondence is up-linked to a feature.
-// May be a simple get is enough to access this data.
-// -
-
-//template class CorrespondenceBase
-template <const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE = 0,
- unsigned int BLOCK_2_SIZE = 0,
- unsigned int BLOCK_3_SIZE = 0,
- unsigned int BLOCK_4_SIZE = 0,
- unsigned int BLOCK_5_SIZE = 0,
- unsigned int BLOCK_6_SIZE = 0,
- unsigned int BLOCK_7_SIZE = 0,
- unsigned int BLOCK_8_SIZE = 0,
- unsigned int BLOCK_9_SIZE = 0>
-class CorrespondenceSparse: public CorrespondenceBase
-{
- protected:
- std::vector<WolfScalar*> state_block_ptr_vector_;
- std::vector<unsigned int> state_block_sizes_vector_;
-
- public:
- static const unsigned int measurementSize = MEASUREMENT_SIZE;
- static const unsigned int block0Size = BLOCK_0_SIZE;
- static const unsigned int block1Size = BLOCK_1_SIZE;
- static const unsigned int block2Size = BLOCK_2_SIZE;
- static const unsigned int block3Size = BLOCK_3_SIZE;
- static const unsigned int block4Size = BLOCK_4_SIZE;
- static const unsigned int block5Size = BLOCK_5_SIZE;
- static const unsigned int block6Size = BLOCK_6_SIZE;
- static const unsigned int block7Size = BLOCK_7_SIZE;
- static const unsigned int block8Size = BLOCK_8_SIZE;
- static const unsigned int block9Size = BLOCK_9_SIZE;
-
- /** \brief Contructor with state pointer array
- *
- * Constructor with state pointer array
- * JVN: Potser aquest constructor no l'utilitzarem mai.. no?
- *
- **/
- CorrespondenceSparse(const FeatureBasePtr& _ftr_ptr, CorrespondenceType _tp, WolfScalar** _blockPtrArray) :
- CorrespondenceBase(_ftr_ptr,_tp),
- state_block_ptr_vector_(10),
- state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
- {
- for (uint ii = 0; ii<state_block_sizes_vector_.size(); ii++)
- {
- if (state_block_sizes_vector_.at(ii) != 0)
- {
- state_block_ptr_vector_.at(ii) = _blockPtrArray[ii];
- }
- else //at the end the vector is cropped to just relevant components
- {
- state_block_ptr_vector_.resize(ii);
- break;
- }
- }
- }
-
- /** \brief Contructor with state pointer separated
- *
- * Constructor with state pointers separated
- *
- **/
- CorrespondenceSparse(const FeatureBasePtr& _ftr_ptr,
- CorrespondenceType _tp,
- WolfScalar* _state0Ptr,
- WolfScalar* _state1Ptr = nullptr,
- WolfScalar* _state2Ptr = nullptr,
- WolfScalar* _state3Ptr = nullptr,
- WolfScalar* _state4Ptr = nullptr,
- WolfScalar* _state5Ptr = nullptr,
- WolfScalar* _state6Ptr = nullptr,
- WolfScalar* _state7Ptr = nullptr,
- WolfScalar* _state8Ptr = nullptr,
- WolfScalar* _state9Ptr = nullptr ) :
- CorrespondenceBase(_ftr_ptr,_tp),
- state_block_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
- {
- for (uint ii = 0; ii<state_block_sizes_vector_.size(); ii++)
- {
- if ( (state_block_ptr_vector_.at(ii) == nullptr) && (state_block_sizes_vector_.at(ii) == 0) )
- {
- state_block_sizes_vector_.resize(ii);
- state_block_ptr_vector_.resize(ii);
- break;
- }
- else // check error cases
- {
- assert(state_block_ptr_vector_.at(ii) != nullptr);
- assert(state_block_sizes_vector_.at(ii) != 0);
- }
- }
- }
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~CorrespondenceSparse()
- {
- //
- }
-
- /** \brief Returns a vector of pointers to the state blocks
- *
- * Returns a vector of pointers to the state blocks in which this correspondence depends
- *
- **/
- virtual const std::vector<WolfScalar*> getStateBlockPtrVector()
- {
- return state_block_ptr_vector_;
- }
-
- // Ja és a correspondence_base...
- /** \brief Returns a pointer to the mesaurement associated to this correspondence
- *
- * Returns a pointer to the mesaurement associated to this correspondence.
- * Measurement is owned by upper-level feature
- **/
-// const Eigen::VectorXs * getMeasurement() const
-// {
-// return upperNode().getMeasurement();
-// }
-};
-#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
deleted file mode 100644
index 319877efb..000000000
--- a/src/examples/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-# NodeLinked class test
-ADD_EXECUTABLE(test_node_linked test_node_linked.cpp)
-TARGET_LINK_LIBRARIES(test_node_linked ${PROJECT_NAME})
-
-# numeric test
-#ADD_EXECUTABLE(test_ceres_wrapper_numeric test_ceres_wrapper_numeric.cpp)
-#TARGET_LINK_LIBRARIES(test_ceres_wrapper_numeric ${PROJECT_NAME})
-
-# jet test
-ADD_EXECUTABLE(test_ceres_wrapper_jet test_ceres_wrapper_jet.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet ${PROJECT_NAME})
-
-# jet individual blocks test
-ADD_EXECUTABLE(test_ceres_wrapper_jet_ind_block test_ceres_wrapper_jet_ind_block.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_ind_block ${PROJECT_NAME})
-
-# jet test with 2 sensors
-ADD_EXECUTABLE(test_ceres_wrapper_jet_2_sensors test_ceres_wrapper_jet_2_sensors.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_2_sensors ${PROJECT_NAME})
-
-# jet test different bla
-# ADD_EXECUTABLE(test_ceres_wrapper_state_units test_ceres_wrapper_state_units.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_wrapper_state_units ${PROJECT_NAME})
-
-# jet test manager
-ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME})
-
-# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-ADD_EXECUTABLE(test_ceres_odom_batch test_ceres_odom_batch.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_odom_batch ${PROJECT_NAME})
-
-# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-ADD_EXECUTABLE(test_ceres_odom_iterative test_ceres_odom_iterative.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME})
-
-# Building and populating the wolf tree
-# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
-
-# test ceres manager, wolf manager and wolf tree
-ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME})
\ No newline at end of file
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
deleted file mode 100644
index 8cdb8f659..000000000
--- a/src/examples/test_ceres_manager.cpp
+++ /dev/null
@@ -1,818 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "sensor_base.h"
-#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "capture_base.h"
-#include "state_base.h"
-#include "wolf.h"
-
-// ceres wrapper includes
-#include "ceres_wrapper/complex_angle_parametrization.h"
-
-/**
- * This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data.
- *
- **/
-
-using namespace Eigen;
-
-class CaptureXBase;
-class CorrespondenceXBase;
-typedef std::shared_ptr<CorrespondenceXBase> CorrespondenceXShPtr;
-typedef std::shared_ptr<CaptureXBase> CaptureXShPtr;
-
-class CorrespondenceXBase
-{
- protected:
- WolfScalar *measurement_ptr_;
-
- public:
-
- CorrespondenceXBase(WolfScalar * _measurement_ptr) :
- measurement_ptr_(_measurement_ptr)
- {
- }
-
- virtual ~CorrespondenceXBase()
- {
- }
-
- virtual CorrespondenceType getType() const = 0;
- virtual const std::vector<WolfScalar*> getBlockPtrVector() = 0;
-};
-
-template <const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE = 0,
- unsigned int BLOCK_2_SIZE = 0,
- unsigned int BLOCK_3_SIZE = 0,
- unsigned int BLOCK_4_SIZE = 0,
- unsigned int BLOCK_5_SIZE = 0,
- unsigned int BLOCK_6_SIZE = 0,
- unsigned int BLOCK_7_SIZE = 0,
- unsigned int BLOCK_8_SIZE = 0,
- unsigned int BLOCK_9_SIZE = 0>
-class CorrespondenceSparse: public CorrespondenceXBase
-{
- protected:
- std::vector<WolfScalar*> state_block_ptr_vector_;
- std::vector<unsigned int> block_sizes_vector_;
-
- public:
- static const unsigned int measurementSize = MEASUREMENT_SIZE;
- static const unsigned int block0Size = BLOCK_0_SIZE;
- static const unsigned int block1Size = BLOCK_1_SIZE;
- static const unsigned int block2Size = BLOCK_2_SIZE;
- static const unsigned int block3Size = BLOCK_3_SIZE;
- static const unsigned int block4Size = BLOCK_4_SIZE;
- static const unsigned int block5Size = BLOCK_5_SIZE;
- static const unsigned int block6Size = BLOCK_6_SIZE;
- static const unsigned int block7Size = BLOCK_7_SIZE;
- static const unsigned int block8Size = BLOCK_8_SIZE;
- static const unsigned int block9Size = BLOCK_9_SIZE;
-
- CorrespondenceSparse(WolfScalar* _measurementPtr, WolfScalar** _blockPtrArray) :
- CorrespondenceXBase(_measurementPtr),
- block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
- {
- for (uint i = 0; i<block_sizes_vector_.size(); i++)
- {
- if (block_sizes_vector_.at(i) == 0)
- {
- block_sizes_vector_.resize(i);
- break;
- }
- else
- state_block_ptr_vector_.push_back(_blockPtrArray[i]);
- }
- }
-
- CorrespondenceSparse(WolfScalar* _measurementPtr,
- WolfScalar* _state0Ptr,
- WolfScalar* _state1Ptr = nullptr,
- WolfScalar* _state2Ptr = nullptr,
- WolfScalar* _state3Ptr = nullptr,
- WolfScalar* _state4Ptr = nullptr,
- WolfScalar* _state5Ptr = nullptr,
- WolfScalar* _state6Ptr = nullptr,
- WolfScalar* _state7Ptr = nullptr,
- WolfScalar* _state8Ptr = nullptr,
- WolfScalar* _state9Ptr = nullptr ) :
- CorrespondenceXBase(_measurementPtr),
- state_block_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
- {
- for (uint i = 0; i<block_sizes_vector_.size(); i++)
- {
- if (block_sizes_vector_.at(i) == 0)
- {
- block_sizes_vector_.resize(i);
- state_block_ptr_vector_.resize(i);
- break;
- }
- }
-
- //TODO: Check if while size OK, pointers NULL
- }
-
- virtual ~CorrespondenceSparse()
- {
- }
-
- virtual CorrespondenceType getType() const = 0;
-
- virtual const std::vector<WolfScalar *> getBlockPtrVector()
- {
- return state_block_ptr_vector_;
- }
-};
-
-class CorrespondenceGPS2D : public CorrespondenceSparse<2,2>
-{
- public:
- static const unsigned int N_BLOCKS = 1;
- const double stdev_ = 1;
-
- CorrespondenceGPS2D(WolfScalar* _measurementPtr, WolfScalar* _statePtr) :
- CorrespondenceSparse<2,2>(_measurementPtr, _statePtr)
- {
- }
-
- CorrespondenceGPS2D(WolfScalar* _measurementPtr, const StateBaseShPtr& _statePtr) :
- CorrespondenceSparse<2,2>(_measurementPtr, _statePtr->getPtr())
- {
- }
-
- virtual ~CorrespondenceGPS2D()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _x, T* _residuals) const
- {
- _residuals[0] = (T(*this->measurement_ptr_) - _x[0]) / T(stdev_);
- _residuals[1] = (T(*(this->measurement_ptr_+1)) - _x[1]) / T(stdev_);
- return true;
- }
-
- virtual CorrespondenceType getType() const
- {
- return CORR_GPS_FIX_2D;
- }
-};
-
-class Correspondence2DOdometry : public CorrespondenceSparse<2,2,2,2,2>
-{
- public:
- static const unsigned int N_BLOCKS = 4;
- const double stdev_ = 0.01; //model parameters
-
- Correspondence2DOdometry(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) :
- CorrespondenceSparse<2,2,2,2,2>(_measurementPtr, _blockPtrs)
- {
- }
-
- Correspondence2DOdometry(WolfScalar* _measurementPtr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- CorrespondenceSparse<2,2,2,2,2>(_measurementPtr, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
- {
- }
-
- Correspondence2DOdometry(WolfScalar* _measurementPtr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
- CorrespondenceSparse<2,2,2,2,2>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
- {
- }
-
- virtual ~Correspondence2DOdometry()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- T expected_range = (_p1[0]-_p2[0])*(_p1[0]-_p2[0]) + (_p1[1]-_p2[1])*(_p1[1]-_p2[1]); //square of the range
- T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
- //T expected_rotation = atan2(_o2[1], _o2[0]) -atan2(_o1[1],_o1[0]);
-
- // Residuals
- _residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_);
- _residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_);
-
- return true;
- }
-
- virtual CorrespondenceType getType() const
- {
- return CORR_ODOM_2D_COMPLEX_ANGLE;
- }
-};
-
-class Correspondence2DOdometryTheta : public CorrespondenceSparse<2,2,1,2,1>
-{
- public:
- static const unsigned int N_BLOCKS = 4;
- const double stdev_ = 0.01; //model parameters
-
- Correspondence2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _blockPtrs)
- {
- }
-
- Correspondence2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
- {
- }
-
- Correspondence2DOdometryTheta(WolfScalar* _measurementPtr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
- {
- }
-
- virtual ~Correspondence2DOdometryTheta()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
- T expected_rotation = _o2[0]-_o1[0];
-
- // Residuals
- _residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_);
- _residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_);
-
- return true;
- }
-
- virtual CorrespondenceType getType() const
- {
- return CORR_ODOM_2D_THETA;
- }
-};
-
-class CaptureXBase
-{
- public:
- VectorXs capture;
- TimeStamp time_stamp;
- SensorBaseShPtr sensor_ptr_; ///< Pointer to sensor
-
- CaptureXBase(const VectorXs& _capture, const WolfScalar& _time_stamp, const SensorBaseShPtr& _sensor_ptr) :
- capture(_capture),
- time_stamp(_time_stamp),
- sensor_ptr_(_sensor_ptr)
- {
- }
-
- virtual ~CaptureXBase()
- {
- }
-
- const SensorType getSensorType() const
- {
- return sensor_ptr_->getSensorType();
- }
-
- WolfScalar* getPtr()
- {
- return capture.data();
- }
-};
-
-class WolfManager
-{
- protected:
- VectorXs state_;
- unsigned int first_empty_state_;
- bool use_complex_angles_;
- std::vector<FrameBaseShPtr> frames_;
- std::vector<CorrespondenceXShPtr> correspondences_;
- std::vector<VectorXs> odom_captures_;
- std::vector<VectorXs> gps_captures_;
- std::queue<CaptureXShPtr> new_captures_;
- std::vector<CaptureXShPtr> captures_;
-
- public:
- WolfManager(const unsigned int& _state_length=1000, const bool _complex_angle=false) :
- state_(_state_length),
- first_empty_state_(0),
- use_complex_angles_(_complex_angle),
- frames_(0),
- correspondences_(0)
- {
- VectorXs init_frame(use_complex_angles_ ? 4 : 3);
- if (use_complex_angles_)
- init_frame << 0, 0, 1, 0;
- else
- init_frame << 0, 0, 0;
- createFrame(init_frame, 0);
- }
-
- virtual ~WolfManager()
- {
-// std::cout << "Destroying WolfManager...\n";
-// std::cout << "Clearing correspondences_...\n";
-// correspondences_.clear();
-// std::cout << "Clearing frames...\n";
-// frames_.clear();
-// std::cout << "Clearing odom_captures_...\n";
-// odom_captures_.clear();
-// std::cout << "Clearing gps_captures_...\n";
-// gps_captures_.clear();
-// captures_.clear();
-//
-// std::cout << "all cleared...\n";
- }
-
- unsigned int getCorrespondencesSize()
- {
- return correspondences_.size();
- }
-
- void createFrame(const VectorXs& _frame_state, const TimeStamp& _time_stamp)
- {
- // Store in state_
- state_.segment(first_empty_state_, use_complex_angles_ ? 4 : 3) << _frame_state;
-
- // Create frame
- if (use_complex_angles_)
- frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2)))));
-
- else
- frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2)))));
-
- // Update first free state location index
- first_empty_state_ += use_complex_angles_ ? 4 : 3;
- }
-
- void addCapture(const VectorXs& _odom_capture, const WolfScalar& _time_stamp, const SensorBaseShPtr& _sensor_ptr)
- {
- new_captures_.push(CaptureXShPtr(new CaptureXBase(_odom_capture, _time_stamp, _sensor_ptr)));
- }
-
- void computeOdomCapture(const CaptureXShPtr& _odom_capture)
- {
- FrameBaseShPtr prev_frame_ptr = frames_.back();
-
- // STORE CAPTURE
- captures_.push_back(_odom_capture);
- VectorXs capture_data = _odom_capture->capture;
-
- // PRIOR
- VectorXs pose_predicted(use_complex_angles_ ? 4 : 3);
- Map<VectorXs> previous_pose(prev_frame_ptr->getPPtr()->getPtr(), use_complex_angles_ ? 4 : 3);
- if (use_complex_angles_)
- {
- double new_pose_predicted_2 = previous_pose(2) * cos(capture_data(1)) - previous_pose(3) * sin(capture_data(1));
- double new_pose_predicted_3 = previous_pose(2) * sin(capture_data(1)) + previous_pose(3) * cos(capture_data(1));
- pose_predicted(0) = previous_pose(0) + capture_data(0) * new_pose_predicted_2;
- pose_predicted(1) = previous_pose(1) + capture_data(0) * new_pose_predicted_3;
- pose_predicted(2) = new_pose_predicted_2;
- pose_predicted(3) = new_pose_predicted_3;
- }
- else
- {
- pose_predicted(0) = previous_pose(0) + capture_data(0) * cos(previous_pose(2) + (capture_data(1)));
- pose_predicted(1) = previous_pose(1) + capture_data(0) * sin(previous_pose(2) + (capture_data(1)));
- pose_predicted(2) = previous_pose(2) + (capture_data(1));
- }
-
- // NEW FRAME
- createFrame(pose_predicted, _odom_capture->time_stamp);
-
- // CORRESPONDENCE ODOMETRY
- if (use_complex_angles_)
- correspondences_.push_back(CorrespondenceXShPtr(new Correspondence2DOdometry(_odom_capture->getPtr(),
- prev_frame_ptr->getPPtr()->getPtr(),
- prev_frame_ptr->getOPtr()->getPtr(),
- frames_.back()->getPPtr()->getPtr(),
- frames_.back()->getOPtr()->getPtr())));
-
- else
- correspondences_.push_back(CorrespondenceXShPtr(new Correspondence2DOdometryTheta(_odom_capture->getPtr(),
- prev_frame_ptr->getPPtr()->getPtr(),
- prev_frame_ptr->getOPtr()->getPtr(),
- frames_.back()->getPPtr()->getPtr(),
- frames_.back()->getOPtr()->getPtr())));
- }
-
- void computeGPSCapture(const CaptureXShPtr& _gps_capture)
- {
- // STORE CAPTURE
- captures_.push_back(_gps_capture);
-
- // CORRESPONDENCE GPS
- correspondences_.push_back(CorrespondenceXShPtr(new CorrespondenceGPS2D(_gps_capture->getPtr(), frames_.back()->getPPtr()->getPtr())));
- }
-
- void update(std::queue<StateBaseShPtr>& new_state_units, std::queue<CorrespondenceXShPtr>& new_correspondences)
- {
- while (!new_captures_.empty())
- {
- switch (new_captures_.front()->getSensorType())
- {
- case GPS_FIX:
- computeGPSCapture(new_captures_.front());
- new_correspondences.push(correspondences_.back());
- break;
- case ODOM_2D:
- computeOdomCapture(new_captures_.front());
- new_correspondences.push(correspondences_.back());
- new_state_units.push(frames_.back()->getPPtr());
- new_state_units.push(frames_.back()->getOPtr());
- break;
- default:
- std::cout << "unknown capture...\n";
- }
- new_captures_.pop();
- }
- }
-
- VectorXs getState()
- {
- return state_;
- }
-
- CorrespondenceXShPtr getCorrespondencePrt(unsigned int i)
- {
- return correspondences_.at(i);
- }
-
- std::queue<StateBaseShPtr> getStateUnitsPtrs(unsigned int i)
- {
- return std::queue<StateBaseShPtr>({frames_.at(i)->getPPtr(),frames_.at(i)->getOPtr()});
- }
-};
-
-class CeresManager
-{
- protected:
-
- std::vector<std::pair<ceres::ResidualBlockId, CorrespondenceXShPtr>> correspondence_list_;
- ceres::Problem* ceres_problem_;
-
- public:
- CeresManager(ceres::Problem* _ceres_problem) :
- ceres_problem_(_ceres_problem)
- {
- }
-
- ~CeresManager()
- {
-// std::vector<double*> state_units;
-// ceres_problem_->GetParameterBlocks(&state_units);
-//
-// for (uint i = 0; i< state_units.size(); i++)
-// removeStateUnit(state_units.at(i));
-//
-// std::cout << "all state units removed! \n";
- std::cout << "residuals: " << ceres_problem_->NumResiduals() << "\n";
- std::cout << "parameters: " << ceres_problem_->NumParameters() << "\n";
- }
-
- ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options)
- {
- // create summary
- ceres::Solver::Summary ceres_summary_;
-
- // run Ceres Solver
- ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
-
- //display results
- return ceres_summary_;
- }
-
- void addCorrespondences(std::queue<CorrespondenceXShPtr>& _new_correspondences)
- {
- //std::cout << _new_correspondences.size() << " new correspondences\n";
- while (!_new_correspondences.empty())
- {
- addCorrespondence(_new_correspondences.front());
- _new_correspondences.pop();
- }
- }
-
- void removeCorrespondences()
- {
- for (uint i = 0; i<correspondence_list_.size(); i++)
- {
- ceres_problem_->RemoveResidualBlock(correspondence_list_.at(i).first);
- }
- correspondence_list_.clear();
- std::cout << ceres_problem_->NumResidualBlocks() << " residual blocks \n";
- }
-
- void addCorrespondence(const CorrespondenceXShPtr& _corr_ptr)
- {
- ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getBlockPtrVector());
- correspondence_list_.push_back(std::pair<ceres::ResidualBlockId, CorrespondenceXShPtr>(blockIdx,_corr_ptr));
- }
-
- void addStateUnits(std::queue<StateBaseShPtr>& _new_state_units)
- {
- while (!_new_state_units.empty())
- {
- addStateUnit(_new_state_units.front());
- _new_state_units.pop();
- }
- }
-
- void removeStateUnit(WolfScalar* _st_ptr)
- {
- ceres_problem_->RemoveParameterBlock(_st_ptr);
- }
-
- void addStateUnit(const StateBaseShPtr& _st_ptr)
- {
- //std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr.get())->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
-// case PARAM_QUATERNION:
-// {
-// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
-// ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
-// ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
-// break;
-// }
- case ST_POINT_1D:
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr.get())->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_2D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr.get())->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr.get())->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- }
-
- ceres::CostFunction* createCostFunction(const CorrespondenceXShPtr& _corrPtr)
- {
- switch (_corrPtr->getType())
- {
- case CORR_GPS_FIX_2D:
- {
- CorrespondenceGPS2D* specific_ptr = (CorrespondenceGPS2D*)(_corrPtr.get());
- return new ceres::AutoDiffCostFunction<CorrespondenceGPS2D,
- specific_ptr->measurementSize,
- 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 CORR_ODOM_2D_COMPLEX_ANGLE:
- {
- Correspondence2DOdometry* specific_ptr = (Correspondence2DOdometry*)(_corrPtr.get());
- return new ceres::AutoDiffCostFunction<Correspondence2DOdometry,
- specific_ptr->measurementSize,
- 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 CORR_ODOM_2D_THETA:
- {
- Correspondence2DOdometryTheta* specific_ptr = (Correspondence2DOdometryTheta*)(_corrPtr.get());
- return new ceres::AutoDiffCostFunction<Correspondence2DOdometryTheta,
- specific_ptr->measurementSize,
- 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 correspondence type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
- }
-};
-
-int main(int argc, char** argv)
-{
- std::cout << " ========= 2D Robot with odometry and GPS ===========\n\n";
-
- // USER INPUT ============================================================================================
- if (argc!=3 || atoi(argv[1])<1 || atoi(argv[2]) < 0 || atoi(argv[2]) > 1)
- {
- std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl;
- std::cout << " - NI is the number of iterations (NI > 0)" << std::endl;
- std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- clock_t t1, t2;
- t1=clock();
-
- NodeLinked<NodeTerminus,NodeTerminus> node(TOP,"TRAJECTORY");
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
- bool complex_angle = (bool) atoi(argv[2]);
-
- // INITIALIZATION ============================================================================================
- //init random generators
- std::default_random_engine generator(1);
- std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
- std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
-
- //init google log
- google::InitGoogleLogging(argv[0]);
-
- // Ceres initialization
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- ceres::Problem* ceres_problem = new ceres::Problem();
- CeresManager* ceres_manager = new CeresManager(ceres_problem);
- std::ofstream log_file; //output file
- // Wolf manager initialization
- WolfManager* wolf_manager = new WolfManager(n_execution * (complex_angle ? 4 : 3), complex_angle);
-
- //variables
- Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion
- Eigen::VectorXs pose_true(3); //current true pose
- Eigen::VectorXs pose_odom(3); //current true pose
- Eigen::VectorXs ground_truth(n_execution*3); //all true poses
- Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses
- Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
- Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
- std::queue<StateBaseShPtr> new_state_units; // new state units in wolf that must be added to ceres
- std::queue<CorrespondenceXShPtr> new_correspondences; // new correspondences in wolf that must be added to ceres
- SensorBaseShPtr odom_sensor = SensorBaseShPtr(new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1),0));
- SensorBaseShPtr gps_sensor = SensorBaseShPtr(new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0));
-
- // Initial pose
- pose_true << 0,0,0;
- pose_odom << 0,0,0;
- ground_truth.head(3) = pose_true;
- odom_trajectory.head(3) = pose_true;
-
- // SENSOR DATA ============================================================================================
- for (unsigned int ii = 1; ii<n_execution; ii++)
- {
- // inventing odometry ground truth
- if ( ii < (unsigned int)floor(n_execution/2) )
- odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments.
- else
- odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments.
-
- // Computing ground truth trajectory
- pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2) + odom_inc_true(ii*2+1));
- pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2) + odom_inc_true(ii*2+1));
- pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
- ground_truth.segment(ii*3,3) << pose_true;
-
- // corrupting sensor readings (odometry and GPS)
- odom_readings.segment(ii*2,2) << odom_inc_true(ii*2) + distribution_odom(generator),
- odom_inc_true(ii*2+1) + distribution_odom(generator); //true range and theta with noise
- gps_fix_readings.segment(ii*3,3) << pose_true(0) + distribution_gps(generator),
- pose_true(1) + distribution_gps(generator),
- 0. + distribution_gps(generator);
-
- // Computing ground truth trajectory
- pose_odom(0) = pose_odom(0) + odom_readings(ii*2) * cos(pose_odom(2) + odom_readings(ii*2+1));
- pose_odom(1) = pose_odom(1) + odom_readings(ii*2) * sin(pose_odom(2) + odom_readings(ii*2+1));
- pose_odom(2) = pose_odom(2) + odom_readings(ii*2+1);
- odom_trajectory.segment(ii*3,3) << pose_odom;
- }
- std::cout << "sensor data created!\n";
-
- // START TRAJECTORY ============================================================================================
- new_state_units = wolf_manager->getStateUnitsPtrs(0); // First pose to be added in ceres
- for (uint step=1; step < n_execution; step++)
- {
- // adding sensor captures
- wolf_manager->addCapture(odom_readings.segment(step*2,2),step,odom_sensor);
- wolf_manager->addCapture(gps_fix_readings.segment(step*3,3),step,gps_sensor);
-
- // updating problem
- wolf_manager->update(new_state_units, new_correspondences);
-
- // adding new state units and correspondences to ceres
- ceres_manager->addStateUnits(new_state_units);
- ceres_manager->addCorrespondences(new_correspondences);
- }
-
- // SOLVE OPTIMIZATION ============================================================================================
- ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
- t2=clock();
- double seconds = ((double)t2-t1)/CLOCKS_PER_SEC;
-
- // DISPLAY RESULTS ============================================================================================
- std::cout << summary.FullReport() << std::endl;
- std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl;
- std::cout << "total seconds: " << seconds << std::endl;
-
- // change from complex angle to theta
- VectorXs state = wolf_manager->getState();
- VectorXs state_theta(n_execution * 3);
- if (complex_angle)
- for (uint ii = 0; ii<n_execution; ii++)
- state_theta.segment(ii*3,3) << state(ii*4), state(ii*4+1), atan2(state(ii*4+2), state(ii*4+3));
- else
- state_theta = state;
-
- // Print log file
- std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
- log_file.open(filepath, std::ofstream::out); //open log file
-
- if (log_file.is_open())
- {
- log_file << seconds << std::endl;
- for (unsigned int ii = 0; ii<n_execution; ii++)
- log_file << state_theta.segment(ii*3,3).transpose()
- << "\t" << ground_truth.segment(ii*3,3).transpose()
- << "\t" << (state_theta.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose()
- << "\t" << odom_trajectory.segment(ii*3,3).transpose()
- << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl;
- log_file.close(); //close log file
- std::cout << std::endl << "Result file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the file " << filepath << std::endl;
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
- //ceres_manager->removeCorrespondences();
- delete wolf_manager;
- std::cout << "everything deleted!\n";
- delete ceres_manager;
- std::cout << "...deleted!\n";
- delete ceres_problem;
- std::cout << "amost... deleted!\n";
-
- //exit
- return 0;
-}
-
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
deleted file mode 100644
index dc44a0315..000000000
--- a/src/examples/test_ceres_manager_tree.cpp
+++ /dev/null
@@ -1,374 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "sensor_base.h"
-#include "sensor_odom_2D.h"
-#include "sensor_gps_fix.h"
-#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "capture_base.h"
-#include "capture_relative.h"
-#include "capture_odom_2D.h"
-#include "capture_gps_fix.h"
-#include "state_base.h"
-#include "correspondence_sparse.h"
-#include "correspondence_gps_2D.h"
-#include "correspondence_odom_2D_theta.h"
-#include "correspondence_odom_2D_complex_angle.h"
-
-// ceres wrapper include
-#include "ceres_wrapper/ceres_manager.h"
-
-/**
- * This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data.
- *
- **/
-
-using namespace Eigen;
-
-class WolfManager
-{
- protected:
- VectorXs state_;
- unsigned int first_empty_state_;
- bool use_complex_angles_;
- TrajectoryBasePtr trajectory_;
- std::vector<VectorXs> odom_captures_;
- std::vector<VectorXs> gps_captures_;
- std::queue<CaptureBaseShPtr> new_captures_;
- SensorBasePtr sensor_prior_;
-
- public:
- WolfManager(const SensorBasePtr& _sensor_prior, const unsigned int& _state_length=1000, const bool _complex_angle=false) :
- state_(_state_length),
- first_empty_state_(0),
- use_complex_angles_(_complex_angle),
- trajectory_(new TrajectoryBase()),
- sensor_prior_(_sensor_prior)
- {
- VectorXs init_frame(use_complex_angles_ ? 4 : 3);
- if (use_complex_angles_)
- init_frame << 0, 0, 1, 0;
- else
- init_frame << 0, 0, 0;
- createFrame(init_frame, 0);
- }
-
- virtual ~WolfManager()
- {
- delete trajectory_;
-// std::cout << "Destroying WolfManager...\n";
-// std::cout << "Clearing correspondences_...\n";
-// correspondences_.clear();
-// std::cout << "Clearing frames...\n";
-// frames_.clear();
-// std::cout << "Clearing odom_captures_...\n";
-// odom_captures_.clear();
-// std::cout << "Clearing gps_captures_...\n";
-// gps_captures_.clear();
-// captures_.clear();
-//
-// std::cout << "all cleared...\n";
- }
-
- void createFrame(const VectorXs& _frame_state, const TimeStamp& _time_stamp)
- {
- // Store in state_
- state_.segment(first_empty_state_, use_complex_angles_ ? 4 : 3) << _frame_state;
-
- // Create frame and add it to the trajectory
- if (use_complex_angles_)
- {
- FrameBaseShPtr new_frame(new FrameBase(trajectory_,
- _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2))));
- trajectory_->addFrame(new_frame);
- }
- else
- {
- FrameBaseShPtr new_frame(new FrameBase(trajectory_,
- _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2))));
- trajectory_->addFrame(new_frame);
- }
-
- // Update first free state location index
- first_empty_state_ += use_complex_angles_ ? 4 : 3;
- }
-
- void addCapture(const CaptureBaseShPtr& _capture)
- {
- new_captures_.push(_capture);
- }
-
- void update(std::list<StateBasePtr>& new_state_units, std::list<CorrespondenceBasePtr>& new_correspondences)
- {
- // TODO: management due to time stamps
- while (!new_captures_.empty())
- {
- // EXTRACT NEW CAPTURE
- CaptureBaseShPtr new_capture = new_captures_.front();
- new_captures_.pop();
-
- // NEW FRAME (if the specific sensor)
- // TODO: accumulate odometries
- if (new_capture->getSensorPtr() == sensor_prior_)
- {
- createFrame(VectorXs::Zero(use_complex_angles_ ? 4 : 3), new_capture->getTimeStamp());
-
- // ADD CAPTURE TO THE NEW FRAME
- trajectory_->getFrameListPtr()->back()->addCapture(new_capture);
-
- // COMPUTE PRIOR
- trajectory_->getFrameListPtr()->back()->setState(new_capture->computePrior());
-
- // TODO: Change by something like...
- //new_state_units.insert(new_state_units.end(), trajectory_.getFrameList.back()->getStateList().begin(), trajectory_.getFrameList.back()->getStateList().end());
- new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getPPtr().get());
- new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getOPtr().get());
- }
- else
- {
- // ADD CAPTURE TO THE NEW FRAME
- trajectory_->getFrameListPtr()->back()->addCapture(new_capture);
- }
-
- // COMPUTE CAPTURE (features, correspondences)
- new_capture->processCapture();
- new_capture->findCorrespondences();
-
- // ADD CORRESPONDENCES TO THE new_correspondences OUTPUT PARAM
- for (FeatureBaseIter feature_list_iter=new_capture->getFeatureListPtr()->begin(); feature_list_iter!=new_capture->getFeatureListPtr()->end(); feature_list_iter++)
- {
- for (CorrespondenceBaseIter correspondence_list_iter=(*feature_list_iter)->getCorrespondenceListPtr()->begin(); correspondence_list_iter!=(*feature_list_iter)->getCorrespondenceListPtr()->end(); correspondence_list_iter++)
- {
- new_correspondences.push_back((*correspondence_list_iter).get());
- }
- }
- }
- }
-
- VectorXs getState()
- {
- return state_;
- }
-
- std::list<StateBasePtr> getStateList()
- {
- std::list<StateBasePtr> st_list;
-
- for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
- {
- //st_list.insert(st_list.end(), (*frame_list_iter)->getStateList().begin(), (*frame_list_iter)->getStateList().end());
- st_list.push_back((*frame_list_iter)->getPPtr().get());
- st_list.push_back((*frame_list_iter)->getOPtr().get());
- }
-
- return st_list;
- }
-
- std::list<CorrespondenceBaseShPtr> getCorrespondencesList()
- {
- std::list<CorrespondenceBaseShPtr> corr_list;
-
- for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
- {
- for (CaptureBaseIter capture_list_iter=(*frame_list_iter)->getCaptureListPtr()->begin(); capture_list_iter!=(*frame_list_iter)->getCaptureListPtr()->end(); capture_list_iter++)
- {
- for (FeatureBaseIter feature_list_iter=(*capture_list_iter)->getFeatureListPtr()->begin(); feature_list_iter!=(*capture_list_iter)->getFeatureListPtr()->end(); feature_list_iter++)
- {
- corr_list.insert(corr_list.end(),(*feature_list_iter)->getCorrespondenceListPtr()->begin(), (*feature_list_iter)->getCorrespondenceListPtr()->end());
- }
- }
- }
- return corr_list;
- }
-
- void printTree()
- {
- trajectory_->print();
- }
-};
-
-int main(int argc, char** argv)
-{
- std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n";
-
- // USER INPUT ============================================================================================
- if (argc!=3 || atoi(argv[1])<1 || atoi(argv[2]) < 0 || atoi(argv[2]) > 1)
- {
- std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl;
- std::cout << " - NI is the number of iterations (NI > 0)" << std::endl;
- std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- clock_t t1, t2;
- t1=clock();
-
- NodeLinked<NodeTerminus,NodeTerminus> node(TOP,"TRAJECTORY");
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
- bool complex_angle = (bool) atoi(argv[2]);
-
- // INITIALIZATION ============================================================================================
- //init random generators
- WolfScalar odom_std= 0.01;
- WolfScalar gps_std= 1;
- std::default_random_engine generator(1);
- std::normal_distribution<WolfScalar> distribution_odom(0.001, odom_std); //odometry noise
- std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
-
- //init google log
- google::InitGoogleLogging(argv[0]);
-
- // Ceres initialization
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- ceres::Problem* ceres_problem = new ceres::Problem();
- CeresManager* ceres_manager = new CeresManager(ceres_problem);
- std::ofstream log_file; //output file
-
-
- //variables
- Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion
- Eigen::VectorXs pose_true(3); //current true pose
- Eigen::VectorXs pose_odom(3); //current true pose
- Eigen::VectorXs ground_truth(n_execution*3); //all true poses
- Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses
- Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
- Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
- std::list<StateBasePtr> new_state_units; // new state units in wolf that must be added to ceres
- std::list<CorrespondenceBasePtr> new_correspondences; // new correspondences in wolf that must be added to ceres
-
- // Wolf manager initialization
- SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(3,1), odom_std, odom_std);
- SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(3,1), gps_std);
- WolfManager* wolf_manager = new WolfManager(&odom_sensor, n_execution * (complex_angle ? 4 : 3), complex_angle);
-
- // Initial pose
- pose_true << 0,0,0;
- pose_odom << 0,0,0;
- ground_truth.head(3) = pose_true;
- odom_trajectory.head(3) = pose_true;
-
- // SENSOR DATA ============================================================================================
- for (unsigned int ii = 1; ii<n_execution; ii++)
- {
- // inventing odometry ground truth
- if ( ii < (unsigned int)floor(n_execution/2) )
- odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments.
- else
- odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments.
-
- // Computing ground truth trajectory
- pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2) + odom_inc_true(ii*2+1));
- pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2) + odom_inc_true(ii*2+1));
- pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
- ground_truth.segment(ii*3,3) << pose_true;
-
- // corrupting sensor readings (odometry and GPS)
- odom_readings.segment(ii*2,2) << odom_inc_true(ii*2) + distribution_odom(generator),
- odom_inc_true(ii*2+1) + distribution_odom(generator); //true range and theta with noise
- gps_fix_readings.segment(ii*3,3) << pose_true(0) + distribution_gps(generator),
- pose_true(1) + distribution_gps(generator),
- 0. + distribution_gps(generator);
-
- // Computing ground truth trajectory
- pose_odom(0) = pose_odom(0) + odom_readings(ii*2) * cos(pose_odom(2) + odom_readings(ii*2+1));
- pose_odom(1) = pose_odom(1) + odom_readings(ii*2) * sin(pose_odom(2) + odom_readings(ii*2+1));
- pose_odom(2) = pose_odom(2) + odom_readings(ii*2+1);
- odom_trajectory.segment(ii*3,3) << pose_odom;
- }
-
- // START TRAJECTORY ============================================================================================
- new_state_units = wolf_manager->getStateList(); // First pose to be added in ceres
- for (uint step=1; step < n_execution; step++)
- {
- // adding new sensor captures
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureOdom2D(TimeStamp(step*0.01), &odom_sensor, odom_readings.segment(step*2,2), odom_std * MatrixXs::Identity(2,2))));
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureGPSFix(TimeStamp(step*0.01), &gps_sensor, gps_fix_readings.segment(step*3,3), gps_std * MatrixXs::Identity(3,3))));
-
- // updating problem
- wolf_manager->update(new_state_units, new_correspondences);
-
- // adding new state units and correspondences to ceres
- ceres_manager->addStateUnits(new_state_units);
- ceres_manager->addCorrespondences(new_correspondences);
- }
-
- //std::cout << "Resulting tree:\n";
- //wolf_manager->printTree();
-
- // SOLVE OPTIMIZATION ============================================================================================
- ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
- t2=clock();
- double seconds = ((double)t2-t1)/CLOCKS_PER_SEC;
-
- // DISPLAY RESULTS ============================================================================================
- std::cout << summary.FullReport() << std::endl;
- std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl;
- std::cout << "total seconds: " << seconds << std::endl;
-
- // change from complex angle to theta
- VectorXs state = wolf_manager->getState();
- VectorXs state_theta(n_execution * 3);
- if (complex_angle)
- for (uint ii = 0; ii<n_execution; ii++)
- state_theta.segment(ii*3,3) << state(ii*4), state(ii*4+1), atan2(state(ii*4+2), state(ii*4+3));
- else
- state_theta = state;
-
- // Print log file
- std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
- log_file.open(filepath, std::ofstream::out); //open log file
-
- if (log_file.is_open())
- {
- log_file << seconds << std::endl;
- for (unsigned int ii = 0; ii<n_execution; ii++)
- log_file << state_theta.segment(ii*3,3).transpose()
- << "\t" << ground_truth.segment(ii*3,3).transpose()
- << "\t" << (state_theta.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose()
- << "\t" << odom_trajectory.segment(ii*3,3).transpose()
- << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl;
- log_file.close(); //close log file
- std::cout << std::endl << "Result file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the file " << filepath << std::endl;
-
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- delete ceres_manager;
- delete ceres_problem;
- delete wolf_manager;
-
- //exit
- return 0;
-}
diff --git a/src/examples/test_ceres_odom_batch.cpp b/src/examples/test_ceres_odom_batch.cpp
deleted file mode 100644
index 0178bd5c1..000000000
--- a/src/examples/test_ceres_odom_batch.cpp
+++ /dev/null
@@ -1,450 +0,0 @@
-// Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-
-//std includes
-#include <ctime>
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-// #include <memory>
-// #include <typeinfo>
-
-//ceres
-#include "ceres/ceres.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_terminus.h"
-#include "node_linked.h"
-
-//forward declarations
-class TrajectoryBaseX;
-class FrameBaseX;
-class CaptureBaseX;
-class FeatureBaseX;
-class CorrespondenceBaseX;
-
-//class TrajectoryBaseX
-class TrajectoryBaseX : public NodeLinked<NodeTerminus,FrameBaseX>
-{
- protected:
- unsigned int length_; //just something to play
-
- public:
- TrajectoryBaseX(const unsigned int _len) :
- NodeLinked(TOP, "TRAJECTORY"),
- length_(_len)
- {
- //
- };
-
- virtual ~TrajectoryBaseX()
- {
-
- };
-};
-
-//class FrameBaseX
-class FrameBaseX : public NodeLinked<TrajectoryBaseX,CaptureBaseX>
-{
- protected:
- double time_stamp_; //frame ts
-
- public:
- FrameBaseX(double _ts) :
- NodeLinked(MID, "FRAME"),
- time_stamp_(_ts)
- {
- //
- };
-
- virtual ~FrameBaseX()
- {
-
- };
-};
-
-//class CaptureBaseX
-class CaptureBaseX : public NodeLinked<FrameBaseX,FeatureBaseX>
-{
- protected:
- double time_stamp_; //capture ts
-
- public:
- CaptureBaseX(double _ts) :
- NodeLinked(MID, "CAPTURE"),
- time_stamp_(_ts)
- {
- //
- };
-
- virtual ~CaptureBaseX()
- {
-
- };
-};
-
-//class FeatureBaseX
-class FeatureBaseX : public NodeLinked<CaptureBaseX,CorrespondenceBaseX>
-{
- protected:
-
- public:
- FeatureBaseX() :
- NodeLinked(MID, "FEATURE")
- {
- //
- };
-
- virtual ~FeatureBaseX()
- {
- //
- };
-};
-
-//class CorrespondenceBaseX
-class CorrespondenceBaseX : public NodeLinked<FeatureBaseX,NodeTerminus>
-{
- protected:
- unsigned int nblocks_; //number of state blocks in which the correspondence depends on.
- std::vector<unsigned int> block_indexes_; //state vector indexes indicating start of each state block. This vector has nblocks_ size.
- std::vector<unsigned int> block_sizes_; //sizes of each state block. This vector has nblocks_ size.
- ceres::CostFunction* cost_function_ptr_;
-
- public:
- CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bindexes, const std::vector<unsigned int> & _bsizes) :
- NodeLinked(BOTTOM, "CORRESPONDENCE"),
- nblocks_(_nb),
- block_indexes_(_bindexes),
- block_sizes_(_bsizes)
- {
- assert(block_sizes_.size() == nblocks_);
- };
-
- virtual ~CorrespondenceBaseX()
- {
- //
- };
-
- ceres::CostFunction * getCostFunctionPtr()
- {
- return cost_function_ptr_;
- };
-
- virtual void display() const
- {
- unsigned int ii;
- std::cout << "number of blocks: " << nblocks_ << std::endl;
- std::cout << "block indexes: ";
- for (ii=0; ii<block_indexes_.size(); ii++) std::cout << block_indexes_.at(ii) << " ";
- std::cout << std::endl;
- std::cout << "block sizes: ";
- for (ii=0; ii<block_sizes_.size(); ii++) std::cout << block_sizes_.at(ii) << " ";
- std::cout << std::endl;
- };
-};
-
-class Odom2DFunctor
-{
- protected:
- Eigen::Vector2s odom_inc_; //incremental odometry measurements (range, theta). Could be a map to data hold by capture or feature
- const double odom_stdev_ = 0.01; //model parameters
-
- public:
- //constructor
- Odom2DFunctor(const Eigen::Vector2s & _odom):
- odom_inc_(_odom)
- {
- //
- };
-
- //destructor
- virtual ~Odom2DFunctor()
- {
- //
- };
-
- //cost function
- template <typename T>
- bool operator()(const T* const _x0, const T* const _x1, T* _residual) const
- {
- T dr2, dth;
-
- //expected range and theta increments, given the state points
- dr2 = (_x0[0]-_x1[0])*(_x0[0]-_x1[0]) + (_x0[1]-_x1[1])*(_x0[1]-_x1[1]); //square of the range
- dth = _x1[2] - _x0[2]; //
-
- //residuals in range and theta components
- _residual[0] = (dr2 - T(odom_inc_(0)*odom_inc_(0))) / T(odom_stdev_);
- _residual[1] = (dth - T(odom_inc_(1))) / T(odom_stdev_);
-
- //return
- return true;
- };
-};
-
-//Specialized correspondence class for 2D odometry
-class CorrespondenceOdom2D : public CorrespondenceBaseX
-{
- protected:
- Eigen::Map<Eigen::Vector3s> pose_previous_;
- Eigen::Map<Eigen::Vector3s> pose_current_;
- Eigen::Map<const Eigen::Vector2s> odom_inc_;
-
- public:
- CorrespondenceOdom2D(WolfScalar * _st, const Eigen::Vector2s & _odom) :
- CorrespondenceBaseX(2,{0,3},{3,3}),
- pose_previous_(_st + block_indexes_.at(0), block_sizes_.at(0)),
- pose_current_(_st + block_indexes_.at(1), block_sizes_.at(1)),
- odom_inc_(_odom.data())
- {
- cost_function_ptr_ = new ceres::AutoDiffCostFunction<Odom2DFunctor,2,3,3>(new Odom2DFunctor(_odom));
- };
-
- virtual ~CorrespondenceOdom2D()
- {
- //delete cost_function_ptr_;
- };
-
- double * getPosePreviousPtr()
- {
- return pose_previous_.data();
- };
-
- double * getPoseCurrentPtr()
- {
- return pose_current_.data();
- };
-
- virtual void display() const
- {
- std::cout << "---- Odom Correspondence ----" << std::endl;
- CorrespondenceBaseX::display();
- std::cout << "pose_previous_: " << pose_previous_.transpose() << std::endl;
- std::cout << "pose_current_: " << pose_current_.transpose() << std::endl;
- std::cout << "odom_inc_: " << odom_inc_.transpose() << std::endl;
- };
-};
-
-class GPSFixFunctor
-{
- protected:
- Eigen::Vector3s gps_fix_; //GPS fix XYZ. Could be a map to data hold by capture or feature
- const double gps_stdev_ = 1; //model parameters
-
- public:
- //constructor
- GPSFixFunctor(const Eigen::Vector3s & _gps_fix):
- gps_fix_(_gps_fix)
- {
- //
- };
-
- //destructor
- virtual ~GPSFixFunctor()
- {
- //
- };
-
- //cost function
- template <typename T>
- bool operator()(const T* const _x0, T* _residual) const
- {
- T dist_x = T( gps_fix_(0) ) - _x0[0];
- T dist_y = T( gps_fix_(1) ) - _x0[1];
- _residual[0] = dist_x / T(gps_stdev_);
- _residual[1] = dist_y / T(gps_stdev_);
- _residual[2] = T(0.); //T( gps_fix_(2) ) - _x0[2]; //in this case, third component of the state is heading, not Z, so it is ignored
- return true;
- };
-};
-
-//Specialized correspondence class for GPS Fix data
-class CorrespondenceGPSFix : public CorrespondenceBaseX
-{
- protected:
- Eigen::Map<Eigen::Vector3s> location_;
- Eigen::Map<const Eigen::Vector3s> gps_fix_;
- //TODO: add ceres_residual_block_id_ to manage add/remove block
-
- public:
- CorrespondenceGPSFix(WolfScalar * _st, const Eigen::Vector3s & _gps_fix) :
- CorrespondenceBaseX(1,{0},{3}),
- location_(_st + block_indexes_.at(0) , block_sizes_.at(0)),
- gps_fix_(_gps_fix.data())
- {
- cost_function_ptr_ = new ceres::AutoDiffCostFunction<GPSFixFunctor,3,3>(new GPSFixFunctor(_gps_fix));
- };
-
- virtual ~CorrespondenceGPSFix()
- {
- //delete cost_function_ptr_;
- };
-
- double * getLocation()
- {
- return location_.data();
- }
-
- virtual void display() const
- {
- std::cout << "---- GPS Fix Correspondence ----" << std::endl;
- CorrespondenceBaseX::display();
- std::cout << "location_: " << location_.transpose() << std::endl;
- std::cout << "gps_fix_: " << gps_fix_.transpose() << std::endl;
- };
-};
-
-
-int main(int argc, char** argv)
-{
- clock_t t1, t2;
- t1 = clock();
-
- //Welcome message
- std::cout << std::endl << " ========= WOLF-CERES test. Simple Odometry + GPS fix problem (with non-template classes) ===========" << std::endl << std::endl;
-
- //user input
- if (argc!=2)
- {
- std::cout << "Please call me with: [./test_ceres_wrapper_non_template NI NW], where:" << std::endl;
- std::cout << " - NI is the number of iterations" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
- //init google log
- google::InitGoogleLogging(argv[0]);
-
- //variables
- std::list<unsigned int> block_ids; //id of each added block to ceres problem
- Eigen::VectorXs odom_inc_true;//invented motion
- Eigen::Vector3s pose_true; //current true pose
- Eigen::VectorXs ground_truth; //accumulated true poses
- Eigen::Vector3s pose_predicted; // current predicted pose
- Eigen::VectorXs predicted_trajectory; // current predicted pose
- Eigen::VectorXs state; //running window winth solver result
- Eigen::Vector2s odom_reading; //current odometry reading
- Eigen::Vector3s gps_fix_reading; //current GPS fix reading
- Eigen::VectorXs gps_log; //log of all gps readings
- CorrespondenceOdom2D *odom_corresp; //pointer to odometry correspondence
- CorrespondenceGPSFix *gps_fix_corresp; //pointer to GPS fix correspondence
- ceres::Problem problem; //ceres problem
- ceres::Solver::Options options; //ceres solver options
- ceres::Solver::Summary summary; //ceres solver summary
- std::ofstream log_file; //output file
-
- //resize vectors according user input number of iterations
- odom_inc_true.resize(n_execution*2); //2 odometry components per iteration
- ground_truth.resize(n_execution*3);// 3 components per iteration
- gps_log.resize(n_execution*3); //3 components per iteration
- state.resize(n_execution*3); //3 components per window element
- predicted_trajectory.resize(n_execution*3); //3 components per window element
-
- //init true odom and true pose
- for (unsigned int ii = 0; ii<n_execution; ii++)
- {
- if ( ii < (unsigned int)floor(n_execution/2) )
- odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments.
- else
- odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments.
- }
- pose_true << 0,0,0;
- pose_predicted << 0,0,0;
- ground_truth.middleRows(0,3) << pose_true; //init point pushed to ground truth
- state.middleRows(0,3) << 0,0,0; //init state at origin
- predicted_trajectory.middleRows(0,3) << pose_predicted;
-
- //init random generators
- std::default_random_engine generator(1);
- std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
- std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
-
- //test loop
- for (unsigned int ii = 1; ii<n_execution; ii++) //ii over iterations
- {
- //inventing a simple motion
- pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2)+odom_inc_true(ii*2+1));
- pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2)+odom_inc_true(ii*2+1));
- pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
- ground_truth.middleRows(ii*3,3) << pose_true;
- //std::cout << "pose_true(" << ii << ") = " << pose_true.transpose() << std::endl;
-
- //inventing sensor readings for odometry and GPS
- odom_reading << odom_inc_true(ii*2)+distribution_odom(generator), odom_inc_true(ii*2+1)+distribution_odom(generator); //true range and theta with noise
- gps_fix_reading << pose_true(0) + distribution_gps(generator), pose_true(1) + distribution_gps(generator), 0. + distribution_gps(generator);
- gps_log.middleRows(ii*3,3) << gps_fix_reading;//log the reading
-
- //setting initial guess as an odometry prediction, using noisy odometry
- pose_predicted(0) = pose_predicted(0) + odom_reading(0) * cos(pose_predicted(2)+odom_reading(1));
- pose_predicted(1) = pose_predicted(1) + odom_reading(0) * sin(pose_predicted(2)+odom_reading(1));
- pose_predicted(2) = pose_predicted(2) + odom_reading(1);
- state.middleRows(ii*3,3) << pose_predicted;
- predicted_trajectory.middleRows(ii*3,3) << pose_predicted;
- //std::cout << "pose_predicted(" << ii << ") = " << pose_predicted.transpose() << std::endl;
-
- //creating odom correspondence, exceptuating first iteration. Adding it to the problem
- odom_corresp = new CorrespondenceOdom2D(state.data()+(ii-1)*3, odom_reading);
- //odom_corresp->display();
- problem.AddResidualBlock(odom_corresp->getCostFunctionPtr(),nullptr, odom_corresp->getPosePreviousPtr(), odom_corresp->getPoseCurrentPtr());
- delete odom_corresp;
-
- //creating gps correspondence and adding it to the problem
- gps_fix_corresp = new CorrespondenceGPSFix(state.data()+ii*3, gps_fix_reading);
- //gps_fix_corresp->display();
- problem.AddResidualBlock(gps_fix_corresp->getCostFunctionPtr(),nullptr, gps_fix_corresp->getLocation());
- delete gps_fix_corresp;
-
- //set options and solve (sliding window)
-// options.minimizer_progress_to_stdout = true;
-// ceres::Solve(options, &problem, &summary);
- }
-
- //display initial guess
- //std::cout << "INITIAL GUESS IS: " << state.transpose() << std::endl;
-
- //set options and solve (batch mode)
- //options.minimizer_progress_to_stdout = true;
- options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;//
- options.max_line_search_step_contraction = 1e-3;
- ceres::Solve(options, &problem, &summary);
-
- t2=clock();
- double seconds = ((double)t2-t1)/CLOCKS_PER_SEC;
- std::cout << summary.FullReport() << std::endl;
- std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl;
- std::cout << "total seconds: " << seconds << std::endl;
-
- //display/log results, by setting cout flags properly
- std::string homepath = getenv("HOME");
- log_file.open(homepath + "/Desktop/log_file.txt", std::ofstream::out); //open log file
- if (log_file.is_open())
- {
- log_file << seconds << std::endl;
- for (unsigned int ii = 0; ii<n_execution; ii++)
- log_file << state.middleRows(ii*3,3).transpose()
- << " " << ground_truth.middleRows(ii*3,3).transpose()
- << " " << (state.middleRows(ii*3,3)-ground_truth.middleRows(ii*3,3)).transpose()
- << " " << gps_log.middleRows(ii*3,3).transpose()
- << " " << predicted_trajectory.middleRows(ii*3,3).transpose() << std::endl;
- log_file.close(); //close log file
- std::cout << std::endl << " Result to file ~/Desktop/log_data.txt" << std::endl;
- }
- else
- std::cout << std::endl << " Failed to write the file ~/Desktop/log_data.txt" << std::endl;
-
- //free memory (not necessary since ceres::problem holds their ownership)
-// delete odom_corresp;
-// delete gps_fix_corresp;
-
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/src/examples/test_ceres_odom_iterative.cpp b/src/examples/test_ceres_odom_iterative.cpp
deleted file mode 100644
index 91e755082..000000000
--- a/src/examples/test_ceres_odom_iterative.cpp
+++ /dev/null
@@ -1,459 +0,0 @@
-// Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-
-//std includes
-#include <cstdlib>
-#include <stdlib.h> //getenv
-#include <iostream>
-#include <fstream>
-#include <string>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-// #include <memory>
-// #include <typeinfo>
-
-//ceres
-#include "ceres/ceres.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "node_terminus.h"
-#include "node_linked.h"
-
-//forward declarations
-class TrajectoryBaseX;
-class FrameBaseX;
-class CaptureBaseX;
-class FeatureBaseX;
-class CorrespondenceBaseX;
-
-//class TrajectoryBaseX
-class TrajectoryBaseX : public NodeLinked<NodeTerminus,FrameBaseX>
-{
- protected:
- unsigned int length_; //just something to play
-
- public:
- TrajectoryBaseX(const unsigned int _len) :
- NodeLinked(TOP, "TRAJECTORY"),
- length_(_len)
- {
- //
- };
-
- virtual ~TrajectoryBaseX()
- {
-
- };
-};
-
-//class FrameBaseX
-class FrameBaseX : public NodeLinked<TrajectoryBaseX,CaptureBaseX>
-{
- protected:
- double time_stamp_; //frame ts
-
- public:
- FrameBaseX(double _ts) :
- NodeLinked(MID, "FRAME"),
- time_stamp_(_ts)
- {
- //
- };
-
- virtual ~FrameBaseX()
- {
-
- };
-};
-
-//class CaptureBaseX
-class CaptureBaseX : public NodeLinked<FrameBaseX,FeatureBaseX>
-{
- protected:
- double time_stamp_; //capture ts
-
- public:
- CaptureBaseX(double _ts) :
- NodeLinked(MID, "CAPTURE"),
- time_stamp_(_ts)
- {
- //
- };
-
- virtual ~CaptureBaseX()
- {
-
- };
-};
-
-//class FeatureBaseX
-class FeatureBaseX : public NodeLinked<CaptureBaseX,CorrespondenceBaseX>
-{
- protected:
-
- public:
- FeatureBaseX() :
- NodeLinked(MID, "FEATURE")
- {
- //
- };
-
- virtual ~FeatureBaseX()
- {
- //
- };
-};
-
-//class CorrespondenceBaseX
-class CorrespondenceBaseX : public NodeLinked<FeatureBaseX,NodeTerminus>
-{
- protected:
- unsigned int nblocks_; //number of state blocks in which the correspondence depends on.
- //std::vector<unsigned int> block_indexes_; //state vector indexes indicating start of each state block. This vector has nblocks_ size.
- std::vector<unsigned int> block_sizes_; //sizes of each state block. This vector has nblocks_ size.
- ceres::CostFunction* cost_function_ptr_;
- ceres::ResidualBlockId ceres_residual_block_id_;
-
- public:
- //CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bindexes, const std::vector<unsigned int> & _bsizes) :
- CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bsizes) :
- NodeLinked(BOTTOM, "CORRESPONDENCE"),
- nblocks_(_nb),
- //block_indexes_(_bindexes),
- block_sizes_(_bsizes)
- {
- assert(block_sizes_.size() == nblocks_);
- };
-
- virtual ~CorrespondenceBaseX()
- {
- //
- };
-
-// ceres::CostFunction * getCostFunctionPtr()
-// {
-// return cost_function_ptr_;
-// };
-
- virtual void addToProblem(ceres::Problem & _problem) = 0;
-
- virtual void removeFromProblem(ceres::Problem & _problem)
- {
- _problem.RemoveResidualBlock(ceres_residual_block_id_);
- }
-
- virtual void display() const
- {
- unsigned int ii;
- std::cout << "number of state blocks: " << nblocks_ << std::endl;
- //std::cout << "state block indexes: ";
- //for (ii=0; ii<block_indexes_.size(); ii++) std::cout << block_indexes_.at(ii) << " ";
- std::cout << std::endl;
- std::cout << "state block sizes: ";
- for (ii=0; ii<block_sizes_.size(); ii++) std::cout << block_sizes_.at(ii) << " ";
- std::cout << std::endl;
- std::cout << "ceres residual block id: " << ceres_residual_block_id_ << std::endl;
- };
-};
-
-class Odom2DFunctor
-{
- protected:
- Eigen::Vector2s odom_inc_; //incremental odometry measurements (range, theta). Could be a map to data hold by capture or feature
- const double odom_stdev_ = 0.01; //model parameters
-
- public:
- //constructor
- Odom2DFunctor(const Eigen::Vector2s & _odom):
- odom_inc_(_odom)
- {
- //
- };
-
- //destructor
- virtual ~Odom2DFunctor()
- {
- //
- };
-
- //cost function
- template <typename T>
- bool operator()(const T* const _x0, const T* const _x1, T* _residual) const
- {
- T dr2, dth;
-
- //expected range and theta increments, given the state points
- dr2 = (_x0[0]-_x1[0])*(_x0[0]-_x1[0]) + (_x0[1]-_x1[1])*(_x0[1]-_x1[1]); //square of the range
- dth = _x1[2] - _x0[2]; //
-
- //residuals in range and theta components
- _residual[0] = (T(dr2) - T(odom_inc_(0)*odom_inc_(0))) / T(odom_stdev_);
- _residual[1] = (T(dth) - T(odom_inc_(1))) / T(odom_stdev_);
-
- //return
- return true;
- };
-};
-
-//Specialized correspondence class for 2D odometry
-class CorrespondenceOdom2D : public CorrespondenceBaseX
-{
- protected:
- Eigen::Map<Eigen::Vector3s> pose_previous_;
- Eigen::Map<Eigen::Vector3s> pose_current_;
- Eigen::Map<const Eigen::Vector2s> odom_inc_;
-
- public:
- CorrespondenceOdom2D(WolfScalar * _st_prev, WolfScalar * _st_curr, const Eigen::Vector2s & _odom) :
- CorrespondenceBaseX(2,{3,3}),
- pose_previous_(_st_prev),//, block_sizes_.at(0)), //size 3 is already defined at declaration
- pose_current_(_st_curr),//, block_sizes_.at(1)), //size 3 is already defined at declaration
- odom_inc_(_odom.data())
- {
- cost_function_ptr_ = new ceres::AutoDiffCostFunction<Odom2DFunctor,2,3,3>(new Odom2DFunctor(_odom));
- };
-
- virtual ~CorrespondenceOdom2D()
- {
- //delete cost_function_ptr_;
- };
-
-// double * getPosePreviousPtr()
-// {
-// return pose_previous_.data();
-// };
-//
-// double * getPoseCurrentPtr()
-// {
-// return pose_current_.data();
-// };
-
- virtual void addToProblem(ceres::Problem & _problem)
- {
- ceres_residual_block_id_ = _problem.AddResidualBlock(cost_function_ptr_,nullptr, pose_previous_.data(), pose_current_.data());
- }
-
- virtual void display() const
- {
- std::cout << "---- Odom Correspondence ----" << std::endl;
- CorrespondenceBaseX::display();
- std::cout << "pose_previous_: " << pose_previous_.transpose() << std::endl;
- std::cout << "pose_current_: " << pose_current_.transpose() << std::endl;
- std::cout << "odom_inc_: " << odom_inc_.transpose() << std::endl;
- };
-};
-
-class GPSFixFunctor
-{
- protected:
- Eigen::Vector3s gps_fix_; //GPS fix XYZ. Could be a map to data hold by capture or feature
- const double gps_stdev_ = 1; //model parameters
-
- public:
- //constructor
- GPSFixFunctor(const Eigen::Vector3s & _gps_fix):
- gps_fix_(_gps_fix)
- {
- //
- };
-
- //destructor
- virtual ~GPSFixFunctor()
- {
- //
- };
-
- //cost function
- template <typename T>
- bool operator()(const T* const _x0, T* _residual) const
- {
- T dist_x = T( gps_fix_(0) ) - _x0[0];
- T dist_y = T( gps_fix_(1) ) - _x0[1];
- _residual[0] = dist_x / T(gps_stdev_);
- _residual[1] = dist_y / T(gps_stdev_);
- _residual[2] = T(0.); //T( gps_fix_(2) ) - _x0[2]; //in this case, third component of the state is heading, not Z, so it is ignored
- return true;
- };
-};
-
-//Specialized correspondence class for GPS Fix data
-class CorrespondenceGPSFix : public CorrespondenceBaseX
-{
- protected:
- Eigen::Map<Eigen::Vector3s> state_position_;
- Eigen::Map<const Eigen::Vector3s> gps_fix_;
-
- public:
- CorrespondenceGPSFix(WolfScalar * _st, const Eigen::Vector3s & _gps_fix) :
- CorrespondenceBaseX(1,{3}),
- state_position_(_st),// block_sizes_.at(0)), //size 3 is already defined at declaration
- gps_fix_(_gps_fix.data())
- {
- cost_function_ptr_ = new ceres::AutoDiffCostFunction<GPSFixFunctor,3,3>(new GPSFixFunctor(_gps_fix));
- };
-
- virtual ~CorrespondenceGPSFix()
- {
- //delete cost_function_ptr_;
- };
-
-// double * getLocation()
-// {
-// return location_.data();
-// }
-
- virtual void addToProblem(ceres::Problem & _problem)
- {
- ceres_residual_block_id_ = _problem.AddResidualBlock(cost_function_ptr_,nullptr,state_position_.data());
- }
-
- virtual void display() const
- {
- std::cout << "---- GPS Fix Correspondence ----" << std::endl;
- CorrespondenceBaseX::display();
- std::cout << "state_position_: " << state_position_.transpose() << std::endl;
- std::cout << "gps_fix_: " << gps_fix_.transpose() << std::endl;
- };
-};
-
-
-int main(int argc, char** argv)
-{
- //Welcome message
- std::cout << std::endl << " ========= WOLF-CERES test. Simple Odometry + GPS fix problem (with non-template classes) ===========" << std::endl << std::endl;
-
- //user input
- if (argc!=3)
- {
- std::cout << "Please call me with: [./test_ceres_odom_iterative NI NW], where:" << std::endl;
- std::cout << " - NI is the number of iterations" << std::endl;
- std::cout << " - NW is the size of the window" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
- unsigned int n_window = (unsigned int) atoi(argv[2]); //size of the window.
-
- //init google log
- google::InitGoogleLogging(argv[0]);
-
- //variables
- unsigned int ii, jj, jj_previous; //iterators
- Eigen::VectorXs odom_inc_true;//invented motion
- Eigen::Vector3s pose_true; //current true pose
- Eigen::VectorXs ground_truth; //accumulated true poses
- Eigen::Vector3s pose_predicted; // current predicted pose
- Eigen::VectorXs state; //running window winth solver result
- Eigen::VectorXs state_prior; //state prior, just before solving the problem
- Eigen::Vector2s odom_reading; //current odometry reading
- Eigen::Vector3s gps_fix_reading; //current GPS fix reading
- Eigen::VectorXs gps_log; //log of all gps readings
- Eigen::VectorXs results_log; //log of optimized poses
- CorrespondenceOdom2D *odom_corresp; //pointer to odometry correspondence
- CorrespondenceGPSFix *gps_fix_corresp; //pointer to GPS fix correspondence
- ceres::Problem problem; //ceres problem
- ceres::Solver::Options options; //ceres solver options
- ceres::Solver::Summary summary; //ceres solver summary
- std::ofstream log_file; //output file
-
- //resize vectors according user input number of iterations
- odom_inc_true.resize(n_execution*2); //2 odometry components per iteration
- ground_truth.resize(n_execution*3);// 3 components per iteration
- gps_log.resize(n_execution*3); //3 components per iteration
- results_log.resize(n_execution*3); //3 components per iteration
- state.resize(n_window*3); //3 components per window element
- state_prior.resize(n_execution*3); //3 components per window element
-
- //init true odom and true pose
- for (ii = 0; ii<n_execution; ii++)
- {
- if ( ii < (unsigned int)floor(n_execution/2) )
- odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments.
- else
- odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments.
- }
- pose_true << 0,0,0;
- pose_predicted << 0,0,0;
- ground_truth.middleRows(0,3) << pose_true; //init point pushed to ground truth
- state.middleRows(0,3) << 0,0,0; //init state at origin
-
- //init random generators
- std::default_random_engine generator;
- std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
- std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
-
- //test loop
- for (ii = 1; ii<n_execution; ii++) //ii over iterations, jj over the window
- {
- //set jj index (over the window)
- jj = ii%n_window;
- jj_previous = (ii-1)%n_window;
-
- //inventing a simple motion
- pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2)+odom_inc_true(ii*2+1));
- pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2)+odom_inc_true(ii*2+1));
- pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
- ground_truth.middleRows(ii*3,3) << pose_true;
- //std::cout << "pose_true(" << ii << ") = " << pose_true.transpose() << std::endl;
-
- //inventing sensor readings for odometry and GPS
- odom_reading << odom_inc_true(ii*2)+distribution_odom(generator), odom_inc_true(ii*2+1)+distribution_odom(generator); //true range and theta with noise
- gps_fix_reading << pose_true(0) + distribution_gps(generator), pose_true(1) + distribution_gps(generator), 0. + distribution_gps(generator);
- gps_log.middleRows(ii*3,3) << gps_fix_reading;//log the reading
-
- //setting initial guess from the last optimized pose, using noisy odometry
- pose_predicted(0) = state(jj_previous*3) + odom_reading(0) * cos(state(jj_previous*3+2)+odom_reading(1));
- pose_predicted(1) = state(jj_previous*3+1) + odom_reading(0) * sin(state(jj_previous*3+2)+odom_reading(1));
- pose_predicted(2) = state(jj_previous*3+2) + odom_reading(1);
-
- //window management
- state.middleRows(jj*3,3) << pose_predicted;
-
- //creating odom correspondence. Adding it to the problem
- odom_corresp = new CorrespondenceOdom2D(state.data()+jj_previous*3, state.data()+jj*3, odom_reading);
- odom_corresp->addToProblem(problem);
- //odom_corresp->display();
- delete odom_corresp;
-
- //creating gps correspondence and adding it to the problem
- gps_fix_corresp = new CorrespondenceGPSFix(state.data()+ii*3, gps_fix_reading);
- gps_fix_corresp->addToProblem(problem);
- //gps_fix_corresp->display();
- delete gps_fix_corresp;
-
- //set options and solve (sliding window)
-// options.minimizer_progress_to_stdout = true;
-// ceres::Solve(options, &problem, &summary);
- }
-
- //display initial guess
- //std::cout << "INITIAL GUESS IS: " << state.transpose() << std::endl;
-
- //set options and solve (batch mode)
- //options.minimizer_progress_to_stdout = true;
- state_prior = state;
- options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
- options.max_line_search_step_contraction = 1e-3;
- ceres::Solve(options, &problem, &summary);
-
- //display/log results, by setting cout flags properly
- std::string filename( getenv("HOME") );
- filename += "/Desktop/log_data.txt";
- std::cout << std::endl << " Result to file " << filename << std::endl;
- log_file.open(filename, std::ofstream::out); //open log file
- for (unsigned int ii = 0; ii<n_execution; ii++)
- log_file << state.middleRows(ii*3,3).transpose() << " " << ground_truth.middleRows(ii*3,3).transpose() << " " << (state.middleRows(ii*3,3)-ground_truth.middleRows(ii*3,3)).transpose() << " " << gps_log.middleRows(ii*3,3).transpose() << " " << state_prior.middleRows(ii*3,3).transpose() <<std::endl;
- log_file.close(); //close log file
-
- //free memory (not necessary since ceres::problem holds their ownership)
-// delete odom_corresp;
-// delete gps_fix_corresp;
-
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/src/examples/test_ceres_odom_plot.sce b/src/examples/test_ceres_odom_plot.sce
deleted file mode 100644
index 9cea76ea6..000000000
--- a/src/examples/test_ceres_odom_plot.sce
+++ /dev/null
@@ -1,75 +0,0 @@
-//plot log data from ceres test
-
-// clear all
-xdel(winsid());
-clear;
-
-// CERES ODOM BATCH
-//load log file
-data = read('~/Desktop/log_file.txt',-1,14);
-
-//plot
-fig1 = figure(0);
-fig1.background = 8;
-plot(data(2:$,10),data(2:$,11),"g.");
-plot(data(2:$,1),data(2:$,2),"b-");
-plot(data(2:$,4),data(2:$,5),"r-");
-plot(data(2:$,13),data(2:$,14),"c--");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$GPS$";"$Optimization$";"$Ground\ Truth$";"$ODOM$"],4);
-lh.font_size = 3;
-title(strcat(["CERES_ODOM_BATCH - Time: ",string(data(1,1))," s"]));
-ah.title.font_size = 4;
-
-// MANAGER - THETA
-//load log file
-data2 = read('~/Desktop/log_file_2.txt',-1,15);
-
-//plot
-fig2 = figure(1);
-fig2.background = 8;
-plot(data2(2:$,13),data2(2:$,14),"g.");
-plot(data2(2:$,1),data2(2:$,2),"b-");
-plot(data2(2:$,4),data2(2:$,5),"r-");
-plot(data2(2:$,10),data2(2:$,11),"c--");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$GPS$";"$Optimization$";"$Ground\ Truth$";"$ODOM$"],4);
-lh.font_size = 3;
-title(strcat(["CERES_MANAGER: Theta - Time: ",string(data2(1,1))," s"]));
-ah.title.font_size = 4;
-
-// MANAGER - COMPLEX ANGLE
-//load log file
-data3 = read('~/Desktop/log_file_3.txt',-1,15);
-
-//plot
-fig3 = figure(2);
-fig3.background = 8;
-plot(data3(2:$,13),data3(2:$,14),"g.");
-plot(data3(2:$,1),data3(2:$,2),"b-");
-plot(data3(2:$,4),data3(2:$,5),"r-");
-plot(data3(2:$,10),data3(2:$,11),"c--");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$GPS$";"$Optimization$";"$Ground\ Truth$";"$ODOM$"],4);
-lh.font_size = 3;
-title(strcat(["CERES_MANAGER: Complex Angle - Time: ",string(data3(1,1))," s"]));
-ah.title.font_size = 4;
-
diff --git a/src/examples/test_ceres_wrapper_jet.cpp b/src/examples/test_ceres_wrapper_jet.cpp
deleted file mode 100644
index c2091942d..000000000
--- a/src/examples/test_ceres_wrapper_jet.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "sensor_base.h"
-#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "capture_base.h"
-#include "state_base.h"
-#include "wolf.h"
-
-// ceres wrapper includes
-#include "ceres_wrapper/complex_angle_parametrization.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "node_terminus.h"
-#include "node_linked.h"
-
-//namespaces
-using namespace std;
-using namespace Eigen;
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle.
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class.
- * It holds:
- * - a map to a state vector
- * - a map to an error vector
- * - a method to compute the error from the state
- *
- **/
-
-class WolfVehicle
-{
- protected:
- VectorXs state_prior_; //state storage where to compute prior
- Map<VectorXs> state_map_; //state point to be evaluated by Wolf tree constraints
-
- //Just to generate fake measurements
- std::default_random_engine generator_; //just to generate measurements
- std::normal_distribution<double> distribution_; //just to generate measurements
- MatrixXs measurements_; //just a set of invented measurements
-
- unsigned int measurements_size_, state_size_;
-
- public:
- WolfVehicle() :
- state_prior_(),
- state_map_(nullptr,0),
- distribution_(0.0,0.01),
- measurements_size_(0),
- state_size_(0)
- {
- //
- }
-
- virtual ~WolfVehicle()
- {
- //
- }
-
- WolfScalar *getPrior()
- {
- return state_prior_.data();
- }
-
- void resizeState(unsigned int _state_size)
- {
- state_size_ = _state_size;
- state_prior_.resize(_state_size);
- }
-
- void inventMeasurements(unsigned int _sz)
- {
- // for testing
- measurements_size_ = _sz;
- measurements_.resize(state_size_,measurements_size_);
-
- for(unsigned int ii=0; ii<measurements_size_*state_size_; ii++)
- measurements_(ii) = 1 + distribution_(generator_); //just inventing a sort of noise measurements
-
- //std::cout << "invented measurements_ = " << std::endl << measurements_.transpose() << std::endl;
- }
-
- void computePrior()
- {
- state_prior_.setZero();
- }
-
- template <typename T>
- bool operator()(const T* const _x, T* _residuals) const
- {
- // Remap the vehicle state to the const evaluation point
- Map<const Matrix<T,Dynamic,1>> state_map_const_(_x, state_size_);
-
- // Map residuals vector to matrix (with sizes of the measurements matrix)
- Map<Matrix<T,Dynamic,Dynamic>> mapped_residuals(_residuals, state_size_, measurements_size_);
-
- // Compute error or residuals
- mapped_residuals = measurements_.cast<T>() - state_map_const_.replicate(1,measurements_size_);
-
- // std::cout << typeid(T).name() << std::endl;
- // for (unsigned int j = 0; j<state_size_; j++)
- // {
- // std::cout << "_x(" << j <<") = " << _x[j] << std::endl;
- // for (unsigned int i = 0; i<measurements_size_; i++)
- // std::cout << "mapped_residuals(" << j << "," << i <<") = " << mapped_residuals(j,i) << std::endl;
- // }
-
- return true;
- }
-
-// bool operator()(const WolfScalar* const _x, WolfScalar* _residuals) const
-// {
-// // Remap the vehicle state to the const evaluation point
-// Map<const VectorXs> state_map_const_(_x, state_size_);
-//
-// // Map residuals vector to matrix (with sizes of the measurements matrix)
-// Map<MatrixXs> mapped_residuals(_residuals, state_size_, measurements_size_);
-//
-// // Compute error or residuals
-// mapped_residuals = measurements_ - state_map_const_.replicate(1,measurements_size_);
-//
-// return true;
-// }
-
- void print()
- {
- //std::cout << "measurements_: " << std::endl << measurements_.transpose() << std::endl << std::endl;
- std::cout << "state_prior_ : " << std::endl << state_prior_.transpose() << std::endl << std::endl;
- //std::cout << "state_const_ : " << std::endl << state_map_const_.transpose() << std::endl << std::endl;
- }
-};
-
-//This main is a single iteration of the WOLF.
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv)
-{
- std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-
-// NodeLinked<NodeTerminus,NodeTerminus> node(TOP,"TRAJECTORY");
-
- //dimension
- const unsigned int STATE_DIM = 5; //just to test, all will be DIM-dimensional
- const unsigned int MEASUREMENT_DIM = 1; //just to test, all will be DIM-dimensional
-
- // init
- google::InitGoogleLogging(argv[0]);
-
- using ceres::AutoDiffCostFunction;
- using ceres::CostFunction;
-
- //wolf vehicle & ceres functor
- WolfVehicle *functorPtr = new WolfVehicle();
-
- // Ceres problem initialization
- ceres::Solver::Options options;
- options.minimizer_progress_to_stdout = false;
- options.minimizer_type = ceres::TRUST_REGION;
- options.line_search_direction_type = ceres::LBFGS;
- options.max_num_iterations = 10;
- ceres::Solver::Summary summary;
- ceres::Problem problem;
-
- // fixed dim problem
- functorPtr->resizeState(STATE_DIM);
- functorPtr->computePrior();
-
- // set measures. This will be replaced by the WOLF-ROS front-end, getting sensor readings from sensors and performing measurements to build the whole wolf tree
- functorPtr->inventMeasurements(MEASUREMENT_DIM);
-
- // cost function
- CostFunction* cost_function = new AutoDiffCostFunction<WolfVehicle,1,1>(functorPtr);
- for (uint st=0; st < STATE_DIM; st++)
- {
- problem.AddResidualBlock(cost_function, NULL, functorPtr->getPrior()+st);
- }
- //CostFunction* cost_function = new AutoDiffCostFunction<WolfVehicle,STATE_DIM*MEASUREMENT_DIM,STATE_DIM>(functorPtr);
- //problem.AddResidualBlock(cost_function, NULL, functorPtr->getPrior());
-
- // run Ceres Solver
- ceres::Solve(options, &problem, &summary);
-
- //display results
- std::cout << summary.BriefReport() << "\n";
- functorPtr->print();
-
- //clean
- std::cout << "Cleaning ... " << std::endl << std::endl;
- //problem.RemoveResidualBlock(rbId);
- delete cost_function;
- delete functorPtr;
-
- //end Wolf iteration
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
-
diff --git a/src/examples/test_ceres_wrapper_jet_2_sensors.cpp b/src/examples/test_ceres_wrapper_jet_2_sensors.cpp
deleted file mode 100644
index 7564a3034..000000000
--- a/src/examples/test_ceres_wrapper_jet_2_sensors.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "ceres/loss_function.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle.
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class.
- * It holds:
- * - a map to a state vector
- * - a map to an error vector
- * - a method to compute the error from the state
- *
- **/
-using namespace Eigen;
-
-class CorrespondenceBase
-{
- protected:
- VectorXs measurement_;
-
- public:
-
- CorrespondenceBase(const unsigned int & _measurement_size) :
- measurement_(_measurement_size)
- {
- }
-
- virtual ~CorrespondenceBase()
- {
- }
-
- virtual void inventMeasurement(const VectorXs& _measurement, std::default_random_engine& _generator, std::normal_distribution<WolfScalar>& _distribution)
- {
- measurement_ = _measurement;
- for(unsigned int ii=0; ii<measurement_.size(); ii++)
- measurement_(ii) += _distribution(_generator); //just inventing a sort of noise measurements
- //std::cout << "measurement_" << measurement_ << std::endl;
- }
-};
-
-class CorrespondenceCeresBase
-{
- protected:
- ceres::CostFunction* cost_function_;
-
- public:
-
- CorrespondenceCeresBase()
- {
- }
-
- virtual ~CorrespondenceCeresBase()
- {
- }
-
- virtual void addBlock(ceres::Problem & ceres_problem) = 0;
-};
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_SIZE = 1>
-class Correspondence1Sparse: public CorrespondenceBase
-{
- protected:
- Map<Matrix<WolfScalar, BLOCK_SIZE, 1>> state_block_map_;
-
- public:
-
- Correspondence1Sparse(WolfScalar* _statePtr) :
- CorrespondenceBase(MEASUREMENT_SIZE),
- state_block_map_(_statePtr,BLOCK_SIZE)
- {
- }
-
- virtual ~Correspondence1Sparse()
- {
- }
-
- virtual WolfScalar* getBlockPtr()
- {
- return state_block_map_.data();
- }
-
- template <typename T>
- //void compute_residuals(const Matrix<T,Dynamic,1>& _x, Matrix<T,Dynamic,1> residuals)
- void compute_residuals(Map<const Matrix<T,Dynamic,1>>& _st, Map<Matrix<T,Dynamic,1>> residuals) const
- {
- residuals = measurement_.cast<T>() - _st;
- // std::cout << typeid(T).name() << std::endl;
- // for (unsigned int j = 0; j<BLOCK_SIZE; j++)
- // {
- // std::cout << "_x(" << j <<") = " << _x[j] << std::endl;
- // std::cout << "mapped_residuals(" << j <<") = " << mapped_residuals(j) << std::endl;
- // }
- }
-};
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_SIZE = 1>
-class Correspondence1SparseCeres: public CorrespondenceCeresBase, public Correspondence1Sparse<MEASUREMENT_SIZE, BLOCK_SIZE>
-{
- public:
-
- Correspondence1SparseCeres(WolfScalar* _statePtr) :
- CorrespondenceCeresBase(),
- Correspondence1Sparse<MEASUREMENT_SIZE, BLOCK_SIZE>(_statePtr)
- {
- cost_function_ = new ceres::AutoDiffCostFunction<Correspondence1SparseCeres,MEASUREMENT_SIZE,BLOCK_SIZE>(this);
- }
-
- virtual ~Correspondence1SparseCeres()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _x, T* _residuals) const
- {
- //std::cout << "adress of x: " << _x << std::endl;
-
- // Remap the vehicle state to the const evaluation point
- Map<const Matrix<T,Dynamic,1>> state_map_const(_x, BLOCK_SIZE);
-
- // Map residuals vector to matrix (with sizes of the measurements matrix)
- Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, MEASUREMENT_SIZE);
-
- // Compute error or residuals
- this->template compute_residuals<T>(state_map_const, mapped_residuals);
-
- return true;
- }
-
- virtual void addBlock(ceres::Problem & ceres_problem)
- {
- //std::cout << " adding correspondence_1_sparse_ceres...";
- ceres_problem.AddResidualBlock(cost_function_, NULL, this->state_block_map_.data());
- //std::cout << " added!" << std::endl;
- }
-};
-
-
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_1_SIZE = 1, unsigned int BLOCK_2_SIZE = 1>
-class Correspondence2Sparse: public CorrespondenceBase
-{
- protected:
- Map<Matrix<WolfScalar, BLOCK_1_SIZE, 1>> state_block_1_map_;
- Map<Matrix<WolfScalar, BLOCK_2_SIZE, 1>> state_block_2_map_;
-
- public:
-
- Correspondence2Sparse(WolfScalar* _block1Ptr, WolfScalar* _block2Ptr) :
- CorrespondenceBase(MEASUREMENT_SIZE),
- state_block_1_map_(_block1Ptr,BLOCK_1_SIZE),
- state_block_2_map_(_block2Ptr,BLOCK_2_SIZE)
- {
- }
-
- virtual ~Correspondence2Sparse()
- {
- }
-
- template <typename T>
- void compute_residuals(Map<const Matrix<T,Dynamic,1>>& _st1, Map<const Matrix<T,Dynamic,1>>& _st2, Map<Matrix<T,Dynamic,1>> residuals) const
- {
- Matrix<T,Dynamic,1> expected_measurement = ((_st1 - _st2).transpose() * (_st1 - _st2)).cwiseSqrt();
- VectorXd meas = this->measurement_;
- residuals = (meas).cast<T>() - expected_measurement;
- }
-
- WolfScalar *getBlock1Ptr()
- {
- return state_block_1_map_.data();
- }
-
- WolfScalar *getBlock2Ptr()
- {
- return state_block_2_map_.data();
- }
-};
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_1_SIZE = 1, unsigned int BLOCK_2_SIZE = 1>
-class Correspondence2SparseCeres: public CorrespondenceCeresBase, public Correspondence2Sparse<MEASUREMENT_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE>
-{
- public:
-
- Correspondence2SparseCeres(WolfScalar* _block1Ptr, WolfScalar* _block2Ptr) :
- CorrespondenceCeresBase(),
- Correspondence2Sparse<MEASUREMENT_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE>(_block1Ptr, _block2Ptr)
- {
- cost_function_ = new ceres::AutoDiffCostFunction<Correspondence2SparseCeres,MEASUREMENT_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE>(this);
- }
-
- virtual ~Correspondence2SparseCeres()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _x1, const T* const _x2, T* _residuals) const
- {
- // print inputs
- // std::cout << "_x1 = ";
- // for (int i=0; i < BLOCK_1_SIZE; i++)
- // std::cout << _x1[i] << " ";
- // std::cout << std::endl;
- // std::cout << "_x2 = ";
- // for (int i=0; i < BLOCK_2_SIZE; i++)
- // std::cout << _x2[i] << " ";
- // std::cout << std::endl;
- // std::cout << "measurement = ";
- // for (int i=0; i < MEASUREMENT_SIZE; i++)
- // std::cout << this->measurement_(i) << " ";
- // std::cout << std::endl;
-
- // Remap the vehicle state to the const evaluation point
- Map<const Matrix<T,Dynamic,1>> x1_map_const(_x1, BLOCK_1_SIZE);
- Map<const Matrix<T,Dynamic,1>> x2_map_const(_x2, BLOCK_2_SIZE);
-
- // Map residuals vector to matrix (with sizes of the measurements matrix)
- Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, MEASUREMENT_SIZE);
-
- // Compute error or residuals
- this->template compute_residuals(x1_map_const, x2_map_const, mapped_residuals);
-
- // print outputs
- // std::cout << "expected = ";
- // for (int i=0; i < MEASUREMENT_SIZE; i++)
- // std::cout << expected_measurement(i) << " ";
- // std::cout << std::endl;
- // std::cout << "_residuals = ";
- // for (int i=0; i < MEASUREMENT_SIZE; i++)
- // std::cout << _residuals[i] << " ";
- // std::cout << std::endl << std::endl;
-
- return true;
- }
-
- virtual void addBlock(ceres::Problem & ceres_problem)
- {
- //std::cout << " adding correspondence_2_sparse_ceres...";
- ceres_problem.AddResidualBlock(cost_function_, NULL, this->state_block_1_map_.data(), this->state_block_2_map_.data());
- //std::cout << " added!" << std::endl;
- }
-};
-
-class WolfProblem
-{
- protected:
- VectorXs state_; //state storage
- std::vector<CorrespondenceCeresBase*> correspondences_;
-
- unsigned int state_size_;
-
- public:
- WolfProblem() :
- state_(),
- correspondences_(0),
- state_size_(0)
- {
- }
-
- virtual ~WolfProblem()
- {
- }
-
- WolfScalar *getPrior()
- {
- return state_.data();
- }
-
- CorrespondenceCeresBase* getCorrespondence(const unsigned int _idx)
- {
- //std::cout << correspondences_.size() << " correspondences" << std::endl;
- return correspondences_[_idx];
- }
-
- unsigned int getCorrespondencesSize()
- {
- return correspondences_.size();
- }
-
- void resizeState(unsigned int _state_size)
- {
- state_size_ = _state_size;
- state_.resize(_state_size);
- }
-
- void addCorrespondence(CorrespondenceCeresBase* _absCorrPtr)
- {
- correspondences_.push_back(_absCorrPtr);
- //std::cout << correspondences_.size() << " correspondence added!" << std::endl;
- }
-
- void computePrior()
- {
- state_.setRandom();
- }
-
- void print()
- {
- std::cout << "state_ : " << std::endl << state_.transpose() << std::endl << std::endl;
- }
-};
-
-//This main is a single iteration of the WOLF.
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv)
-{
- std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-
- //dimension
- const unsigned int DIM = 3;
- const unsigned int N_STATES = 10;
- const unsigned int STATE_DIM = DIM * N_STATES;
- const unsigned int MEAS_A_DIM = 3;
- const unsigned int MEAS_B_DIM = 1;
- const unsigned int N_MEAS_A = 100;
- const unsigned int N_MEAS_B = 50;
- //const double w_A = 1;
- //const double w_B = 10;
-
- // init
- google::InitGoogleLogging(argv[0]);
- std::default_random_engine generator;
- std::normal_distribution<WolfScalar> distribution_A(0.0,0.01);
- std::normal_distribution<WolfScalar> distribution_B(0.0,0.1);
- VectorXs actualState(STATE_DIM);
- for (uint i=0;i<STATE_DIM; i++)
- actualState(i) = i;
-
- using ceres::AutoDiffCostFunction;
- using ceres::CostFunction;
-
- // Ceres problem initialization
- ceres::Solver::Options options;
- options.minimizer_progress_to_stdout = false;
- options.minimizer_type = ceres::TRUST_REGION;
- options.line_search_direction_type = ceres::LBFGS;
- options.max_num_iterations = 100;
- ceres::Solver::Summary summary;
- ceres::Problem ceres_problem;
-
- //wolf problem
- WolfProblem *wolf_problem = new WolfProblem();
- wolf_problem->resizeState(STATE_DIM);
- wolf_problem->computePrior();
- std::cout << "Real state: " << actualState.transpose() << std::endl;
- wolf_problem->print();
-
- // Correspondences
- // SENSOR A: Absolute measurements of the whole state
- for(uint mA=0; mA < N_MEAS_A; mA++)
- {
- for (uint st=0; st < N_STATES; st++)
- {
- Correspondence1SparseCeres<MEAS_A_DIM, DIM>* corrAPtr = new Correspondence1SparseCeres<MEAS_A_DIM, DIM>(wolf_problem->getPrior()+st*DIM);
- VectorXs actualMeasurement = actualState.segment(st*DIM,DIM);
- corrAPtr->inventMeasurement(actualMeasurement,generator,distribution_A);
- wolf_problem->addCorrespondence(corrAPtr);
- }
- }
- // SENSOR B: Relative distances between points
- for(uint mB=0; mB < N_MEAS_B; mB++)
- {
- for (uint st_from=0; st_from < N_STATES-1; st_from++)
- {
- for (uint st_to=st_from+1; st_to < N_STATES; st_to++)
- {
- Correspondence2SparseCeres<MEAS_B_DIM, DIM, DIM>* corrBPtr = new Correspondence2SparseCeres<MEAS_B_DIM, DIM, DIM>(wolf_problem->getPrior()+st_from*DIM,wolf_problem->getPrior()+st_to*DIM);
- VectorXs actualMeasurement = ((actualState.segment(st_from*DIM,DIM) - actualState.segment(st_to*DIM,DIM)).transpose() * (actualState.segment(st_from*DIM,DIM) - actualState.segment(st_to*DIM,DIM))).cwiseSqrt();
- corrBPtr->inventMeasurement(actualMeasurement,generator,distribution_B);
- wolf_problem->addCorrespondence(corrBPtr);
- }
- }
-// correspondence_2_sparse_ceres<MEAS_B_DIM, DIM, DIM>* corrBPtr = new correspondence_2_sparse_ceres<MEAS_B_DIM, DIM, DIM>(wolf_problem->getPrior(),wolf_problem->getPrior()+DIM);
-// VectorXs actualMeasurement = ((actualState.head(DIM) - actualState.tail(DIM)).transpose() * (actualState.head(DIM) - actualState.tail(DIM))).cwiseSqrt();
-// corrBPtr->inventMeasurement(actualMeasurement,generator,distribution_B);
-// wolf_problem->addCorrespondence(corrBPtr);
- }
-
- // cost function
- std::cout << "Number of blocks: " << std::endl << wolf_problem->getCorrespondencesSize() << std::endl;
- for (uint block=0; block < wolf_problem->getCorrespondencesSize(); block++)
- {
- //std::cout << "block " << block << "...";
- wolf_problem->getCorrespondence(block)->addBlock(ceres_problem);
- //std::cout << " added!" << std::endl;
- }
-
- // run Ceres Solver
- ceres::Solve(options, &ceres_problem, &summary);
-
- //display results
- std::cout << summary.FullReport() << "\n";
- wolf_problem->print();
-
- //clean
- std::cout << "Cleaning ... " << std::endl << std::endl;
- //ceres_problem.RemoveResidualBlock(rbId);
- delete wolf_problem;
-
- //end Wolf iteration
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
-
diff --git a/src/examples/test_ceres_wrapper_jet_ind_block.cpp b/src/examples/test_ceres_wrapper_jet_ind_block.cpp
deleted file mode 100644
index 075483b41..000000000
--- a/src/examples/test_ceres_wrapper_jet_ind_block.cpp
+++ /dev/null
@@ -1,214 +0,0 @@
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle.
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class.
- * It holds:
- * - a map to a state vector
- * - a map to an error vector
- * - a method to compute the error from the state
- *
- **/
-using namespace Eigen;
-
-class AbsoluteCorrespondence
-{
- protected:
- Map<VectorXs> state_block_mapped_;
-
- //Just to generate fake measurements
- std::default_random_engine generator_; //just to generate measurements
- std::normal_distribution<WolfScalar> distribution_; //just to generate measurements
- VectorXs measurement_; // invented measurement
- ceres::CostFunction* cost_function;
-
- public:
-
- AbsoluteCorrespondence(WolfScalar* _statePtr, const unsigned int & _block_size) :
- state_block_mapped_(_statePtr,_block_size),
- distribution_(0.0,0.01),
- measurement_(_block_size),
- cost_function(new ceres::AutoDiffCostFunction<AbsoluteCorrespondence,1,1>(this))
- {
- }
-
- virtual ~AbsoluteCorrespondence()
- {
- }
-
- WolfScalar *getPrior()
- {
- return state_block_mapped_.data();
- }
-
- void inventMeasurement(std::default_random_engine& _generator)
- {
- for(unsigned int ii=0; ii<state_block_mapped_.size(); ii++)
- measurement_(ii) = 1 + distribution_(_generator); //just inventing a sort of noise measurements
- }
-
- template <typename T>
- bool operator()(const T* const _x, T* _residuals) const
- {
- // Remap the vehicle state to the const evaluation point
- Map<const Matrix<T,Dynamic,1>> state_map_const_(_x, state_block_mapped_.size());
-
- // Map residuals vector to matrix (with sizes of the measurements matrix)
- Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, state_block_mapped_.size());
-
- // Compute error or residuals
- mapped_residuals = measurement_.cast<T>() - state_map_const_;
-
- // std::cout << typeid(T).name() << std::endl;
- // for (unsigned int j = 0; j<block_size_; j++)
- // {
- // std::cout << "_x(" << j <<") = " << _x[j] << std::endl;
- // std::cout << "mapped_residuals(" << j <<") = " << mapped_residuals(j) << std::endl;
- // }
-
- return true;
- }
-
- void addBlock(ceres::Problem & ceres_problem)
- {
- ceres_problem.AddResidualBlock(cost_function, NULL, getPrior());
- }
-};
-
-class WolfProblem
-{
- protected:
- VectorXs state_; //state storage
- std::vector<AbsoluteCorrespondence*> correspondences_;
-
- unsigned int state_size_;
-
- public:
- WolfProblem() :
- state_(),
- correspondences_(0),
- state_size_(0)
- {
- }
-
- virtual ~WolfProblem()
- {
- }
-
- WolfScalar *getPrior()
- {
- return state_.data();
- }
-
- AbsoluteCorrespondence* getCorrespondence(const unsigned int _idx)
- {
- return correspondences_[_idx];
- }
-
- unsigned int getCorrespondencesSize()
- {
- return correspondences_.size();
- }
-
- void resizeState(unsigned int _state_size)
- {
- state_size_ = _state_size;
- state_.resize(_state_size);
- }
-
- void addCorrespondence(AbsoluteCorrespondence* _absCorrPtr)
- {
- correspondences_.push_back(_absCorrPtr);
- }
-
- void inventMeasurements(std::default_random_engine& _generator)
- {
- for (std::vector<AbsoluteCorrespondence*>::iterator it = correspondences_.begin(); it != correspondences_.end(); it++)
- (*it)->inventMeasurement(_generator);
- }
-
- void computePrior()
- {
- state_.setZero();
- }
-
- void print()
- {
- std::cout << "state_ : " << std::endl << state_.transpose() << std::endl << std::endl;
- }
-};
-
-//This main is a single iteration of the WOLF.
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv)
-{
- std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-
- //dimension
- const unsigned int STATE_DIM = 50; //just to test, all will be DIM-dimensional
- const unsigned int N_MEASUREMENTS = 1000;
- // init
- google::InitGoogleLogging(argv[0]);
- std::default_random_engine generator;
-
- using ceres::AutoDiffCostFunction;
- using ceres::CostFunction;
-
- // Ceres problem initialization
- ceres::Solver::Options options;
- options.minimizer_progress_to_stdout = false;
- options.minimizer_type = ceres::TRUST_REGION;
- options.line_search_direction_type = ceres::LBFGS;
- options.max_num_iterations = 10;
- ceres::Solver::Summary summary;
- ceres::Problem ceres_problem;
-
- //wolf problem
- WolfProblem *wolf_problem = new WolfProblem();
- wolf_problem->resizeState(STATE_DIM);
- wolf_problem->computePrior();
- for(uint st=0; st < N_MEASUREMENTS; st++)
- for (uint st=0; st < STATE_DIM; st++)
- wolf_problem->addCorrespondence(new AbsoluteCorrespondence(wolf_problem->getPrior()+st,1));
-
- // set measures. This will be replaced by the WOLF-ROS front-end, getting sensor readings from sensors and performing measurements to build the whole wolf tree
- wolf_problem->inventMeasurements(generator);
-
- // cost function
- std::cout << "Number of blocks: " << std::endl << wolf_problem->getCorrespondencesSize() << std::endl;
- for (uint block=0; block < wolf_problem->getCorrespondencesSize(); block++)
- wolf_problem->getCorrespondence(block)->addBlock(ceres_problem);
-
- // run Ceres Solver
- ceres::Solve(options, &ceres_problem, &summary);
-
- //display results
- std::cout << summary.FullReport() << "\n";
- wolf_problem->print();
-
- //clean
- std::cout << "Cleaning ... " << std::endl << std::endl;
- //ceres_problem.RemoveResidualBlock(rbId);
- delete wolf_problem;
-
- //end Wolf iteration
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
-
diff --git a/src/examples/test_ceres_wrapper_numeric.cpp b/src/examples/test_ceres_wrapper_numeric.cpp
deleted file mode 100644
index 60528eafb..000000000
--- a/src/examples/test_ceres_wrapper_numeric.cpp
+++ /dev/null
@@ -1,220 +0,0 @@
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle.
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class.
- * It holds:
- * - a map to a state vector
- * - a map to an error vector
- * - a method to compute the error from the state
- *
- **/
-class WolfVehicle
-{
- protected:
- Eigen::VectorXs state_; //state storage where to compute prior, and where result is placed. Is the allocation for the state vector
- Eigen::Map<const Eigen::VectorXs> state_map_; //state point to be evaluated by Wolf tree constraints
- Eigen::Map<Eigen::VectorXs> error_map_; //error computed by the wolf tree
- unsigned int state_size_;
- unsigned int error_size_;
-
- //Just to generate fake measurements
- std::default_random_engine generator_; //just to generate measurements
- std::normal_distribution<double> distribution_; //just to generate measurements
- Eigen::VectorXs measurements_; //just a set of invented measurements
-
- public:
- WolfVehicle() :
- state_(),
- state_map_(nullptr,0),
- error_map_(nullptr,0),
- state_size_(0),
- error_size_(0),
- distribution_(0.0,0.1)
- {
- //
- }
-
- virtual ~WolfVehicle()
- {
- //
- }
-
- void setSizes(unsigned int _state_size, unsigned int _error_size)
- {
- state_size_ = _state_size;
- error_size_ = _error_size;
- state_.resize(_state_size);
- }
-
- void remapState(const WolfScalar *_ptr)
- {
- new (&state_map_) Eigen::Map<const Eigen::VectorXs>(_ptr, state_size_);
- }
-
- void remapError(WolfScalar *_ptr)
- {
- new (&error_map_) Eigen::Map<Eigen::VectorXs>(_ptr, error_size_);
- }
-
- void inventMeasurements(unsigned int _sz)
- {
- measurements_.resize(_sz);
- for(unsigned int ii=0; ii<_sz; ii++)
- {
- measurements_(ii) = 1 + distribution_(generator_); //just inventing a sort of noise measurements
- }
- }
-
- WolfScalar *getState()
- {
- return state_.data();
- }
-
- void getState(Eigen::VectorXs & _v)
- {
- _v.resize(state_.size());
- _v = state_;
- }
-
- void computePrior()
- {
- state_.setOnes();//just a fake prior
- }
-
- void computeError()
- {
- for(unsigned int ii=0; ii<error_map_.size(); ii++)
- {
- error_map_(ii) = measurements_(ii) - state_map_(ii); //just a trivial error function
- }
- }
-
- void print()
- {
- std::cout << "measurements_: " << measurements_.transpose() << std::endl;
- std::cout << "state_: " << state_.transpose() << std::endl;
- //std::cout << "state_map_: " << state_map_.transpose() << std::endl;
- }
-};
-
-/**
- * This class wrapps the Wolf Vehicle and implements the operator(), to be used by CERES optimizer
- * - the operator() overloaded
-**/
-class CeresWolfFunctor
-{
- protected:
- std::shared_ptr<WolfVehicle> vehicle_ptr_; //pointer to Wolf Vehicle object
-
- public:
- CeresWolfFunctor(std::shared_ptr<WolfVehicle> & _wv) :
- vehicle_ptr_(_wv)
- {
- //std::cout << "CeresWolfFunctor(): " << vehicle_ptr_.use_count() << " " << state_ptr_.use_count() << " " << std::endl;
- }
-
- virtual ~CeresWolfFunctor()
- {
- //
- }
- bool operator()(const WolfScalar * const _x, double* _residuals) const
- {
- // 1. Remap the vehicle state to the provided x
- vehicle_ptr_->remapState(_x);
-
- // 2. Remap the error to the provided address
- vehicle_ptr_->remapError(_residuals);
-
- // 3. Compute error
- vehicle_ptr_->computeError();
-
- // 4. return
- return true;
- }
-
-};
-
-//This main is a single iteration of the WOLF.
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv)
-{
- std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-
- //dimension
- const unsigned int DIM = 19; //just to test, all will be DIM-dimensional
-
- // init
- google::InitGoogleLogging(argv[0]);
-
- //wolf vehicle
- std::shared_ptr<WolfVehicle> vehiclePtr(std::make_shared<WolfVehicle>());
-
- //ceres functor
- CeresWolfFunctor *functorPtr = new CeresWolfFunctor(vehiclePtr);
-
- // Allocate the cost function !!!! Difference is to create in the same line both objects -> in the last (): (new ... ) SEE test_ceres_basic.cpp
- ceres::NumericDiffCostFunction<CeresWolfFunctor,ceres::CENTRAL,DIM,DIM>*
- cost_function_static = new ceres::NumericDiffCostFunction<CeresWolfFunctor,ceres::CENTRAL,DIM,DIM>(functorPtr);
-
- //********************** start Wolf iteration *************************
-
- // set measures. This will be replaced by the WOLF-ROS front-end, getting sensor reading from sensors (callbacks) and performing measurements to build the whole wolf tree
- vehiclePtr->inventMeasurements(DIM);
-
- // Resizing & remapping. Dimensions may come from a call to WolfVehicle::getSizes()
- vehiclePtr->setSizes(DIM,DIM);
-
- // Compute and gets the Prior (initial guess). This would be the prior given by Wolf
- vehiclePtr->computePrior();
-
- // build Ceres problem
- ceres::Problem *problem = new ceres::Problem();
- problem->AddResidualBlock(cost_function_static, nullptr, vehiclePtr->getState());
-
- // run Ceres Solver
- ceres::Solver::Options options;
- options.minimizer_progress_to_stdout = true;
- options.minimizer_type = ceres::TRUST_REGION;
- options.line_search_direction_type = ceres::LBFGS;
- options.max_num_iterations = 10;
- ceres::Solver::Summary summary;
- ceres::Solve(options, problem, &summary);
-
- //display results
- std::cout << std::endl<< "Ceres Report:" << std::endl;
- std::cout << summary.BriefReport() << "\n";
- std::cout << std::endl << "Wolf vectors:" << std::endl;
- vehiclePtr->print();
-
- //********************** end Wolf iteration *************************
-
- //clean
- //std::cout << "Cleaning ... " << std::endl << std::endl;
- //something to do ??
- delete problem;
- //delete vehiclePtr;
- //delete cost_function_static;
- //delete cost_function_static;
-
- //end Wolf iteration
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
-
diff --git a/src/examples/test_ceres_wrapper_state_units.cpp b/src/examples/test_ceres_wrapper_state_units.cpp
deleted file mode 100644
index b83f7f478..000000000
--- a/src/examples/test_ceres_wrapper_state_units.cpp
+++ /dev/null
@@ -1,1021 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-//#include "ceres/loss_function.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-
-/**
- * This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data.
- *
- **/
-
-using namespace Eigen;
-enum costFunctionType {
- AUTO,
- NUMERIC};
-enum parametrizationType {
- NONE,
- COMPLEX_ANGLE,
- QUATERNION,
- PO_2D};
-
-class StateBase
-{
- protected:
- WolfScalar* state_ptr_;
-
- public:
-
- StateBase(VectorXs& _st_remote, const unsigned int _idx) :
- state_ptr_(_st_remote.data() + _idx)
- {
- }
-
- StateBase(WolfScalar* _st_ptr) :
- state_ptr_(_st_ptr)
- {
- }
-
- StateBase(StateBase& _st) :
- state_ptr_(_st.state_ptr_)
- {
- }
-
-
- virtual ~StateBase()
- {
- }
-
- virtual WolfScalar* getPtr()
- {
- return state_ptr_;
- }
-
- virtual parametrizationType getStateType() const = 0;
-
- virtual void print() const = 0;
-};
-
-class StatePoint2D: public StateBase
-{
- public:
- static const unsigned int BLOCK_SIZE = 2;
-
- StatePoint2D(VectorXs& _st_remote, const unsigned int _idx) :
- StateBase(_st_remote, _idx)
- {
- }
-
- StatePoint2D(WolfScalar* _st_ptr) :
- StateBase(_st_ptr)
- {
- }
-
- virtual ~StatePoint2D()
- {
- }
-
- virtual parametrizationType getParametrizationType() const
- {
- return NONE;
- }
-
- virtual void print() const
- {
- std::cout << *this->state_ptr_ << " " << *(this->state_ptr_+1) << std::endl;
- }
-};
-
-class StateThetaAngle: public StateBase
-{
- public:
- static const unsigned int BLOCK_SIZE = 1;
-
- StateThetaAngle(VectorXs& _st_remote, const unsigned int _idx) :
- StateBase(_st_remote, _idx)
- {
- }
-
- StateThetaAngle(WolfScalar* _st_ptr) :
- StateBase(_st_ptr)
- {
- }
-
- virtual ~StateThetaAngle()
- {
- }
-
- virtual parametrizationType getParametrizationType() const
- {
- return NONE;
- }
-
- virtual void print() const
- {
- std::cout << *this->state_ptr_ << std::endl;
- }
-};
-
-class StateComplexAngle: public StateBase
-{
- public:
- static const unsigned int BLOCK_SIZE = 2;
-
- StateComplexAngle(VectorXs& _st_remote, const unsigned int _idx) :
- StateBase(_st_remote, _idx)
- {
- }
-
- StateComplexAngle(WolfScalar* _st_ptr) :
- StateBase(_st_ptr)
- {
- }
-
- virtual ~StateComplexAngle()
- {
- }
-
- virtual parametrizationType getParametrizationType() const
- {
- return COMPLEX_ANGLE;
- }
-
- virtual void print() const
- {
- std::cout << *this->state_ptr_ << " " << *(this->state_ptr_+1) << std::endl;
- }
-};
-
-class ComplexAngleParameterization : public ceres::LocalParameterization
-{
- public:
- virtual ~ComplexAngleParameterization()
- {
- }
-
- virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
- {
- x_plus_delta_raw[0] = x_raw[0] * cos(delta_raw[0]) - x_raw[1] * sin(delta_raw[0]);
- x_plus_delta_raw[1] = x_raw[1] * cos(delta_raw[0]) + x_raw[0] * sin(delta_raw[0]);
-
- //normalize
- //double norm = sqrt(x_plus_delta_raw[0] * x_plus_delta_raw[0] + x_plus_delta_raw[1] * x_plus_delta_raw[1]);
- //std::cout << "(before normalization) norm = " << norm << std::endl;
- //x_plus_delta_raw[0] /= norm;
- //x_plus_delta_raw[1] /= norm;
-
- return true;
- }
-
- virtual bool ComputeJacobian(const double* x, double* jacobian) const
- {
- jacobian[0] = -x[1];
- jacobian[1] = x[0];
- return true;
- }
-
- virtual int GlobalSize() const
- {
- return 2;
- }
-
- virtual int LocalSize() const
- {
- return 1;
- }
-};
-
-class CorrespondenceBase
-{
- protected:
- WolfScalar *measurement_ptr_;
-
- public:
-
- CorrespondenceBase(WolfScalar * _measurement_ptr) :
- measurement_ptr_(_measurement_ptr)
- {
- }
-
- virtual ~CorrespondenceBase()
- {
- }
-
- virtual costFunctionType getCostFunctionType() const = 0;
- virtual const std::vector<WolfScalar *> getBlockPtrVector() = 0;
-};
-
-template <const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE = 0,
- unsigned int BLOCK_2_SIZE = 0,
- unsigned int BLOCK_3_SIZE = 0,
- unsigned int BLOCK_4_SIZE = 0,
- unsigned int BLOCK_5_SIZE = 0,
- unsigned int BLOCK_6_SIZE = 0,
- unsigned int BLOCK_7_SIZE = 0,
- unsigned int BLOCK_8_SIZE = 0,
- unsigned int BLOCK_9_SIZE = 0>
-class CorrespondenceSparse: public CorrespondenceBase
-{
- protected:
- std::vector<Map<VectorXs>> state_block_map_vector_;
- std::vector<WolfScalar*> state_block_ptr_vector_;
- std::vector<unsigned int> block_sizes_vector_;
-
- public:
- static const unsigned int measurementSize = MEASUREMENT_SIZE;
- static const unsigned int block0Size = BLOCK_0_SIZE;
- static const unsigned int block1Size = BLOCK_1_SIZE;
- static const unsigned int block2Size = BLOCK_2_SIZE;
- static const unsigned int block3Size = BLOCK_3_SIZE;
- static const unsigned int block4Size = BLOCK_4_SIZE;
- static const unsigned int block5Size = BLOCK_5_SIZE;
- static const unsigned int block6Size = BLOCK_6_SIZE;
- static const unsigned int block7Size = BLOCK_7_SIZE;
- static const unsigned int block8Size = BLOCK_8_SIZE;
- static const unsigned int block9Size = BLOCK_9_SIZE;
-
- CorrespondenceSparse(WolfScalar* _measurementPtr, WolfScalar** _blockPtrArray) :
- CorrespondenceBase(_measurementPtr),
- block_sizes_vector_({BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE})
- {
- for (uint i = 0; i<block_sizes_vector_.size(); i++)
- {
- if (block_sizes_vector_.at(i) == 0)
- {
- block_sizes_vector_.resize(i);
- break;
- }
- else
- {
- state_block_map_vector_.push_back(Map<VectorXs>(_blockPtrArray[i],block_sizes_vector_.at(i)));
- state_block_ptr_vector_.push_back(_blockPtrArray[i]);
- }
- }
- }
-
- CorrespondenceSparse(WolfScalar* _measurementPtr,
- WolfScalar* _state0Ptr,
- WolfScalar* _state1Ptr = NULL,
- WolfScalar* _state2Ptr = NULL,
- WolfScalar* _state3Ptr = NULL,
- WolfScalar* _state4Ptr = NULL,
- WolfScalar* _state5Ptr = NULL,
- WolfScalar* _state6Ptr = NULL,
- WolfScalar* _state7Ptr = NULL,
- WolfScalar* _state8Ptr = NULL,
- WolfScalar* _state9Ptr = NULL ) :
- CorrespondenceBase(_measurementPtr),
- state_block_ptr_vector_({_state0Ptr,
- _state1Ptr,
- _state2Ptr,
- _state3Ptr,
- _state4Ptr,
- _state5Ptr,
- _state6Ptr,
- _state7Ptr,
- _state8Ptr,
- _state9Ptr}),
- block_sizes_vector_({BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE})
- {
- for (uint i = 0; i<block_sizes_vector_.size(); i++)
- {
- if (block_sizes_vector_.at(i) == 0)
- {
- block_sizes_vector_.resize(i);
- state_block_ptr_vector_.resize(i);
- break;
- }
- else
- {
- state_block_map_vector_.push_back(Map<VectorXs>(state_block_ptr_vector_.at(i),block_sizes_vector_.at(i)));
- //std::cout << "state " << i << ":" << std::endl;
- //for (uint j = 0; j<block_sizes_vector_.at(i); j++ )
- // std::cout << *(state_block_ptr_vector_.at(i)+j) << ", ";
- //std::cout << std::endl;
- }
- }
-
- //TODO: Check if while size OK, pointers NULL
- }
-
- virtual ~CorrespondenceSparse()
- {
- }
-
- virtual costFunctionType getCostFunctionType() const
- {
- return AUTO;
- }
-
- virtual const std::vector<WolfScalar *> getBlockPtrVector()
- {
- return state_block_ptr_vector_;
- }
-};
-
-class CorrespondenceGPS2D : public CorrespondenceSparse<2,2>
-{
- public:
- static const unsigned int N_BLOCKS = 1;
- const double stdev_ = 1;
-
- CorrespondenceGPS2D(WolfScalar* _measurementPtr, WolfScalar* _statePtr) :
- CorrespondenceSparse<2,2>(_measurementPtr, _statePtr)
- {
- }
-
- CorrespondenceGPS2D(WolfScalar* _measurementPtr, StateBase* _statePtr) :
- CorrespondenceSparse<2,2>(_measurementPtr, _statePtr->getPtr())
- {
- }
-
- virtual ~CorrespondenceGPS2D()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _x, T* _residuals) const
- {
- _residuals[0] = (T(*this->measurement_ptr_) - _x[0]) / T(stdev_);
- _residuals[1] = (T(*(this->measurement_ptr_+1)) - _x[1]) / T(stdev_);
-
- return true;
- }
-
- virtual costFunctionType getCostFunctionType() const
- {
- return AUTO;
- }
-};
-
-//class Correspondence2DRange : public CorrespondenceSparse<1,2,2>
-//{
-// public:
-// static const unsigned int N_BLOCKS = 2;
-//
-// Correspondence2DRange(WolfScalar** _blockPtrs) :
-// CorrespondenceSparse(_blockPtrs)
-// {
-// }
-//
-// Correspondence2DRange(WolfScalar* _block1Ptr, WolfScalar* _block2Ptr) :
-// CorrespondenceSparse(_block1Ptr, _block2Ptr)
-// {
-// }
-//
-//// Correspondence2DRange(StateBase* _state1Ptr, StateBase* _state2Ptr) :
-//// CorrespondenceSparse(_state1Ptr, _state2Ptr)
-//// {
-//// }
-//
-// ~Correspondence2DRange()
-// {
-// }
-//
-// template <typename T>
-// bool operator()(const T* const _x1, const T* const _x2, T* _residuals) const
-// {
-//
-// // Remap the vehicle state to the const evaluation point
-// Map<const Matrix<T,Dynamic,1>> x1_map_const(_x1, this->block1Size);
-// Map<const Matrix<T,Dynamic,1>> x2_map_const(_x2, this->block2Size);
-//
-// // Map residuals vector to matrix (with sizes of the measurements matrix)
-// Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, this->measurementSize);
-//
-// // Compute error or residuals
-// Matrix<T,Dynamic,1> expected_measurement = ((x1_map_const - x2_map_const).transpose() * (x1_map_const - x2_map_const)).cwiseSqrt();
-// VectorXd meas = this->measurement_;
-// mapped_residuals = (meas).cast<T>() - expected_measurement;
-//
-// return true;
-// }
-//
-// virtual costFunctionType getCostFunctionType() const
-// {
-// return AUTO;
-// }
-//};
-
-class Correspondence2DOdometry : public CorrespondenceSparse<2,2,2,2,2>
-{
- public:
- static const unsigned int N_BLOCKS = 4;
- const double stdev_ = 0.01; //model parameters
-
- Correspondence2DOdometry(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) :
- CorrespondenceSparse<2,2,2,2,2>(_measurementPtr, _blockPtrs)
- {
- }
-
- Correspondence2DOdometry(WolfScalar* _measurementPtr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr, WolfScalar* _block4Ptr) :
- CorrespondenceSparse<2,2,2,2,2>(_measurementPtr, _block1Ptr, _block2Ptr, _block3Ptr, _block4Ptr)
- {
- }
-
- Correspondence2DOdometry(WolfScalar* _measurementPtr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr, StateBase* _state4Ptr) :
- CorrespondenceSparse<2,2,2,2,2>(_measurementPtr, _state1Ptr->getPtr(), _state2Ptr->getPtr(),_state3Ptr->getPtr(), _state4Ptr->getPtr())
- {
- }
-
- virtual ~Correspondence2DOdometry()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // print inputs
- // std::cout << "_p1:" << std::endl;
- //for (int i=0; i < this->block0Size; i++)
- // std::cout << _p1[i] << std::endl;
- // std::cout << std::endl;
- // std::cout << "_o1:" << std::endl;
- //for (int i=0; i < this->block1Size; i++)
- // std::cout << _o1[i] << std::endl;
- //std::cout << std::endl;
- // std::cout << "_p2:" << std::endl;
- // for (int i=0; i < this->block2Size; i++)
- // std::cout << _p2[i] << std::endl;
- // std::cout << std::endl;
- //std::cout << "_o2:" << std::endl;
- //for (int i=0; i < this->block3Size; i++)
- // std::cout << _o2[i] << std::endl;
- //std::cout << std::endl;
- // std::cout << "measurement:" << std::endl;
- //for (int i=0; i < this->measurementSize; i++)
- // std::cout << *(this->measurement_ptr_+i) << std::endl;
- //std::cout << std::endl;
-
- // Expected measurement
- T expected_range = (_p1[0]-_p2[0])*(_p1[0]-_p2[0]) + (_p1[1]-_p2[1])*(_p1[1]-_p2[1]); //square of the range
- T expected_rotation = atan2(-_o1[0]*_o2[1] + _o1[1]*_o2[0], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
-
- // Residuals
- _residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_);
- _residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_);
-
- return true;
- }
-
- virtual costFunctionType getCostFunctionType() const
- {
- return AUTO;
- }
-};
-
-class Correspondence2DOdometryTheta : public CorrespondenceSparse<2,2,1,2,1>
-{
- public:
- static const unsigned int N_BLOCKS = 4;
- const double stdev_ = 0.01; //model parameters
-
- Correspondence2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _blockPtrs)
- {
- }
-
- Correspondence2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr, WolfScalar* _block4Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _block1Ptr, _block2Ptr, _block3Ptr, _block4Ptr)
- {
- }
-
- Correspondence2DOdometryTheta(WolfScalar* _measurementPtr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr, StateBase* _state4Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _state1Ptr->getPtr(), _state2Ptr->getPtr(),_state3Ptr->getPtr(), _state4Ptr->getPtr())
- {
- }
-
- virtual ~Correspondence2DOdometryTheta()
- {
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
- T expected_rotation = _o2[0]-_o1[0];
-
- // Residuals
- _residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_);
- _residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_);
-
- return true;
- }
-
- virtual costFunctionType getCostFunctionType() const
- {
- return AUTO;
- }
-};
-
-class Correspondence2DOdometryThetaNumeric : public CorrespondenceSparse<2,2,1,2,1>
-{
- public:
- static const unsigned int N_BLOCKS = 4;
- const double stdev_ = 0.01; //model parameters
-
- Correspondence2DOdometryThetaNumeric(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _blockPtrs)
- {
- }
-
- Correspondence2DOdometryThetaNumeric(WolfScalar* _measurementPtr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr, WolfScalar* _block4Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _block1Ptr, _block2Ptr, _block3Ptr, _block4Ptr)
- {
- }
-
- Correspondence2DOdometryThetaNumeric(WolfScalar* _measurementPtr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr, StateBase* _state4Ptr) :
- CorrespondenceSparse<2,2,1,2,1>(_measurementPtr, _state1Ptr->getPtr(), _state2Ptr->getPtr(),_state3Ptr->getPtr(), _state4Ptr->getPtr())
- {
- }
-
- virtual ~Correspondence2DOdometryThetaNumeric()
- {
- }
-
- bool operator()(const double* const _p1, const double* const _o1, const double* const _p2, const double* const _o2, double* _residuals) const
- {
- // Expected measurement
- double expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
- double expected_rotation = _o2[0]-_o1[0];
-
- // Residuals
- _residuals[0] = (expected_range - ((*this->measurement_ptr_)*(*this->measurement_ptr_))) / stdev_;
- _residuals[1] = (expected_rotation - (*(this->measurement_ptr_+1))) / stdev_;
-
- return true;
- }
-
- virtual costFunctionType getCostFunctionType() const
- {
- return NUMERIC;
- }
-};
-
-
-class WolfProblem
-{
- protected:
- std::vector<StateBase*> state_units_;
- std::vector<CorrespondenceBase*> correspondences_;
-
- public:
- WolfProblem() :
- state_units_(0),
- correspondences_(0)
- {
- }
-
- virtual ~WolfProblem()
- {
- }
-
- unsigned int getCorrespondencesSize()
- {
- return correspondences_.size();
- }
-
- unsigned int addCorrespondence(CorrespondenceBase* _corr_ptr)
- {
- correspondences_.push_back(_corr_ptr);
- return correspondences_.size()-1;
- }
-
- CorrespondenceBase* getCorrespondencePrt(unsigned int i)
- {
- return correspondences_.at(i);
- }
-
- unsigned int addStateUnit(StateBase* _st_ptr)
- {
- //std::cout << "Adding state unit to the wolf list..." << std::endl;
- state_units_.push_back(_st_ptr);
- //std::cout << "Added!" << std::endl;
- return state_units_.size()-1;
- }
-
- StateBase* getStateUnitPtr(unsigned int i)
- {
- return state_units_.at(i);
- }
-};
-
-class CeresManager
-{
- protected:
-
- std::vector<std::pair<ceres::ResidualBlockId, CorrespondenceBase*>> correspondence_list_;
- ceres::Problem ceres_problem_;
-
- public:
- CeresManager()
- {
- }
-
- ~CeresManager()
- {
- }
-
- ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options)
- {
- // create summary
- ceres::Solver::Summary ceres_summary_;
-
- // run Ceres Solver
- ceres::Solve(_ceres_options, &ceres_problem_, &ceres_summary_);
-
- //display results
- return ceres_summary_;
- }
-
- template <class CorrespondenceType>
- void addCorrespondence(CorrespondenceType* _corr_ptr)
- {
- ceres::ResidualBlockId blockIdx = ceres_problem_.AddResidualBlock(createCostFunction<CorrespondenceType>(_corr_ptr), NULL, _corr_ptr->getBlockPtrVector());
- correspondence_list_.push_back(std::pair<ceres::ResidualBlockId, CorrespondenceBase*>(blockIdx,_corr_ptr));
- }
-
- template <class StateUnitType>
- void addStateUnit(StateUnitType* _st_ptr)
- {
- //std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getParametrizationType())
- {
- case COMPLEX_ANGLE:
- {
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_.SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
- ceres_problem_.AddParameterBlock(_st_ptr->getPtr(), _st_ptr->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
-// case QUATERNION:
-// {
-// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
-// local_parametrization_list_.push_back(LocalParametrizationWrapper{_st_ptr, new EigenQuaternionParameterization});
-// ceres_problem_.SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
-// break;
-// }
- case NONE:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- }
-
- template <typename CorrespondenceDerived>
- ceres::CostFunction* createCostFunction(CorrespondenceDerived* _corrPtr)
- {
- switch (_corrPtr->getCostFunctionType())
- {
- case AUTO:
- {
- return new ceres::AutoDiffCostFunction<CorrespondenceDerived,
- _corrPtr->measurementSize,
- _corrPtr->block0Size,
- _corrPtr->block1Size,
- _corrPtr->block2Size,
- _corrPtr->block3Size,
- _corrPtr->block4Size,
- _corrPtr->block5Size,
- _corrPtr->block6Size,
- _corrPtr->block7Size,
- _corrPtr->block8Size,
- _corrPtr->block9Size>(_corrPtr);
- break;
- }
- case NUMERIC:
- {
- return new ceres::NumericDiffCostFunction<CorrespondenceDerived,
- ceres::CENTRAL,
- _corrPtr->measurementSize,
- _corrPtr->block0Size,
- _corrPtr->block1Size,
- _corrPtr->block2Size,
- _corrPtr->block3Size,
- _corrPtr->block4Size,
- _corrPtr->block5Size,
- _corrPtr->block6Size,
- _corrPtr->block7Size,
- _corrPtr->block8Size,
- _corrPtr->block9Size>(_corrPtr);
- break;
- }
- default:
- std::cout << "Unknown cost function type!" << std::endl;
- return NULL;
- }
- }
-};
-
-int main(int argc, char** argv)
-{
- clock_t t1, t2;
- t1=clock();
-
- std::cout << " ========= 2D Robot with odometry and GPS ===========" << std::endl << std::endl;
-
- //user input
- if (argc!=4)
- {
- std::cout << "Please call me with: [./test_ceres_wrapper_states NI PRINT ORIENTATION_MODE], where:" << std::endl;
- std::cout << " - NI is the number of iterations" << std::endl;
- std::cout << " - PRINT = 1 for print results" << std::endl;
- std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
- bool print = (bool) atoi(argv[2]);
- bool complex_angle = (bool) atoi(argv[3]);
-
- //init google log
- google::InitGoogleLogging(argv[0]);
-
- // wolf problem
- WolfProblem* wolf_problem = new WolfProblem;
-
- //variables
- int dim = (complex_angle ? 4 : 3);
- Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion
- Eigen::VectorXs pose_true(dim); //current true pose
- Eigen::VectorXs ground_truth(n_execution*dim); //all true poses
- Eigen::VectorXs pose_predicted(dim); // current predicted pose
- Eigen::VectorXs predicted_trajectory(n_execution*dim); // current predicted pose
- Eigen::VectorXs state(n_execution*dim); //running window winth solver result
- Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
- Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
- std::vector<StateBase> state_vector_; // vector of state units
- std::vector<StateBase*> state_ptr_vector_; // vector of pointers to state units
- int id_p, id_o, id_p_prev, id_o_prev;
-
- //init true odom and true pose
- for (unsigned int ii = 0; ii<n_execution; ii++)
- {
- if ( ii < (unsigned int)floor(n_execution/2) )
- odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments.
- else
- odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments.
- }
- if (complex_angle)
- {
- pose_true << 0,0,1,0;
- pose_predicted << 0,0,1,0;
- }
- else
- {
- pose_true << 0,0,0;
- pose_predicted << 0,0,0;
- }
- ground_truth.head(dim) << pose_true; //init point pushed to ground truth
- state.head(dim) << pose_predicted; //init state at origin
- predicted_trajectory.head(dim) << pose_predicted;
-
- //init random generators
- std::default_random_engine generator(1);
- std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
- std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
-
- // TODO: incorporar weights a les funcions residu (via LossFunction o directament a operador())
-
- // Ceres problem initialization
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
-// ceres_options.minimizer_progress_to_stdout = false;
-// ceres_options.line_search_direction_type = ceres::LBFGS;
-// ceres_options.max_num_iterations = 2;
- CeresManager ceres_wrapper;
- std::ofstream log_file; //output file
-
- // Start trajectory
- id_p = wolf_problem->addStateUnit(new StatePoint2D(state.data()));
- ceres_wrapper.addStateUnit<StatePoint2D>((StatePoint2D*)(wolf_problem->getStateUnitPtr(id_p)));
- state_ptr_vector_.push_back(wolf_problem->getStateUnitPtr(id_p));
- if (complex_angle)
- {
- id_o = wolf_problem->addStateUnit(new StateComplexAngle(state.data()+2));
- ceres_wrapper.addStateUnit<StateComplexAngle>((StateComplexAngle*)(wolf_problem->getStateUnitPtr(id_o)));
- state_ptr_vector_.push_back(wolf_problem->getStateUnitPtr(id_o));
- }
- else
- {
- id_o = wolf_problem->addStateUnit(new StateThetaAngle(state.data()+2));
- ceres_wrapper.addStateUnit<StateThetaAngle>((StateThetaAngle*)(wolf_problem->getStateUnitPtr(id_o)));
- state_ptr_vector_.push_back(wolf_problem->getStateUnitPtr(id_o));
- }
-
- for (uint step=1; step < n_execution; step++)
- {
- // NEW STATE
- //inventing a simple motion
- if (complex_angle)
- {
- double new_pose_true_2 = pose_true(2) * cos(odom_inc_true(step*2+1)) - pose_true(3) * sin(odom_inc_true(step*2+1));
- double new_pose_true_3 = pose_true(2) * sin(odom_inc_true(step*2+1)) + pose_true(3) * cos(odom_inc_true(step*2+1));
- pose_true(0) = pose_true(0) + odom_inc_true(step*2) * new_pose_true_2;
- pose_true(1) = pose_true(1) + odom_inc_true(step*2) * new_pose_true_3;
- pose_true(2) = new_pose_true_2;
- pose_true(3) = new_pose_true_3;
- }
- else
- {
- double new_pose_true_2 = pose_true(2) + (odom_inc_true(step*2+1));
- pose_true(0) = pose_true(0) + odom_inc_true(step*2) * cos(new_pose_true_2);
- pose_true(1) = pose_true(1) + odom_inc_true(step*2) * sin(new_pose_true_2);
- pose_true(2) = new_pose_true_2;
- }
-
- //inventing sensor readings for odometry and GPS
- odom_readings.segment(step*2,2) << odom_inc_true(step*2)+distribution_odom(generator), odom_inc_true(step*2+1)+distribution_odom(generator); //true range and theta with noise
- gps_fix_readings.segment(step*3,3) << pose_true(0) + distribution_gps(generator), pose_true(1) + distribution_gps(generator), 0. + distribution_gps(generator);
-
- //setting initial guess as an odometry prediction, using noisy odometry
- if (complex_angle)
- {
- double new_pose_predicted_2 = pose_predicted(2) * cos(odom_readings(step*2+1)) - pose_predicted(3) * sin(odom_readings(step*2+1));
- double new_pose_predicted_3 = pose_predicted(2) * sin(odom_readings(step*2+1)) + pose_predicted(3) * cos(odom_readings(step*2+1));
- pose_predicted(0) = pose_predicted(0) + odom_readings(step*2) * new_pose_predicted_2;
- pose_predicted(1) = pose_predicted(1) + odom_readings(step*2) * new_pose_predicted_3;
- pose_predicted(2) = new_pose_predicted_2;
- pose_predicted(3) = new_pose_predicted_3;
- }
- else
- {
- double new_pose_predicted_2 = pose_predicted(2) + (odom_readings(step*2+1));
- pose_predicted(0) = pose_predicted(0) + odom_readings(step*2) * cos(new_pose_predicted_2);
- pose_predicted(1) = pose_predicted(1) + odom_readings(step*2) * sin(new_pose_predicted_2);
- pose_predicted(2) = new_pose_predicted_2;
- }
- predicted_trajectory.segment(step*dim,dim) = pose_predicted;
-
- // store
- state.segment(step*dim,dim) << pose_predicted;
- ground_truth.segment(step*dim,dim) << pose_true;
-
- // STATE UNITS
- id_p_prev = id_p;
- id_o_prev = id_o;
- // p
- id_p = wolf_problem->addStateUnit(new StatePoint2D(state.data() + step * dim));
- ceres_wrapper.addStateUnit<StatePoint2D>((StatePoint2D*)(wolf_problem->getStateUnitPtr(id_p)));
- state_ptr_vector_.push_back(wolf_problem->getStateUnitPtr(id_p));
- // o
- if (complex_angle)
- {
- id_o = wolf_problem->addStateUnit(new StateComplexAngle(state.data() + step * dim + 2));
- ceres_wrapper.addStateUnit<StateComplexAngle>((StateComplexAngle*)(wolf_problem->getStateUnitPtr(id_o)));
- state_ptr_vector_.push_back(wolf_problem->getStateUnitPtr(id_o));
- }
- else
- {
- id_o = wolf_problem->addStateUnit(new StateThetaAngle(state.data() + step * dim + 2));
- ceres_wrapper.addStateUnit<StateThetaAngle>((StateThetaAngle*)(wolf_problem->getStateUnitPtr(id_o)));
- state_ptr_vector_.push_back(wolf_problem->getStateUnitPtr(id_o));
- }
-// std::cout << "New state ptr in the vector: " << std::endl;
-// for (int j = 0; j < state_ptr_vector_.size(); j++)
-// {
-// std::cout << j << ":" << std::endl;
-// state_ptr_vector_.at(j)->print();
-// }
-
- // CORRESPONDENCE ODOMETRY
- if (complex_angle)
- {
-
- // int odomIdx = wolf_manager.addOdomMeasurement(odom_readings.data() + step*2);
- // Correspondence2DOdometry* odomCorrPtr = wolf_manager.getCorrespondence(odomIdx);
- // ceres_wrapper.addCorrespondence<Correspondence2DOdometry>(odomCorrPtr, odomCorrPtr->getP1(), odomCorrPtr->getO1(), odomCorrPtr->getP2(), odomCorrPtr->getO2());
-
- uint id_corr = wolf_problem->addCorrespondence(new Correspondence2DOdometry(odom_readings.data() + step*2,
- wolf_problem->getStateUnitPtr(id_p_prev),
- wolf_problem->getStateUnitPtr(id_o_prev),
- wolf_problem->getStateUnitPtr(id_p),
- wolf_problem->getStateUnitPtr(id_o)));
- ceres_wrapper.addCorrespondence<Correspondence2DOdometry>((Correspondence2DOdometry*)wolf_problem->getCorrespondencePrt(id_corr));
- }
- else
- {
- uint id_corr = wolf_problem->addCorrespondence(new Correspondence2DOdometryTheta(odom_readings.data() + step*2,
- wolf_problem->getStateUnitPtr(id_p_prev),
- wolf_problem->getStateUnitPtr(id_o_prev),
- wolf_problem->getStateUnitPtr(id_p),
- wolf_problem->getStateUnitPtr(id_o)));
-
- ceres_wrapper.addCorrespondence<Correspondence2DOdometryTheta>((Correspondence2DOdometryTheta*)wolf_problem->getCorrespondencePrt(id_corr));
- }
-
- // CORRESPONDENCE GPS (2D)
- uint id_corr = wolf_problem->addCorrespondence(new CorrespondenceGPS2D(gps_fix_readings.data() + step*3, wolf_problem->getStateUnitPtr(id_p)));
- ceres_wrapper.addCorrespondence<CorrespondenceGPS2D>((CorrespondenceGPS2D*)wolf_problem->getCorrespondencePrt(id_corr));
-
- // SOLVE CERES PROBLEM
- //ceres::Solver::Summary summary = ceres_wrapper.solve();
- if (print)
- {
- std::cout << " ========= STEP " << step << "===========" << std::endl << std::endl;
- std::cout << "odom : " << odom_inc_true.segment(step*2,2).transpose() << std::endl << std::endl;
- std::cout << "pose_predicted : " << pose_predicted.transpose() << std::endl << std::endl;
- std::cout << "pose_true : " << pose_true.transpose() << std::endl << std::endl;
- std::cout << "gps measurement : " << gps_fix_readings.segment(step*3,3).transpose() << std::endl << std::endl;
- std::cout << "state : " << std::endl << state.head(step*dim).transpose() << std::endl;
- std::cout << "ground_truth : " << std::endl << ground_truth.head(step*dim).transpose() << std::endl;
- std::cout << "error : " << std::endl << (ground_truth.head(step*dim) - state.head(step*dim)).transpose() << std::endl << std::endl;
- //std::cout << "total time (s):" << summary.total_time_in_seconds << std::endl;
- }
- }
-
- ceres::Solver::Summary summary = ceres_wrapper.solve(ceres_options);
- std::cout << summary.FullReport() << std::endl;
-
- t2=clock();
- double seconds = ((double)t2-t1)/CLOCKS_PER_SEC;
- std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl;
- std::cout << "total seconds: " << seconds << std::endl;
-
- //display/log results, by setting cout flags properly
- VectorXs state_theta(n_execution * 3);
- VectorXs ground_truth_theta(n_execution * 3);
- VectorXs predicted_trajectory_theta(n_execution * 3);
- if (complex_angle)
- {
- // change from complex angle to theta
- for (uint ii = 0; ii<n_execution; ii++)
- {
- state_theta.segment(ii*3,3) << state(ii*4), state(ii*4+1), atan2(state(ii*4+2), state(ii*4+3));
- ground_truth_theta.segment(ii*3,3) << ground_truth(ii*4), ground_truth(ii*4+1), atan2(ground_truth(ii*4+2), ground_truth(ii*4+3));
- predicted_trajectory_theta.segment(ii*3,3) << predicted_trajectory(ii*4), predicted_trajectory(ii*4+1), atan2(predicted_trajectory(ii*4+2), predicted_trajectory(ii*4+3));
- }
- }
- else
- {
- state_theta = state;
- ground_truth_theta = ground_truth;
- predicted_trajectory_theta = predicted_trajectory;
- }
- std::string homepath = getenv("HOME");
- log_file.open(homepath + "/Desktop/log_file_2.txt", std::ofstream::out); //open log file
- if (log_file.is_open())
- {
- log_file << seconds << std::endl;
- for (unsigned int ii = 0; ii<n_execution; ii++)
- log_file << state_theta.segment(ii*3,3).transpose()
- << " " << ground_truth_theta.segment(ii*3,3).transpose()
- << " " << (state_theta.segment(ii*3,3)-ground_truth_theta.segment(ii*3,3)).transpose()
- << " " << gps_fix_readings.segment(ii*3,3).transpose()
- << " " << predicted_trajectory_theta.segment(ii*3,3).transpose() << std::endl;
- log_file.close(); //close log file
- std::cout << std::endl << " Result file ~/Desktop/log_data.txt" << std::endl;
- }
- else
- std::cout << std::endl << " Failed to write the file ~/Desktop/log_data.txt" << std::endl;
- //end Wolf iteration
- std::cout << " ========= END ===========" << std::endl << std::endl;
-
- //exit
- return 0;
-}
-
diff --git a/src/examples/test_node_linked.cpp b/src/examples/test_node_linked.cpp
deleted file mode 100644
index 3c0fff955..000000000
--- a/src/examples/test_node_linked.cpp
+++ /dev/null
@@ -1,147 +0,0 @@
-//wolf
-#include "node_terminus.h"
-#include "node_linked.h"
-
-//namespaces
-using namespace std;
-
-//id count init
-// unsigned int NodeBase::node_id_count_ = 0;
-
-//forward declarations
-class TrajectoryN;
-class FrameN;
-class MeasurementN;
-
-//class TrajectoryN
-class TrajectoryN : public NodeLinked<NodeTerminus,FrameN>
-{
- protected:
- unsigned int length_; //just something to play
-
- public:
- TrajectoryN(const unsigned int _len) :
- NodeLinked(TOP, "TRAJECTORY"),
- length_(_len)
- {
- //
- };
-
- ~TrajectoryN()
- {
-
- };
-
-// virtual void printLabel(ostream & _ost = cout) const
-// {
-// _ost <<"TRAJECTORY";
-// }
-};
-
-//class FrameN
-class FrameN : public NodeLinked<TrajectoryN,MeasurementN>
-{
- protected:
- double time_stamp_; //just something to play
-
- public:
- FrameN(double _ts) :
- NodeLinked(MID, "FRAME"),
- time_stamp_(_ts)
- {
- //
- };
-
- ~FrameN()
- {
-
- };
-
-// virtual void printLabel(ostream & _ost = cout) const
-// {
-// _ost <<"FRAME";
-// }
-
-};
-
-//class MeasurementN
-class MeasurementN : public NodeLinked<FrameN,NodeTerminus>
-{
- protected:
- unsigned int size_; //just something to play
-
- public:
- MeasurementN(const unsigned int _sz) :
- NodeLinked(BOTTOM, "MEASUREMENT"),
- size_(_sz)
- {
- //
- };
-
- ~MeasurementN()
- {
-
- };
-
-// virtual void printLabel(ostream & _ost = cout) const
-// {
-// _ost <<"MEASUREMENT";
-// }
-
-};
-
-
-int main()
-{
- cout << endl << "Node class test" << endl;
- cout << "========================================================" << endl;
-
- cout << endl << "TEST 1. Constructors" << endl;
- shared_ptr<TrajectoryN> trajectory_(new TrajectoryN(2));
- shared_ptr<FrameN> frame_1_(new FrameN(1.011));
- shared_ptr<FrameN> frame_2_(new FrameN(2.022));
- shared_ptr<MeasurementN> sensor_data_cam_1_(new MeasurementN(640));
- shared_ptr<MeasurementN> sensor_data_laser_(new MeasurementN(180));
- shared_ptr<MeasurementN> sensor_data_cam_2_(new MeasurementN(480));
- shared_ptr<MeasurementN> sensor_data_radar_(new MeasurementN(90));
- trajectory_->print();
- cout << "========================================================" << endl;
-
- cout << endl << "TEST 2. Build tree dependencies" << endl;
- frame_1_->addDownNode(sensor_data_cam_1_);
- frame_1_->addDownNode(sensor_data_laser_);
- frame_2_->addDownNode(sensor_data_cam_2_);
- trajectory_->addDownNode(frame_1_);
- trajectory_->addDownNode(frame_2_);
- trajectory_->print();
- cout << "========================================================" << endl;
-
- cout << endl << "TEST 3. Modify one of the nodes (add new node), once tree has been constructed" << endl;
- frame_2_->addDownNode(sensor_data_radar_);
- trajectory_->print();
- cout << "========================================================" << endl;
-
- cout << endl << "TEST 4. Remove nodes" << endl;
- //check if resetting ptr previously, effectively removes object when calling removeDownNode()
- unsigned int f1_id_ = frame_1_->nodeId();
- frame_1_.reset();
- sensor_data_cam_1_.reset();
- sensor_data_laser_.reset();
- trajectory_->removeDownNode(f1_id_);
- trajectory_->print();
- cout << "========================================================" << endl;
-
- cout << endl << "TEST 5. getTop()" << endl;
- NodeBase* nb_ptr = sensor_data_radar_->getTop();
- //shared_ptr<TrajectoryN> nb_shptr((TrajectoryN*)nb_ptr);
- cout << "TOP node is: " << nb_ptr->nodeId() << endl;
- //cout << "nb_shptr.use_count(): " << nb_shptr.use_count() << "; value: " << nb_shptr.get() << endl;
- //cout << "trajectory_.use_count(): " << trajectory_.use_count() << "; value: " << trajectory_.get() << endl;
- //nb_shptr.reset();
- //COMMENTS: It seems that if shared pointer is used here, since type is NodeBase, there is no sharing ownership with trajectory_, which has type TrajectoryN, so the program crashes at the end: two shared pointers, pointing to the same object but each one with use_count()=1"
- cout << "========================================================" << endl;
-
- cout << endl << "End NodeLinked test" << endl;
- return 0;
-}
-
diff --git a/src/examples/test_wolf_tree.cpp b/src/examples/test_wolf_tree.cpp
deleted file mode 100644
index 7dfb31390..000000000
--- a/src/examples/test_wolf_tree.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-// Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//ceres
-#include "ceres/ceres.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "capture_base.h"
-#include "sensor_base.h"
-
-
-int main(int argc, char** argv)
-{
- //wolf manager variables
- std::queue<FrameBase> trajectory_; //this will be owned by the wolf manager
- std::list<CaptureBaseShPtr> pending_captures_; //this will be owned by the wolf manager
- std::list<CaptureBaseShPtr>::iterator pending_it_; //this will be owned by the wolf manager
- Eigen::VectorXs sp(6);
- sp << 0.1,0.1,0.1,0,0,0;
- SensorBase sensor1(ABSOLUTE_POSE,sp); //just one sensor. This will be owned by the wolf manager
- sp << 0.2,0.2,0.2,0,0,0;
- SensorBase sensor2(ABSOLUTE_POSE,sp); //just another sensor. This will be owned by the wolf manager
-
- //ROS callbacks varaibles (will be get from message)
- TimeStamp ros_ts; //this plays the role of ros::Time, or msg->header.stamp
- Eigen::VectorXs sensor_reading(4); //this plays the role of the ROS message content (sensor reading). Reading of dim=4 (example)
-
- //Welcome message
- std::cout << std::endl << " ========= WOLF TREE test ===========" << std::endl << std::endl;
-
- //main loop
- for (unsigned int ii = 0; ii<10; ii++)
- {
- //1. a new sensor data arrives (this part will be placed on ROS callbacks)
- ros_ts.setToNow();
- sensor_reading << 1,2,3,4;
- std::shared_ptr<CaptureBase> capture( new CaptureBase(ros_ts.get(), &sensor1, sensor_reading) );
- pending_captures_.push_back(capture);
-
- //2. Process pending_captures_, deciding for each new capture wheter a Frame has to be created or they have to be linked to the last one
- for (pending_it_ = pending_captures_.begin(); pending_it_ != pending_captures_.end(); ++pending_it_)
- {
- capture->processCapture(); //This should create features
-
- }
-
- //3. Stablish correspondences
-
- //4. Call ceres solver
-
- //5. publish results
-
- }
-
- //End message
- std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
- return 0;
-}
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
deleted file mode 100644
index 201e265df..000000000
--- a/src/feature_base.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-#include "feature_base.h"
-
-FeatureBase::FeatureBase(const CaptureBasePtr& _capt_ptr, unsigned int _dim_measurement) :
- NodeLinked(MID, "FEATURE", _capt_ptr),
- measurement_(_dim_measurement)
-{
- //
-}
-
-//FeatureBase::FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
-// NodeLinked(MID, "FEATURE", _capt_ptr),
-// measurement_(_measurement)
-//{
-// //
-//}
-
-FeatureBase::FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- NodeLinked(MID, "FEATURE", _capt_ptr),
- measurement_(_measurement),
- measurement_covariance_(_meas_covariance)
-{
- //
-}
-
-FeatureBase::~FeatureBase()
-{
- //
-}
-
-//inline void FeatureBase::linkToCapture(const CaptureBaseShPtr& _capt_ptr)
-//{
-// linkToUpperNode(_capt_ptr.get());
-//}
-
-void FeatureBase::addCorrespondence(CorrespondenceBaseShPtr& _co_ptr)
-{
- addDownNode(_co_ptr);
-}
-
-const CaptureBasePtr FeatureBase::getCapturePtr() const
-{
- return upperNodePtr();
-}
-
-const FrameBasePtr FeatureBase::getFramePtr() const
-{
- return upperNodePtr()->upperNodePtr();
-}
-
-// inline const CorrespondenceBaseList & FeatureBase::getCorrespondenceList() const
-// {
-// return downNodeList();
-// }
-
-CorrespondenceBaseList* FeatureBase::getCorrespondenceListPtr()
-{
- return getDownNodeListPtr();
-}
-
-Eigen::VectorXs * FeatureBase::getMeasurementPtr()
-{
- return &measurement_;
-}
-
-Eigen::MatrixXs * FeatureBase::getMeasurementCovariancePtr()
-{
- return &measurement_covariance_;
-}
-
-void FeatureBase::setMeasurement(const Eigen::VectorXs & _meas)
-{
- measurement_ = _meas;
-}
-
-void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
-{
- measurement_covariance_ = _meas_cov;
-}
-
-void FeatureBase::printSelf(unsigned int _ntabs, std::ostream & _ost) const
-{
- NodeLinked::printSelf(_ntabs, _ost);
- printTabs(_ntabs);
- _ost << "\tMeasurement: ( " << measurement_.transpose() << " )" << std::endl;
- printTabs(_ntabs);
- _ost << "\tMeasurement covariance: ( " << measurement_covariance_ << " )" << std::endl;
-}
diff --git a/src/feature_base.h b/src/feature_base.h
deleted file mode 100644
index 912e1b564..000000000
--- a/src/feature_base.h
+++ /dev/null
@@ -1,80 +0,0 @@
-#ifndef FEATURE_BASE_H_
-#define FEATURE_BASE_H_
-
-// Forward declarations for node templates
-class CaptureBase;
-class CorrespondenceBase;
-
-//std includes
-
-//Wolf includes
-#include "wolf.h"
-#include "node_linked.h"
-#include "capture_base.h"
-#include "correspondence_base.h"
-
-//class FeatureBase
-class FeatureBase : public NodeLinked<CaptureBase,CorrespondenceBase>
-{
- protected:
- Eigen::VectorXs measurement_;
- Eigen::MatrixXs measurement_covariance_; ///< Noise of the measurement
-
- public:
- /** \brief Constructor from capture pointer and measure dim
- *
- * \param _capt_ptr a pointer to the Capture up node
- * \param _dim_measurement the dimension of the measurement space
- *
- */
- FeatureBase(const CaptureBasePtr& _capt_ptr, unsigned int _dim_measurement);
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _capt_ptr a pointer to the Capture up node
- * \param _measurement the measurement
- *
- */
- // measurement ha d'anar amb covariance, si cal, posem Identity com a default...
- //FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _capt_ptr a shared pointer to the Capture up node
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
-
- virtual ~FeatureBase();
-
- //void linkToCapture(const CaptureBaseShPtr& _capt_ptr); //JVN: inútil, el constructor ja crea el feature penjant d'una captura
-
- void addCorrespondence(CorrespondenceBaseShPtr& _co_ptr);
-
- const CaptureBasePtr getCapturePtr() const;
-
- const FrameBasePtr getFramePtr() const;
-
-// const CorrespondenceBaseList & getCorrespondenceList() const;
-
- CorrespondenceBaseList* getCorrespondenceListPtr();
-
- Eigen::VectorXs * getMeasurementPtr();
-
- Eigen::MatrixXs * getMeasurementCovariancePtr();
-
- void setMeasurement(const Eigen::VectorXs & _meas);
-
- void setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov);
-
- /** \brief prints object's info
- *
- * prints object's info
- *
- **/
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-
-};
-#endif
diff --git a/src/feature_gps_fix.cpp b/src/feature_gps_fix.cpp
deleted file mode 100644
index 999138773..000000000
--- a/src/feature_gps_fix.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#include "feature_gps_fix.h"
-
-FeatureGPSFix::FeatureGPSFix(const CaptureBasePtr& _capt_ptr, unsigned int _dim_measurement) :
- FeatureBase(_capt_ptr, _dim_measurement)
-{
- //
-}
-
-//FeatureGPSFix::FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
-// FeatureBase(_capt_ptr, _measurement)
-//{
-// //
-//}
-
-FeatureGPSFix::FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase(_capt_ptr, _measurement, _meas_covariance)
-{
- //
-}
-
-FeatureGPSFix::~FeatureGPSFix()
-{
- //
-}
-
-void FeatureGPSFix::findCorrespondences()
-{
- CorrespondenceBaseShPtr gps_correspondence(new CorrespondenceGPS2D(this, getCapturePtr()->getFramePtr()->getPPtr()));
- addCorrespondence(gps_correspondence);
-}
diff --git a/src/feature_gps_fix.h b/src/feature_gps_fix.h
deleted file mode 100644
index 19b99b794..000000000
--- a/src/feature_gps_fix.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef FEATURE_GPS_FIX_H_
-#define FEATURE_GPS_FIX_H_
-
-//std includes
-
-//Wolf includes
-#include "feature_base.h"
-#include "correspondence_gps_2D.h"
-
-//class FeatureGPSFix
-class FeatureGPSFix : public FeatureBase
-{
- public:
- /** \brief Constructor from capture pointer and measure dim
- *
- * \param _capt_ptr a shared pointer to the Capture up node
- * \param _dim_measurement the dimension of the measurement space
- *
- */
- FeatureGPSFix(const CaptureBasePtr& _capt_ptr, unsigned int _dim_measurement);
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _capt_ptr a shared pointer to the Capture up node
- * \param _measurement the measurement
- *
- */
- //FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _capt_ptr a shared pointer to the Capture up node
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
-
- virtual ~FeatureGPSFix();
-
- /** \brief Generic interface to find correspondences
- *
- * TBD
- * Generic interface to find correspondences between this feature and a map (static/slam) or a previous feature
- *
- **/
- virtual void findCorrespondences();
-
-};
-#endif
diff --git a/src/feature_odom_2D.cpp b/src/feature_odom_2D.cpp
deleted file mode 100644
index b68c08386..000000000
--- a/src/feature_odom_2D.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-#include "feature_odom_2D.h"
-
-FeatureOdom2D::FeatureOdom2D(const CaptureBasePtr& _capt_ptr, unsigned int _dim_measurement) :
- FeatureBase(_capt_ptr, _dim_measurement)
-{
- //
-}
-
-//FeatureOdom2D::FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
-// FeatureBase(_capt_ptr, _measurement)
-//{
-// //
-//}
-
-FeatureOdom2D::FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase(_capt_ptr, _measurement, _meas_covariance)
-{
- //
-}
-
-FeatureOdom2D::~FeatureOdom2D()
-{
- //
-}
-
-void FeatureOdom2D::findCorrespondences()
-{
-
-}
diff --git a/src/feature_odom_2D.h b/src/feature_odom_2D.h
deleted file mode 100644
index d958216d9..000000000
--- a/src/feature_odom_2D.h
+++ /dev/null
@@ -1,51 +0,0 @@
-#ifndef FEATURE_ODOM_2D_H_
-#define FEATURE_ODOM_2D_H_
-
-//std includes
-
-//Wolf includes
-#include "feature_base.h"
-#include "correspondence_odom_2D_theta.h"
-#include "correspondence_odom_2D_complex_angle.h"
-
-//class FeatureOdom2D
-class FeatureOdom2D : public FeatureBase
-{
- public:
- /** \brief Constructor from capture pointer and measure dim
- *
- * \param _capt_ptr a shared pointer to the Capture up node
- * \param _dim_measurement the dimension of the measurement space
- *
- */
- FeatureOdom2D(const CaptureBasePtr& _capt_ptr, unsigned int _dim_measurement);
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _capt_ptr a shared pointer to the Capture up node
- * \param _measurement the measurement
- *
- */
- //FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _capt_ptr a shared pointer to the Capture up node
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
-
- virtual ~FeatureOdom2D();
-
- /** \brief Generic interface to find correspondences
- *
- * TBD
- * Generic interface to find correspondences between this feature and a map (static/slam) or a previous feature
- *
- **/
- virtual void findCorrespondences();
-
-};
-#endif
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
deleted file mode 100644
index 1f878f671..000000000
--- a/src/frame_base.cpp
+++ /dev/null
@@ -1,161 +0,0 @@
-
-#include "frame_base.h"
-
-FrameBase::FrameBase(const TrajectoryBasePtr & _traj_ptr, const TimeStamp& _ts, const StateBaseShPtr& _p_ptr, const StateBaseShPtr& _o_ptr, const StateBaseShPtr& _v_ptr, const StateBaseShPtr& _w_ptr) :
- NodeLinked(MID, "FRAME", _traj_ptr),
- type_(REGULAR_FRAME),
- time_stamp_(_ts),
- p_ptr_(_p_ptr),
- o_ptr_(_o_ptr),
- v_ptr_(_v_ptr),
- w_ptr_(_w_ptr)
-{
- //
-}
-
-FrameBase::FrameBase(const TrajectoryBasePtr & _traj_ptr, const FrameType & _tp, const TimeStamp& _ts, const StateBaseShPtr& _p_ptr, const StateBaseShPtr& _o_ptr, const StateBaseShPtr& _v_ptr, const StateBaseShPtr& _w_ptr) :
- NodeLinked(MID, "FRAME", _traj_ptr),
- type_(_tp),
- time_stamp_(_ts),
- p_ptr_(_p_ptr),
- o_ptr_(_o_ptr),
- v_ptr_(_v_ptr),
- w_ptr_(_w_ptr)
-{
- //
-}
-
-FrameBase::~FrameBase()
-{
- //
-}
-
-inline bool FrameBase::isKey() const
-{
- if ( type_ == KEY_FRAME ) return true;
- else return false;
-}
-
-inline void FrameBase::setType(FrameType _ft)
-{
- type_ = _ft;
-}
-
-inline void FrameBase::setTimeStamp(const TimeStamp & _ts)
-{
- time_stamp_ = _ts;
-}
-
-TimeStamp FrameBase::getTimeStamp() const
-{
- return time_stamp_.get();
-}
-
-inline void FrameBase::getTimeStamp(TimeStamp & _ts) const
-{
- _ts = time_stamp_.get();
-}
-
-void FrameBase::setState(const Eigen::VectorXs& _st)
-{
- assert(_st.size() == ((!p_ptr_ ? 0 : p_ptr_->getStateSize()) +
- (!o_ptr_ ? 0 : o_ptr_->getStateSize()) +
- (!v_ptr_ ? 0 : v_ptr_->getStateSize()) +
- (!w_ptr_ ? 0 : w_ptr_->getStateSize())) &&
- "In FrameBase::setState wrong state size");
- assert(!!p_ptr_ && "in FrameBase::setState(), p_ptr_ is nullptr");
-
- Eigen::Map<Eigen::VectorXs> state_map(p_ptr_->getPtr(), _st.size());
- state_map = _st;
-}
-
-void FrameBase::addCapture(CaptureBaseShPtr & _capt_ptr)
-{
- addDownNode(_capt_ptr);
-}
-
-inline const TrajectoryBasePtr FrameBase::getTrajectoryPtr() const
-{
- return upperNodePtr();
-}
-
-// inline const CaptureBaseList & FrameBase::captureList() const
-// {
-// return downNodeList();
-// }
-
-CaptureBaseList* FrameBase::getCaptureListPtr()
-{
- return getDownNodeListPtr();
-}
-
-FrameBase* FrameBase::getPreviousFrame() const
-{
- std::list<FrameBaseShPtr>::iterator f_it;
- std::list<FrameBaseShPtr>* f_list_ptr = this->up_node_ptr_->getFrameListPtr();
-
- //look for the position of this node in the upper list (frame list of trajectory)
- for ( f_it = f_list_ptr->begin(); f_it != f_list_ptr->end(); ++f_it )
- {
- if ( this->node_id_ == (f_it->get())->nodeId() ){
- f_it--;
- return f_it->get();
- }
- }
- std::cout << "previous frame not found!" << std::endl;
- return nullptr;
-}
-
-//inline const Eigen::Vector3s & FrameBase::state() const
-//{
-// return state_;
-//}
-
-StateBaseShPtr FrameBase::getPPtr() const
-{
- return p_ptr_;
-}
-
-StateBaseShPtr FrameBase::getOPtr() const
-{
- return o_ptr_;
-}
-
-StateBaseShPtr FrameBase::getVPtr() const
-{
- return v_ptr_;
-}
-
-StateBaseShPtr FrameBase::getWPtr() const
-{
- return w_ptr_;
-}
-
-void FrameBase::printSelf(unsigned int _ntabs, std::ostream& _ost) const
-{
- NodeLinked::printSelf(_ntabs, _ost);
- if (p_ptr_.get() != nullptr)
- {
- _ost << "\tPosition : \n";
- p_ptr_->printSelf(_ntabs,_ost);
- }
- if (o_ptr_.get() != nullptr)
- {
- _ost << "\tOrientation : \n";
- o_ptr_->printSelf(_ntabs,_ost);
- }
- if (v_ptr_.get() != nullptr)
- {
- _ost << "\tVelocity : \n";
- v_ptr_->printSelf(_ntabs,_ost);
- }
- if (w_ptr_.get() != nullptr)
- {
- _ost << "\tAngular velocity : \n";
- v_ptr_->printSelf(_ntabs,_ost);
- }
-
-}
-
-
-
diff --git a/src/frame_base.h b/src/frame_base.h
deleted file mode 100644
index dd4350e82..000000000
--- a/src/frame_base.h
+++ /dev/null
@@ -1,109 +0,0 @@
-
-#ifndef FRAME_BASE_H_
-#define FRAME_BASE_H_
-
-class TrajectoryBase;
-class CaptureBase;
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_linked.h"
-#include "trajectory_base.h"
-#include "capture_base.h"
-#include "state_base.h"
-
-//class FrameBase
-class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
-{
- protected:
- FrameType type_; //type of frame. Either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- TimeStamp time_stamp_; //frame time stamp
- //Eigen::Vector3s state_; //TBD: Instead , It could be a vector/list/map of pointers to state units
- StateBaseShPtr p_ptr_; // Position state unit pointer
- StateBaseShPtr o_ptr_; // Orientation state unit pointer
- StateBaseShPtr v_ptr_; // Velocity state unit pointer
- StateBaseShPtr w_ptr_; // Angular velocity state unit pointer
- //TBD: accelerations?
-
- public:
- /** \brief Constructor with only time stamp
- *
- * Constructor with only time stamp
- * \param _traj_ptr a pointer to the Capture up node
- * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _p_ptr StateBase pointer to the position (default: nullptr)
- * \param _o_ptr StateBase pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBase pointer to the velocity (default: nullptr)
- * \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
- *
- **/
- FrameBase(const TrajectoryBasePtr & _traj_ptr, const TimeStamp& _ts, const StateBaseShPtr& _p_ptr = {}, const StateBaseShPtr& _o_ptr = {}, const StateBaseShPtr& _v_ptr = {}, const StateBaseShPtr& _w_ptr = {});
-
- /** \brief Constructor with type, time stamp and state pointer
- *
- * Constructor with type, time stamp and state pointer
- * \param _traj_ptr a pointer to the Capture up node
- * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _ts is the time stamp associated to this frame, provided in seconds
- * \param _p_ptr StateBase pointer to the position (default: nullptr)
- * \param _o_ptr StateBase pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBase pointer to the velocity (default: nullptr)
- * \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
- *
- **/
- FrameBase(const TrajectoryBasePtr & _traj_ptr, const FrameType & _tp, const TimeStamp& _ts, const StateBaseShPtr& _p_ptr = {}, const StateBaseShPtr& _o_ptr = {}, const StateBaseShPtr& _v_ptr = {}, const StateBaseShPtr& _w_ptr = {});
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~FrameBase();
-
- /** \brief Checks if this frame is KEY_FRAME
- *
- * Returns true if type_ is KEY_FRAME. Oterwise returns false.
- *
- **/
- bool isKey() const;
-
- void setType(FrameType _ft);
-
- void setTimeStamp(const TimeStamp& _ts);
-
- TimeStamp getTimeStamp() const;
-
- void getTimeStamp(TimeStamp & _ts) const;
-
- void setState(const Eigen::VectorXs& _st);
-
- void addCapture(CaptureBaseShPtr& _capt_ptr);
-
- const TrajectoryBasePtr getTrajectoryPtr() const;
-
- //const CaptureBaseList & captureList() const;
-
- CaptureBaseList* getCaptureListPtr();
-
- FrameBasePtr getPreviousFrame() const;
-
- StateBaseShPtr getPPtr() const;
-
- StateBaseShPtr getOPtr() const;
-
- StateBaseShPtr getVPtr() const;
-
- StateBaseShPtr getWPtr() const;
-
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-
-};
-#endif
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
deleted file mode 100644
index b4dce09da..000000000
--- a/src/landmark_base.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-
-#include "landmark_base.h"
-
-LandmarkBase::LandmarkBase(const MapBasePtr& _traj_ptr, const LandmarkType & _tp,const StateBaseShPtr& _p_ptr) :
- NodeLinked(MID, "FRAME", _traj_ptr),
- type_(_tp),
- st_list_({_p_ptr})
-{
- //
-}
-LandmarkBase::LandmarkBase(const MapBasePtr& _traj_ptr, const LandmarkType & _tp, const StateBaseList& _st_list) :
- NodeLinked(MID, "FRAME", _traj_ptr),
- type_(_tp),
- st_list_(_st_list)
-{
- //
-}
-
-LandmarkBase::~LandmarkBase()
-{
- //
-}
-
-void LandmarkBase::setType(LandmarkType _ft)
-{
- type_ = _ft;
-}
-
-const StateBaseShPtr LandmarkBase::getPPtr()
-{
- return st_list_.front();
-}
-
-StateBaseList* LandmarkBase::getStateListPtr()
-{
- return &st_list_;
-}
-
-//void LandmarkBase::printSelf(unsigned int _ntabs, std::ostream& _ost) const
-//{
-// NodeLinked::printSelf(_ntabs, _ost);
-// if (p_ptr_.get() != nullptr)
-// {
-// _ost << "\tPosition : \n";
-// p_ptr_->printSelf(_ntabs,_ost);
-// }
-// if (o_ptr_.get() != nullptr)
-// {
-// _ost << "\tOrientation : \n";
-// o_ptr_->printSelf(_ntabs,_ost);
-// }
-// if (v_ptr_.get() != nullptr)
-// {
-// _ost << "\tVelocity : \n";
-// v_ptr_->printSelf(_ntabs,_ost);
-// }
-// if (w_ptr_.get() != nullptr)
-// {
-// _ost << "\tAngular velocity : \n";
-// v_ptr_->printSelf(_ntabs,_ost);
-// }
-//}
-
-
-
diff --git a/src/landmark_base.h b/src/landmark_base.h
deleted file mode 100644
index 364af38d8..000000000
--- a/src/landmark_base.h
+++ /dev/null
@@ -1,72 +0,0 @@
-
-#ifndef LANDMARK_BASE_H_
-#define LANDMARK_BASE_H_
-
-class MapBase;
-class NodeTerminus;
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_linked.h"
-#include "map_base.h"
-
-//class LandmarkBase
-class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
-{
- protected:
- LandmarkType type_; //type of frame. Either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- StateBaseList st_list_; //List of pointers to the state corresponding to the landmark estimation
-
- public:
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, time stamp and state pointer
- * \param _traj_ptr pointer to the trajectory.
- * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _p_ptr StateBase pointer to the position (default: nullptr)
- *
- **/
- LandmarkBase(const MapBasePtr& _map_ptr, const LandmarkType & _tp, const StateBaseShPtr& _p_ptr);
-
-
- /** \brief Constructor with type, time stamp and state list
- *
- * Constructor with type, time stamp and state pointer
- * \param _traj_ptr pointer to the trajectory.
- * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _st_list StateBase list of the landmark estimation
- *
- **/
- LandmarkBase(const MapBasePtr& _map_ptr, const LandmarkType & _tp, const StateBaseList& _st_list);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~LandmarkBase();
-
- void setType(LandmarkType _ft);
-
- const StateBaseShPtr getPPtr();
-
- StateBaseList* getStateListPtr();
-
- //StateBaseShPtr getOPtr() const;
-
- //StateBaseShPtr getVPtr() const;
-
- //StateBaseShPtr getWPtr() const;
-
- //virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-
-};
-#endif
diff --git a/src/lineFit.sce b/src/lineFit.sce
deleted file mode 100644
index e44bed567..000000000
--- a/src/lineFit.sce
+++ /dev/null
@@ -1,111 +0,0 @@
-//info about 2D homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
-
-// clear all
-xdel(winsid());
-clear;
-
-//User Tunning params
-Nw = 6; //window size
-theta_th = %pi/6;
-K = 3; //How many std_dev are tolerated to count that a point is supporting a line
-r_stdev = 0.1; //ranging std dev
-
-//init
-result_lines = [];
-line_indexes = [];
-corners = [];
-
-//invent a set of points + noise
-points = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43;
- 7 6 5 4 3 2 1 2 3 4 5 6 7 8 9 10 9 8 7 6 5 4 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 7.4 7.3 7.2 7.1 7 6.9 6.8 6.7 6.6 6.5 6.4];
-points = points + rand(points,"normal")*r_stdev;
-[xx Np] = size(points);
-
-//Main loop. Runs over a sliding window of Nw points
-for (i = Nw:Np)
- //set the window
- points_w = points(:,(i-Nw+1):i)
-
- //Found the best fitting line over the window. Build the system: Ax=0. Matrix A = a_ij
-// a_00 = sum( points_w(1,:).^2 );
-// a_01 = sum( points_w(1,:).*points_w(2,:) );
-// a_02 = sum( points_w(1,:) );
-// a_10 = a_01;
-// a_11 = sum( points_w(2,:).^2 );
-// a_12 = sum( points_w(2,:) );
-// a_20 = a_02;
-// a_21 = a_12;
-// a_22 = Nw;
-// A = [a_00 a_01 a_02; a_10 a_11 a_12; a_20 a_21 a_22; 0 0 1];
- a_00 = sum( points_w(1,:).^2 );
- a_01 = sum( points_w(1,:).*points_w(2,:) );
- a_02 = sum( points_w(1,:) );
- a_10 = a_01;
- a_11 = sum( points_w(2,:).^2 );
- a_12 = sum( points_w(2,:) );
- A = [a_00 a_01 a_02; a_10 a_11 a_12; 0 0 1];
-
- //solve
-// line = pinv(A)*[zeros(3,1);1];
- line = inv(A)*[0; 0; 1];
-
- //compute error
- err = 0;
- for j=1:Nw
- err = err + abs(line'*[points_w(:,j);1])/sqrt(line(1)^2+line(2)^2);
- end
- err = err/Nw;
- //disp("error: "); disp(err);
-
- //if error below stdev, add line to result set
- if err < K*r_stdev then
- result_lines = [result_lines [line;points_w(:,1);points_w(:,$)]];
- line_indexes = [line_indexes i]; //ray index where the segment ends
- end
-end
-
-//num of lines
-[xx Nl] = size(result_lines);
-
-//corner detection
-for (i = 2:Nl)
- //compute angle diff between consecutive segments
- cos_theta = result_lines(1:2,i-1)'*result_lines(1:2,i) / ( norm(result_lines(1:2,i-1)) * norm(result_lines(1:2,i)) )
- theta = abs(acos(cos_theta));
-
- //if angle diff greater than threshold && indexes are less than Nw, we decide corner
- if theta > theta_th then
- if (line_indexes(i)-line_indexes(i-1)) < Nw then
- //Corner found! Compute "sub-pixel" corner location as the intersection of two lines
- corner = cross(result_lines(1:3,i-1),result_lines(1:3,i))
- corner = corner./corner(3);//norlamlize homogeneous point
- corners = [corners corner];
-
- //display
- disp("theta: "); disp(theta);
- disp("index:" ); disp(line_indexes(i)-Nw+1);//line_indexes(i) indicates the end point of the segment
- end
- end
-end
-
-//Set figure
-fig1 = figure(0);
-fig1.background = 8;
-
-//plot points
-plot(points(1,:),points(2,:),"g.");
-
-//plot lines
-for i=1:Nl
- m = -result_lines(1,i)/result_lines(2,i);
- xc = -result_lines(3,i)/result_lines(2,i);
- point1 = [result_lines(4,i) m*result_lines(4,i)+xc];
- point2 = [result_lines(6,i) m*result_lines(6,i)+xc];
- xpoly([point1(1) point2(1)],[point1(2) point2(2)]);
-end
-
-//plot corners
-plot(corners(1,:),corners(2,:),"ro");
-
-
-
diff --git a/src/map_base.cpp b/src/map_base.cpp
deleted file mode 100644
index e56aa781c..000000000
--- a/src/map_base.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "map_base.h"
-
-MapBase::MapBase() :
- NodeLinked(TOP, "TRAJECTORY")
-{
- //
-}
-
-MapBase::~MapBase()
-{
- //
-}
-
-const LandmarkBaseList* MapBase::getLandmarkListPtr()
-{
- return getDownNodeListPtr();
-}
diff --git a/src/map_base.h b/src/map_base.h
deleted file mode 100644
index 01cce79d7..000000000
--- a/src/map_base.h
+++ /dev/null
@@ -1,57 +0,0 @@
-
-#ifndef MAP_BASE_H_
-#define MAP_BASE_H_
-
-class NodeTerminus;
-class LandmarkBase;
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "node_linked.h"
-#include "node_terminus.h"
-#include "landmark_base.h"
-
-//class MapBase
-class MapBase : public NodeLinked<NodeTerminus,LandmarkBase>
-{
- protected:
-
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- **/
- MapBase();
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- ~MapBase();
-
- /** \brief Returns Frame list
- *
- * Returns FrameBase list
- *
- **/
- const LandmarkBaseList* getLandmarkListPtr();
-
- /** \brief Prints self info to std out
- *
- * Prints self info to std out
- *
- **/
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const {};
-
-};
-#endif
diff --git a/src/node_base.cpp b/src/node_base.cpp
deleted file mode 100644
index 481789436..000000000
--- a/src/node_base.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "node_base.h"
-
-//init static node counter
-unsigned int NodeBase::node_id_count_ = 0;
-
-NodeBase::NodeBase(std::string _label) :
- label_(_label), //
- node_id_(++node_id_count_)
-{
- // std::cout << "NodeID::constructor. Id: " << node_id_ << std::endl;
-}
-
-NodeBase::~NodeBase()
-{
- // std::cout << "NodeID::destructor. Id: " << node_id_ << std::endl;
-}
-
diff --git a/src/node_base.h b/src/node_base.h
deleted file mode 100644
index 4b9168384..000000000
--- a/src/node_base.h
+++ /dev/null
@@ -1,92 +0,0 @@
-#ifndef NODE_BASE_H_
-#define NODE_BASE_H_
-
-#include <iostream>
-
-/** \brief Base class for Nodes
- *
- * Base class for all Nodes in the Wolf tree.
- * It implements the ID factory.
- *
- **/
-class NodeBase
-{
- protected:
- std::string label_; ///< Text label identifying the node
- unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
- static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
-
- public:
-
- /** \brief Constructor from label.
- *
- * Constructor from label
- *
- */
- NodeBase(std::string _label);
-
- /** \brief Default destructor
- *
- * Default destructor
- *
- */
- virtual ~NodeBase();
-
- //public:
- /** \brief Gets node ID
- *
- * Gets node ID. Inline function.
- *
- */
- unsigned int nodeId() const
- {
- return node_id_;
- };
-
- /** \brief Gets node label
- *
- * Gets node label. Inline function.
- *
- */
- std::string nodeLabel() const
- {
- return label_;
- };
-
- /** \brief Inherited classes should implement that method
- *
- * Inherited classes should implement that method. It is required to implement a getTop() at NodeLinked level
- *
- **/
- virtual NodeBase* getTop() = 0;
-
- /** \brief Print node information
- *
- * Prints node information. Inine function.
- * \param _ntabs number of tabulations to print at the left of the printed information
- * \param _ost output stream
- *
- * Overload this function in derived classes to adapt the printed output to each object's relevant info.
- *
- */
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const
- {
- _ost << label_ << " " << node_id_ << std::endl;
- };
-
- protected:
-
- /** \brief prints a number of tabulations.
- *
- * Prints a number of tabulations, i.e., "\t". Inline function.
- * \param _ntabs number of tabulations to print
- * \param _ost output stream
- *
- */
- void printTabs(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const
- {
- for (unsigned int i = 0; i < _ntabs; i++) _ost << "\t";
- }
-};
-
-#endif /* NODE_BASE_H_ */
diff --git a/src/node_linked.h b/src/node_linked.h
deleted file mode 100644
index 1c5580e6b..000000000
--- a/src/node_linked.h
+++ /dev/null
@@ -1,416 +0,0 @@
-/**
- * This file defines the basic template class for linked nodes.
- *
- * As a definition of a template class of a significant complexity,
- * this file is organized with member definitions within the class declaration.
- * This has some advantages:
- * - Functions can be inlined.
- * - typedefs within the class can be used straightforwardly in definitions.
- * - Circular references are minimized.
- */
-
-#ifndef NODE_LINKED_H_
-#define NODE_LINKED_H_
-
-//wof includes
-#include "node_base.h"
-#include "wolf.h"
-
-/** \brief Linked node element in the Wolf Tree
- *
- * \param UpperType the type of node one level up in the Wolf tree.
- * \param LowerType the type of node one level down in the Wolf tree.
- *
- * Inherit from this class to implement a node element to be placed somewhere in the Wolf Tree.
- * A node has five main data members:
- * - An unique ID to identify it over the whole Wolf Tree (inherited from Node)
- * - A label indicating the node nature (inherited from Node)
- * - An enum indicating tree location (see NodeLocation enum at wolf.h)
- * - down_node_list_: A list of shared pointers to derived node objects, specified by the template parameter LowerType.
- * - up_node_: A regular pointer to a derived node object, specified by the template parameter UpperType.
- *
- */
-template<class UpperType, class LowerType>
-class NodeLinked : public NodeBase
-{
- public:
- typedef UpperType* UpperNodePtr;
-
- protected:
- typedef LowerType* LowerNodePtr;
- typedef std::shared_ptr<LowerType> LowerNodeShPtr;
- //typedef std::shared_ptr<UpperType> UpperNodeShPtr;
- typedef std::list<LowerNodeShPtr> LowerNodeList;
- typedef typename LowerNodeList::iterator LowerNodeIter;
-
- protected:
- NodeLocation location_;
- UpperNodePtr up_node_ptr_; //it is not a shared pointer because the ownership of upper node should not be shared by lower nodes
- LowerNodeList down_node_list_;
-
- public:
-
- /** \brief Constructor without specify up node
- *
- * Constructor without specify up node
- *
- */
- NodeLinked(const NodeLocation _loc, const std::string& _label);
-
- /** \brief Constructor specifying up node
- *
- * Constructor specifying up node
- *
- */
- NodeLinked(const NodeLocation _loc, const std::string& _label, const UpperNodePtr& _up_node_ptr);
-
- /** \brief Default destructor
- *
- * Default destructor
- *
- */
- virtual ~NodeLinked();
-
- /** \brief Checks if node is on the top of Wolf tree
- *
- * Check if node is on the top of Wolf tree
- *
- */
- bool isTop() const;
-
- /** \brief Checks if node is at the bottom of Wolf tree
- *
- * Check if node is at the bottom of Wolf tree
- *
- */
- bool isBottom() const;
-
- /** \brief Sets link to up node
- *
- * Sets link to up node
- *
- */
- void linkToUpperNode(UpperNodePtr _pptr);
-
- /** \brief Clears link to up node
- *
- * Sets link to up node
- *
- */
- void unlinkFromUpperNode();
-
- /** \brief Access the pointer to parent.
- *
- * Access the pointer to parent.
- * Throw if parent nullptr.
- *
- */
- const UpperNodePtr upperNodePtr() const;
-
- /** \brief Gets a reference to parent.
- *
- * Get a reference to parent.
- * Throw if parent is nullptr.
- *
- */
- const UpperType& upperNode() const;
-
- /** \brief Adds a down node
- *
- * Adds a down node
- *
- */
- void addDownNode(LowerNodeShPtr& _ptr);
-
- /** \brief Gets a reference to down node list
- *
- * Gets a reference to down node list
- *
- */
- LowerNodeList& downNodeList() const;
-
- /** \brief Gets a constant reference to down node list
- *
- * Gets constant reference to down node list
- *
- */
-// const LowerNodeList& downNodeList() const;
-
- /** \brief Gets a pointer to down node list
- *
- * Gets a pointer to down node list
- *
- */
- LowerNodeList* getDownNodeListPtr();
-
- /** \brief Gets a const pointer to down node list
- *
- * Gets a const pointer to down node list
- *
- */
-// const LowerNodeList* downNodeListPtr() const;
-
- /** \brief Removes a down node from list, given an iterator
- *
- * Removes a down node from the list
- * @param _iter an iterator to the particular down node in the list that will be removed
- *
- */
- void removeDownNode(const LowerNodeIter& _iter);
-
- /** \brief Removes a down node from the list, given a node id
- *
- * Removes a down node from the multimap
- * @param _id node id of the node that will nbe removed
- *
- */
- void removeDownNode(const unsigned int _id);
-
- /** \brief Gets a pointer to the tree top node
- *
- * Gets a pointer to the tree top node
- * TODO: Review if it could return a pointer to a derived class instead of NodeBase
- *
- **/
- virtual NodeBase* getTop();
-
- /** \brief Prints node information
- *
- * Prints node information.
- * \param _ntabs number of tabulations to print at the left of the printed information
- * \param _ost output stream
- *
- * Overload this function in derived classes to adapt the printed output to each object's relevant info.
- *
- */
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-
-
- protected:
-
- /** \brief Prints tabulated information about this node.
- *
- * Prints information about this node. It adds a number of tabs given by _ntabs.
- *\param _ntabs the number of tabs.
- *\param _ost the stream it prints to
- *
- */
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-
- /** \brief Prints upper node info
- *
- * Prints upper node info
- *
- **/
- void printUpper(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-
- /** \brief Prints recursively lower node info
- *
- * Prints recursively lower node info
- *
- **/
- void printLower(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-};
-
-//////////////////////////////////////////
-// IMPLEMENTATION
-//////////////////////////////////////////
-template<class UpperType, class LowerType>
-NodeLinked<UpperType, LowerType>::NodeLinked(const NodeLocation _loc, const std::string& _label) :
- NodeBase(_label), //
- location_(_loc), //
- up_node_ptr_(nullptr),
- down_node_list_()
-{
-}
-
-template<class UpperType, class LowerType>
-NodeLinked<UpperType, LowerType>::NodeLinked(const NodeLocation _loc, const std::string& _label, const UpperNodePtr& _up_node_ptr) :
- NodeBase(_label), //
- location_(_loc),
- down_node_list_()
-{
- linkToUpperNode(_up_node_ptr);
-}
-
-template<class UpperType, class LowerType>
-NodeLinked<UpperType, LowerType>::~NodeLinked()
-{
- up_node_ptr_ = nullptr;
- down_node_list_.clear();
-}
-
-template<class UpperType, class LowerType>
-inline bool NodeLinked<UpperType, LowerType>::isTop() const
-{
- if (location_ == TOP)
- return true;
- else
- return false;
-}
-
-template<class UpperType, class LowerType>
-inline bool NodeLinked<UpperType, LowerType>::isBottom() const
-{
- if (location_ == BOTTOM)
- return true;
- else
- return false;
-}
-
-template<class UpperType, class LowerType>
-inline void NodeLinked<UpperType, LowerType>::linkToUpperNode(UpperNodePtr _pptr)
-{
- if (isTop())
- {
- up_node_ptr_ = nullptr;
- }
- else
- {
- up_node_ptr_ = _pptr;
- }
-}
-
-template<class UpperType, class LowerType>
-inline void NodeLinked<UpperType, LowerType>::unlinkFromUpperNode()
-{
- up_node_ptr_ = nullptr;
-}
-
-template<class UpperType, class LowerType>
-inline const typename NodeLinked<UpperType, LowerType>::UpperNodePtr NodeLinked<UpperType, LowerType>::upperNodePtr() const
-{
- assert(up_node_ptr_ != nullptr);
- return up_node_ptr_;
-}
-
-template<class UpperType, class LowerType>
-inline const UpperType& NodeLinked<UpperType, LowerType>::upperNode() const
-{
- assert(up_node_ptr_ != nullptr);
- return *up_node_ptr_;
-}
-
-template<class UpperType, class LowerType>
-inline void NodeLinked<UpperType, LowerType>::addDownNode(LowerNodeShPtr& _ptr)
-{
- assert(!isBottom() && "Trying to add a down node to a bottom node");
- down_node_list_.push_back(_ptr);
- down_node_list_.back()->linkToUpperNode( (typename LowerType::UpperNodePtr)(this) );
-}
-
-template<class UpperType, class LowerType>
-inline typename NodeLinked<UpperType, LowerType>::LowerNodeList& NodeLinked<UpperType, LowerType>::downNodeList() const
-{
- return down_node_list_;
-}
-
-// template<class UpperType, class LowerType>
-// inline const typename NodeLinked<UpperType, LowerType>::LowerNodeList& NodeLinked<UpperType, LowerType>::downNodeList() const
-// {
-// return down_node_list_;
-// }
-
-template<class UpperType, class LowerType>
-inline typename NodeLinked<UpperType, LowerType>::LowerNodeList* NodeLinked<UpperType, LowerType>::getDownNodeListPtr()
-{
- return &down_node_list_;
-}
-
-// template<class UpperType, class LowerType>
-// inline const typename NodeLinked<UpperType, LowerType>::LowerNodeList* NodeLinked<UpperType, LowerType>::downNodeListPtr() const
-// {
-// return &down_node_list_;
-// }
-
-template<class UpperType, class LowerType>
-inline void NodeLinked<UpperType, LowerType>::removeDownNode(const unsigned int _id)
-{
- for (auto iter = down_node_list_.begin(); iter != down_node_list_.end(); ++iter)
- {
- if ( (*iter)->nodeId() == _id)
- {
- removeDownNode(iter);
- break; //avoid comparison of iter and list.end(), otherwise Valgrind claimed
- }
- }
-}
-
-template<class UpperType, class LowerType>
-inline void NodeLinked<UpperType, LowerType>::removeDownNode(const LowerNodeIter& _iter)
-{
- (*_iter)->unlinkFromUpperNode();
- down_node_list_.erase(_iter);
-}
-
-template<class UpperType, class LowerType>
-NodeBase* NodeLinked<UpperType, LowerType>::getTop()
-{
- if (location_ == TOP)
- return this;
- else
- return up_node_ptr_->getTop();
-}
-
-template<class UpperType, class LowerType>
-void NodeLinked<UpperType, LowerType>::print(unsigned int _ntabs, std::ostream& _ost) const
-{
- printSelf(_ntabs, _ost); //one line
-
- if ((location_ != TOP) && (up_node_ptr_ != nullptr))
- {
- printUpper(_ntabs, _ost);
- }
- if (location_ != BOTTOM)
- {
- printLower(_ntabs, _ost);
- }
-}
-
-template<class UpperType, class LowerType>
-void NodeLinked<UpperType, LowerType>::printSelf(unsigned int _ntabs, std::ostream& _ost) const
-{
- printTabs(_ntabs);
- _ost << nodeLabel() << " " << nodeId() << " : ";
- switch (location_)
- {
- case TOP:
- _ost << "TOP" << std::endl;
- break;
- case MID:
- _ost << "MID" << std::endl;
- break;
- case BOTTOM:
- _ost << "BOT" << std::endl;
- break;
- default:
- _ost << "*" << std::endl;
- break;
- }
-}
-
-template<class UpperType, class LowerType>
-void NodeLinked<UpperType, LowerType>::printUpper(unsigned int _ntabs, std::ostream& _ost) const
-{
- printTabs(_ntabs);
- _ost << "\tUpper Node --> " << up_node_ptr_->nodeId() << std::endl;
-}
-
-template<class UpperType, class LowerType>
-void NodeLinked<UpperType, LowerType>::printLower(unsigned int _ntabs, std::ostream& _ost) const
-{
- printTabs(_ntabs);
- _ost << "\tLower Nodes ==> [ ";
- for (auto const & down_node_sh_ptr : down_node_list_)
- {
- _ost << down_node_sh_ptr->nodeId() << " ";
- }
- _ost << "]" << std::endl;
- _ntabs++;
- for (auto const & down_node_ptr : down_node_list_)
- {
- down_node_ptr->print(_ntabs, _ost);
- }
-}
-
-#endif /* NODE_LINKED_H_ */
diff --git a/src/node_terminus.cpp b/src/node_terminus.cpp
deleted file mode 100644
index d63d2ba9c..000000000
--- a/src/node_terminus.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-
-#include "node_terminus.h"
-
-inline NodeTerminus::~NodeTerminus()
-{
- //
-}
-
-NodeBase* NodeTerminus::getTop()
-{
- return nullptr;
-}
diff --git a/src/node_terminus.h b/src/node_terminus.h
deleted file mode 100644
index 04339eafd..000000000
--- a/src/node_terminus.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef NODE_TERMINUS_H_
-#define NODE_TERMINUS_H_
-
-//wolf includes
-#include "node_base.h"
-
-/**
- * \brief Dummy node to terminate the tree in both the Top and Bottom levels.
- *
- * This node does nothing. It only shares basic types with other nodes
- * to be able to be passed to Top and Bottom nodes as
- * their template parameters.
- *
- * Top node will usually be Frame, while Bottom should be where error function is implemented.
- * Depending on the applcation it can be at Correspondence level, but also on feature of capture levels.
- *
- * Use it as template parameter for TOP and BOTTOM nodes.
- *
- */
-class NodeTerminus : public NodeBase
-{
- public:
- virtual ~NodeTerminus();
- virtual NodeBase* getTop();
-};
-#endif /* NODE_TERMINUS_H_ */
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
deleted file mode 100644
index 042abf708..000000000
--- a/src/sensor_base.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "sensor_base.h"
-
-SensorBase::SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, const Eigen::VectorXs & _params) :
- type_(_tp),
- sensor_pose_vehicle_(_pose),
- params_(_params.size())
-{
- params_ = _params;
-}
-
-SensorBase::SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, unsigned int _params_size) :
- type_(_tp),
- sensor_pose_vehicle_(_pose),
- params_(_params_size)
-{
- //
-}
-
-SensorBase::~SensorBase()
-{
- //
-}
-
-const SensorType SensorBase::getSensorType() const
-{
- return type_;
-}
-
-const Eigen::VectorXs * SensorBase::getSensorPose() const
-{
- return & sensor_pose_vehicle_;
-}
-
diff --git a/src/sensor_base.h b/src/sensor_base.h
deleted file mode 100644
index 174c37d02..000000000
--- a/src/sensor_base.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef SENSOR_BASE_H_
-#define SENSOR_BASE_H_
-
-
-//std includes
-//
-
-//Wolf includes
-#include "wolf.h"
-
-class SensorBase
-{
- protected:
- SensorType type_;//indicates sensor type. Enum defined at wolf.h
- Eigen::VectorXs sensor_pose_vehicle_;//sensor pose in the vehicle frame
- Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ...
- //bool generate_prior_; //flag indicating if this sensor generates the prior or not
-
- public:
- SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, const Eigen::VectorXs & _params);
-
- SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, unsigned int _params_size);
-
- ~SensorBase();
-
- const SensorType getSensorType() const;
-
- const Eigen::VectorXs * getSensorPose() const;
-};
-#endif
-
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
deleted file mode 100644
index f12a2cd29..000000000
--- a/src/sensor_gps_fix.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "sensor_gps_fix.h"
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise) :
- SensorBase(GPS_FIX, _sp, Eigen::VectorXs::Constant(1,_noise))
-{
- //
-}
-
-SensorGPSFix::~SensorGPSFix()
-{
- //
-}
-
-WolfScalar SensorGPSFix::getNoise() const
-{
- return params_(0);
-}
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
deleted file mode 100644
index 65a3ae038..000000000
--- a/src/sensor_gps_fix.h
+++ /dev/null
@@ -1,35 +0,0 @@
-
-#ifndef SENSOR_GPS_FIX_H_
-#define SENSOR_GPS_FIX_H_
-
-//wolf includes
-#include "sensor_base.h"
-
-class SensorGPSFix : public SensorBase
-{
- public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _sp sensor 3D pose with respect to vehicle base frame
- * \param _noise noise standard deviation
- *
- **/
- SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~SensorGPSFix();
-
- /** \brief Returns noise standard deviation
- *
- * Returns noise standard deviation
- *
- **/
- double getNoise() const;
-
-};
-#endif
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
deleted file mode 100644
index 6b670fc7d..000000000
--- a/src/sensor_laser_2D.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "sensor_laser_2D.h"
-
-SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax, WolfScalar _range_stdev) :
- SensorBase(LIDAR, _sp, 5)
-// SensorBase(LIDAR, _sp,{(WolfScalar)(_nrays), _apert, _rmin, _rmax, _range_stdev})
-{
- params_ << (WolfScalar)(_nrays), _apert, _rmin, _rmax, _range_stdev;
-}
-
-SensorLaser2D::~SensorLaser2D()
-{
- //
-}
-
-unsigned int SensorLaser2D::getNumRays() const
-{
- return (unsigned int)(params_(0));
-}
-
-WolfScalar SensorLaser2D::getAperture() const
-{
- return params_(1);
-}
-
-WolfScalar SensorLaser2D::getRangeMin() const
-{
- return params_(2);
-}
-
-WolfScalar SensorLaser2D::getRangeMax() const
-{
- return params_(3);
-}
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
deleted file mode 100644
index a8ee94d23..000000000
--- a/src/sensor_laser_2D.h
+++ /dev/null
@@ -1,67 +0,0 @@
-// sensor_laser_2D.h
-
-#ifndef SENSOR_LASER_2D_H_
-#define SENSOR_LASER_2D_H_
-
-//wolf includes
-#include "sensor_base.h"
-
-class SensorLaser2D : public SensorBase
-{
- private:
-// unsigned int n_rays_; ///numper of rays per scan [#]
-// double aperture_; ///scan aperture in [rad]
-// double range_min_; ///minimum range [m]
-// double range_max_; ///maximum range [m]
-
- public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _sp sensor 3D pose with respect to vehicle base frame
- * \param _nrays number of rays per scan
- * \param _apert angular aperture [rad]
- * \param _rmin minimum range [m]
- * \param _rmax maximum range [m]
- * \param _range_stdev range standard deviation [m]
- *
- **/
- SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax, WolfScalar _range_stdev);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~SensorLaser2D();
-
- /** \brief Returns n_rays_
- *
- * Returns n_rays_
- *
- **/
- unsigned int getNumRays() const;
-
- /** \brief Returns aperture_
- *
- * Returns aperture_
- *
- **/
- double getAperture() const;
-
- /** \brief Returns range_min_
- *
- * Returns range_min_
- *
- **/
- double getRangeMin() const;
-
- /** \brief Returns range_max_
- *
- * Returns range_max_
- *
- **/
- double getRangeMax() const;
-
-};
-#endif /*SENSOR_LASER_2D_H_*/
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
deleted file mode 100644
index 5bf66af15..000000000
--- a/src/sensor_odom_2D.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "sensor_odom_2D.h"
-
-SensorOdom2D::SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
- SensorBase(ODOM_2D, _sp, 2)
-{
- params_ << _disp_noise_factor, _rot_noise_factor;
-}
-
-SensorOdom2D::~SensorOdom2D()
-{
- //
-}
-
-WolfScalar SensorOdom2D::getDisplacementNoiseFactor() const
-{
- return params_(0);
-}
-
-WolfScalar SensorOdom2D::getRotationNoiseFactor() const
-{
- return params_(1);
-}
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
deleted file mode 100644
index c81628ba3..000000000
--- a/src/sensor_odom_2D.h
+++ /dev/null
@@ -1,43 +0,0 @@
-
-#ifndef SENSOR_ODOM_2D_H_
-#define SENSOR_ODOM_2D_H_
-
-//wolf includes
-#include "sensor_base.h"
-
-class SensorOdom2D : public SensorBase
-{
- public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _sp sensor 3D pose with respect to vehicle base frame
- * \param _disp_noise_factor displacement noise factor
- * \param _rot_noise_factor rotation noise factor
- *
- **/
- SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~SensorOdom2D();
-
- /** \brief Returns displacement noise factor
- *
- * Returns displacement noise factor
- *
- **/
- double getDisplacementNoiseFactor() const;
-
- /** \brief Returns rotation noise factor
- *
- * Returns rotation noise factor
- *
- **/
- double getRotationNoiseFactor() const;
-
-};
-#endif
diff --git a/src/state_base.cpp b/src/state_base.cpp
deleted file mode 100644
index 95ddfac32..000000000
--- a/src/state_base.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-
-#include "state_base.h"
-
-StateBase::StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- state_ptr_(_st_remote.data() + _idx)
-{
- //
-}
-
-
-StateBase::StateBase(WolfScalar* _st_ptr) :
- state_ptr_(_st_ptr)
-{
- //
-}
-
-StateBase::~StateBase()
-{
- //
-}
-
-WolfScalar* StateBase::getPtr()
-{
- return state_ptr_;
-}
-
-void StateBase::printTabs(unsigned int _ntabs, std::ostream& _ost) const
-{
- for (unsigned int i = 0; i < _ntabs; i++) _ost << "\t";
-}
diff --git a/src/state_base.h b/src/state_base.h
deleted file mode 100644
index 5e41bf6a9..000000000
--- a/src/state_base.h
+++ /dev/null
@@ -1,81 +0,0 @@
-
-#ifndef STATE_BASE_H_
-#define STATE_BASE_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-
-//class StateBase
-class StateBase
-{
- protected:
- WolfScalar* state_ptr_;
-
- public:
- /** \brief Constructor with whole state storage and index where starts the state unit
- *
- * Constructor with whole state storage and index where starts the state unit
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx);
-
- /** \brief Constructor with scalar pointer
- *
- * Constructor with scalar pointer
- * \param _st_ptr is the pointer of the first element of the state unit
- *
- **/
- StateBase(WolfScalar* _st_ptr);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StateBase();
-
- /** \brief Returns the pointer to the first element of the state
- *
- * Returns the scalar pointer to the first element of the state
- *
- **/
- virtual WolfScalar* getPtr();
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual stateType getStateType() const = 0;
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const = 0;
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const = 0;
-
- /** \brief prints a number of tabulations.
- *
- * Prints a number of tabulations, i.e., "\t". Inline function.
- * \param _ntabs number of tabulations to print
- * \param _ost output stream
- *
- */
- void printTabs(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-};
-#endif
diff --git a/src/state_complex_angle.cpp b/src/state_complex_angle.cpp
deleted file mode 100644
index 3282eb378..000000000
--- a/src/state_complex_angle.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-
-#include "state_complex_angle.h"
-
-StateComplexAngle::StateComplexAngle(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateBase(_st_remote, _idx)
-{
- //
- std::cout << "state complex angle created\n";
-}
-
-StateComplexAngle::StateComplexAngle(WolfScalar* _st_ptr) :
- StateBase(_st_ptr)
-{
- //
-}
-
-StateComplexAngle::~StateComplexAngle()
-{
- //
-}
-
-stateType StateComplexAngle::getStateType() const
-{
- return ST_COMPLEX_ANGLE;
-}
-
-unsigned int StateComplexAngle::getStateSize() const
-{
- return BLOCK_SIZE;
-}
-
-void StateComplexAngle::printSelf(unsigned int _ntabs, std::ostream& _ost) const
-{
- printTabs(_ntabs);
- _ost << *this->state_ptr_<< std::endl;
- printTabs(_ntabs);
- _ost << *(this->state_ptr_+1) << std::endl;
-}
diff --git a/src/state_complex_angle.h b/src/state_complex_angle.h
deleted file mode 100644
index 8f3e42b9c..000000000
--- a/src/state_complex_angle.h
+++ /dev/null
@@ -1,65 +0,0 @@
-
-#ifndef STATE_COMPLEX_ANGLE_H_
-#define STATE_COMPLEX_ANGLE_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "state_base.h"
-
-//class StateComplexAngle
-class StateComplexAngle : public StateBase
-{
- public:
- static const unsigned int BLOCK_SIZE = 2;
-
- /** \brief Constructor with whole state storage and index where starts the state unit
- *
- * Constructor with whole state storage and index where starts the state unit
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StateComplexAngle(Eigen::VectorXs& _st_remote, const unsigned int _idx);
-
- /** \brief Constructor with scalar pointer
- *
- * Constructor with scalar pointer
- * \param _st_ptr is the pointer of the first element of the state unit
- *
- **/
- StateComplexAngle(WolfScalar* _st_ptr);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StateComplexAngle();
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual stateType getStateType() const;
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const;
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-};
-#endif
diff --git a/src/state_point.h b/src/state_point.h
deleted file mode 100644
index 5ad8f9700..000000000
--- a/src/state_point.h
+++ /dev/null
@@ -1,98 +0,0 @@
-
-#ifndef STATE_POINT_H_
-#define STATE_POINT_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-
-//class StateBase
-template <unsigned int SIZE>
-class StatePoint : public StateBase
-{
- public:
- static const unsigned int BLOCK_SIZE = SIZE;
-
- /** \brief Constructor with whole state storage and index where starts the state unit
- *
- * Constructor with whole state storage and index where starts the state unit
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StatePoint(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateBase(_st_remote, _idx)
- {
- //
- std::cout << "state point created\n";
- }
-
- /** \brief Constructor with scalar pointer
- *
- * Constructor with scalar pointer
- * \param _st_ptr is the pointer of the first element of the state unit
- *
- **/
- StatePoint(WolfScalar* _st_ptr) :
- StateBase(_st_ptr)
- {
- //
- }
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StatePoint()
- {
- //
- }
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual stateType getStateType() const
- {
- switch (BLOCK_SIZE)
- {
- case 1:
- return ST_POINT_1D;
- case 2:
- return ST_POINT_2D;
- case 3:
- return ST_POINT_3D;
- }
- }
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const
- {
- return BLOCK_SIZE;
- }
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const
- {
- for (uint i = 0; i < BLOCK_SIZE; i++)
- {
- printTabs(_ntabs);
- _ost << *(this->state_ptr_+i) << std::endl;
- }
- }
-};
-#endif
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
deleted file mode 100644
index 409847e16..000000000
--- a/src/time_stamp.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-
-#include "time_stamp.h"
-
-TimeStamp::TimeStamp() :
- time_stamp_(0)
-{
- setToNow();
-}
-
-TimeStamp::TimeStamp(const WolfScalar _ts) :
- time_stamp_(_ts)
-{
- //
-}
-
-TimeStamp::~TimeStamp()
-{
- //nothing to do
-}
-
-void TimeStamp::setToNow()
-{
- timeval ts;
- gettimeofday(&ts, NULL);
- time_stamp_ = (WolfScalar)(ts.tv_sec + ts.tv_usec / 1e6);
-}
-
-void TimeStamp::set(const timeval & ts)
-{
- time_stamp_ = (WolfScalar)(ts.tv_sec + ts.tv_usec / 1e6);
-}
-
-void TimeStamp::set(const unsigned long int sec, const unsigned long int nanosec)
-{
- time_stamp_ = (WolfScalar)(sec + nanosec / 1e9);
-}
-
-void TimeStamp::set(const WolfScalar ts)
-{
- time_stamp_ = ts;
-}
-
-WolfScalar TimeStamp::get() const
-{
- return time_stamp_;
-}
-
-unsigned long int TimeStamp::getSeconds() const
-{
- unsigned long int ts;
- ts = (unsigned long int)floor(time_stamp_);
- return ts;
-}
-
-unsigned long int TimeStamp::getNanoSeconds() const
-{
- WolfScalar ts;
- ts = floor(time_stamp_);
- return (unsigned long int)((time_stamp_ - ts) * 1e9);
-}
-
-void TimeStamp::print(std::ostream & ost) const
-{
- std::streamsize nn;
- std::ios_base::fmtflags fmtfl;
-
- //get/set ostream flags and precision digits
- fmtfl = ost.flags(std::ios::left);
- ost.setf(std::ios::fixed, std::ios::floatfield);
- nn = ost.precision(TIME_STAMP_DIGITS_);
-
- //send to ostream
- ost << this->time_stamp_;
-
- //restore flags and precision
- ost.flags(fmtfl);
- ost.precision(nn);
-}
-
-void TimeStamp::operator=(const WolfScalar & ts)
-{
- time_stamp_ = ts;
-}
-
-void TimeStamp::operator=(const TimeStamp & ts)
-{
- time_stamp_ = ts.get();
-}
-
-bool TimeStamp::operator<(const TimeStamp & ts) const
-{
- if (time_stamp_ < ts.get())
- return true;
- else
- return false;
-}
-
-bool TimeStamp::operator<=(const TimeStamp & ts) const
-{
- if (time_stamp_ <= ts.get())
- return true;
- else
- return false;
-}
-
-WolfScalar TimeStamp::operator-(const TimeStamp & ts) const
-{
- return (time_stamp_ - ts.get());
-}
-
diff --git a/src/time_stamp.h b/src/time_stamp.h
deleted file mode 100644
index bd2d2da30..000000000
--- a/src/time_stamp.h
+++ /dev/null
@@ -1,142 +0,0 @@
-
-#ifndef TIME_STAMP_H_
-#define TIME_STAMP_H_
-
-//C, std
-#include <time.h>
-#include <sys/time.h>
-#include <math.h>
-#include <iostream>
-
-//wolf
-#include "wolf.h"
-
-
-/**
- *
- * \brief TimeStamp implements basic functionalities for time stamps
- *
- * TimeStamp implements basic functionalities for time stamps
- *
- */
-class TimeStamp
-{
- private:
- WolfScalar time_stamp_; ///< Time stamp. Expressed in seconds from 1th jan 1970.
- static const unsigned int TIME_STAMP_DIGITS_ = 10; ///< Number of digits to print time stamp values
-
- public:
- /** \brief Constructor
- *
- * Constructor without arguments. Sets time stamp to now.
- *
- */
- TimeStamp();
-
- /** \brief Constructor with argument
- *
- * Constructor with arguments
- *
- */
- TimeStamp(const WolfScalar _ts);
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~TimeStamp();
-
- /** \brief Time stamp to now
- *
- * Sets time stamp to now
- *
- */
- void setToNow();
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a timeval struct
- *
- */
- void set(const timeval & ts);
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds)
- *
- */
- void set(const unsigned long int sec, const unsigned long int nanosec);
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a scalar_t (seconds)
- *
- */
- void set(const WolfScalar ts);
-
- /** \brief Get time stamp
- *
- * Returns time stamp
- *
- */
- WolfScalar get() const;
-
- /** \brief Get time stamp (only seconds)
- *
- * Returns seconds of time stamp
- *
- */
- unsigned long int getSeconds() const;
-
- /** \brief Get time stamp (only nano seconds)
- *
- * Returns nanoseconds part of time stamp
- *
- */
- unsigned long int getNanoSeconds() const;
-
- /** \brief Prints time stamp to a given ostream
- *
- * Prints time stamp to a given ostream
- *
- */
- void print(std::ostream & ost = std::cout) const;
-
- /** \brief Assignement operator
- *
- * Assignement operator
- *
- */
- void operator=(const WolfScalar & ts);
-
- /** \brief Assignement operator
- *
- * Assignement operator
- *
- */
- void operator=(const TimeStamp & ts);
-
- /** \brief comparison operator
- *
- * Comparison operator
- *
- */
- bool operator<(const TimeStamp & ts) const;
-
- /** \brief comparison operator
- *
- * Comparison operator
- *
- */
- bool operator<=(const TimeStamp & ts) const;
-
- /** \brief difference operator
- *
- * difference operator
- *
- */
- WolfScalar operator-(const TimeStamp & ts) const;
-
-};
-#endif
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
deleted file mode 100644
index 0b81337c7..000000000
--- a/src/trajectory_base.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-#include "trajectory_base.h"
-
-TrajectoryBase::TrajectoryBase() :
- NodeLinked(TOP, "TRAJECTORY")
-{
- //
-}
-
-TrajectoryBase::~TrajectoryBase()
-{
- //
-}
-
-void TrajectoryBase::addFrame(FrameBaseShPtr& _frame_ptr)
-{
- addDownNode(_frame_ptr);
-}
-
-FrameBaseList* TrajectoryBase::getFrameListPtr()
-{
- return getDownNodeListPtr();
-}
-
-// const inline FrameBaseList* TrajectoryBase::frameList() const
-// {
-// return downNodeListPtr();
-// }
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
deleted file mode 100644
index e803be9bf..000000000
--- a/src/trajectory_base.h
+++ /dev/null
@@ -1,73 +0,0 @@
-
-#ifndef TRAJECTORY_BASE_H_
-#define TRAJECTORY_BASE_H_
-
-class NodeTerminus;
-class FrameBase;
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "node_linked.h"
-#include "node_terminus.h"
-#include "frame_base.h"
-#include "state_base.h"
-
-//class TrajectoryBase
-class TrajectoryBase : public NodeLinked<NodeTerminus,FrameBase>
-{
- protected:
- // JVN: No seria millor que la trajectòria de tamany fix fos una classe derivada? i implementar funcions virtuals de l'estil "manageFrames()" que elimini/remapegi
- unsigned int fixed_size_; // Limits the number of frames forming the trajectory
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- **/
- TrajectoryBase();
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- ~TrajectoryBase();
-
- /** \brief Add a frame to the trajectory
- *
- * Add a frame to the trajectory
- *
- **/
- void addFrame(FrameBaseShPtr& _frame_ptr);
-
- /** \brief Returns a pointer to Frame list
- *
- * Returns a pointer to Frame list
- *
- **/
- FrameBaseList* getFrameListPtr();
-
- /** \brief Returns a const pointer to Frame list
- *
- * Returns a const pointer to Frame list
- *
- **/
-// const FrameBaseList* frameList() const;
-
- /** \brief Prints self info to std out
- *
- * Prints self info to std out
- *
- **/
- virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const {};
-
-};
-#endif
diff --git a/src/wolf.h b/src/wolf.h
deleted file mode 100644
index aec9f17a5..000000000
--- a/src/wolf.h
+++ /dev/null
@@ -1,293 +0,0 @@
-/*
- * wolf.h
- *
- * Created on: 28/05/2014
- * \author: jsola
- */
-
-/**
- * \file wolf.h
- * \brief General typedefs for the Wolf project
- * \author Joan Sola
- */
-#ifndef WOLF_H_
-#define WOLF_H_
-
-//includes from std lib
-#include <memory>
-#include <list>
-#include <map>
-
-//includes from Eigen lib
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-/**
- * \brief scalar type for the Wolf project
- *
- * This typedef makes it possible to recompile the whole Wolf project with double, float, or other precisions.
- *
- * To change the project precision, just edit wolf.h and change the precision of this typedef.
- *
- * Do NEVER forget to use Wolf scalar definitions when you code!!!
- *
- * Do NEVER use plain Eigen scalar types!!! (This is redundant with the above. But just to make sure you behave!)
- *
- * The ONLY exception to this rule is when you need special precision. The ONLY example by now is the time stamp which uses double.
- */
-//typedef float WolfScalar; // Use this for float, 32 bit precision
-typedef double WolfScalar; // Use this for double, 64 bit precision
-//typedef long double WolfScalar; // Use this for long double, 128 bit precision
-
-///////////////////////////////////////////
-// Construct types for any scalar defined in the typedef scalar_t above
-////////////////////////////////////////////
-/** \brief Namespace extending Eigen definitions
- *
- * We redefine all algebra and gemoetry types to hold double or single precision floats.
- * The appended letter indicating this is 's', so that we have, e.g.,
- * - VectorXf Vector of floats - defined by Eigen
- * - VectorXd Vector of doubles - defined by Eigen
- * - VectorXs Vector of either double of float, depending on the type \b scalar_t, defined by Wolf.
- *
- */
-namespace Eigen // Eigen namespace extension
-{
-// 1. Vectors and Matrices
-typedef Matrix<WolfScalar, 2, 2> Matrix2s; ///< 2x2 matrix of real scalar_t type
-typedef Matrix<WolfScalar, 3, 3> Matrix3s; ///< 3x3 matrix of real scalar_t type
-typedef Matrix<WolfScalar, 4, 4> Matrix4s; ///< 4x4 matrix of real scalar_t type
-typedef Matrix<WolfScalar, Dynamic, Dynamic> MatrixXs; ///< variable size matrix of real scalar_t type
-typedef Matrix<WolfScalar, 2, 1> Vector2s; ///< 2-vector of real scalar_t type
-typedef Matrix<WolfScalar, 3, 1> Vector3s; ///< 3-vector of real scalar_t type
-typedef Matrix<WolfScalar, 4, 1> Vector4s; ///< 4-vector of real scalar_t type
-typedef Matrix<WolfScalar, Dynamic, 1> VectorXs; ///< variable size vector of real scalar_t type
-typedef Matrix<WolfScalar, 1, 2> RowVector2s; ///< 2-row-vector of real scalar_t type
-typedef Matrix<WolfScalar, 1, 3> RowVector3s; ///< 3-row-vector of real scalar_t type
-typedef Matrix<WolfScalar, 1, 4> RowVector4s; ///< 4-row-vector of real scalar_t type
-typedef Matrix<WolfScalar, 1, Dynamic> RowVectorXs; ///< variable size row-vector of real scalar_t type
-
-// 2. Quaternions and other rotation things
-typedef Quaternion<WolfScalar> Quaternions; ///< Quaternion of real scalar_t type
-typedef AngleAxis<WolfScalar> AngleAxiss; ///< Angle-Axis of real scalar_t type
-}
-
-/** \brief Enumeration of node locations at Wolf Tree
- *
- * Enumeration of node locations at Wolf Tree.
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- *
- */
-typedef enum
-{
- TOP, ///< root node location. This is the one that commands jobs down the tree.
- MID,///< middle nodes. These delegate jobs to lower nodes.
- BOTTOM ///< lowest level nodes. These are the ones that do not delegate any longer and have to do the job.
-} NodeLocation;
-
-/** \brief Enumeration of all possible frames
- *
- * Enumeration of all possible frames.
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- *
- */
-typedef enum
-{
- KEY_FRAME, ///< marks a key frame. It will stay in the frames window and play at optimizations.
- REGULAR_FRAME ///< marks a regular frame. It does play at optimizations but it will be discarded from the window once a newer frame arrives.
-} FrameType;
-
-/** \brief Enumeration of all possible correspondences
- *
- * Enumeration of all possible correspondences.
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- *
- */
-typedef enum
-{
- CORR_GPS_FIX_2D, ///< marks a 2D GPS Fix correspondence.
- CORR_ODOM_2D_COMPLEX_ANGLE, ///< marks a 2D Odometry using complex angles.
- CORR_ODOM_2D_THETA ///< marks a 2D Odometry using theta angles.
-
-} CorrespondenceType;
-
-/** \brief Enumeration of all possible state parametrizations
- *
- * Enumeration of all possible state parametrizations.
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- *
- */
-typedef enum
-{
- ST_POINT_1D, ///< A 1D point. No parametrization.
- ST_POINT_2D, ///< A 2D point. No parametrization.
- ST_POINT_3D, ///< A 3D point. No parametrization.
- ST_THETA, ///< A 2D orientation represented by a single angle. No parametrization (equivalent to ST_POINT_1D).
- ST_COMPLEX_ANGLE, ///< A 2D orientation represented by a complex number.
- ST_QUATERNION ///< A 3D orientation represented by a quaternion.
-} stateType;
-
-/** \brief Enumeration of all possible sensor types
- *
- * Enumeration of all possible sensor types.
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- *
- */
-typedef enum
-{
- ODOM_2D, ///< Odometry measurement from encoders: displacement and rotation.
- IMU, ///< Inertial measurement unit with 3 acceleros, 3 gyros
- CAMERA, ///< Regular pinhole camera
- GPS_FIX, ///< GPS fix calculated from a GPS receiver
- GPS_RAW, ///< GPS pseudo ranges, doppler and satellite ephemerides
- LIDAR, ///< Laser Range Finder, 2D
- RADAR, ///< Radar
- ABSOLUTE_POSE ///< Full absolute pose (XYZ+quaternion)
-} SensorType;
-
-/** \brief Enumeration of all possible landmark types
- *
- * Enumeration of all possible landmark types.
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- *
- */
-typedef enum
-{
- LANDMARK_POINT, ///< A point landmark, either 3D or 2D
- LANDMARK_CORNER, ///< A corner landmark (2D)
- LANDMARK_CONTAINER ///< A container landmark
-} LandmarkType;
-
-/////////////////////////////////////////////////////////////////////////
-// TYPEDEFS FOR POINTERS AND ITERATORS IN THE WOLF TREE
-/////////////////////////////////////////////////////////////////////////
-// - forwards for pointers
-//class VehicleBase;
-
-
-class NodeTerminus;
-class MapBase;
-class LandmarkBase;
-class TrajectoryBase;
-class FrameBase;
-class CaptureBase;
-class CaptureLaser2D;
-class FeatureBase;
-class FeatureCorner2D;
-class CorrespondenceBase;
-class RawBase;
-class RawLaser2D;
-class SensorBase;
-class SensorLaser2D;
-class TransSensor;
-class StateBase;
-template <unsigned int SIZE> class StatePoint;
-class PinHole;
-
-
-// - Vehicle
-// typedef std::shared_ptr<VehicleBase> VehicleShPtr;
-// typedef VehicleBase* VehiclePtr;
-// typedef std::list<VehicleShPtr> VehicleList;
-// typedef VehicleList::iterator VehicleIter;
-
-//Map
-typedef std::shared_ptr<MapBase> MapBaseShPtr;
-typedef MapBase* MapBasePtr;
-typedef std::list<MapBaseShPtr> MapBaseList;
-typedef MapBaseList::iterator MapBaseIter;
-
-//Landmark
-typedef std::shared_ptr<LandmarkBase> LandmarkBaseShPtr;
-typedef LandmarkBase* LandmarkBasePtr;
-typedef std::list<LandmarkBaseShPtr> LandmarkBaseList;
-typedef LandmarkBaseList::iterator LandmarkBaseIter;
-
-//Trajectory
-typedef std::shared_ptr<TrajectoryBase> TrajectoryBaseShPtr;
-typedef TrajectoryBase* TrajectoryBasePtr;
-// typedef std::list<TrajectoryBaseShPtr> TrajectoryBaseList;
-// typedef TrajectoryBaseList::iterator TrajectoryBaseIter;
-
-// - Frame
-typedef std::shared_ptr<FrameBase> FrameBaseShPtr;
-typedef FrameBase* FrameBasePtr;
-typedef std::list<FrameBaseShPtr> FrameBaseList;
-typedef FrameBaseList::iterator FrameBaseIter;
-
-// - Capture
-typedef std::shared_ptr<CaptureBase> CaptureBaseShPtr;
-typedef CaptureBase* CaptureBasePtr;
-typedef std::list<CaptureBaseShPtr> CaptureBaseList;
-typedef CaptureBaseList::iterator CaptureBaseIter;
-
-// - Feature
-typedef std::shared_ptr<FeatureBase> FeatureBaseShPtr;
-typedef FeatureBase* FeatureBasePtr; ///< Feature pointer type
-typedef std::list<FeatureBaseShPtr> FeatureBaseList;
-typedef FeatureBaseList::iterator FeatureBaseIter;
-
-// - Correspondence
-typedef std::shared_ptr<CorrespondenceBase> CorrespondenceBaseShPtr;
-typedef CorrespondenceBase* CorrespondenceBasePtr;
-typedef std::list<CorrespondenceBaseShPtr> CorrespondenceBaseList;
-typedef CorrespondenceBaseList::iterator CorrespondenceBaseIter;
-
-// - Raw data
-typedef std::shared_ptr<RawBase> RawShPtr;
-typedef RawBase* RawPtr;
-
-// - Sensors
-typedef std::shared_ptr<SensorBase> SensorBaseShPtr;
-typedef SensorBase* SensorBasePtr;
-typedef SensorLaser2D* SensorLaser2DPtr;
-
-// - transSensor
-typedef std::shared_ptr<TransSensor> TransSensorShPtr;
-typedef TransSensor* TransSensorPtr;
-typedef std::map<unsigned int, TransSensorShPtr > TransSensorMap;
-typedef TransSensorMap::iterator TransSensorIter;
-
-// - State Pose
-typedef std::shared_ptr<StateBase> StateBaseShPtr;
-typedef StateBase* StateBasePtr;
-typedef std::list<StateBaseShPtr> StateBaseList;
-typedef StateBaseList::iterator StateBaseIter;
-
-typedef StatePoint<1> StateTheta;
-typedef StatePoint<1> StatePoint1D;
-typedef StatePoint<2> StatePoint2D;
-typedef StatePoint<3> StatePoint3D;
-
-// - Pin hole
-typedef PinHole* PinHolePtr;
-
-
-
-///** \brief Enumeration of all possible feature types
-// *
-// * Enumeration of all possible feature types.
-// *
-// * You may add items to this list as needed. Be concise with names, and document your entries.
-// *
-// */
-//typedef enum FeatureType
-//{
-// BASE, ///< A feature of FeatureBase -- just for completeness
-// POINT, ///< A point feature, either 3D or 2D
-// LINE, ///< a line feature, 2D or 3D
-// P_RANGE, ///< A pseudo-range measurement from GPS satellite
-// V_DOPPLER, ///< Doppler velocity measurement from GPS satellite
-// GPS_FIX, ///< A GPS fix
-// LIDAR_SCAN, ///< Full 2D laser scan
-// LIDAR_RAY ///< A single laser ray
-//} FeatureType;
-
-
-#endif /* WOLF_H_ */
--
GitLab