Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Showing
with 45 additions and 503 deletions
map name: "Example of map of Apriltag landmarks"
nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error.
landmarks:
- id : 1 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 1
tag width: 0.1
position: [0, 0, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
- id : 3 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 3
tag width: 0.1
position: [1, 1, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
- id : 5 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 5
tag width: 0.1
position: [1, 0, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
- id : 2 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 2
tag width: 0.1
position: [0, 1, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
map name: "Example of map of Polyline2D landmarks"
nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error.
landmarks:
- id: 1
type: "POLYLINE 2D" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
position: [1, 1]
orientation: [1]
position fixed: true
orientation fixed: true
classification: 0
first_id: 0
first_defined: false
last_defined: false
points:
- [1, 1]
- [1, 2]
- [1, 3]
- id: 4
type: "POLYLINE 2D" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
position: [4, 4]
orientation: [4]
position fixed: true
orientation fixed: true
classification: 0
first_id: -2
first_defined: false
last_defined: true
points:
- [4, 1]
- [4, 2]
- [4, 3]
- [4, 4]
- id: 6
type: "POLYLINE 2D" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
position: [6, 6]
orientation: [6]
position fixed: true
orientation fixed: true
classification: 0
first_id: 0
first_defined: true
last_defined: false
points:
- [6, 1]
- [6, 2]
- id: 7
type: "AHP"
position: [1, 2, 3, 4]
descriptor: [1, 0, 1, 0, 1, 1, 0, 0]
\ No newline at end of file
map name: "4 tags printed on a A4 sheet vertical recorded at IRI with logitech webcam."
nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error.
######################
# World frame is considered to be the top left corner of tag id 0. Reference frame is corherent with a camera
# looking straight at the sheet with RBF convention.
######################
landmarks:
- id : 0 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 0
tag width: 0.055
position: [0.0225, 0.0225, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
- id : 1 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 1
tag width: 0.055
position: [0.1525, 0.0225, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
- id : 2 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 2
tag width: 0.055
position: [0.0225, 0.2125, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
- id : 3 # use same as tag id
type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
tag id: 3
tag width: 0.055
position: [0.1525, 0.2125, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
position fixed: true
orientation fixed: true
processor type: "IMAGE ORB"
processor name: "ORB feature tracker"
vision_utils:
YAML file params: processor_image_vision_utils.yaml
algorithm:
maximum new features: 40
minimum features for new keyframe: 40
minimum response for new features: 80 #0.0005
time tolerance: 0.01
distance: 20
noise:
pixel noise std: 1 # pixels
draw: # Used to control drawing options
primary draw: true
secondary draw: true
detection roi: true
tracking roi: false
sensor:
type: "USB_CAM"
detector:
type: "ORB"
nfeatures: 100
scale factor: 1.2
nlevels: 8
edge threshold: 8 # 16
first level: 0
WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
patch size: 15 # 31
descriptor:
type: "ORB"
nfeatures: 100
scale factor: 1.2
nlevels: 8
edge threshold: 8 # 16
first level: 0
WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
patch size: 15 # 31
matcher:
type: "FLANNBASED"
match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
roi:
width: 20
height: 20
min normalized score: 0.85
algorithm:
type: "ACTIVESEARCH"
draw results: false
grid horiz cells: 12
grid vert cells: 8
separation: 10
min features to track: 5
max new features: 40
min response new features: 80
\ No newline at end of file
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
unmeasured perturbation std: 0.00001
time tolerance: 0.0025 # Time tolerance for joining KFs
keyframe vote:
max time span: 2.0 # seconds
max buffer length: 20000 # motion deltas
dist traveled: 2.0 # meters
angle turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
voting_active: false
\ No newline at end of file
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
time tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 999999.0 # seconds
max buffer length: 999999 # motion deltas
dist traveled: 999999.0 # meters
angle turned: 999999 # radians (1 rad approx 57 deg, approx 60 deg)
voting_active: false
\ No newline at end of file
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
unmeasured perturbation std: 0.00001
time tolerance: 0.0025 # Time tolerance for joining KFs
keyframe vote:
max time span: 0.9999 # seconds
max buffer length: 10000 # motion deltas
dist traveled: 10000 # meters
angle turned: 10000 # radians (1 rad approx 57 deg, approx 60 deg)
voting_active: true
\ No newline at end of file
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
unmeasured perturbation std: 0.00001
time tolerance: 0.0025 # Time tolerance for joining KFs
keyframe vote:
max time span: 5.9999 # seconds
max buffer length: 10000 # motion deltas
dist traveled: 10000 # meters
angle turned: 10000 # radians (1 rad approx 57 deg, approx 60 deg)
voting_active: true
\ No newline at end of file
processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
time tolerance: 0.01 # seconds
keyframe vote:
max time span: 0.2 # seconds
max buffer length: 10 # motion deltas
dist traveled: 0.5 # meters
angle turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
time_tolerance: 0.01 # seconds
unmeasured_perturbation_std: 0.001
keyframe_vote:
voting_active: false
voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 0.5 # meters
angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
processor type: "TRACKER FEATURE TRIFOCAL"
processor name: "tracker feature trifocal example"
vision_utils:
YAML file params: ACTIVESEARCH.yaml
algorithm:
time tolerance: 0.01
voting active: true
minimum features for keyframe: 40
maximum new features: 40
grid horiz cells: 10
grid vert cells: 10
min response new features: 50
max euclidean distance: 20
noise:
pixel noise std: 1 # pixels
processor type: "TRACKER LANDMARK APRILTAG"
processor name: "tracker landmark apriltag example"
detector parameters:
quad_decimate: 1.5 # doing quad detection at lower resolution to speed things up (see end of file)
quad_sigma: 0.8 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file)
nthreads: 8 # how many thread during tag detection (does not change much?)
debug: false # write some debugging images
refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff)
ippe_min_ratio: 3.0 # quite arbitrary, always > 1 (to deactive, set at 0 for example)
ippe_max_rep_error: 2.0 # to deactivate, set at something big (100)
tag widths:
0: 0.055
1: 0.055
2: 0.055
3: 0.055
tag parameters:
tag_family: "tag36h11"
# tag_black_border: 1
tag_width_default: 0.165 # used if tag width not specified
noise:
std_xy : 0.1 # m
std_z : 0.1 # m
std_rpy_degrees : 5 # degrees
std_pix: 20 # pixel error
vote:
voting active: true
min_time_vote: 0 # s
max_time_vote: 0 # s
min_features_for_keyframe: 12
max_features_diff: 17
nb_vote_for_every_first: 50
enough_info_necessary: true # create kf if enough information to uniquely determine the KF position (necessary for apriltag_only slam)
reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor
add_3D_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only
# Annexes:
### Quad decimate: the higher, the lower the resolution
# Does nothing if <= 1.0
# Only values taken into account:
# 1.5, 2, 3, 4
# 1.5 -> ~*2 speed up
# Higher values use a "bad" code according to commentaries in the library, smaller do nothing
### Gaussian blur table:
# max sigma kernel size
# 0.499 1 (disabled)
# 0.999 3
# 1.499 5
# 1.999 7
sensor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
sensor name: "Main IMU" # This is ignored. The name provided to the SensorFactory prevails
motion variances:
a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
ab_initial_stdev: 0.800 # m/s2 - initial bias
wb_initial_stdev: 0.350 # rad/sec - initial bias
ab_rate_stdev: 0.1 # m/s2/sqrt(s)
wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
\ No newline at end of file
sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
motion variances:
disp_to_disp: 0.02 # m^2 / m
disp_to_rot: 0.02 # rad^2 / m
rot_to_rot: 0.01 # rad^2 / rad
min_disp_var: 0.01 # m^2
min_rot_var: 0.01 # rad^2
type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
k_rot_to_rot: 0.01 # rad^2 / rad
min_disp_var: 0.01 # m^2
min_rot_var: 0.01 # rad^2
sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
motion variances:
disp_to_disp: 0.000001 # m^2 / m
disp_to_rot: 0.000001 # rad^2 / m
rot_to_rot: 0.000001 # rad^2 / rad
min_disp_var: 0.00000001 # m^2
min_rot_var: 0.00000001 # rad^2
\ No newline at end of file
type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.000001 # m^2 / m
k_disp_to_rot: 0.000001 # rad^2 / m
k_rot_to_rot: 0.000001 # rad^2 / rad
min_disp_var: 0.00000001 # m^2
min_rot_var: 0.00000001 # rad^2
\ No newline at end of file
sensor:
type: "USB_CAM"
detector:
type: "ORB"
nfeatures: 100
scale factor: 1.2
nlevels: 8
edge threshold: 8 # 16
first level: 0
WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
patch size: 15 # 31
descriptor:
type: "ORB"
nfeatures: 100
scale factor: 1.2
nlevels: 8
edge threshold: 8 # 16
first level: 0
WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
patch size: 15 # 31
matcher:
type: "FLANNBASED"
match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
roi:
width: 20
height: 20
min normalized score: 0.85
algorithm:
type: "ACTIVESEARCH"
draw results: true
grid horiz cells: 12
grid vert cells: 8
separation: 10
min features to track: 5
max new features: 40
min response new features: 80
\ No newline at end of file
......@@ -139,12 +139,12 @@ int main()
// SELF CALIBRATION ===================================================
// NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
// SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
sensor_rb->getO()->unfix();
// sensor_rb->getO()->unfix();
// NOTE: SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
// SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
// sensor_rb->getP()->unfix();
// CONFIGURE ==========================================================
......
......@@ -101,13 +101,14 @@ int main()
using namespace wolf;
WOLF_TRACE("======== CONFIGURE PROBLEM =======")
WOLF_TRACE("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
std::string file = std::string(_WOLF_ROOT_DIR) + "/hello_wolf/hello_wolf_config.yaml";
std::string config_file = "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:
ParserYAML parser = ParserYAML(file);
ParserYAML parser = ParserYAML(config_file, wolf_path);
parser.parse();
ParamsServer server = ParamsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
......@@ -118,8 +119,8 @@ int main()
problem->print(4,0,1,0);
// recover sensor pointers for later use (access by sensor name)
SensorBasePtr sensor_odo = problem->getSensor("odom");
SensorBasePtr sensor_rb = problem->getSensor("rb");
SensorBasePtr sensor_odo = problem->getSensor("sen odom");
SensorBasePtr sensor_rb = problem->getSensor("sen rb");
// Solver. Configure a Ceres solver
ceres::Solver::Options options;
......@@ -131,12 +132,12 @@ int main()
// SELF CALIBRATION ===================================================
// These few lines control whether we calibrate some sensor parameters or not.
// NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
// SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
sensor_rb->getO()->unfix();
// sensor_rb->getO()->unfix();
// NOTE: SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
// SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
// sensor_rb->getP()->unfix();
// CONFIGURE input data (motion and measurements) ==============================================
......@@ -151,7 +152,7 @@ int main()
// SET OF EVENTS: make things happen =======================================================
std::cout << std::endl;
WOLF_TRACE("======== BUILD PROBLEM =======")
WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======")
// We'll do 3 steps of motion and landmark observations.
......
config:
problem:
frame structure: "PO"
dimension: 2
prior timestamp: 0.0
prior state: [0,0,0]
prior cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
prior time tolerance: 0.1
sensors:
-
type: "ODOM 2D"
name: "odom"
-
type: "RANGE BEARING"
name: "rb"
noise range metres std: 0.1
noise bearing degrees std: 0.5
processors:
-
type: "ODOM 2D"
name: "odom"
sensor name: "odom"
voting active: true
time tolerance: 0.1
max time span: 999
dist_traveled: 0.95
angle_turned: 999
cov_det: 999
unmeasured_perturbation_std: 0.001
-
type: "RANGE BEARING"
name: "rb"
sensor name: "rb"
files:
# - "/Users/jsola/dev/wolf_lib/core/lib/libhellowolf.dylib"
\ No newline at end of file
......@@ -14,10 +14,10 @@
namespace wolf
{
ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) :
ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
ProcessorBase("RANGE BEARING", _params)
{
H_r_s = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState());
//
}
void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
......@@ -75,7 +75,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
{
// new landmark:
// - create landmark
lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)));
lmk = LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing));
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
// - add to known landmarks
known_lmks.emplace(id, lmk);
......@@ -99,39 +99,6 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr _sen_ptr)
{
SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr);
ProcessorParamsRangeBearingPtr params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
// construct processor
ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
// setup processor
prc->setName(_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
{
......@@ -189,10 +156,6 @@ Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const
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