Commit 844481a1 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Implemented all object slam classes, very simple processor WIP

parents
.cproject
.project
.settings/
README.txt
bin/
build/
build_release/
lib/
.idea/
./Wolf.user
./Wolf.config
./Wolf.creator
./Wolf.files
./Wolf.includes
./Wolf/
/CMakeLists.txt.user
src/examples/map_polyline_example_write.yaml
*.gch
/CMakeFiles/
/CMakeCache.txt
*.dat
.DS_Store
*.graffle
/Default/
src/CMakeCache.txt
src/CMakeFiles/cmake.check_cache
objectslam.found
\.vscode/
build_release/
Testing/Temporary/LastTest.log
Testing/Temporary/CTestCostData.txt
# 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)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
# The project name
PROJECT(objectslam)
set(PLUGIN_NAME wolf${PROJECT_NAME})
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()
if(UNIX)
# GCC is not strict enough by default, so enable most of the warnings.
set(CMAKE_CXX_FLAGS
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
IF(NOT BUILD_DEMOS)
OPTION(BUILD_DEMOS "Build Demos" OFF)
ENDIF(NOT BUILD_DEMOS)
IF(NOT BUILD_DOC)
OPTION(BUILD_DOC "Build Documentation" OFF)
ENDIF(NOT BUILD_DOC)
#############
## Testing ##
#############
#
if(BUILD_TESTS)
# Enables testing for this directory and below.
# Note that ctest expects to find a test file in the build directory root.
# Therefore, this command should be in the source directory root.
#include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
enable_testing()
endif()
MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
#CMAKE modules
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
MESSAGE(STATUS ${CMAKE_MODULE_PATH})
# Some wolf compilation options
IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
set(_WOLF_DEBUG true)
ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# Does this has any other interest
# but for the examples ?
# yes, for the tests !
IF(BUILD_EXAMPLES OR BUILD_TESTS)
string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
#find dependencies.
# ============EXAMPLE==================
FIND_PACKAGE(wolfcore REQUIRED)
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
# Create the specified output directory if it does not exist.
IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
message(STATUS "Creating config output directory: ${WOLF_CONFIG_DIR}")
file(MAKE_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
"${WOLF_CONFIG_DIR} exists, but is not a directory.")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
#INCLUDES SECTION
# ============EXAMPLE==================
INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
include_directories(BEFORE "include")
#HEADERS
SET(HDRS_CAPTURE
)
SET(HDRS_COMMON
)
SET(HDRS_FACTOR
include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h
)
SET(HDRS_FEATURE
include/objectslam/feature/feature_object.h
)
SET(HDRS_LANDMARK
include/objectslam/landmark/landmark_object.h
)
SET(HDRS_MATH
)
SET(HDRS_PROCESSOR
include/objectslam/processor/processor_tracker_landmark_object.h
)
SET(HDRS_SENSOR
)
SET(HDRS_SOLVER
)
SET(HDRS_UTILS
)
SET(HDRS_YAML
)
#SOURCES
SET(SRCS_CAPTURE
src/capture/capture_object.cpp
)
SET(SRCS_COMMON
)
SET(SRCS_FACTOR
)
SET(SRCS_FEATURE
src/feature/feature_object.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_object.cpp
)
SET(SRCS_MATH
)
SET(SRCS_PROCESSOR
src/processor/processor_tracker_landmark_object.cpp
)
SET(SRCS_SENSOR
)
SET(SRCS_SOLVER
)
SET(SRCS_UTILS
)
SET(SRCS_YAML
src/yaml/processor_tracker_landmark_object_yaml.cpp
)
# create the shared library
ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS_CAPTURE}
${SRCS_COMMON}
${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_LANDMARK}
${SRCS_MATH}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
${SRCS_SOLVER}
${SRCS_UTILS}
${SRCS_YAML}
)
# Set compiler options
# ====================
if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
message(STATUS "Using C++ compiler clang")
target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
# using Clang
elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
message(STATUS "Using C++ compiler gnu")
target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
# using GCC
endif()
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
#Link the created libraries
#===============EXAMPLE=========================
# IF (Ceres_FOUND)
# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
# ENDIF(Ceres_FOUND)
#Build tests
#===============EXAMPLE=========================
IF(BUILD_TESTS)
MESSAGE("Building tests.")
add_subdirectory(test)
ENDIF(BUILD_TESTS)
#install library
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iri-algorithms
ARCHIVE DESTINATION lib/iri-algorithms)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_COMMON}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/common)
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
INSTALL(FILES ${HDRS_MATH}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
INSTALL(FILES ${HDRS_SOLVER}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
INSTALL(FILES ${HDRS_UTILS}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/utils)
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
FILE(WRITE ${PROJECT_NAME}.found "")
INSTALL(FILES ${PROJECT_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
export(PACKAGE ${PLUGIN_NAME})
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)
WOLF - Windowed Localization Frames | objectslam Plugin
===================================
For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
#edit the following line to add the librarie's header files
FIND_PATH(
wolfobjectslam_INCLUDE_DIRS
NAMES objectslam.found
PATHS /usr/local/include/iri-algorithms/wolf/plugin_objectslam)
IF(wolfobjectslam_INCLUDE_DIRS)
MESSAGE("Found wolf objectslam include dirs: ${wolfobjectslam_INCLUDE_DIRS}")
ELSE(wolfobjectslam_INCLUDE_DIRS)
MESSAGE("Couldn't find wolf objectslam include dirs")
ENDIF(wolfobjectslam_INCLUDE_DIRS)
FIND_LIBRARY(
wolfobjectslam_LIBRARIES
NAMES libwolfobjectslam.so libwolfobjectslam.dylib
PATHS /usr/local/lib/iri-algorithms)
IF(wolfobjectslam_LIBRARIES)
MESSAGE("Found wolf objectslam lib: ${wolfobjectslam_LIBRARIES}")
ELSE(wolfobjectslam_LIBRARIES)
MESSAGE("Couldn't find wolf objectslam lib")
ENDIF(wolfobjectslam_LIBRARIES)
IF (wolfobjectslam_INCLUDE_DIRS AND wolfobjectslam_LIBRARIES)
SET(wolfobjectslam_FOUND TRUE)
ELSE(wolfobjectslam_INCLUDE_DIRS AND wolfobjectslam_LIBRARIES)
set(wolfobjectslam_FOUND FALSE)
ENDIF (wolfobjectslam_INCLUDE_DIRS AND wolfobjectslam_LIBRARIES)
IF (wolfobjectslam_FOUND)
IF (NOT wolfobjectslam_FIND_QUIETLY)
MESSAGE(STATUS "Found wolf objectslam: ${wolfobjectslam_LIBRARIES}")
ENDIF (NOT wolfobjectslam_FIND_QUIETLY)
ELSE (wolfobjectslam_FOUND)
IF (wolfobjectslam_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find wolf objectslam")
ENDIF (wolfobjectslam_FIND_REQUIRED)
ENDIF (wolfobjectslam_FOUND)
macro(wolf_report_not_found REASON_MSG)
set(wolfobjectslam_FOUND FALSE)
unset(wolfobjectslam_INCLUDE_DIRS)
unset(wolfobjectslam_LIBRARIES)
# Reset the CMake module path to its state when this script was called.
set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
# FindPackage() use the camelcase library name, not uppercase.
if (wolfobjectslam_FIND_QUIETLY)
message(STATUS "Failed to find wolf objectslam- " ${REASON_MSG} ${ARGN})
else (wolfobjectslam_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find wolf objectslam - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
# that prevents generation, but continues configuration.
message(SEND_ERROR "Failed to find wolf objectslam - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(wolf_report_not_found)
if(NOT wolfobjectslam_FOUND)
wolf_report_not_found("Something went wrong while setting up wolf objectslam.")
endif(NOT wolfobjectslam_FOUND)
# Set the include directories for wolf (itself).
set(wolfobjectslam_FOUND TRUE)
# Now we gather all the required dependencies for Wolf Laser
FIND_PACKAGE(wolfvision REQUIRED)
list(APPEND wolfobjectslam_INCLUDE_DIRS ${wolfvision_INCLUDE_DIR})
list(APPEND wolfobjectslam_LIBRARIES ${wolfvision_LIBRARY})
if(NOT OpenCV_FOUND)
FIND_PACKAGE(OpenCV REQUIRED)
list(APPEND wolfobjectslam_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
list(APPEND wolfobjectslam_LIBRARIES ${OpenCV_LIBS})
endif()
#Making sure wolf is looked for
if(NOT wolf_FOUND)
FIND_PACKAGE(wolfcore REQUIRED)
#We reverse in order to insert at the start
list(REVERSE wolfobjectslam_INCLUDE_DIRS)
list(APPEND wolfobjectslam_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
list(REVERSE wolfobjectslam_INCLUDE_DIRS)
list(REVERSE wolfobjectslam_LIBRARIES)
list(APPEND wolfobjectslam_LIBRARIES ${wolfcore_LIBRARIES})
list(REVERSE wolfobjectslam_LIBRARIES)
endif()
# Vision
IF(vision_utils_FOUND)
IF(Ceres_FOUND)
# IF (APRILTAG_LIBRARY)
# ADD_EXECUTABLE(demo_apriltag demo_apriltag.cpp)
# TARGET_LINK_LIBRARIES(demo_apriltag ${PROJECT_NAME})
# ENDIF(APRILTAG_LIBRARY)
ENDIF(Ceres_FOUND)
ENDIF(vision_utils_FOUND)
#ifndef CAPTURE_OBJECT_H_
#define CAPTURE_OBJECT_H_
//Wolf includes
#include "core/capture/capture_base.h"
namespace wolf {
typedef struct ObjectDetection
{
const Eigen::Vector7d & measurement;
const Eigen::Matrix6d & meas_covariance;
const std::string & object_type;
} ObjectDetection;
typedef std::list<ObjectDetection> ObjectDetections;
WOLF_PTR_TYPEDEFS(CaptureObject);
class CaptureObject : public CaptureBase
{
public:
CaptureObject(const TimeStamp& _ts, const ObjectDetections& _object_dets);
~CaptureObject() override;
/** \brief Returns the object detected in current sensor input (image for instance)
*
**/
ObjectDetections getObjectDetections() const;
private:
ObjectDetections object_detections_;
};
} // namespace wolf
#endif
#ifndef FEATURE_OBJECT_H_
#define FEATURE_OBJECT_H_
//Wolf includes
#include "core/feature/feature_base.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureObject);
//class FeatureObject
class FeatureObject : public FeatureBase
{
public:
FeatureObject(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_covariance,
const std::string & _object_type,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
~FeatureObject() override;
/** \brief Returns object type
*
* Returns object type
*
**/
std::string getObjectType() const;
private:
std::string object_type_;
};
} // namespace wolf
#endif
#ifndef LANDMARK_OBJECT_H_
#define LANDMARK_OBJECT_H_
//Wolf includes
#include "core/landmark/landmark_base.h"
#include "core/state_block/state_quaternion.h"
// Std includes
namespace wolf {
WOLF_PTR_TYPEDEFS(LandmarkObject);
//class LandmarkObject
class LandmarkObject : public LandmarkBase
{
public:
/** \brief Constructor with type, time stamp and the position state pointer
*
* Constructor with type, and state pointer
* \param _pose: Concatenation of landmark position and quaternion vectors
* \param _object_type : type of the object represented by the landmark
*
**/
LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type);
~LandmarkObject() override;
/** \brief Returns the object type
*
**/
const std::string& getObjectType() const;
/** Factory method to create new landmarks from YAML nodes
*/
static LandmarkBasePtr create(const YAML::Node& _lmk_node);
YAML::Node saveToYaml() const override;
private:
const std::string object_type_;
};
} // namespace wolf
#endif
#ifndef _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_
#define _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_
//Wolf includes
#include "core/common/wolf.h"
#include "core/processor/processor_tracker_landmark.h"
#include <core/factor/factor_distance_3d.h>
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmarkObject);
struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandmark
{
double min_time_vote_;
double max_time_vote_;
int nb_vote_for_every_first_;
bool add_3d_cstr_;
bool reestimate_last_frame_;
ParamsProcessorTrackerLandmarkObject() = default;
ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorTrackerLandmark(_unique_name, _server)