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

Merge branch 'devel' into 298-params-and-factory-renaming

parents e6921d4d b2a1bd15
No related branches found
No related tags found
1 merge request!351WIP: Resolve "Params and factory renaming"
Hide whitespace changes
Inline Side-by-side
Showing
with 56 additions and 140 deletions
CMakeLists.txt
+ 40
− 31
View file @ 81deebf4
......@@ -246,6 +246,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_pose_3d.h
include/core/factor/factor_quaternion_absolute.h
include/core/factor/factor_relative_2d_analytic.h
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
......@@ -285,7 +286,11 @@ SET(HDRS_SOLVER
include/core/solver/solver_manager.h
include/core/solver/factory_solver.h
)
SET(HDRS_TREE_MANAGER
include/core/tree_manager/factory_tree_manager.h
include/core/tree_manager/tree_manager_base.h
include/core/tree_manager/tree_manager_sliding_window.h
)
SET(HDRS_YAML
include/core/yaml/parser_yaml.h
include/core/yaml/yaml_conversion.h
......@@ -371,6 +376,9 @@ SET(SRCS_SENSOR
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
SET(SRCS_TREE_MANAGER
src/tree_manager/tree_manager_sliding_window.cpp
)
SET(SRCS_YAML
src/yaml/parser_yaml.cpp
src/yaml/processor_odom_3d_yaml.cpp
......@@ -400,8 +408,6 @@ ELSE(Ceres_FOUND)
SET(SRCS_WRAPPER)
ENDIF(Ceres_FOUND)
#SUBDIRECTORIES
add_subdirectory(hello_wolf)
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
......@@ -431,6 +437,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
${SRCS_TRAJECTORY}
${SRCS_TREE_MANAGER}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML}
......@@ -471,44 +478,46 @@ INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
#install headers
INSTALL(FILES ${HDRS_MATH}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
INSTALL(FILES ${HDRS_UTILS}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
INSTALL(FILES ${HDRS_PROBLEM}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
INSTALL(FILES ${HDRS_HARDWARE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
INSTALL(FILES ${HDRS_TRAJECTORY}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
INSTALL(FILES ${HDRS_MAP}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
INSTALL(FILES ${HDRS_FRAME}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
INSTALL(FILES ${HDRS_COMMON}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
INSTALL(FILES ${HDRS_COMMON}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature)
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
INSTALL(FILES ${HDRS_FRAME}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
INSTALL(FILES ${HDRS_HARDWARE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/landmark)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
INSTALL(FILES ${HDRS_SOLVER}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
INSTALL(FILES ${HDRS_MAP}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
INSTALL(FILES ${HDRS_MATH}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
INSTALL(FILES ${HDRS_PROBLEM}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
INSTALL(FILES ${HDRS_SERIALIZATION}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/serialization)
INSTALL(FILES ${HDRS_SOLVER}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
INSTALL(FILES ${HDRS_TRAJECTORY}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
INSTALL(FILES ${HDRS_TREE_MANAGER}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
INSTALL(FILES ${HDRS_UTILS}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
......
demos/CMakeLists.txt 0 → 100644
+ 2
− 0
View file @ 81deebf4
#Add hello_wolf demo
add_subdirectory(hello_wolf)
demos/demo_map_yaml.cpp deleted 100644 → 0
+ 0
− 101
View file @ e6921d4d
/**
* \file test_map_yaml.cpp
*
* Created on: Jul 27, 2016
* \author: jsola
*/
#include "core/common/wolf.h"
#include "core/problem/problem.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_polyline_2d.h"
#include "core/landmark/landmark_AHP.h"
#include "core/state_block/state_block.h"
#include "core/yaml/yaml_conversion.h"
#include <iostream>
using namespace wolf;
void print(MapBase& _map)
{
for (auto lmk_ptr : _map.getLandmarkList())
{
std::cout << "Lmk ID: " << lmk_ptr->id();
std::cout << "\nLmk type: " << lmk_ptr->getType();
if (lmk_ptr->getType() == "POLYLINE 2d")
{
LandmarkPolyline2dPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2d>(lmk_ptr);
std::cout << "\npos: " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
std::cout << "\nori: " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
std::cout << "\nn points: " << poly_ptr->getNPoints();
std::cout << "\nFirst idx: " << poly_ptr->getFirstId();
std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
std::cout << "\nLast def: " << poly_ptr->isLastDefined();
for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
break;
}
else if (lmk_ptr->getType() == "AHP")
{
LandmarkAHPPtr ahp_ptr = std::static_pointer_cast<LandmarkAHP>(lmk_ptr);
std::cout << "\npos: " << ahp_ptr->getP()->getState().transpose() << " -- fixed: " << ahp_ptr->getP()->isFixed();
std::cout << "\ndescript: " << ahp_ptr->getCvDescriptor().t();
break;
}
else
break;
std::cout << std::endl;
}
}
int main()
{
using namespace Eigen;
std::cout << "\nTesting Lmk creator and node saving..." << std::endl;
Vector4d v;
v << 1, 2, 3, 4;
cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
LandmarkAHP lmk_1(v, nullptr, nullptr, d);
std::cout << "Pos 1 = " << lmk_1.getP()->getState().transpose() << std::endl;
std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
YAML::Node n = lmk_1.saveToYaml();
std::cout << "Pos n = " << n["position"].as<VectorXd>().transpose() << std::endl;
std::cout << "Des n = " << n["descriptor"].as<VectorXd>().transpose() << std::endl;
LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
std::cout << "Des 2 = " << lmk_2.getCvDescriptor().t() << std::endl;
std::string filename;
std::string wolf_root = _WOLF_ROOT_DIR;
std::string wolf_config = wolf_root + "/src/examples";
std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
ProblemPtr problem = Problem::create("PO", 2);
filename = wolf_config + "/map_polyline_example.yaml";
std::cout << "Reading map from file: " << filename << std::endl;
problem->loadMap(filename);
std::cout << "Printing map..." << std::endl;
print(*(problem->getMap()));
filename = wolf_config + "/map_polyline_example_write.yaml";
std::cout << "Writing map to file: " << filename << std::endl;
std::string thisfile = __FILE__;
problem->getMap()->save(filename, "Example generated by test file " + thisfile);
std::cout << "Clearing map... " << std::endl;
problem->getMap()->getLandmarkList().clear();
std::cout << "Re-reading map from file: " << filename << std::endl;
problem->loadMap(filename);
std::cout << "Printing map..." << std::endl;
print(*(problem->getMap()));
return 0;
}
hello_wolf/CMakeLists.txt → demos/hello_wolf/CMakeLists.txt
+ 0
− 0
View file @ 81deebf4
File moved
hello_wolf/capture_range_bearing.cpp → demos/hello_wolf/capture_range_bearing.cpp
+ 1
− 1
View file @ 81deebf4
......@@ -11,7 +11,7 @@ namespace wolf
{
CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
CaptureBase("RANGE BEARING", _ts, _scanner),
CaptureBase("CaptureRangeBearing", _ts, _scanner),
ids_(_ids),
ranges_(_ranges),
bearings_(_bearings)
......
hello_wolf/factor_bearing.h → demos/hello_wolf/factor_bearing.h
+ 0
− 0
View file @ 81deebf4
File moved
hello_wolf/feature_range_bearing.cpp → demos/hello_wolf/feature_range_bearing.cpp
+ 1
− 1
View file @ 81deebf4
......@@ -11,7 +11,7 @@ namespace wolf
{
FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
FeatureBase("RANGE BEARING", _measurement, _meas_covariance)
FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
{
//
}
......
hello_wolf/hello_wolf.cpp → demos/hello_wolf/hello_wolf.cpp
+ 0
− 0
View file @ 81deebf4
File moved
hello_wolf/hello_wolf_autoconf.cpp → demos/hello_wolf/hello_wolf_autoconf.cpp
+ 1
− 1
View file @ 81deebf4
......@@ -98,7 +98,7 @@ int main()
WOLF_TRACE("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
std::string config_file = "hello_wolf/yaml/hello_wolf_config.yaml";
std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
std::string wolf_path = std::string(_WOLF_ROOT_DIR);
// parse file into params server: each param will be retrievable from this params server:
......
hello_wolf/landmark_point_2d.cpp → demos/hello_wolf/landmark_point_2d.cpp
+ 1
− 1
View file @ 81deebf4
......@@ -11,7 +11,7 @@ namespace wolf
{
LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
LandmarkBase("POINT 2d", std::make_shared<StateBlock>(_xy))
LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
{
setId(_id);
}
......
hello_wolf/yaml/hello_wolf_config.yaml → demos/hello_wolf/yaml/hello_wolf_config.yaml
+ 9
− 4
View file @ 81deebf4
......@@ -2,6 +2,11 @@ config:
problem:
tree_manager:
type: "TreeManagerSlidingWindow"
n_key_frames: -1
fix_first_key_frame: false
viral_remove_empty_parent: true
frame_structure: "PO" # keyframes have position and orientation
dimension: 2 # space is 2d
prior:
......@@ -17,7 +22,7 @@ config:
name: "sen odom"
extrinsic:
pose: [0,0, 0]
follow: "hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
- type: "SensorRangeBearing"
plugin: "core"
......@@ -25,7 +30,7 @@ config:
extrinsic:
pose: [1,1, 0]
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
follow: hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
processors:
......@@ -33,10 +38,10 @@ config:
plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
follow: hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
follow: demos/hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
- type: "ProcessorRangeBearing"
plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
follow: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
follow: demos/hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
hello_wolf/yaml/processor_odom_2d.yaml → demos/hello_wolf/yaml/processor_odom_2d.yaml
+ 1
− 0
View file @ 81deebf4
......@@ -10,3 +10,4 @@ keyframe_vote:
angle_turned: 999
max_buff_length: 999
cov_det: 999
apply_loss_function: true
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment