Skip to content
Snippets Groups Projects
Commit 75a05b04 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Merge remote-tracking branch 'wolf/plugin/core' into devel

parents 2647c44c daf2f472
No related branches found
No related tags found
No related merge requests found
Pipeline #3095 passed
Showing
with 649 additions and 50 deletions
......@@ -2,7 +2,7 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# MAC OSX RPATH
......@@ -169,7 +169,7 @@ ELSE (Suitesparse_INCLUDE_DIRS)
ENDIF (Suitesparse_INCLUDE_DIRS)
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/internal)
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal)
# Create the specified output directory if it does not exist.
IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
......@@ -182,6 +182,7 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
# include spdlog (logging library)
......@@ -191,8 +192,8 @@ IF (SPDLOG_INCLUDE_DIR)
MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
ELSE (SPDLOG_INCLUDE_DIR)
MESSAGE(FATAL_ERROR "Could not find spdlog")
ENDIF (SPDLOG_INCLUDE_DIR)
ENDIF (SPDLOG_INCLUDE_DIR)
INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
include_directories("include")
include_directories(.)
......@@ -243,6 +244,9 @@ SET(HDRS_UTILS
include/core/utils/logging.h
include/core/utils/make_unique.h
include/core/utils/singleton.h
include/core/utils/params_server.hpp
include/core/utils/converter.h
include/core/utils/loader.hpp
)
SET(HDRS_PROBLEM
include/core/problem/problem.h
......@@ -311,6 +315,7 @@ SET(HDRS_LANDMARK
include/core/landmark/landmark_match.h
)
SET(HDRS_PROCESSOR
include/core/processor/autoconf_processor_factory.h
include/core/processor/diff_drive_tools.h
include/core/processor/diff_drive_tools.hpp
include/core/processor/motion_buffer.h
......@@ -332,6 +337,7 @@ SET(HDRS_PROCESSOR
include/core/processor/track_matrix.h
)
SET(HDRS_SENSOR
include/core/sensor/autoconf_sensor_factory.h
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
include/core/sensor/sensor_factory.h
......@@ -347,8 +353,12 @@ SET(HDRS_DTASSC
include/core/association/association_solver.h
include/core/association/association_tree.h
include/core/association/matrix.h
include/core/association/association_nnls.h
)
SET(HDRS_YAML
include/core/yaml/parser_yaml.hpp
)
#SOURCES
SET(SRCS_PROBLEM
src/problem/problem.cpp
......@@ -457,18 +467,19 @@ IF (Ceres_FOUND)
src/solver/solver_manager.cpp
)
ELSE(Ceres_FOUND)
SET(HDRS_WRAPPER)
SET(SRCS_WRAPPER)
SET(HDRS_WRAPPER)
SET(SRCS_WRAPPER)
ENDIF(Ceres_FOUND)
#SUBDIRECTORIES
add_subdirectory(hello_wolf)
add_subdirectory(hello_plugin)
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
IF (Suitesparse_FOUND)
#DOES NOTHING?!
#DOES NOTHING?!
#ADD_SUBDIRECTORY(solver_suitesparse)
ENDIF(Suitesparse_FOUND)
# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
......@@ -487,6 +498,7 @@ ENDIF(YAMLCPP_FOUND)
# create the shared library
ADD_LIBRARY(${PROJECT_NAME}
SHARED
${SRCS}
${SRCS_BASE}
${SRCS_CAPTURE}
${SRCS_COMMON}
......@@ -508,7 +520,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_WRAPPER}
${SRCS_YAML}
)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
#Link the created libraries
#=============================================================
......@@ -605,7 +617,7 @@ configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
"${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/internal)
DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
......@@ -632,12 +644,12 @@ IF (UNIX)
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)
......@@ -648,12 +660,12 @@ IF (UNIX)
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)
......@@ -670,4 +682,3 @@ ELSE(UNIX)
COMMENT "packaging only implemented in unix"
TARGET uninstall)
ENDIF(UNIX)
......@@ -130,8 +130,8 @@ endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
# Set the include directories for wolf (itself).
set(wolf_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include/iri-algorithms")
# if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
# # if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/plugin_core)
wolf_report_not_found(
"wolf install root: ${CURRENT_ROOT_INSTALL_DIR}, "
"determined from relative path from wolfConfig.cmake install location: "
......@@ -139,8 +139,8 @@ if (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
"Either the install directory was deleted, or the install tree was only "
"partially relocated outside of CMake after wolf was built.")
# endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/wolf.h)
endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf)
list(APPEND wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIR}/wolf)
endif (NOT EXISTS ${wolf_INCLUDE_DIR}/wolf/plugin_core)
list(APPEND wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIR}/wolf/plugin_core)
# Set the version.
set(wolf_VERSION 0.0.1)
......@@ -156,7 +156,6 @@ list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3)
# match and reject with an explanation below.
find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET)
# Flag set with currently found Eigen version.
set(EIGEN_VERSION @EIGEN_VERSION@)
if (EIGEN3_FOUND)
......
INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
# INCLUDE_DIRECTORIES(/home/jcasals/workspace/wolf/src)
# link_directories(/home/jcasals/workspace/wolf/lib)
ADD_EXECUTABLE(hello_plugin hello_plugin.cpp)
ADD_EXECUTABLE(params_autoconf params_autoconf.cpp)
# target_link_libraries(hello_plugin class_loader boost_system console_bridge wolf yaml-cpp ${CERES_LIBRARIES})
# target_link_libraries(params_autoconf class_loader boost_system console_bridge wolf yaml-cpp )
target_link_libraries(hello_plugin wolf yaml-cpp ${CERES_LIBRARIES} dl)
target_link_libraries(params_autoconf wolf yaml-cpp dl)
# These lines always at the end
SET(HDRS_PLUGIN ${HDRS_PLUGIN} PARENT_SCOPE )
SET(SRCS_PLUGIN ${SRCS_PLUGIN} PARENT_SCOPE )
/*
* hello_plugin.cpp
*
* Created on: Nov 12, 2018
* Author: jcasals
*/
#include "core/sensor/sensor_base.h"
#include "core/common/wolf.h"
// #include "sensor_odom_2D.cpp"
#include <yaml-cpp/yaml.h>
#include "core/yaml/parser_yaml.hpp"
#include "core/utils/params_server.hpp"
#include "../hello_wolf/capture_range_bearing.h"
#include "../hello_wolf/feature_range_bearing.h"
#include "../hello_wolf/factor_range_bearing.h"
#include "../hello_wolf/landmark_point_2D.h"
#include "core/utils/loader.hpp"
#include "core/processor/processor_odom_2D.h"
#include "core/solver/solver_factory.h"
#include "core/ceres_wrapper/ceres_manager.h"
using namespace std;
using namespace wolf;
using namespace Eigen;
int main(int argc, char** argv) {
string file = "";
if(argc > 1) file = argv[1];
parserYAML parser = parserYAML(file);
parser.parse();
paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
cout << "PRINTING SERVER MAP" << endl;
server.print();
cout << "-----------------------------------" << endl;
/**
It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
**/
// vector<ClassLoader*> class_loaders = vector<ClassLoader*>();
// for(auto it : parser.getFiles()) {
// auto c = new ClassLoader(it);
// class_loaders.push_back(c);
// }
auto loaders = vector<Loader*>();
for(auto it : parser.getFiles()) {
auto l = new LoaderRaw(it);
loaders.push_back(l);
}
ProblemPtr problem = Problem::create("PO", 2);
auto sensorMap = map<string, SensorBasePtr>();
auto procesorMap = map<string, ProcessorBasePtr>();
for(auto s : server.getSensors()){
cout << s._name << " " << s._type << endl;
sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server)));
}
for(auto s : server.getProcessors()){
cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
}
problem->print(4,1,1,1);
Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
// landmark observations data
VectorXi ids;
VectorXs ranges, bearings;
// SET OF EVENTS =======================================================
std::cout << std::endl;
WOLF_TRACE("======== BUILD PROBLEM =======");
// ceres::Solver::Options options;
// options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
// CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
auto ceres = SolverFactory::get().create("Solver", problem, server);
// We'll do 3 steps of motion and landmark observations.
// STEP 1 --------------------------------------------------------------
// initialize
TimeStamp t(0.0); // t : 0.0
Vector3s x(0,0,0);
Matrix3s P = Matrix3s::Identity() * 0.1;
problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
auto sensor_rb = sensorMap.find("rb")->second;
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
ranges << 1.0; // see drawing
bearings << M_PI/2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2)
// STEP 2 --------------------------------------------------------------
t += 1.0; // t : 1.0
// motion
auto sensor_odometry = sensorMap.find("odom")->second;
CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odometry, motion_data, motion_cov);
sensor_odometry ->process(cap_motion); // KF2 : (1,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 1, 2; // will observe Lmks 1 and 2
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
t += 1.0; // t : 2.0
// motion
cap_motion ->setTimeStamp(t);
sensor_odometry ->process(cap_motion); // KF3 : (2,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 2, 3; // will observe Lmks 2 and 3
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1,0,1,0);
// SOLVE ================================================================
// SOLVE with exact initial guess
WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
problem->print(1,0,1,0);
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
for (auto sen : problem->getHardware()->getSensorList())
for (auto sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
for (auto sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto lmk : problem->getMap()->getLandmarkList())
for (auto sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1,0,1,0);
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto kf : problem->getTrajectory()->getFrameList()){
if (kf->isKey())
{
Eigen::MatrixXs cov;
WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
}
for (auto lmk : problem->getMap()->getLandmarkList()) {
Eigen::MatrixXs cov;
WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
}
}
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
problem->print(4,1,1,1);
return 0;
}
config:
sensors:
-
type: "ODOM 2D"
name: "odom"
intrinsic:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
extrinsic:
pos: [1,2,3]
-
type: "RANGE BEARING"
name: "rb"
processors:
-
type: "ODOM 2D"
name: "processor1"
sensor name: "odom"
-
type: "RANGE BEARING"
name: "rb_processor"
sensor name: "rb"
-
type: "ODOM 2D"
name: "my_proc_test"
sensor name: "odom"
follow: "../hello_plugin/params_conf.yaml" #Config continues in this file
files:
- "/home/jcasals/workspace/wip/wolf/lib/libsensor_odom.so"
- "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
/*
* params_autoconf.cpp
*
* Created on: Feb 15, 2019
* Author: jcasals
*/
#include "core/sensor/sensor_base.h"
#include "core/common/wolf.h"
// #include "sensor_odom_2D.cpp"
#include <yaml-cpp/yaml.h>
#include "core/yaml/parser_yaml.hpp"
#include "core/utils/params_server.hpp"
#include "../hello_wolf/capture_range_bearing.h"
#include "../hello_wolf/feature_range_bearing.h"
#include "../hello_wolf/factor_range_bearing.h"
#include "../hello_wolf/landmark_point_2D.h"
#include "core/utils/loader.hpp"
#include "core/processor/processor_odom_2D.h"
#include "core/solver/solver_factory.h"
#include "core/ceres_wrapper/ceres_manager.h"
using namespace std;
using namespace wolf;
using namespace Eigen;
int main(int argc, char** argv) {
string file = "";
if(argc > 1) file = argv[1];
parserYAML parser = parserYAML(file);
parser.parse();
paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
cout << "PRINTING SERVER MAP" << endl;
server.print();
cout << "-----------------------------------" << endl;
/**
It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
**/
auto loaders = vector<Loader*>();
for(auto it : parser.getFiles()) {
auto l = new LoaderRaw(it);
l->load();
loaders.push_back(l);
}
ProblemPtr problem = Problem::create("PO", 2);
auto sensorMap = map<string, SensorBasePtr>();
auto procesorMap = map<string, ProcessorBasePtr>();
for(auto s : server.getSensors()){
cout << s._name << " " << s._type << endl;
sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server)));
}
for(auto s : server.getProcessors()){
cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
}
auto prc = ProcessorParamsOdom2D("my_proc_test", server);
std::cout << "prc.cov_det " << prc.cov_det << std::endl;
std::cout << "prc.unmeasured_perturbation_std " << prc.unmeasured_perturbation_std << std::endl;
std::cout << "prc.angle_turned " << prc.angle_turned << std::endl;
std::cout << "prc.dist_traveled " << prc.dist_traveled << std::endl;
std::cout << "prc.max_buff_length " << prc.max_buff_length << std::endl;
std::cout << "prc.max_time_span " << prc.max_time_span << std::endl;
std::cout << "prc.time_tolerance " << prc.time_tolerance << std::endl;
std::cout << "prc.voting_active " << prc.voting_active << std::endl;
return 0;
}
cov_det: 100
unmeasured_perturbation_std: 100
angle_turned: 100
dist_traveled: 100
max_buff_length: 100
max_time_span: 100
time_tolerance: 100
voting_active: false
\ No newline at end of file
......@@ -13,22 +13,21 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp
${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp
${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
)
ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME})
add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME})
add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME})
# These lines always at the end
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} PARENT_SCOPE )
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} PARENT_SCOPE )
......@@ -118,6 +118,24 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
return prc;
}
ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _unique_name,
const paramsServer& _server,
const SensorBasePtr _sensor_ptr)
{
SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr);
ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>();
params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.01");
// construct processor
ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
// setup processor
prc->setName(_unique_name);
return prc;
}
Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const
{
return polar(toSensor(lmk_w));
......@@ -175,4 +193,9 @@ namespace wolf
{
WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing)
} // namespace wolf
#include "core/processor/autoconf_processor_factory.h"
namespace wolf
{
WOLF_REGISTER_PROCESSOR_AUTO("RANGE BEARING", ProcessorRangeBearing)
} // namespace wolf
......@@ -40,6 +40,9 @@ class ProcessorRangeBearing : public ProcessorBase
static ProcessorBasePtr create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr sensor_ptr = nullptr);
static ProcessorBasePtr createAutoConf(const std::string& _unique_name,
const paramsServer& _server,
const SensorBasePtr _sensor_ptr = nullptr);
protected:
// Implementation of pure virtuals from ProcessorBase
......
......@@ -42,6 +42,16 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
return sensor;
}
SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name, //
const paramsServer& _server)
{
Eigen::Vector2s noise_std(0.1,1.0);
SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(Eigen::Vector3s(1,1,0), noise_std);
sensor->setName(_unique_name);
// sensor->node_name_ = _unique_name;
return sensor;
}
} /* namespace wolf */
// Register in the SensorFactory
......@@ -50,4 +60,8 @@ namespace wolf
{
WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing)
} // namespace wolf
#include "core/sensor/autoconf_sensor_factory.h"
namespace wolf
{
WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing)
} // namespace wolf
......@@ -9,6 +9,7 @@
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.hpp"
namespace wolf
{
......@@ -18,6 +19,16 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
{
Scalar noise_range_metres_std = 0.05;
Scalar noise_bearing_degrees_std = 0.5;
IntrinsicsRangeBearing()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
IntrinsicsRangeBearing(std::string _unique_name, const paramsServer& _server):
IntrinsicsBase(_unique_name, _server)
{
noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std", "0.05");
noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std", "0.5");
}
};
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
......@@ -32,6 +43,8 @@ class SensorRangeBearing : public SensorBase
static SensorBasePtr create(const std::string& _unique_name, //
const Eigen::VectorXs& _extrinsics, //
const IntrinsicsBasePtr _intrinsics);
static SensorBasePtr createAutoConf(const std::string& _unique_name, //
const paramsServer& _server);
};
} /* namespace wolf */
......
......@@ -46,6 +46,8 @@ class CeresManager : public SolverManager
~CeresManager();
static SolverManagerPtr create(const ProblemPtr& wolf_problem, const paramsServer& _server);
ceres::Solver::Summary getSummary();
std::unique_ptr<ceres::Problem>& getCeresProblem()
......
......@@ -9,8 +9,9 @@
#define WOLF_H_
// Enable project-specific definitions and macros
#include "internal/config.h"
#include "core/internal/config.h"
#include "core/utils/logging.h"
#include "core/utils/params_server.hpp"
//includes from Eigen lib
#include <Eigen/Dense>
......@@ -364,7 +365,16 @@ bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
}
//===================================================
struct ParamsBase
{
ParamsBase() = default;
ParamsBase(std::string _unique_name, const paramsServer&)
{
//
}
virtual ~ParamsBase() = default;
};
} // namespace wolf
#endif /* WOLF_H_ */
......@@ -18,6 +18,9 @@ struct ProcessorParamsBase;
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
#include "core/utils/params_server.hpp"
#include "core/sensor/autoconf_sensor_factory.h"
#include "core/processor/autoconf_processor_factory.h"
// std includes
#include <mutex>
......@@ -58,6 +61,7 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file);
virtual ~Problem();
// Properties -----------------------------------------
......@@ -91,7 +95,12 @@ class Problem : public std::enable_shared_from_this<Problem>
const std::string& _unique_sensor_name, //
const Eigen::VectorXs& _extrinsics, //
const std::string& _intrinsics_filename);
/**
Custom installSensor using the parameters server
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name,
const paramsServer& _server);
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
......@@ -125,7 +134,14 @@ class Problem : public std::enable_shared_from_this<Problem>
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename = "");
/** \brief Set the processor motion
/**
Custom installProcessor to be used with parameters server
*/
ProcessorBasePtr installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
const std::string& _corresponding_sensor_name, //
const paramsServer& _server);
/** \brief Set the processor motion
*
* Set the processor motion.
*/
......
/**
* \file processor_factory.h
*
* Created on: May 4, 2016
* \author: jsola
*/
#ifndef AUTOCONF_PROCESSOR_FACTORY_H_
#define AUTOCONF_PROCESSOR_FACTORY_H_
namespace wolf
{
class ProcessorBase;
struct ProcessorParamsBase;
}
// wolf
#include "core/common/factory.h"
// std
namespace wolf
{
/** \brief Processor factory class
*
* This factory can create objects of classes deriving from ProcessorBase.
*
* Specific object creation is invoked by create(TYPE, params), and the TYPE of processor is identified with a string.
* For example, the following processor types are implemented,
* - "ODOM 3D" for ProcessorOdom3D
* - "ODOM 2D" for ProcessorOdom2D
* - "GPS" for ProcessorGPS
*
* The rule to make new TYPE strings unique is that you skip the prefix 'Processor' from your class name,
* and you build a string in CAPITALS with space separators.
* - ProcessorImageFeature -> ````"IMAGE"````
* - ProcessorLaser2D -> ````"LASER 2D"````
* - etc.
*
* The methods to create specific processors are called __creators__.
* Creators must be registered to the factory before they can be invoked for processor creation.
*
* This documentation shows you how to:
* - Access the Factory
* - Register and unregister creators
* - Create processors
* - Write a processor creator for ProcessorOdom2D (example).
*
* #### Accessing the Factory
* The ProcessorFactory class is a singleton: it can only exist once in your application.
* To obtain an instance of it, use the static method get(),
*
* \code
* ProcessorFactory::get()
* \endcode
*
* You can then call the methods you like, e.g. to create a processor, you type:
*
* \code
* ProcessorFactory::get().create(...); // see below for creating processors ...
* \endcode
*
* #### Registering processor creators
* Prior to invoking the creation of a processor of a particular type,
* you must register the creator for this type into the factory.
*
* Registering processor creators into the factory is done through registerCreator().
* You provide a processor type string (above), and a pointer to a static method
* that knows how to create your specific processor, e.g.:
*
* \code
* ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
* \endcode
*
* The method ProcessorOdom2D::create() exists in the ProcessorOdom2D class as a static method.
* All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type.
* This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBasePtr,
* that can be derived for each derived processor.
*
* Here is an example of ProcessorOdom2D::create() extracted from processor_odom_2D.h:
*
* \code
* static ProcessorBasePtr create(const std::string& _name, ProcessorParamsBasePtr _params)
* {
* // cast _params to good type
* ProcessorParamsOdom2D* params = (ProcessorParamsOdom2D*)_params;
*
* ProcessorBasePtr prc = new ProcessorOdom2D(params);
* prc->setName(_name); // pass the name to the created ProcessorOdom2D.
* return prc;
* }
* \endcode
*
* #### Achieving automatic registration
* Currently, registering is performed in each specific ProcessorXxxx source file, processor_xxxx.cpp.
* For example, in processor_odom_2D.cpp we find the line:
*
* \code
* const bool registered_odom_2D = ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2D class).
* Therefore, at application level, all processors that have a .cpp file compiled are automatically registered.
*
* #### Unregister processor creators
* The method unregisterCreator() unregisters the ProcessorXxx::create() method.
* It only needs to be passed the string of the processor type.
*
* \code
* ProcessorFactory::get().unregisterCreator("ODOM 2D");
* \endcode
*
* #### Creating processors
* Prior to invoking the creation of a processor of a particular type,
* you must register the creator for this type into the factory.
*
* To create a ProcessorOdom2D, you type:
*
* \code
* ProcessorFactory::get().create("ODOM 2D", "main odometry", params_ptr);
* \endcode
*
* #### Example 1 : using the Factories alone
* We provide the necessary steps to create a processor of class ProcessorOdom2D in our application,
* and bind it to a SensorOdom2D:
*
* \code
* #include "sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory
* #include "processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory
*
* // Note: SensorOdom2D::create() is already registered, automatically.
* // Note: ProcessorOdom2D::create() is already registered, automatically.
*
* // First create the sensor (See SensorFactory for details)
* SensorBasePtr sensor_ptr = SensorFactory::get().create ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
*
* // To create a odometry integrator, provide a type="ODOM 2D", a name="main odometry", and a pointer to the parameters struct:
*
* ProcessorParamsOdom2D params({...}); // fill in the derived struct (note: ProcessorOdom2D actually has no input params)
*
* ProcessorBasePtr processor_ptr =
* ProcessorFactory::get().create ( "ODOM 2D" , "main odometry" , &params );
*
* // Bind processor to sensor
* sensor_ptr->addProcessor(processor_ptr);
* \endcode
*
* #### Example 2: Using the helper API in class Problem
* The WOLF uppermost node, Problem, makes the creation of sensors and processors, and the binding between them, even simpler.
*
* The creation is basically replicating the factories' API. The binding is accomplished by passing the sensor name to the Processor installer.
*
* The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation),
*
* \code
* #include "sensor_odom_2D.h"
* #include "processor_odom_2D.h"
* #include "problem.h"
*
* Problem problem(FRM_PO_2D);
* problem.installSensor ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
* problem.installProcessor ( "ODOM 2D" , "Odometry" , "Main odometer" , &params );
* \endcode
*
* You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
*/
typedef Factory<ProcessorBase,
const std::string&,
const paramsServer&,
const SensorBasePtr> AutoConfProcessorFactory;
template<>
inline std::string AutoConfProcessorFactory::getClass()
{
return "AutoConfProcessorFactory";
}
#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType, ProcessorName) \
namespace{ const bool WOLF_UNUSED ProcessorName##AutoConf##Registered = \
wolf::AutoConfProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::createAutoConf); }\
} /* namespace wolf */
#endif /* PROCESSOR_FACTORY_H_ */
......@@ -11,6 +11,7 @@ class SensorBase;
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
#include "core/frame/frame_base.h"
#include "core/utils/params_server.hpp"
// std
#include <memory>
......@@ -105,10 +106,9 @@ class PackKeyFrameBuffer
*
* Derive from this struct to create structs of processor parameters.
*/
struct ProcessorParamsBase
struct ProcessorParamsBase : public ParamsBase
{
ProcessorParamsBase() = default;
ProcessorParamsBase(bool _voting_active,
Scalar _time_tolerance,
bool _voting_aux_active = false) :
......@@ -118,6 +118,12 @@ struct ProcessorParamsBase
{
//
}
ProcessorParamsBase(std::string _unique_name, const paramsServer& _server):
ParamsBase(_unique_name, _server)
{
voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0");
}
virtual ~ProcessorParamsBase() = default;
......
......@@ -10,6 +10,7 @@
#include "core/processor/processor_motion.h"
#include "core/processor/diff_drive_tools.h"
#include "core/utils/params_server.hpp"
namespace wolf {
......@@ -17,18 +18,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
{
// ProcessorParamsDiffDrive(const Scalar _time_tolerance,
// const Scalar _dist_travel_th,
// const Scalar _theta_traveled_th,
// const Scalar _cov_det_th,
// const Scalar _unmeasured_perturbation_std = 0.0001) :
// dist_traveled_th_(_dist_travel_th),
// theta_traveled_th_(_theta_traveled_th),
// cov_det_th_(_cov_det_th),
// unmeasured_perturbation_std_(_unmeasured_perturbation_std)
// {
// time_tolerance = _time_tolerance;
// }
Scalar unmeasured_perturbation_std = 0.0001;
ProcessorParamsDiffDrive() = default;
ProcessorParamsDiffDrive(std::string _unique_name, const paramsServer& _server):
ProcessorParamsMotion(_unique_name, _server)
{
unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
}
};
/**
......
......@@ -4,6 +4,7 @@
// Wolf related headers
#include "core/processor/processor_loopclosure_base.h"
#include "core/state_block/state_block.h"
#include "core/utils/params_server.hpp"
namespace wolf{
......@@ -38,7 +39,20 @@ struct ProcessorParamsFrameNearestNeighborFilter : public ProcessorParamsLoopClo
{
//
}
ProcessorParamsFrameNearestNeighborFilter(std::string _unique_name, const paramsServer& _server):
ProcessorParamsLoopClosure(_unique_name, _server)
{
buffer_size_ = _server.getParam<int>(_unique_name + "/buffer_size");
sample_step_degree_ = _server.getParam<int>(_unique_name + "/sample_step_degree");
auto distance_type_str = _server.getParam<std::string>(_unique_name + "/distance_type");
if(distance_type_str.compare("LC_POINT_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSE;
else if(distance_type_str.compare("LC_ELLIPSE_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
else if(distance_type_str.compare("LC_POINT_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSOID;
else if(distance_type_str.compare("LC_ELLIPSOID_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID;
else if(distance_type_str.compare("LC_MAHALANOBIS_DISTANCE")) distance_type_ = LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE;
else throw std::runtime_error("Failed to fetch a valid value for the enumerate LoopclosureDistanceType. Value provided: " + distance_type_str);
probability_ = _server.getParam<Scalar>(_unique_name + "/probability");
}
virtual ~ProcessorParamsFrameNearestNeighborFilter() = default;
int buffer_size_;
......
......@@ -10,6 +10,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure);
struct ProcessorParamsLoopClosure : public ProcessorParamsBase
{
using ProcessorParamsBase::ProcessorParamsBase;
// virtual ~ProcessorParamsLoopClosure() = default;
// add neccesery parameters for loop closure initialisation here and initialize
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment