├── res
    ├── iros_1.jpg
    ├── iros_2.jpg
    └── iros_3.jpg
├── .gitignore
├── .gitmodules
├── rrxio
    ├── package.xml
    ├── launch
    │   ├── configs
    │   │   ├── default_params_radar_ego_velocity_estimation.yaml
    │   │   ├── rrxio.rviz
    │   │   ├── rrxio_iros_datasets_visual.info
    │   │   └── rrxio_iros_datasets_thermal.info
    │   ├── rrxio_evaluate_rosbag.launch
    │   ├── rrxio_thermal_iros_demo.launch
    │   └── rrxio_visual_iros_demo.launch
    ├── CMakeLists.txt
    ├── src
    │   └── nodes
    │   │   ├── rrxio_node.cpp
    │   │   └── rrxio_rosbag_loader.cpp
    ├── python
    │   ├── odom_to_path.py
    │   ├── plot_states_uncertainty.py
    │   ├── evaluate_ground_truth.py
    │   └── evaluate_iros_datasets.py
    └── include
    │   └── rrxio
    │       ├── VelocityUpdate.hpp
    │       └── RRxIOFilter.hpp
├── .clang-format
├── README.md
└── LICENSE


/res/iros_1.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/christopherdoer/rrxio/HEAD/res/iros_1.jpg


--------------------------------------------------------------------------------
/res/iros_2.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/christopherdoer/rrxio/HEAD/res/iros_2.jpg


--------------------------------------------------------------------------------
/res/iros_3.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/christopherdoer/rrxio/HEAD/res/iros_3.jpg


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # CMake
 2 | CMakeCache.txt
 3 | CMakeFiles
 4 | Makefile
 5 | cmake_install.cmake
 6 | install_manifest.txt
 7 | gtest
 8 | rovio_custom.info
 9 | *.bag
10 | *.active
11 | 
12 | # Compiled Object files
13 | *.slo
14 | *.lo
15 | *.o
16 | 
17 | # Compiled Dynamic libraries
18 | *.so
19 | *.dylib
20 | 
21 | # Compiled Static libraries
22 | *.lai
23 | *.la
24 | *.a
25 | 
26 | *~
27 | *.pyc
28 | 
29 | html/
30 | latex/
31 | .idea/
32 | CMakeLists.txt.user
33 | 


--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
 1 | [submodule "thirdparty/rpg_trajectory_evaluation"]
 2 | 	path = thirdparty/rpg_trajectory_evaluation
 3 | 	url = https://github.com/christopherdoer/rpg_trajectory_evaluation.git
 4 | [submodule "thirdparty/kindr"]
 5 | 	path = thirdparty/kindr
 6 | 	url = https://github.com/ethz-asl/kindr.git
 7 | [submodule "thirdparty/rovio"]
 8 | 	path = thirdparty/rovio
 9 | 	url = https://github.com/ethz-asl/rovio
10 | [submodule "thirdparty/reve"]
11 | 	path = thirdparty/reve
12 | 	url = https://github.com/christopherdoer/reve.git
13 | 


--------------------------------------------------------------------------------
/rrxio/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>rrxio</name>
 4 |   <version>0.0.0</version>
 5 |   <description>
 6 | 
 7 | 	rrxio - Robust Radar Visual/Thermal Inertial Odometry
 8 | 
 9 |   </description>
10 |   <url></url>
11 |   <author>christopher doer</author>
12 |   <maintainer email="christopher.doer@kit.edu">Christopher Doer</maintainer>
13 | 
14 |   <license>GPLv3 License</license>
15 | 
16 |   <buildtool_depend>catkin</buildtool_depend>
17 |   <depend>catkin_simple</depend>
18 |   <depend>roscpp</depend>
19 |   <depend>roslib</depend>
20 |   <depend>cv_bridge</depend>
21 |   <depend>message_generation</depend>
22 |   <depend>message_runtime</depend>
23 |   <depend>geometry_msgs</depend>
24 |   <depend>sensor_msgs</depend>
25 |   <depend>std_msgs</depend>
26 |   <depend>nav_msgs</depend>
27 |   <depend>tf</depend>
28 |   <depend>rosbag</depend>
29 |   <depend>yaml_cpp_catkin</depend>
30 |   <depend>image_transport</depend>
31 |   <depend>rovio</depend>
32 |   <depend>radar_ego_velocity_estimator</depend>
33 |   <depend>rpg_trajectory_evaluation</depend>
34 | 
35 | </package>
36 | 


--------------------------------------------------------------------------------
/rrxio/launch/configs/default_params_radar_ego_velocity_estimation.yaml:
--------------------------------------------------------------------------------
 1 | ## radar ego velocity estimation
 2 | # filtering
 3 | min_dist: 0.25                        # min distance of valid detection
 4 | max_dist: 100                         # Max distance of valid detection
 5 | min_db: 5                             # min SNR in [db]
 6 | elevation_thresh_deg: 60              # threshold for elevation [deg]
 7 | azimuth_thresh_deg: 60                # threshold for azimuth [deg]
 8 | filter_min_z: -100
 9 | filter_max_z: 100
10 | doppler_velocity_correction_factor: 1.0 # Doppler velocity correction
11 | 
12 | # zero velocity detection
13 | thresh_zero_velocity: 0.05        # all inliers need to smaller than this value
14 | allowed_outlier_percentage: 0.25  # outlier ratio (=percentage of detections which are allowed to be above thresh_zero_velocity)
15 | sigma_zero_velocity_x: 0.025
16 | sigma_zero_velocity_y: 0.025
17 | sigma_zero_velocity_z: 0.025
18 | 
19 | # result filtering
20 | max_sigma_x: 0.5   # max estimated sigma to be considered an inlier (right)
21 | max_sigma_y: 0.5  # (forward)
22 | max_sigma_z: 0.5   # (up)
23 | max_r_cond: 1000  # max conditional number of LSQ Pseudo Inverse to ensure a stable result
24 | use_cholesky_instead_of_bdcsvd: True # faster but less stable
25 | 
26 | # RANSAC parameters
27 | use_ransac: True     # turn on RANSAC LSQ
28 | outlier_prob: 0.4   # worst case outlier probability
29 | success_prob: 0.999999 # probability of successful determination of inliers
30 | N_ransac_points: 3   # number of measurements used for the RANSAC solution
31 | inlier_thresh: 0.25  # inlier threshold for inlier determination
32 | 
33 | # noise offset
34 | sigma_offset_radar_x: 0.1   # offset added to estimated sigma
35 | sigma_offset_radar_y: 0.05
36 | sigma_offset_radar_z: 0.1
37 | 
38 | # ODR refinement
39 | use_odr: True    # turn on odr refinement
40 | min_speed_odr: 1.5 # min speed for ODR refinement
41 | sigma_v_d: 0.125  # noise of v_r measurement used for the refinement
42 | model_noise_offset_deg: 2.0 # min model noise
43 | model_noise_scale_deg: 10.0 # scale model noise


--------------------------------------------------------------------------------
/.clang-format:
--------------------------------------------------------------------------------
 1 | # Based on ROS .clang-format with some additions
 2 | ---
 3 | BasedOnStyle:  Google
 4 | AccessModifierOffset: -2
 5 | ConstructorInitializerIndentWidth: 2
 6 | AlignEscapedNewlinesLeft: false
 7 | AlignTrailingComments: true
 8 | AllowAllParametersOfDeclarationOnNextLine: false
 9 | AllowShortIfStatementsOnASingleLine: false
10 | # AllowShortLoopsOnASingleLine: false
11 | # AllowShortFunctionsOnASingleLine: false
12 | # AllowShortLoopsOnASingleLine: false
13 | AlwaysBreakTemplateDeclarations: true
14 | AlwaysBreakBeforeMultilineStrings: false
15 | BreakBeforeBinaryOperators: false
16 | BreakBeforeTernaryOperators: false
17 | BreakConstructorInitializersBeforeComma: true
18 | # BinPackParameters: true
19 | ColumnLimit:    120
20 | ConstructorInitializerAllOnOneLineOrOnePerLine: true
21 | DerivePointerBinding: false
22 | PointerBindsToType: true
23 | ExperimentalAutoDetectBinPacking: false
24 | IndentCaseLabels: true
25 | MaxEmptyLinesToKeep: 1
26 | NamespaceIndentation: None
27 | ObjCSpaceBeforeProtocolList: true
28 | PenaltyBreakBeforeFirstCallParameter: 19
29 | PenaltyBreakComment: 60
30 | PenaltyBreakString: 1
31 | PenaltyBreakFirstLessLess: 1000
32 | PenaltyExcessCharacter: 1000
33 | PenaltyReturnTypeOnItsOwnLine: 90
34 | SpacesBeforeTrailingComments: 2
35 | # Cpp11BracedListStyle: false
36 | Standard:        Auto
37 | IndentWidth:     2
38 | TabWidth:        2
39 | UseTab:          Never
40 | IndentFunctionDeclarationAfterType: false
41 | SpacesInParentheses: false
42 | SpacesInAngles:  false
43 | SpaceInEmptyParentheses: false
44 | SpacesInCStyleCastParentheses: false
45 | SpaceAfterControlStatementKeyword: true
46 | SpaceBeforeAssignmentOperators: true
47 | ContinuationIndentWidth: 4
48 | SortIncludes: false
49 | SpaceAfterCStyleCast: false
50 | 
51 | # Configure each individual brace in BraceWrapping
52 | BreakBeforeBraces: Custom
53 | 
54 | # Control of individual brace wrapping cases
55 | BraceWrapping: {
56 |     AfterClass: 'true'
57 |     AfterControlStatement: 'true'
58 |     AfterEnum : 'true'
59 |     AfterFunction : 'true'
60 |     AfterNamespace : 'true'
61 |     AfterStruct : 'true'
62 |     AfterUnion : 'true'
63 |     BeforeCatch : 'true'
64 |     BeforeElse : 'true'
65 |     IndentBraces : 'false'
66 | }
67 | 
68 | # Additions
69 | AlignConsecutiveAssignments: true
70 | AlignOperands: true
71 | AllowAllParametersOfDeclarationOnNextLine: false
72 | AllowShortLoopsOnASingleLine: true
73 | AllowShortFunctionsOnASingleLine: true
74 | AllowShortLoopsOnASingleLine: true
75 | BinPackArguments: false
76 | BinPackParameters: false
77 | BreakConstructorInitializers: AfterColon
78 | ConstructorInitializerAllOnOneLineOrOnePerLine: true
79 | Cpp11BracedListStyle: true
80 | FixNamespaceComments: true
81 | ReflowComments: true
82 | IndentPPDirectives: AfterHash
83 | ...
84 | 
85 | 


--------------------------------------------------------------------------------
/rrxio/launch/rrxio_evaluate_rosbag.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <!-- This file is part of RRxIO - Robust Radar Visual/Thermal Odometry
 3 | @author Christopher Doer <christopher.doer@kit.edu> -->
 4 | <launch>
 5 |     <arg name="id" default=""/>
 6 | 
 7 |     <arg name="rosbag_dir"/>
 8 |     <arg name="rosbag"/>
 9 |     <arg name="ground_truth_csv"/>
10 |     <arg name="ground_truth_type"/>
11 | 
12 |     <arg name="export_directory"/>
13 |     <arg name="N"/>
14 |     <arg name="enable_rviz"/>
15 |     <arg name="shutdown_when_done"/>
16 |     <arg name="bag_start"/>
17 |     <arg name="bag_dur" default="10000"/>
18 | 
19 |     <arg name="config"/>
20 |     <arg name="camera_config"/>
21 |     <arg name="timeshift_cam_imu"/>
22 | 
23 |     <arg name="radar_calib" default="$(arg rosbag_dir)/radar_calib.yaml"/>
24 | 
25 |     <arg name="topic_imu" default="/sensor_platform/imu"/>
26 |     <arg name="topic_cam"/>
27 |     <arg name="topic_radar_trigger"/>
28 |     <arg name="topic_radar_scan"/>
29 | 
30 |     <node pkg="rrxio" type="rrxio_rosbag_loader_$(arg N)" name="rovio$(arg id)" output="screen" clear_params="True" launch-prefix="bash -c 'sleep 2; $0 $@' ">
31 |         <param name="filter_config" value="$(arg config)"/>
32 |         <param name="camera0_config" value="$(arg camera_config)"/>
33 |         <param name="rosbag_filename" value="$(arg rosbag_dir)/$(arg rosbag).bag"/>
34 |         <param name="imu_topic_name" value="$(arg topic_imu)"/>
35 |         <param name="cam0_topic_name" value="$(arg topic_cam)"/>
36 |         <param name="topic_vel" type="str" value="not_used"/>
37 |         <param name="bag_start" value="$(arg bag_start)"/>
38 |         <param name="bag_dur" value="$(arg bag_dur)"/>
39 |         <param name="timeshift_cam_imu" value="$(arg timeshift_cam_imu)"/>
40 | 
41 |         <param name="topic_radar_trigger" value="$(arg topic_radar_trigger)"/>
42 |         <param name="topic_radar_scan" value="$(arg topic_radar_scan)"/>
43 | 
44 |         <rosparam file="$(find rrxio)/launch/configs/default_params_radar_ego_velocity_estimation.yaml" command="load"/>
45 |         <rosparam file="$(arg radar_calib)" command="load"/>
46 | 
47 |         <param name="world_frame" value="odom"/>
48 |         <param name="imu_frame" value="base_link"/>
49 |         <param name="camera_frame" value="camera"/>
50 |     </node>
51 | 
52 |     <node pkg="rrxio" type="evaluate_ground_truth.py" name="evaluate_ground_truth$(arg id)" output="screen" clear_params="true"
53 |           required="$(arg shutdown_when_done)">
54 |         <param name="rosbag_name" type="str" value="$(arg rosbag)"/>
55 |         <param name="rosbag_dir" type="str" value="$(arg rosbag_dir)"/>
56 |         <param name="ground_truth_type" type="str" value="$(arg ground_truth_type)"/>
57 |         <param name="ground_truth_csv" type="str" value="$(arg ground_truth_csv)"/>
58 |         <param name="pose_topic" type="str" value="pose_topic_not_used"/>
59 |         <param name="odom_topic" type="str" value="/rovio$(arg id)/rovio/odometry"/>
60 |         <param name="export_directory" type="str" value="$(arg export_directory)"/>
61 |     </node>
62 | </launch>
63 | 


--------------------------------------------------------------------------------
/rrxio/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(rrxio)
 3 | 
 4 | find_package(catkin_simple REQUIRED)
 5 | find_package(OpenCV)
 6 | 
 7 | catkin_simple()
 8 | 
 9 | add_compile_options(-std=c++11)
10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -march=native")
11 | set(ROVIO_NMAXFEATURE 15 CACHE STRING "Number of features for ROVIO")
12 | set(ROVIO_NCAM 1 CACHE STRING "Number of enabled cameras")
13 | set(ROVIO_NLEVELS 4 CACHE STRING "Number of image leavels for the features")
14 | set(ROVIO_PATCHSIZE 6 CACHE STRING "Size of patch (edge length in pixel)")
15 | set(ROVIO_NPOSE 0 CACHE STRING "Additional estimated poses for external pose measurements")
16 | add_definitions(-DROVIO_NMAXFEATURE=${ROVIO_NMAXFEATURE})
17 | add_definitions(-DROVIO_NCAM=${ROVIO_NCAM})
18 | add_definitions(-DROVIO_NLEVELS=${ROVIO_NLEVELS})
19 | add_definitions(-DROVIO_PATCHSIZE=${ROVIO_PATCHSIZE})
20 | add_definitions(-DROVIO_NPOSE=${ROVIO_NPOSE})
21 | 
22 | cs_add_executable(rrxio_rosbag_loader_10 src/nodes/rrxio_rosbag_loader.cpp)
23 | target_compile_definitions(rrxio_rosbag_loader_10 PRIVATE ROVIO_NMAXFEATURE=10 ROVIO_UPDATE_SOURCE=0)
24 | target_link_libraries(rrxio_rosbag_loader_10 ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenCV_LIBRARIES})
25 | 
26 | cs_add_executable(rrxio_rosbag_loader_15 src/nodes/rrxio_rosbag_loader.cpp)
27 | target_compile_definitions(rrxio_rosbag_loader_15 PRIVATE ROVIO_NMAXFEATURE=15 ROVIO_UPDATE_SOURCE=0)
28 | target_link_libraries(rrxio_rosbag_loader_15 ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenCV_LIBRARIES})
29 | 
30 | cs_add_executable(rrxio_rosbag_loader_25 src/nodes/rrxio_rosbag_loader.cpp)
31 | target_compile_definitions(rrxio_rosbag_loader_25 PRIVATE ROVIO_NMAXFEATURE=25 ROVIO_UPDATE_SOURCE=0)
32 | target_link_libraries(rrxio_rosbag_loader_25 ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenCV_LIBRARIES})
33 | 
34 | ## umcomment the following lines for online mode
35 | 
36 | #cd_add_executable(rrxio_node_10 src/nodes/rrxio_node.cpp)
37 | #target_compile_definitions(rrxio_node_10 PRIVATE ROVIO_NMAXFEATURE=10 ROVIO_UPDATE_SOURCE=0)
38 | #target_link_libraries(rrxio_node_10 ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenCV_LIBRARIES})
39 | 
40 | #cd_add_executable(rrxio_node_15 src/nodes/rrxio_node.cpp)
41 | #target_compile_definitions(rrxio_node_15 PRIVATE ROVIO_NMAXFEATURE=15 ROVIO_UPDATE_SOURCE=0)
42 | #target_link_libraries(rrxio_node_15 ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenCV_LIBRARIES})
43 | 
44 | #cd_add_executable(rrxio_node_25 src/nodes/rrxio_node.cpp)
45 | #target_compile_definitions(rrxio_node_25 PRIVATE ROVIO_NMAXFEATURE=25 ROVIO_UPDATE_SOURCE=0)
46 | #target_link_libraries(rrxio_node_25 ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenCV_LIBRARIES})
47 | 
48 | cs_install()
49 | 
50 | cs_export(INCLUDE_DIRS include)
51 | 
52 | ## qtcreator project tree
53 | FILE(GLOB_RECURSE LibFiles "include/*")
54 | add_custom_target(headers SOURCES ${LibFiles})
55 | 
56 | file(GLOB_RECURSE CFG "cfg/*")
57 | add_custom_target(CFG SOURCES ${CFG})
58 | 
59 | file(GLOB_RECURSE LAUNCH "launch/*")
60 | add_custom_target(LAUNCH SOURCES ${LAUNCH})
61 | 
62 | file(GLOB_RECURSE PYTHON "python/*")
63 | add_custom_target(PYTHON SOURCES ${PYTHON})
64 | 
65 | file(GLOB_RECURSE CONFIG "config/*")
66 | add_custom_target(CONFIG SOURCES ${CONFIG})
67 | 


--------------------------------------------------------------------------------
/rrxio/launch/rrxio_thermal_iros_demo.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <!--This file is part of RRxIO - Robust Radar Visual/Thermal Odometry
 3 | @author Christopher Doer <christopher.doer@kit.edu>-->
 4 | <launch>
 5 |     <arg name="rosbag_dir"/>
 6 |     <arg name="rosbag"/>
 7 | 
 8 |     <!--    number of features to be tracked: 10,15,25-->
 9 |     <arg name="N" default="15"/>
10 | 
11 |     <!--  odometry path will be exported to <export_file>_odometry.csv if not empty  -->
12 |     <arg name="export_file" default=""/>
13 | 
14 |     <arg name="enable_rviz" default="True"/>
15 |     <arg name="enable_plots" default="True"/>
16 |     <arg name="shutdown_when_done" default="False"/>
17 |     <arg name="bag_start" default="0"/>
18 |     <arg name="bag_duration" default="10000"/>
19 |     <arg name="max_frame_ctr" default="2000000"/>
20 | 
21 |     <arg name="config" default="$(find rrxio)/launch/configs/rrxio_iros_datasets_thermal.info"/>
22 |     <arg name="camera_config" default="$(arg rosbag_dir)/rovio_thermal.yaml"/>
23 |     <arg name="timeshift_cam_imu" default="-0.004"/>
24 |     <arg name="topic_cam" default="/sensor_platform/camera_thermal/img"/>
25 | 
26 |     <arg name="radar_calib" default="$(arg rosbag_dir)/radar_calib.yaml"/>
27 | 
28 |     <arg name="topic_imu" default="/sensor_platform/imu"/>
29 |     <arg name="topic_radar_trigger" default="/sensor_platform/radar/trigger__"/>
30 |     <arg name="topic_radar_scan" default="/sensor_platform/radar/scan"/>
31 | 
32 |     <node pkg="rrxio" type="rrxio_rosbag_loader_$(arg N)" name="rrxio" output="screen" clear_params="True" launch-prefix="bash -c 'sleep 1; $0 $@' ">
33 |         <!--            <node pkg="rrxio" type="rrxio_rosbag_loader_$(arg N)" name="rrxio" output="screen" clear_params="False" launch-prefix="gdb -ex run &#45;&#45;args">-->
34 |         <param name="filter_config" value="$(arg config)"/>
35 |         <param name="camera0_config" value="$(arg camera_config)"/>
36 |         <param name="rosbag_filename" value="$(arg rosbag_dir)/$(arg rosbag).bag"/>
37 |         <param name="imu_topic_name" value="$(arg topic_imu)"/>
38 |         <param name="cam0_topic_name" value="$(arg topic_cam)"/>
39 |         <param name="cam1_topic_name" value="cam1_not_used"/>
40 |         <param name="topic_vel" type="str" value="not_used"/>
41 |         <param name="bag_start" value="$(arg bag_start)"/>
42 |         <param name="bag_duration" value="$(arg bag_duration)"/>
43 |         <param name="timeshift_cam_imu" value="$(arg timeshift_cam_imu)"/>
44 |         <param name="max_frame_ctr" value="$(arg max_frame_ctr)"/>
45 | 
46 |         <param name="topic_radar_trigger" value="$(arg topic_radar_trigger)"/>
47 |         <param name="topic_radar_scan" value="$(arg topic_radar_scan)"/>
48 | 
49 |         <rosparam file="$(find rrxio)/launch/configs/default_params_radar_ego_velocity_estimation.yaml" command="load"/>
50 |         <rosparam file="$(arg radar_calib)" command="load"/>
51 | 
52 |         <param name="world_frame" value="odom"/>
53 |         <param name="imu_frame" value="base_link"/>
54 |         <param name="camera_frame" value="camera"/>
55 |     </node>
56 | 
57 |     <node pkg="rrxio" type="odom_to_path.py" name="odom_to_path_rrxio_visual" output="screen">
58 |         <param name="topic_odom" type="str" value="/rrxio/rovio/odometry"/>
59 |         <param name="filter_name" type="str" value="RRxIO Visual"/>
60 |         <param name="export_file" type="str" value="$(arg export_file)"/>
61 |     </node>
62 | 
63 |     <node pkg="rrxio" type="plot_states_uncertainty.py" name="plot_states_uncertainty" if="$(arg enable_plots)">
64 |         <param name="topic_prefix" type="str" value="/rrxio/rovio/"/>
65 |     </node>
66 | 
67 |     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrxio)/launch/configs/rrxio.rviz" launch-prefix="bash -c 'sleep 0; $0 $@' "
68 |           if="$(arg enable_rviz)"/>
69 | </launch>
70 | 


--------------------------------------------------------------------------------
/rrxio/launch/rrxio_visual_iros_demo.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <!--This file is part of RRxIO - Robust Radar Visual/Thermal Odometry
 3 | @author Christopher Doer <christopher.doer@kit.edu>-->
 4 | <launch>
 5 |     <arg name="rosbag_dir"/>
 6 |     <arg name="rosbag"/>
 7 | 
 8 |     <!--    number of features to be tracked: 10,15,25-->
 9 |     <arg name="N" default="15"/>
10 | 
11 |     <!--  odometry path will be exported to <export_file>_odometry.csv if not empty  -->
12 |     <arg name="export_file" default=""/>
13 | 
14 |     <arg name="enable_rviz" default="True"/>
15 |     <arg name="enable_plots" default="True"/>
16 |     <arg name="shutdown_when_done" default="False"/>
17 |     <arg name="bag_start" default="0"/>
18 |     <arg name="bag_duration" default="10000"/>
19 |     <arg name="max_frame_ctr" default="2000000"/>
20 | 
21 |     <arg name="config" default="$(find rrxio)/launch/configs/rrxio_iros_datasets_visual.info"/>
22 |     <arg name="camera_config" default="$(arg rosbag_dir)/rovio_visual.yaml"/>
23 |     <arg name="timeshift_cam_imu" default="0.002"/>
24 |     <arg name="topic_cam" default="/sensor_platform/camera_visual/img"/>
25 | 
26 |     <arg name="radar_calib" default="$(arg rosbag_dir)/radar_calib.yaml"/>
27 | 
28 |     <arg name="topic_imu" default="/sensor_platform/imu"/>
29 |     <arg name="topic_radar_trigger" default="/sensor_platform/radar/trigger__"/>
30 |     <arg name="topic_radar_scan" default="/sensor_platform/radar/scan"/>
31 |     <node pkg="rrxio" type="rrxio_rosbag_loader_$(arg N)" name="rrxio" output="screen" clear_params="False" launch-prefix="bash -c 'sleep 1; $0 $@' ">
32 |         <!--    <node pkg="rrxio" type="rrxio_rosbag_loader_$(arg N)" name="rrxio" output="screen" clear_params="False" launch-prefix="gdb -ex run &#45;&#45;args">-->
33 |         <param name="filter_config" value="$(arg config)"/>
34 |         <param name="camera0_config" value="$(arg camera_config)"/>
35 |         <param name="rosbag_filename" value="$(arg rosbag_dir)/$(arg rosbag).bag"/>
36 |         <param name="imu_topic_name" value="$(arg topic_imu)"/>
37 |         <param name="cam0_topic_name" value="$(arg topic_cam)"/>
38 |         <param name="cam1_topic_name" value="cam1_not_used"/>
39 |         <param name="topic_vel" type="str" value="not_used"/>
40 |         <param name="bag_start" value="$(arg bag_start)"/>
41 |         <param name="bag_duration" value="$(arg bag_duration)"/>
42 |         <param name="timeshift_cam_imu" value="$(arg timeshift_cam_imu)"/>
43 |         <param name="max_frame_ctr" value="$(arg max_frame_ctr)"/>
44 | 
45 |         <param name="topic_radar_trigger" value="$(arg topic_radar_trigger)"/>
46 |         <param name="topic_radar_scan" value="$(arg topic_radar_scan)"/>
47 | 
48 |         <rosparam file="$(find rrxio)/launch/configs/default_params_radar_ego_velocity_estimation.yaml" command="load"/>
49 |         <rosparam file="$(arg radar_calib)" command="load"/>
50 | 
51 |         <param name="world_frame" value="odom"/>
52 |         <param name="imu_frame" value="base_link"/>
53 |         <param name="camera_frame" value="camera"/>
54 |     </node>
55 | 
56 |     <node pkg="rrxio" type="odom_to_path.py" name="odom_to_path_rrxio_visual" output="screen" required="$(arg shutdown_when_done)">
57 |         <param name="topic_odom" type="str" value="/rrxio/rovio/odometry"/>
58 |         <param name="filter_name" type="str" value="RRxIO Visual"/>
59 |         <param name="export_file" type="str" value="$(arg export_file)"/>
60 |     </node>
61 | 
62 |     <node pkg="rrxio" type="plot_states_uncertainty.py" name="plot_states_uncertainty" if="$(arg enable_plots)">
63 |         <param name="topic_prefix" type="str" value="/rrxio/rovio/"/>
64 |     </node>
65 | 
66 |     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrxio)/launch/configs/rrxio.rviz" launch-prefix="bash -c 'sleep 0; $0 $@' "
67 |           if="$(arg enable_rviz)"/>
68 | </launch>
69 | 


--------------------------------------------------------------------------------
/rrxio/src/nodes/rrxio_node.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2021, Christopher Doer
  3 |  * Copyright (c) 2014, Autonomous Systems Lab
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  * * Redistributions of source code must retain the above copyright
  9 |  * notice, this list of conditions and the following disclaimer.
 10 |  * * Redistributions in binary form must reproduce the above copyright
 11 |  * notice, this list of conditions and the following disclaimer in the
 12 |  * documentation and/or other materials provided with the distribution.
 13 |  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 14 |  * names of its contributors may be used to endorse or promote products
 15 |  * derived from this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 |  * POSSIBILITY OF SUCH DAMAGE.
 28 |  *
 29 |  */
 30 | 
 31 | #include <memory>
 32 | 
 33 | #include <Eigen/StdVector>
 34 | 
 35 | #pragma GCC diagnostic push
 36 | #pragma GCC diagnostic ignored "-Wunused-parameter"
 37 | #include <geometry_msgs/Pose.h>
 38 | #include <ros/package.h>
 39 | #include <ros/ros.h>
 40 | #pragma GCC diagnostic pop
 41 | 
 42 | #include "rrxio/RRxIOFilter.hpp"
 43 | #include "rrxio/RRxIONode.hpp"
 44 | 
 45 | #ifdef ROVIO_NMAXFEATURE
 46 | static constexpr int nMax_ = ROVIO_NMAXFEATURE;
 47 | #else
 48 | static constexpr int nMax_      = 15;  // Maximal number of considered features in the filter state.
 49 | #endif
 50 | 
 51 | #ifdef ROVIO_NLEVELS
 52 | static constexpr int nLevels_ = ROVIO_NLEVELS;
 53 | #else
 54 | static constexpr int nLevels_   = 4;   // // Total number of pyramid levels considered.
 55 | #endif
 56 | 
 57 | #ifdef ROVIO_PATCHSIZE
 58 | static constexpr int patchSize_ = ROVIO_PATCHSIZE;
 59 | #else
 60 | static constexpr int patchSize_ = 6;   // Edge length of the patches (in pixel). Must be a multiple of 2!
 61 | #endif
 62 | 
 63 | #ifdef ROVIO_NCAM
 64 | static constexpr int nCam_ = ROVIO_NCAM;
 65 | #else
 66 | static constexpr int nCam_      = 1;   // Used total number of cameras.
 67 | #endif
 68 | 
 69 | #ifdef ROVIO_NPOSE
 70 | static constexpr int nPose_ = ROVIO_NPOSE;
 71 | #else
 72 | static constexpr int nPose_     = 0;   // Additional pose states.
 73 | #endif
 74 | 
 75 | typedef rovio::RovioFilter<rovio::FilterState<nMax_, nLevels_, patchSize_, nCam_, nPose_>> mtFilter;
 76 | 
 77 | int main(int argc, char** argv)
 78 | {
 79 |   ros::init(argc, argv, "rovio_node");
 80 |   ros::NodeHandle nh;
 81 |   ros::NodeHandle nh_private("~");
 82 | 
 83 |   std::string rootdir       = ros::package::getPath("rovio");  // Leaks memory
 84 |   std::string filter_config = rootdir + "/cfg/rovio.info";
 85 | 
 86 |   nh_private.param("filter_config", filter_config, filter_config);
 87 | 
 88 |   // Filter
 89 |   std::shared_ptr<mtFilter> mpFilter(new mtFilter);
 90 |   mpFilter->readFromInfo(filter_config);
 91 | 
 92 |   // Force the camera calibration paths to the ones from ROS parameters.
 93 |   for (unsigned int camID = 0; camID < nCam_; ++camID)
 94 |   {
 95 |     std::string camera_config;
 96 |     if (nh_private.getParam("camera" + std::to_string(camID) + "_config", camera_config))
 97 |     {
 98 |       mpFilter->cameraCalibrationFile_[camID] = camera_config;
 99 |     }
100 |   }
101 |   mpFilter->refreshProperties();
102 | 
103 |   // Node
104 |   rovio::RovioNode<mtFilter> rovioNode(nh, nh_private, mpFilter);
105 |   rovioNode.makeTest();
106 | 
107 |   ros::spin();
108 |   return 0;
109 | }
110 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # RRxIO - Robust Radar Visual/Thermal Inertial Odometry
 2 | 
 3 | RRxIO offers robust and accurate state estimation even in challenging visual conditions. RRxIO combines radar ego velocity estimates and Visual Inertial Odometry (VIO) or Thermal
 4 | Inertial Odometry (TIO) in a single filter by extending [rovio](https://github.com/ethz-asl/rovio). Thus, state estimation in challenging visual conditions (e.g. darkness, direct sunlight, fog) or challenging thermal
 5 | conditions (e.g. temperature gradient poor environments or outages caused by non uniformity corrections) is possible. In addition, the drift free radar ego velocity estimates reduce scale errors and the
 6 | overall accuracy as compared to monocular VIO/TIO. RRxIO runs many times faster than real-time on an Intel NUC i7 and achieves real-time on an UpCore embedded computer.
 7 | 
 8 | ## Cite
 9 | 
10 | If you use RRxIO for your academic research, please cite our related [paper](https://christopherdoer.github.io/publication/2021_09_IROS2021):
11 | 
12 | ~~~[bibtex]
13 | @INPROCEEDINGS{DoerIros2021,
14 |   author={Doer, Christopher and Trommer, Gert F.},
15 |   booktitle={2021 IEEE/RSJ International Conference on Intelligent Rotots and Sytems (IROS)}, 
16 |   title={Radar Visual Inertial Odometry and Radar Thermal Inertial Odometry: Robust Navigation even in Challenging Visual Conditions}, 
17 |   year={2021}}
18 | ~~~
19 | 
20 | ## Demo Result: [IRS Radar Thermal Visual Inertial Datasets IROS 2021](https://christopherdoer.github.io/datasets/irs_rtvi_datasets_iros2021)
21 | 
22 | ### Motion Capture Lab (translational RMSE (ATE [m]))
23 | ![image](./res/iros_1.jpg)
24 | 
25 | ### Indoor and Outdoors (translational RMSE (ATE [m]))
26 | ![image](./res/iros_2.jpg)
27 | 
28 | ### Runtime (Real-time factor)
29 | ![image](./res/iros_3.jpg)
30 | 
31 | ## Getting Started
32 | 
33 | RRxIO supports: 
34 |   - Ubuntu 16.04 / ROS Kinetic
35 |   - Ubuntu 18.04 / ROS Melodic
36 | 
37 | RRxIO depends on:
38 | 
39 | - [catkin_simple](https://github.com/catkin/catkin_simple.git)
40 | - [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/)
41 | - [yaml_cpp_catkin](https://github.com/ethz-asl/yaml_cpp_catkin.git)
42 | 
43 | Additional dependencies are required to run the evaluation framework:
44 | 
45 | - sudo apt-get install texlive-latex-extra texlive-fonts-recommended dvipng cm-super
46 | - pip2 install -U PyYAML colorama ruamel.yaml==0.15.0
47 | 
48 | The following dependencies are included via git submodules (run once upon setup: `git submodule update --init --recursive`):
49 | - [rovio](https://github.com/ethz-asl/rovio)
50 | - [kindr](https://github.com/ethz-asl/kindr)
51 | - [reve](https://github.com/christopherdoer/reve)
52 | - [rpg_trajectory_evaluation (own fork)](https://github.com/christopherdoer/rpg_trajectory_evaluation)
53 | 
54 | **Build in Release is highly recommended**:
55 | 
56 | ~~~[shell]
57 | catkin build rrxio -j2 --cmake-args -DCMAKE_BUILD_TYPE=Release
58 | ~~~
59 | 
60 | Depending on the RAM available -j2 or -j1 avoids issues.
61 | Per default, only the rosbag_nodes are built. 
62 | To use the online mode, the [rrxio/CMakeLists.txt](https://github.com/christopherdoer/rrxio/blob/master/rrxio/CMakeLists.txt) needs to be changed.
63 | 
64 | ## Run Demos
65 | 
66 | Download the [IRS Radar Thermal Visual Inertial Datasets IROS 2021](https://christopherdoer.github.io/datasets/irs_rtvi_datasets_iros2021) datasets.
67 | 
68 | Run the mocap_easy datasets with visual RRxIO:
69 | 
70 | ~~~[shell]
71 | roslaunch rrxio rrxio_visual_iros_demo.launch rosbag_dir:=<path-to-rtvi_datastets_iros_2021> rosbag:=mocap_easy
72 | ~~~
73 | 
74 | Run the outdoor_street datasets with thermal RRxIO:
75 | 
76 | ~~~[shell]
77 | roslaunch rrxio rrxio_thermal_iros_demo.launch rosbag_dir:=<path-to-rtvi_datastets_iros_2021> rosbag:=outdoor_street
78 | ~~~
79 | 
80 | ## Run Evaluation IRS Radar Thermal Visual Inertial Datasets IROS 2021
81 | 
82 | The evaluation script is also provided which does an extensive evaluation of RRxIO_10, RRxIO_15, RRxIO_25 on all [IRS Radar Thermal Visual Inertial Datasets IROS 2021](https://christopherdoer.github.io/datasets/irs_rtvi_datasets_iros2021)
83 | datasets:
84 | 
85 | ~~~[shell]
86 | rosrun rrxio evaluate_iros_datasets.py <path-to-rtvi_datastets_iros_2021>
87 | ~~~
88 | 
89 | After some time, the results can be found at <path-to-rtvi_datastets_iros_2021>/results/evaluation/<10/15/25>/evaluation_full_align.
90 | These results are also shown in the table above.
91 | 
92 | 
93 | 


--------------------------------------------------------------------------------
/rrxio/launch/configs/rrxio.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /Grid1
 10 |         - /Image1
 11 |         - /Path1
 12 |       Splitter Ratio: 0.5
 13 |     Tree Height: 567
 14 |   - Class: rviz/Selection
 15 |     Name: Selection
 16 |   - Class: rviz/Tool Properties
 17 |     Expanded:
 18 |       - /2D Pose Estimate1
 19 |       - /2D Nav Goal1
 20 |       - /Publish Point1
 21 |     Name: Tool Properties
 22 |     Splitter Ratio: 0.588679016
 23 |   - Class: rviz/Views
 24 |     Expanded:
 25 |       - /Current View1
 26 |     Name: Views
 27 |     Splitter Ratio: 0.5
 28 |   - Class: rviz/Time
 29 |     Experimental: false
 30 |     Name: Time
 31 |     SyncMode: 0
 32 |     SyncSource: Image
 33 | Toolbars:
 34 |   toolButtonStyle: 2
 35 | Visualization Manager:
 36 |   Class: ""
 37 |   Displays:
 38 |     - Alpha: 0.5
 39 |       Cell Size: 1
 40 |       Class: rviz/Grid
 41 |       Color: 160; 160; 164
 42 |       Enabled: true
 43 |       Line Style:
 44 |         Line Width: 0.0299999993
 45 |         Value: Lines
 46 |       Name: Grid
 47 |       Normal Cell Count: 0
 48 |       Offset:
 49 |         X: 0
 50 |         Y: 0
 51 |         Z: 0
 52 |       Plane: XY
 53 |       Plane Cell Count: 1000
 54 |       Reference Frame: <Fixed Frame>
 55 |       Value: true
 56 |     - Class: rviz/Image
 57 |       Enabled: true
 58 |       Image Topic: /rrxio/rovio/tracker
 59 |       Max Value: 1
 60 |       Median window: 5
 61 |       Min Value: 0
 62 |       Name: Image
 63 |       Normalize Range: true
 64 |       Queue Size: 2
 65 |       Transport Hint: raw
 66 |       Unreliable: false
 67 |       Value: true
 68 |     - Alpha: 1
 69 |       Buffer Length: 1
 70 |       Class: rviz/Path
 71 |       Color: 25; 255; 0
 72 |       Enabled: true
 73 |       Head Diameter: 0.300000012
 74 |       Head Length: 0.200000003
 75 |       Length: 0.300000012
 76 |       Line Style: Lines
 77 |       Line Width: 0.0299999993
 78 |       Name: Path
 79 |       Offset:
 80 |         X: 0
 81 |         Y: 0
 82 |         Z: 0
 83 |       Pose Color: 255; 85; 255
 84 |       Pose Style: None
 85 |       Radius: 0.0299999993
 86 |       Shaft Diameter: 0.100000001
 87 |       Shaft Length: 0.100000001
 88 |       Topic: /rrxio/rovio/odometry/path
 89 |       Unreliable: false
 90 |       Value: true
 91 |   Enabled: true
 92 |   Global Options:
 93 |     Background Color: 48; 48; 48
 94 |     Default Light: true
 95 |     Fixed Frame: odom
 96 |     Frame Rate: 30
 97 |   Name: root
 98 |   Tools:
 99 |     - Class: rviz/Interact
100 |       Hide Inactive Objects: true
101 |     - Class: rviz/MoveCamera
102 |     - Class: rviz/Select
103 |     - Class: rviz/FocusCamera
104 |     - Class: rviz/Measure
105 |     - Class: rviz/SetInitialPose
106 |       Topic: /initialpose
107 |     - Class: rviz/SetGoal
108 |       Topic: /move_base_simple/goal
109 |     - Class: rviz/PublishPoint
110 |       Single click: true
111 |       Topic: /clicked_point
112 |   Value: true
113 |   Views:
114 |     Current:
115 |       Angle: 0
116 |       Class: rviz/TopDownOrtho
117 |       Enable Stereo Rendering:
118 |         Stereo Eye Separation: 0.0599999987
119 |         Stereo Focal Distance: 1
120 |         Swap Stereo Eyes: false
121 |         Value: false
122 |       Invert Z Axis: false
123 |       Name: Current View
124 |       Near Clip Distance: 0.00999999978
125 |       Scale: 93.7048111
126 |       Target Frame: <Fixed Frame>
127 |       Value: TopDownOrtho (rviz)
128 |       X: 0.485030234
129 |       Y: 0.446754932
130 |     Saved: ~
131 | Window Geometry:
132 |   Displays:
133 |     collapsed: false
134 |   Height: 1416
135 |   Hide Left Dock: false
136 |   Hide Right Dock: false
137 |   Image:
138 |     collapsed: false
139 |   QMainWindow State: 000000ff00000000fd0000000400000000000002a3000004fefc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c6000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f4000002320000001600ffffff000000010000010f000004fefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000004fe000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bf0000003efc0100000002fb0000000800540069006d00650100000000000009bf0000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000601000004fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
140 |   Selection:
141 |     collapsed: false
142 |   Time:
143 |     collapsed: false
144 |   Tool Properties:
145 |     collapsed: false
146 |   Views:
147 |     collapsed: false
148 |   Width: 2495
149 |   X: 2625
150 |   Y: 24
151 | 


--------------------------------------------------------------------------------
/rrxio/python/odom_to_path.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | """
  4 | This file is part of RRxIO - Robust Radar Visual/Thermal Odometry
  5 | @author Christopher Doer <christopher.doer@kit.edu>
  6 | """
  7 | 
  8 | from __future__ import with_statement
  9 | import rospy
 10 | import threading
 11 | import time
 12 | from nav_msgs.msg import Odometry, Path
 13 | from geometry_msgs.msg import PoseStamped
 14 | import tf2_ros
 15 | import numpy as np
 16 | 
 17 | import tf.transformations
 18 | 
 19 | 
 20 | def quat_to_rot_mat(q):
 21 |     return tf.transformations.quaternion_matrix(q[[1, 2, 3, 0]])[0:3, 0:3]
 22 | 
 23 | 
 24 | def quat_to_euler(q):
 25 |     return tf.transformations.euler_from_quaternion(q[[1, 2, 3, 0]])
 26 | 
 27 | 
 28 | def rot_mat_to_euler(R):
 29 |     T = np.eye(4)
 30 |     T[:3, :3] = R
 31 |     return tf.transformations.euler_from_matrix(T)
 32 | 
 33 | 
 34 | def from_q_msg(q):
 35 |     return np.array([q.w, q.x, q.y, q.z])
 36 | 
 37 | 
 38 | class OdomToPath:
 39 |     def __init__(self, topic_odom, filter_name, export_file, name):
 40 |         self.export_file = export_file
 41 |         self.name = name
 42 |         self.path_pub = rospy.Publisher(topic_odom + "/path", Path, queue_size=1)
 43 |         self.path = Path()
 44 |         self.tf_broadcaster = tf2_ros.TransformBroadcaster()
 45 |         self.lock = threading.Lock()
 46 |         self.last_time_callback = None
 47 |         self.filter_name = filter_name
 48 | 
 49 |         self.odom_sub = rospy.Subscriber(topic_odom, Odometry, self.callback_rovio, queue_size=10)
 50 | 
 51 |     def callback_rovio(self, msg):
 52 |         cur_pose = PoseStamped()
 53 |         cur_pose.header = msg.header
 54 |         cur_pose.pose.orientation = msg.pose.pose.orientation
 55 |         cur_pose.pose.position.z = msg.pose.pose.position.z
 56 |         cur_pose.pose.position.x = - msg.pose.pose.position.y
 57 |         cur_pose.pose.position.y = msg.pose.pose.position.x
 58 | 
 59 |         if len(self.path.poses) == 0 or (msg.header.stamp - self.path.poses[-1].header.stamp).to_sec() > 1 / 10:
 60 |             self.path.header = msg.header
 61 |             self.path.poses.append(cur_pose)
 62 |             self.path_pub.publish(self.path)
 63 | 
 64 |         with self.lock:
 65 |             self.last_time_callback = time.time()
 66 | 
 67 |     def evaluate(self):
 68 |         t = np.vstack([np.array([msg.header.stamp.to_sec()]) for msg in self.path.poses])
 69 |         positions = np.vstack([np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]) for msg in self.path.poses])
 70 |         quaternions = np.vstack([np.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]) for msg in self.path.poses])
 71 | 
 72 |         step = 2
 73 |         trajectory_lengths = np.cumsum(np.linalg.norm(positions[step::step, :] - positions[:-step:step, :], axis=1))
 74 |         p_err_final = positions[-1, :] - positions[0, :]
 75 |         p_err_final_norm = np.linalg.norm(p_err_final)
 76 | 
 77 |         R_final = quat_to_rot_mat(from_q_msg(self.path.poses[-1].pose.orientation))
 78 |         R_0 = quat_to_rot_mat(from_q_msg(self.path.poses[0].pose.orientation))
 79 |         eul_err = rot_mat_to_euler(R_final.dot(R_0.transpose()))
 80 |         eul_err_deg = np.rad2deg(eul_err)
 81 |         eul_final = rot_mat_to_euler(R_final)
 82 |         eul_final_deg = np.rad2deg(eul_final)
 83 | 
 84 |         yaw_err_deg = np.rad2deg(quat_to_euler(from_q_msg(self.path.poses[-1].pose.orientation))[2] -
 85 |                                  quat_to_euler(from_q_msg(self.path.poses[0].pose.orientation))[2])
 86 | 
 87 |         self.print_("##################################################################")
 88 |         self.print_("Evaluation %s" % self.filter_name)
 89 |         self.print_("##################################################################")
 90 |         self.print_("Trajectory length: %0.2fm" % trajectory_lengths[-1])
 91 |         self.print_("Final pose: %0.2fm, %0.2fm, %0.2fm,  %0.2fdeg, %0.2fdeg, %0.2fdeg"
 92 |                     % (positions[-1, 0], positions[-1, 1], positions[-1, 2], eul_final_deg[0], eul_final_deg[1], eul_final_deg[2]))
 93 |         self.print_("Position error 3D: %0.2fm, %0.2fm, %0.2fm --> %0.2fm --> %0.2f percent"
 94 |                     % (p_err_final[0], p_err_final[1], p_err_final[2], p_err_final_norm, p_err_final_norm / trajectory_lengths[-1] * 100))
 95 |         self.print_("Attitude error 3D: %0.2fdeg, %0.2fdeg, %0.2fdeg" % (eul_final_deg[0], eul_final_deg[1], eul_final_deg[2]))
 96 | 
 97 |         if len(export_file) > 0:
 98 |             self.print_("Exporting to %s" % (export_file + ".txt"))
 99 |             file = open(export_file + ".txt", "a")
100 |             file.write("%s, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f\n" % (
101 |                 self.name, trajectory_lengths[-1], p_err_final[0], p_err_final[1], p_err_final[2], p_err_final_norm,
102 |                 p_err_final_norm / trajectory_lengths[-1] * 100,
103 |                 eul_final_deg[0], eul_final_deg[1], eul_final_deg[2], yaw_err_deg))
104 |             file.close()
105 | 
106 |             np.savetxt(export_file + "_odometry.csv", np.hstack((t, positions, quaternions)), delimiter=' ', fmt='%.3f')
107 | 
108 | 
109 |     def print_(self, s):
110 |         rospy.loginfo("[odom_to_path_%s] : %s" % (self.filter_name, s))
111 | 
112 | 
113 | if __name__ == "__main__":
114 |     rospy.init_node('odom_to_path')
115 | 
116 |     topic_odom = rospy.get_param("~topic_odom", "")
117 |     filter_name = rospy.get_param("~filter_name", "ROVIO")
118 |     export_file = rospy.get_param("~export_file", "")
119 |     name = rospy.get_param("~name", "")
120 | 
121 |     if len(topic_odom) > 0 and len(filter_name) > 0:
122 |         odom_to_path = OdomToPath(topic_odom, filter_name, export_file, name)
123 | 
124 |         while not rospy.is_shutdown():
125 |             with odom_to_path.lock:
126 |                 if odom_to_path.last_time_callback is not None and time.time() - odom_to_path.last_time_callback > 2.0:
127 |                     break
128 |             rospy.sleep(0.5)
129 | 
130 |         odom_to_path.evaluate()
131 | 
132 |     else:
133 |         rospy.logerr("Parameters not setup correctly: topic_odom=%s, filter_name=%s" % (topic_odom, filter_name))
134 | 


--------------------------------------------------------------------------------
/rrxio/python/plot_states_uncertainty.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | """
  4 | This file is part of RRxIO - Robust Radar Visual/Thermal Odometry
  5 | @author Christopher Doer <christopher.doer@kit.edu>
  6 | """
  7 | 
  8 | from __future__ import with_statement
  9 | 
 10 | import threading
 11 | import numpy as np
 12 | import os
 13 | import time
 14 | 
 15 | import rospy
 16 | from nav_msgs.msg import Odometry
 17 | from sensor_msgs.msg import Imu
 18 | import tf.transformations
 19 | 
 20 | import matplotlib.pyplot as plt
 21 | 
 22 | 
 23 | def quat_to_euler(q):
 24 |     return tf.transformations.euler_from_quaternion(q[[1, 2, 3, 0]])
 25 | 
 26 | 
 27 | class DataIdx:
 28 |     def __init__(self):
 29 |         self.t = 0
 30 |         self.p = 1
 31 |         self.sigma_p = 4
 32 |         self.eul_deg = 7
 33 |         self.sigma_eul_deg = 10
 34 |         self.v = 13
 35 |         self.sigma_v = 16
 36 | 
 37 |         self.bias_acc = 1
 38 |         self.sigma_bias_acc = 4
 39 |         self.bias_gyro_deg = 7
 40 |         self.sigma_bias_gyro_deg = 10
 41 | 
 42 | 
 43 | class PlotStatesUncertainty:
 44 |     def __init__(self, topic_prefix):
 45 |         self.last_timestamp_odometry = None
 46 | 
 47 |         self.odometry_data = []
 48 |         self.velocity_data = []
 49 |         self.bias_data = []
 50 | 
 51 |         self.lock = threading.Lock()
 52 | 
 53 |         self.sub_odom = rospy.Subscriber(topic_prefix + "odometry", Odometry, self.callback_odometry, queue_size=1000000)
 54 |         self.sub_bias = rospy.Subscriber(topic_prefix + "imu_biases", Imu, self.callback_imu, queue_size=1000000)
 55 | 
 56 |     def run(self):
 57 |         a = 1
 58 |         while not rospy.is_shutdown():
 59 |             with self.lock:
 60 |                 if self.last_timestamp_odometry is not None:
 61 |                     time_diff = time.time() - self.last_timestamp_odometry
 62 |                 else:
 63 |                     time_diff = 0.
 64 |             if time_diff > 0.25:
 65 |                 break
 66 |             else:
 67 |                 time.sleep(0.1)
 68 | 
 69 |         self.sub_odom.unregister()
 70 |         self.plot()
 71 | 
 72 |     def plot(self):
 73 |         odometry_data = np.vstack(self.odometry_data)
 74 |         odometry_data[:, DataIdx().t] -= odometry_data[0, DataIdx().t]
 75 | 
 76 |         fig_odom, (ax_p, ax_v, ax_eul) = plt.subplots(3, 3, sharex=True)
 77 |         fig_odom.suptitle('Odometry', fontsize="x-large")
 78 | 
 79 |         fig_odom_var, (ax_p_var, ax_v_var, ax_eul_var) = plt.subplots(3, 3, sharex=True)
 80 |         fig_odom_var.suptitle('Odometry Variances', fontsize="x-large")
 81 | 
 82 |         xyz = 'xyz'
 83 |         for k in range(3):
 84 |             ax_p[k].plot(odometry_data[:, DataIdx().t], odometry_data[:, DataIdx().p + k], label=xyz[k])
 85 |             ax_p[k].set_title("p_" + xyz[k])
 86 |             ax_p[k].grid(True)
 87 | 
 88 |             ax_v[k].plot(odometry_data[:, DataIdx().t], odometry_data[:, DataIdx().v + k], label=xyz[k])
 89 |             ax_v[k].set_title("v_" + xyz[k])
 90 |             ax_v[k].grid(True)
 91 | 
 92 |             ax_eul[k].plot(odometry_data[:, DataIdx().t], odometry_data[:, DataIdx().eul_deg + k], label=xyz[k])
 93 |             ax_eul[k].set_title("eul_" + xyz[k])
 94 |             ax_eul[k].grid(True)
 95 | 
 96 |             ax_p_var[k].plot(odometry_data[:, DataIdx().t], odometry_data[:, DataIdx().sigma_p + k], label=xyz[k])
 97 |             ax_p_var[k].set_title("sigma p_" + xyz[k])
 98 |             ax_p_var[k].grid(True)
 99 | 
100 |             ax_v_var[k].plot(odometry_data[:, DataIdx().t], odometry_data[:, DataIdx().sigma_v + k], label=xyz[k])
101 |             ax_v_var[k].set_title("sigma v_" + xyz[k])
102 |             ax_v_var[k].grid(True)
103 | 
104 |             ax_eul_var[k].plot(odometry_data[:, DataIdx().t], odometry_data[:, DataIdx().sigma_eul_deg + k], label=xyz[k])
105 |             ax_eul_var[k].set_title("sigma eul_" + xyz[k])
106 |             ax_eul_var[k].grid(True)
107 | 
108 |         bias_data = np.vstack(self.bias_data)
109 |         bias_data[:, DataIdx().t] -= bias_data[0, DataIdx().t]
110 |         fig_bias, (ax_acc, ax_gyro) = plt.subplots(2, 3, sharex=True)
111 |         fig_bias.suptitle('Bias', fontsize="x-large")
112 | 
113 |         fig_bias_var, (ax_acc_var, ax_gyro_var) = plt.subplots(2, 3, sharex=True)
114 |         fig_bias_var.suptitle('Bias Variances', fontsize="x-large")
115 | 
116 |         for k in range(3):
117 |             ax_acc[k].plot(bias_data[:, DataIdx().t], bias_data[:, DataIdx().bias_acc + k], label=xyz[k])
118 |             ax_acc[k].set_title("acc_" + xyz[k])
119 |             ax_acc[k].grid(True)
120 | 
121 |             ax_gyro[k].plot(bias_data[:, DataIdx().t], bias_data[:, DataIdx().bias_gyro_deg + k], label=xyz[k])
122 |             ax_gyro[k].set_title("gyro_" + xyz[k])
123 |             ax_gyro[k].grid(True)
124 | 
125 |             ax_acc_var[k].plot(bias_data[:, DataIdx().t], bias_data[:, DataIdx().sigma_bias_acc + k], label=xyz[k])
126 |             ax_acc_var[k].set_title("sigma acc_" + xyz[k])
127 |             ax_acc_var[k].grid(True)
128 | 
129 |             ax_gyro_var[k].plot(bias_data[:, DataIdx().t], bias_data[:, DataIdx().sigma_bias_gyro_deg + k], label=xyz[k])
130 |             ax_gyro_var[k].set_title("sigma gyro_" + xyz[k])
131 |             ax_gyro_var[k].grid(True)
132 | 
133 |         while not rospy.is_shutdown():
134 |             plt.pause(0.1)
135 | 
136 |     def callback_odometry(self, msg=Odometry()):
137 |         with self.lock:
138 |             self.last_timestamp_odometry = time.time()
139 |             p = np.hstack((msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z))
140 |             sigma_p = np.sqrt(np.hstack((msg.pose.covariance[0], msg.pose.covariance[7], msg.pose.covariance[14])))
141 |             q = np.array([msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z])
142 |             eul_deg = np.rad2deg(quat_to_euler(q))
143 |             sigma_eul_deg = np.rad2deg(np.sqrt(np.hstack((msg.pose.covariance[21], msg.pose.covariance[28], msg.pose.covariance[35]))))
144 |             v = np.hstack((msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z))
145 |             sigma_v = np.sqrt(np.hstack((msg.twist.covariance[0], msg.twist.covariance[7], msg.twist.covariance[14])))
146 | 
147 |             self.odometry_data.append(np.hstack((msg.header.stamp.to_sec(), p, sigma_p, eul_deg, sigma_eul_deg, v, sigma_v)))
148 | 
149 |     def callback_imu(self, msg=Imu()):
150 |         with self.lock:
151 |             bias_acc = np.hstack((msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z))
152 |             sigma_bias_acc = np.sqrt(
153 |                 np.hstack((msg.linear_acceleration_covariance[0], msg.linear_acceleration_covariance[4], msg.linear_acceleration_covariance[8])))
154 |             bias_gyro_deg = np.rad2deg(np.hstack((msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z)))
155 |             sigma_bias_gyro = np.rad2deg(
156 |                 np.sqrt(np.hstack((msg.angular_velocity_covariance[0], msg.angular_velocity_covariance[4], msg.angular_velocity_covariance[8]))))
157 |             self.bias_data.append(np.hstack((msg.header.stamp.to_sec(), bias_acc, sigma_bias_acc, bias_gyro_deg, sigma_bias_gyro)))
158 | 
159 | 
160 | if __name__ == "__main__":
161 |     rospy.init_node('plot_states_uncertainty', anonymous=True)
162 | 
163 |     topic_prefix = rospy.get_param('~topic_prefix', "/rrxio/rovio/")
164 | 
165 |     plotting = PlotStatesUncertainty(topic_prefix)
166 |     plotting.run()
167 | 


--------------------------------------------------------------------------------
/rrxio/python/evaluate_ground_truth.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | """
  4 | This file is part of RRxIO - Robust Radar Visual/Thermal Odometry
  5 | @author Christopher Doer <christopher.doer@kit.edu>
  6 | """
  7 | 
  8 | from __future__ import with_statement
  9 | 
 10 | import threading
 11 | import numpy as np
 12 | import os
 13 | import time
 14 | 
 15 | import rospy
 16 | from geometry_msgs.msg import PoseStamped
 17 | from nav_msgs.msg import Odometry
 18 | import tf.transformations
 19 | 
 20 | VICON = "vicon"
 21 | PSEUDO_GT = "pseudo_gt"
 22 | FINAL_POSE = "final_pose"
 23 | 
 24 | 
 25 | class GroundTruth:
 26 |     def __init__(self, csv_file_path, dataset_type):
 27 | 
 28 |         if dataset_type == VICON:
 29 |             gt_raw = np.genfromtxt(csv_file_path, delimiter=',', skip_header=1)
 30 |             self.t = gt_raw[:, 0] * 1.0e-9
 31 |             self.p = gt_raw[:, 1:4] - gt_raw[0, 1:4]
 32 |             self.q = gt_raw[:, 4:8]
 33 |             self.v = gt_raw[:, 8:11]
 34 |             self.bw = gt_raw[:, 11:14]
 35 |             self.ba = gt_raw[:, 14:17]
 36 | 
 37 |         elif dataset_type == PSEUDO_GT:
 38 |             gt_raw = np.genfromtxt(csv_file_path, delimiter=' ', skip_header=1)
 39 |             self.t = gt_raw[:, 0]
 40 |             self.p = gt_raw[:, 1:4]
 41 |             q_xyzw = gt_raw[:, 4:8]
 42 |             self.q = np.hstack((q_xyzw[:, 3].reshape(-1, 1), q_xyzw[:, :3]))
 43 |             self.v = None
 44 | 
 45 |         self.step = 5
 46 |         self.trajectory_lengths = np.cumsum(np.linalg.norm(self.p[self.step::self.step, :] - self.p[:-self.step:self.step, :], axis=1))
 47 |         print("##### Loaded trajectory with length %0.2fm and duration %0.2fs" % (self.trajectory_lengths[-1], self.t[-1] - self.t[0]))
 48 | 
 49 | 
 50 | def quat_to_euler(q):
 51 |     return tf.transformations.euler_from_quaternion(q[[1, 2, 3, 0]])
 52 | 
 53 | 
 54 | class EvaluateGroundTruth:
 55 |     def __init__(self, rosbag_name, rosbag_dir, ground_truth_csv, pose_topic, odom_topic, export_directory, ground_truth_type):
 56 | 
 57 |         self.rosbag_name = rosbag_name
 58 |         self.rosbag_dir = rosbag_dir
 59 | 
 60 |         if len(export_directory) > 0:
 61 |             self.export_directory = export_directory
 62 |         else:
 63 |             self.export_directory = ""
 64 | 
 65 |         if ground_truth_type != FINAL_POSE:
 66 |             gt_csv_path = rosbag_dir + ground_truth_csv
 67 |             self.ground_truth = GroundTruth(gt_csv_path, ground_truth_type)
 68 | 
 69 |         self.ground_truth_type = ground_truth_type
 70 |         self.last_timestamp_odometry = None
 71 |         self.timestamp_first_odometry = None
 72 |         self.pose_data = []
 73 |         self.start_time = None
 74 |         self.last_pub_walltime = time.time()
 75 | 
 76 |         self.lock = threading.Lock()
 77 | 
 78 |         self.sub_pose = rospy.Subscriber(pose_topic, PoseStamped, self.callback_pose, queue_size=1000000)
 79 |         self.sub_odom = rospy.Subscriber(odom_topic, Odometry, self.callback_odometry, queue_size=1000000)
 80 | 
 81 |     def callback_pose(self, msg=PoseStamped()):
 82 |         if self.start_time is None:
 83 |             self.start_time = time.time()
 84 | 
 85 |         with self.lock:
 86 |             self.last_timestamp_odometry = time.time()
 87 | 
 88 |         p = np.hstack((msg.pose.position.x, msg.pose.position.y, msg.pose.position.z))
 89 |         q = np.array([msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z])
 90 | 
 91 |         self.pose_data.append(np.hstack((msg.header.stamp.to_sec(), p, q[1:], q[0])))
 92 | 
 93 |     def callback_odometry(self, msg=Odometry()):
 94 |         if self.start_time is None:
 95 |             self.start_time = time.time()
 96 | 
 97 |         with self.lock:
 98 |             self.last_timestamp_odometry = time.time()
 99 | 
100 |         p = np.hstack((msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z))
101 |         q = np.array([msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z])
102 | 
103 |         self.pose_data.append(np.hstack((msg.header.stamp.to_sec(), p, q[1:], q[0])))
104 | 
105 |     def print_(self, s):
106 |         rospy.loginfo("[evaluate_ground_truth] : " + s)
107 | 
108 |     def export(self):
109 | 
110 |         time_taken = self.last_timestamp_odometry - self.start_time
111 |         time_data = self.pose_data[-1][0] - self.pose_data[0][0]
112 |         runtime = "%0.2f &  %0.2f & %0.2f" % (time_taken, time_data, time_data / time_taken)
113 |         self.print_(runtime)
114 | 
115 |         if len(self.export_directory) > 0:
116 |             self.print_("Exporting to %s" % self.export_directory)
117 |             dataset = self.export_directory[self.export_directory.rfind("/") + 1:]
118 |             algo = self.export_directory[self.export_directory[:self.export_directory.rfind("/")].rfind("/") + 1:self.export_directory.rfind("/")]
119 |             export_path_eval = self.export_directory[:self.export_directory[:self.export_directory.rfind("/")].rfind("/")]
120 | 
121 |             timing_file = open(export_path_eval + "/" + "eval.txt", "a")
122 |             timing_file.write(dataset + "_" + algo + " & " + runtime + "\n")
123 |             timing_file.close()
124 | 
125 |             data_vio = np.vstack(self.pose_data)
126 | 
127 |             if not os.path.isdir(self.export_directory):
128 |                 os.makedirs(self.export_directory)
129 |             if self.ground_truth_type != FINAL_POSE:
130 |                 csv_gt = self.export_directory + "/" + "stamped_groundtruth.txt"
131 |                 csv_vio = self.export_directory + "/" + "stamped_traj_estimate.txt"
132 |                 data_gt = np.hstack(
133 |                     (self.ground_truth.t.reshape(-1, 1), self.ground_truth.p, self.ground_truth.q[:, 1:], self.ground_truth.q[:, 0].reshape(-1, 1)))
134 |                 np.savetxt(csv_gt, data_gt, delimiter=' ')
135 |                 np.savetxt(csv_vio, data_vio, delimiter=' ')
136 |             elif self.ground_truth_type == FINAL_POSE:
137 |                 positions = data_vio[:, 1:4]
138 | 
139 |                 step = 2
140 |                 trajectory_lengths = np.cumsum(np.linalg.norm(positions[step::step, :] - positions[:-step:step, :], axis=1))
141 |                 p_err_final = positions[-1, :] - positions[0, :]
142 |                 p_err_final_norm = np.linalg.norm(p_err_final)
143 |                 p_err_final_norm_rel = p_err_final_norm / trajectory_lengths[-1] * 100
144 |                 eul_0 = np.hstack(quat_to_euler(data_vio[0, 4:8]))
145 |                 eul_N = np.hstack(quat_to_euler(data_vio[-1, 4:8]))
146 |                 eul_final_deg = np.rad2deg(np.mod(eul_0 - eul_N + np.pi, 2 * np.pi) - np.pi)
147 | 
148 |                 final_err_file = open(export_path_eval + "/" + "final_err.txt", "a")
149 |                 final_err_file.write("%s, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f\n" % (
150 |                     dataset + "_" + algo, trajectory_lengths[-1], p_err_final[0], p_err_final[1], p_err_final[2], p_err_final_norm,
151 |                     p_err_final_norm_rel, eul_final_deg[0], eul_final_deg[1], eul_final_deg[2]))
152 |                 final_err_file.close()
153 | 
154 | 
155 | if __name__ == "__main__":
156 |     rospy.init_node('evaluate_ground_truth', anonymous=True)
157 | 
158 |     rosbag_name = rospy.get_param('~rosbag_name', "")
159 |     rosbag_dir = rospy.get_param('~rosbag_dir', "")
160 |     ground_truth_csv = rospy.get_param('~ground_truth_csv', "")
161 |     pose_topic = rospy.get_param('~pose_topic', "")
162 |     odom_topic = rospy.get_param('~odom_topic', "")
163 |     export_directory = rospy.get_param('~export_directory', "")
164 |     ground_truth_type = rospy.get_param('~ground_truth_type', VICON)
165 |     evaluator = EvaluateGroundTruth(rosbag_name, rosbag_dir, ground_truth_csv, pose_topic, odom_topic, export_directory, ground_truth_type)
166 |     while not rospy.core.is_shutdown():
167 |         if evaluator.last_timestamp_odometry is not None and time.time() - evaluator.last_timestamp_odometry > 1:
168 |             break
169 |         time.sleep(0.1)
170 | 
171 |     evaluator.export()
172 | 


--------------------------------------------------------------------------------
/rrxio/include/rrxio/VelocityUpdate.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2021 Christopher Doer
  3 |  * Copyright (c) 2014, Autonomous Systems Lab
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  * * Redistributions of source code must retain the above copyright
  9 |  * notice, this list of conditions and the following disclaimer.
 10 |  * * Redistributions in binary form must reproduce the above copyright
 11 |  * notice, this list of conditions and the following disclaimer in the
 12 |  * documentation and/or other materials provided with the distribution.
 13 |  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 14 |  * names of its contributors may be used to endorse or promote products
 15 |  * derived from this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 18 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 19 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 20 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 21 |  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 22 |  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 23 |  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 24 |  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 25 |  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 26 |  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 27 |  *
 28 |  */
 29 | 
 30 | #ifndef ROVIO_VELOCITYUPDATE_HPP_
 31 | #define ROVIO_VELOCITYUPDATE_HPP_
 32 | 
 33 | #include <ros/ros.h>
 34 | 
 35 | #include "lightweight_filtering/common.hpp"
 36 | #include "lightweight_filtering/Update.hpp"
 37 | #include "lightweight_filtering/State.hpp"
 38 | #include "rovio/FilterStates.hpp"
 39 | 
 40 | namespace rovio
 41 | {
 42 | /** \brief Class, defining the innovation.
 43 |  */
 44 | class VelocityInnovation : public LWF::State<LWF::VectorElement<3>>
 45 | {
 46 | public:
 47 |   typedef LWF::State<LWF::VectorElement<3>> Base;
 48 |   using Base::E_;
 49 |   static constexpr unsigned int _vel = 0;
 50 |   VelocityInnovation()
 51 |   {
 52 |     static_assert(_vel + 1 == E_, "Error with indices");
 53 |     this->template getName<_vel>() = "vel";
 54 |   };
 55 |   virtual ~VelocityInnovation(){};
 56 |   inline V3D& vel() { return this->template get<_vel>(); }
 57 |   inline const V3D& vel() const { return this->template get<_vel>(); }
 58 | };
 59 | 
 60 | /** \brief Class, dummy auxillary class for Zero velocity update
 61 |  */
 62 | class VelocityUpdateMeasAuxiliary : public LWF::AuxiliaryBase<VelocityUpdateMeasAuxiliary>
 63 | {
 64 | public:
 65 |   VelocityUpdateMeasAuxiliary(){};
 66 |   virtual ~VelocityUpdateMeasAuxiliary(){};
 67 | };
 68 | 
 69 | /**  \brief Empty measurement
 70 |  */
 71 | class VelocityUpdateMeas : public LWF::State<LWF::VectorElement<3>, VelocityUpdateMeasAuxiliary>
 72 | {
 73 | public:
 74 |   typedef LWF::State<LWF::VectorElement<3>, VelocityUpdateMeasAuxiliary> Base;
 75 |   using Base::E_;
 76 |   static constexpr unsigned int _vel = 0;
 77 |   static constexpr unsigned int _aux = _vel + 1;
 78 |   VelocityUpdateMeas()
 79 |   {
 80 |     static_assert(_aux + 1 == E_, "Error with indices");
 81 |     this->template getName<_vel>() = "vel";
 82 |     this->template getName<_aux>() = "aux";
 83 |   };
 84 |   virtual ~VelocityUpdateMeas(){};
 85 |   inline V3D& vel() { return this->template get<_vel>(); }
 86 |   inline const V3D& vel() const { return this->template get<_vel>(); }
 87 | };
 88 | 
 89 | /**  \brief Class holding the update noise.
 90 |  */
 91 | class VelocityUpdateNoise : public LWF::State<LWF::VectorElement<3>>
 92 | {
 93 | public:
 94 |   typedef LWF::State<LWF::VectorElement<3>> Base;
 95 |   using Base::E_;
 96 |   static constexpr unsigned int _vel = 0;
 97 |   VelocityUpdateNoise()
 98 |   {
 99 |     static_assert(_vel + 1 == E_, "Error with indices");
100 |     this->template getName<_vel>() = "vel";
101 |   };
102 |   virtual ~VelocityUpdateNoise(){};
103 |   inline V3D& vel() { return this->template get<_vel>(); }
104 |   inline const V3D& vel() const { return this->template get<_vel>(); }
105 |   inline void setNoise(const V3D& sigma)
106 |   {
107 |     this->template get<_vel>() = sigma;
108 |     std::cout << "new noise " << this->template get<_vel>() << std::endl;
109 |   }
110 | };
111 | 
112 | /** \brief Outlier Detection.
113 |  * ODEntry<Start entry, dimension of detection>
114 |  */
115 | class VelocityOutlierDetection
116 |   : public LWF::OutlierDetection<LWF::ODEntry<VelocityInnovation::template getId<VelocityInnovation::_vel>(), 3>>
117 | {
118 | public:
119 |   virtual ~VelocityOutlierDetection(){};
120 | };
121 | 
122 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
123 | 
124 | /** \brief Class, holding the zero velocity update
125 |  */
126 | template <typename FILTERSTATE>
127 | class VelocityUpdate : public LWF::Update<VelocityInnovation,
128 |                                           FILTERSTATE,
129 |                                           VelocityUpdateMeas,
130 |                                           VelocityUpdateNoise,
131 |                                           VelocityOutlierDetection,
132 |                                           false>
133 | {
134 | public:
135 |   typedef LWF::
136 |       Update<VelocityInnovation, FILTERSTATE, VelocityUpdateMeas, VelocityUpdateNoise, VelocityOutlierDetection, false>
137 |           Base;
138 |   using Base::doubleRegister_;
139 |   using Base::intRegister_;
140 |   using Base::meas_;
141 |   typedef typename Base::mtState mtState;
142 |   typedef typename Base::mtFilterState mtFilterState;
143 |   typedef typename Base::mtInnovation mtInnovation;
144 |   typedef typename Base::mtMeas mtMeas;
145 |   typedef typename Base::mtNoise mtNoise;
146 |   typedef typename Base::mtOutlierDetection mtOutlierDetection;
147 | 
148 |   QPD qAM_;  // Rotation between IMU (M) and coordinate frame where the velocity is expressed in (A)
149 | 
150 |   /** \brief Constructor.
151 |    *
152 |    *   Loads and sets the needed parameters.
153 |    */
154 |   VelocityUpdate()
155 |   {
156 |     qAM_.setIdentity();
157 |     intRegister_.removeScalarByStr("maxNumIteration");
158 |     doubleRegister_.removeScalarByStr("alpha");
159 |     doubleRegister_.removeScalarByStr("beta");
160 |     doubleRegister_.removeScalarByStr("kappa");
161 |     doubleRegister_.removeScalarByStr("updateVecNormTermination");
162 |     doubleRegister_.registerQuaternion("qAM", qAM_);
163 | 
164 |     Base::disablePreAndPostProcessingWarning_ = true;
165 |   }
166 | 
167 |   /** \brief Destructor
168 |    */
169 |   virtual ~VelocityUpdate(){};
170 | 
171 |   virtual void postProcess(mtFilterState& filterState,
172 |                            const mtMeas& meas,
173 |                            const mtOutlierDetection& outlierDetection,
174 |                            bool& isFinished)
175 |   {
176 |     isFinished = true;
177 |   }
178 | 
179 |   /**
180 |    * @brief Updates the measurement noise
181 |    * @param cov
182 |    */
183 |   void setMeasurementNoise(const Eigen::MatrixXd& cov) { Base::updnoiP_ = cov; }
184 | 
185 |   /** \brief Compute the inovvation term
186 |    *
187 |    *  @param mtInnovation - Class, holding innovation data.
188 |    *  @param state        - Filter %State.
189 |    *  @param meas         - Not Used.
190 |    *  @param noise        - Additive discrete Gaussian noise.
191 |    *  @param dt           - Not used.
192 |    */
193 |   void evalInnovation(mtInnovation& y, const mtState& state, const mtNoise& noise) const
194 |   {
195 |     y.vel() = qAM_.rotate(state.MvM()) + meas_.vel() + noise.vel();  // Velocity of state has a minus sign
196 |   }
197 | 
198 |   /** \brief Computes the Jacobian for the update step of the filter.
199 |    *
200 |    *  @param F     - Jacobian for the update step of the filter.
201 |    *  @param state - Filter state.
202 |    *  @param meas  - Not used.
203 |    *  @param dt    - Not used.
204 |    */
205 |   void jacState(MXD& F, const mtState& state) const
206 |   {
207 |     F.setZero();
208 |     F.template block<3, 3>(mtInnovation::template getId<mtInnovation::_vel>(),
209 |                            mtState::template getId<mtState::_vel>()) = MPD(qAM_).matrix();
210 |   }
211 | 
212 |   /** \brief Computes the Jacobian for the update step of the filter w.r.t. to the noise variables
213 |    *
214 |    *  @param G     - Jacobian for the update step of the filter.
215 |    *  @param state - Filter state.
216 |    *  @param meas  - Not used.
217 |    *  @param dt    - Not used.
218 |    */
219 |   void jacNoise(MXD& G, const mtState& state) const
220 |   {
221 |     G.setZero();
222 |     G.template block<3, 3>(mtInnovation::template getId<mtInnovation::_vel>(),
223 |                            mtNoise::template getId<mtNoise::_vel>()) = Eigen::Matrix3d::Identity();
224 |   }
225 | };
226 | 
227 | }  // namespace rovio
228 | 
229 | #endif /* ROVIO_VELOCITYUPDATE_HPP_ */
230 | 


--------------------------------------------------------------------------------
/rrxio/python/evaluate_iros_datasets.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | """
  4 | This file is part of RRxIO - Robust Radar Visual/Thermal Odometry
  5 | @author Christopher Doer <christopher.doer@kit.edu>
  6 | """
  7 | 
  8 | from __future__ import with_statement
  9 | import sys
 10 | import os
 11 | import rospkg
 12 | import time
 13 | from threading import Thread
 14 | 
 15 | import evaluate_ground_truth
 16 | 
 17 | 
 18 | def run(N_feature):
 19 |     result_base_directory = rosbag_dir + "/results/evaluation/" + str(N_feature) + "/"
 20 | 
 21 |     N_rovio_features = " N:=" + str(N_feature)
 22 | 
 23 |     base_configs = [
 24 |         {"launch_file": "rrxio_evaluate_rosbag.launch", "name": str(N_feature) + "_rrxio_visual", "config": "rrxio_iros_datasets_visual.info",
 25 |          "use_vel": True, "camera_config": camera_config_visual, "params": params_visual + " timeshift_cam_imu:=0.002"},
 26 |         {"launch_file": "rrxio_evaluate_rosbag.launch", "name": str(N_feature) + "_rrxio_thermal", "config": "rrxio_iros_datasets_thermal.info",
 27 |          "use_vel": True, "camera_config": camera_config_thermal, "params": params_thermal + " timeshift_cam_imu:=-0.004"}
 28 |     ]
 29 | 
 30 |     configs = [
 31 |         {'name': "base", 'changes': {}},
 32 |     ]
 33 | 
 34 |     datasets = [
 35 |         {"name": "mocap_easy", "start_time": 0, "ground_truth_type": evaluate_ground_truth.VICON},  # 61m 94s
 36 |         {"name": "mocap_medium", "start_time": 0, "ground_truth_type": evaluate_ground_truth.VICON},  # 65m 87s
 37 |         {"name": "mocap_difficult", "start_time": 0, "ground_truth_type": evaluate_ground_truth.VICON},  # 93m 87s
 38 |         {"name": "mocap_dark", "start_time": 0, "ground_truth_type": evaluate_ground_truth.VICON},  # 111m, 135s
 39 |         {"name": "mocap_dark_fast", "start_time": 0, "ground_truth_type": evaluate_ground_truth.VICON},  # 75m, 86s
 40 |         
 41 |         {"name": "gym", "start_time": 0, "ground_truth_type": evaluate_ground_truth.PSEUDO_GT},  # 74m, 84s
 42 |         {"name": "indoor_floor", "start_time": 0, "ground_truth_type": evaluate_ground_truth.PSEUDO_GT},  # 240m, 206s
 43 |         {"name": "outdoor_campus", "start_time": 0, "ground_truth_type": evaluate_ground_truth.PSEUDO_GT},  # 102m, 100s
 44 |         {"name": "outdoor_street", "start_time": 0, "ground_truth_type": evaluate_ground_truth.PSEUDO_GT},  # 202m, 186s
 45 |     ]
 46 | 
 47 |     platform = "nuc"
 48 |     default_params = "shutdown_when_done:=True enable_rviz:=False"
 49 |     export_directory = result_base_directory + platform + "/"
 50 | 
 51 |     rospack = rospkg.RosPack()
 52 | 
 53 |     evaluation_dir = result_base_directory + "evaluation_full_align/"
 54 |     eval_config = "align_type: posyaw\n" \
 55 |                   "align_num_frames: -1"
 56 | 
 57 |     if suppress_console_output:
 58 |         tail = " >/dev/null 2>&1"
 59 |     else:
 60 |         tail = ""
 61 | 
 62 |     ctr = 0
 63 |     runs = len(base_configs) * len(configs) * len(datasets)
 64 |     start_time = time.time()
 65 |     for base_config in base_configs:
 66 |         for dataset in datasets:
 67 |             euroc_name = dataset["name"]
 68 |             for config in configs:
 69 |                 ctr += 1
 70 |                 print("############################################################")
 71 |                 print(N_feature + ": Progress %d / %d remaining time %0.2fmin" % (ctr, runs, (runs - ctr) * (time.time() - start_time) / ctr / 60.0 * 1.5))
 72 |                 print("############################################################")
 73 | 
 74 |                 config_name = base_config['name'] + "_" + config["name"]
 75 |                 export_directory_run = export_directory + config_name + "/" + platform + "_" + config_name + "_" + euroc_name
 76 | 
 77 |                 # copy config
 78 |                 if not os.path.isdir(export_directory_run):
 79 |                     os.makedirs(export_directory_run)
 80 |                 config_file = rospack.get_path('rrxio') + "/launch/configs/" + base_config["config"]
 81 | 
 82 |                 os.system("cp " + config_file + " " + export_directory_run + "/" + config_name + ".info")
 83 | 
 84 |                 config_file = export_directory_run + "/" + config_name + ".info"
 85 |                 f = open(config_file, "r")
 86 |                 config_str = "".join(f.readlines())
 87 |                 f.close()
 88 |                 for old, new in config['changes'].items():
 89 |                     config_str = config_str.replace(old, new)
 90 | 
 91 |                 f = open(config_file, "w")
 92 |                 f.write(config_str)
 93 |                 f.close()
 94 | 
 95 |                 params = ""
 96 |                 if "params" in config.keys():
 97 |                     params = config["params"]
 98 | 
 99 |                 if base_config["use_vel"]:
100 |                     params += params_radar
101 |                 else:
102 |                     params += "topic_radar_trigger:=no_radar_trigger topic_radar_scan:=no_radar"
103 | 
104 |                 for k in range(N_trials):
105 |                     cmd = "roslaunch rrxio " + base_config["launch_file"] + \
106 |                           " bag_start:=" + str(dataset["start_time"]) + \
107 |                           " " + default_params + \
108 |                           " rosbag:=" + euroc_name + \
109 |                           " ground_truth_csv:=" + euroc_name + "_gt.csv" + \
110 |                           " ground_truth_type:=" + dataset["ground_truth_type"] + \
111 |                           " export_directory:=" + export_directory_run + \
112 |                           " config:=" + config_file + \
113 |                           " rosbag_dir:=" + rosbag_dir + \
114 |                           " " + params + \
115 |                           N_rovio_features + \
116 |                           " " + base_config["params"] + \
117 |                           " id:=" + N_feature
118 |                     print(cmd)
119 |                     os.system(cmd + tail)
120 |                     if N_trials > 1:
121 |                         os.system(
122 |                             "mv " + export_directory_run + "/stamped_traj_estimate.txt" + " " + export_directory_run + "/stamped_traj_estimate" + str(
123 |                                 k) + ".txt")
124 | 
125 |                 config_file_eval = open(export_directory + config_name + "/" + platform + "_" + config_name + "_" + euroc_name + "/eval_cfg.yaml", "w")
126 |                 config_file_eval.write(eval_config)
127 |                 config_file_eval.close()
128 | 
129 |     print(N_feature + ": Starting evaluation...")
130 | 
131 |     if not os.path.isdir(evaluation_dir):
132 |         os.mkdir(evaluation_dir)
133 | 
134 |     ws = "  "
135 |     s = "Datasets:\n"
136 |     for euroc_dataset in datasets:
137 |         if euroc_dataset["ground_truth_type"] != evaluate_ground_truth.FINAL_POSE:
138 |             name = euroc_dataset['name']
139 |             s += ws + name + ":\n"
140 |             s += ws + ws + "label: " + name.replace("_", "") + "\n"
141 | 
142 |     ws = "  "
143 |     s += "Algorithms:\n"
144 |     algos = []
145 |     for base_config in base_configs:
146 |         for config in configs:
147 |             algos.append(base_config['name'] + "_" + config["name"])
148 | 
149 |     for algo in algos:
150 |         s += ws + algo + ":\n"
151 |         s += ws + ws + "fn: traj_est\n"
152 |         s += ws + ws + "label: " + algo.replace("_", "") + "\n"
153 | 
154 |     s += "RelDistances: [12,24,36,48,60]"
155 |     eval_config = open(result_base_directory + "/evaluation_config.yaml", "w")
156 |     eval_config.write(s)
157 |     eval_config.close()
158 | 
159 |     cmd = "rosrun rpg_trajectory_evaluation analyze_trajectories.py " + result_base_directory + "/evaluation_config.yaml" \
160 |                                                                                                 " --output_dir=" + evaluation_dir + \
161 |           " --results_dir=" + result_base_directory + \
162 |           " --platform nuc --odometry_error_per_dataset --overall_odometry_error --plot_trajectories --rmse_table --rmse_boxplot --png --no_sort_names"
163 | 
164 |     if N_trials > 1:
165 |         cmd += " --mul_trials " + str(N_trials)
166 |     print(cmd)
167 |     os.system(cmd + tail)
168 | 
169 | 
170 | def evaluate():
171 |     threads = []
172 |     start = time.time()
173 |     for N_feature in N_features:
174 |         threads.append(Thread(target=run, args=(N_feature,)))
175 |         threads[-1].start()
176 |     time.sleep(1)
177 |     while True:
178 |         for i in range(len(threads)):
179 |             if not threads[i].is_alive():
180 |                 print("###### Thread " + N_features[i] + " is done  :)")
181 |                 threads[i].join(1)
182 |                 if len(threads) == 1:
183 |                     threads = []
184 |                 else:
185 |                     del threads[i]
186 |                 break
187 |             else:
188 |                 time.sleep(1)
189 |         if len(threads) == 0:
190 |             break
191 |     print("Done in %0.2f minutes" % ((time.time() - start) / 60))
192 | 
193 | 
194 | if __name__ == '__main__':
195 |     if len(sys.argv) < 2:
196 |         print("Usage: rosrun eveluate_iros_datasets.py <path-to-radar_thermal_visual_inertial_datasets_iros_2021>")
197 |     else:
198 |         rosbag_dir = sys.argv[1]
199 |         suppress_console_output = True
200 |         N_trials = 1
201 |         camera_config_visual = "rovio_visual"
202 |         camera_config_thermal = "rovio_thermal"
203 | 
204 |         N_features = ["25", "15", "10"]
205 |         params_radar = "topic_radar_trigger:=sensor_platform/radar/trigger topic_radar_scan:=/sensor_platform/radar/scan"
206 |         params_visual = "camera_config:=" + rosbag_dir + "rovio_visual.yaml topic_cam:=/sensor_platform/camera_visual/img"
207 |         params_thermal = "camera_config:=" + rosbag_dir + "rovio_thermal.yaml topic_cam:=/sensor_platform/camera_thermal/img"
208 | 
209 |         evaluate()
210 | 


--------------------------------------------------------------------------------
/rrxio/src/nodes/rrxio_rosbag_loader.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2021, Christopher Doer
  3 |  * Copyright (c) 2014, Autonomous Systems Lab
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  * * Redistributions of source code must retain the above copyright
  9 |  * notice, this list of conditions and the following disclaimer.
 10 |  * * Redistributions in binary form must reproduce the above copyright
 11 |  * notice, this list of conditions and the following disclaimer in the
 12 |  * documentation and/or other materials provided with the distribution.
 13 |  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 14 |  * names of its contributors may be used to endorse or promote products
 15 |  * derived from this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 18 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 19 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 20 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 21 |  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 22 |  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 23 |  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 24 |  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 25 |  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 26 |  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 27 |  *
 28 |  */
 29 | 
 30 | #include <ros/package.h>
 31 | #include <rosbag/bag.h>
 32 | #include <rosbag/view.h>
 33 | #include <sensor_msgs/CompressedImage.h>
 34 | #include <memory>
 35 | #include <iostream>
 36 | #include <locale>
 37 | #include <string>
 38 | #include <Eigen/StdVector>
 39 | #include "rrxio/RRxIOFilter.hpp"
 40 | #include "rrxio/RRxIONode.hpp"
 41 | #include <boost/foreach.hpp>
 42 | #include <boost/date_time/posix_time/posix_time.hpp>
 43 | #include <boost/date_time/posix_time/posix_time_io.hpp>
 44 | #include <random>
 45 | 
 46 | #define foreach BOOST_FOREACH
 47 | 
 48 | #ifdef ROVIO_NMAXFEATURE
 49 | static constexpr int nMax_ = ROVIO_NMAXFEATURE;
 50 | #else
 51 | static constexpr int nMax_      = 15;  // Maximal number of considered features in the filter state.
 52 | #endif
 53 | 
 54 | #ifdef ROVIO_NLEVELS
 55 | static constexpr int nLevels_ = ROVIO_NLEVELS;
 56 | #else
 57 | static constexpr int nLevels_   = 4;   // // Total number of pyramid levels considered.
 58 | #endif
 59 | 
 60 | #ifdef ROVIO_PATCHSIZE
 61 | static constexpr int patchSize_ = ROVIO_PATCHSIZE;
 62 | #else
 63 | static constexpr int patchSize_ = 8;   // Edge length of the patches (in pixel). Must be a multiple of 2!
 64 | #endif
 65 | 
 66 | #ifdef ROVIO_NCAM
 67 | static constexpr int nCam_ = ROVIO_NCAM;
 68 | #else
 69 | static constexpr int nCam_      = 1;   // Used total number of cameras.
 70 | #endif
 71 | 
 72 | #ifdef ROVIO_NPOSE
 73 | static constexpr int nPose_ = ROVIO_NPOSE;
 74 | #else
 75 | static constexpr int nPose_     = 0;   // Additional pose states.
 76 | #endif
 77 | 
 78 | typedef rovio::RovioFilter<rovio::FilterState<nMax_, nLevels_, patchSize_, nCam_, nPose_>> mtFilter;
 79 | 
 80 | int main(int argc, char** argv)
 81 | {
 82 |   ros::init(argc, argv, "rovio");
 83 |   ros::NodeHandle nh;
 84 |   ros::NodeHandle nh_private("~");
 85 | 
 86 |   std::string rootdir       = ros::package::getPath("rovio");  // Leaks memory
 87 |   std::string filter_config = rootdir + "/cfg/rovio.info";
 88 | 
 89 |   nh_private.param("filter_config", filter_config, filter_config);
 90 | 
 91 |   ROS_INFO_STREAM("RRxIO started");
 92 | 
 93 |   // Filter
 94 |   std::shared_ptr<mtFilter> mpFilter(new mtFilter);
 95 |   mpFilter->readFromInfo(filter_config);
 96 | 
 97 |   // Force the camera calibration paths to the ones from ROS parameters.
 98 |   for (unsigned int camID = 0; camID < nCam_; ++camID)
 99 |   {
100 |     std::string camera_config;
101 |     if (nh_private.getParam("camera" + std::to_string(camID) + "_config", camera_config))
102 |     {
103 |       mpFilter->cameraCalibrationFile_[camID] = camera_config;
104 |     }
105 |   }
106 |   mpFilter->refreshProperties();
107 | 
108 |   // Node
109 |   rovio::RovioNode<mtFilter> rovioNode(nh, nh_private, mpFilter);
110 |   //  rovio::RovioNode rovioNode(nh, nh_private, mpFilter);
111 |   rovioNode.makeTest();
112 |   double resetTrigger = 0.0;
113 |   nh_private.param("record_odometry", rovioNode.forceOdometryPublishing_, rovioNode.forceOdometryPublishing_);
114 |   nh_private.param("record_pose_with_covariance_stamped",
115 |                    rovioNode.forcePoseWithCovariancePublishing_,
116 |                    rovioNode.forcePoseWithCovariancePublishing_);
117 |   nh_private.param("record_transform", rovioNode.forceTransformPublishing_, rovioNode.forceTransformPublishing_);
118 |   nh_private.param("record_extrinsics", rovioNode.forceExtrinsicsPublishing_, rovioNode.forceExtrinsicsPublishing_);
119 |   nh_private.param("record_imu_bias", rovioNode.forceImuBiasPublishing_, rovioNode.forceImuBiasPublishing_);
120 |   nh_private.param("record_pcl", rovioNode.forcePclPublishing_, rovioNode.forcePclPublishing_);
121 |   nh_private.param("record_markers", rovioNode.forceMarkersPublishing_, rovioNode.forceMarkersPublishing_);
122 |   nh_private.param("record_patch", rovioNode.forcePatchPublishing_, rovioNode.forcePatchPublishing_);
123 |   nh_private.param("reset_trigger", resetTrigger, resetTrigger);
124 | 
125 |   std::cout << "Recording";
126 |   if (rovioNode.forceOdometryPublishing_)
127 |     std::cout << ", odometry";
128 |   if (rovioNode.forceTransformPublishing_)
129 |     std::cout << ", transform";
130 |   if (rovioNode.forceExtrinsicsPublishing_)
131 |     std::cout << ", extrinsics";
132 |   if (rovioNode.forceImuBiasPublishing_)
133 |     std::cout << ", imu biases";
134 |   if (rovioNode.forcePclPublishing_)
135 |     std::cout << ", point cloud";
136 |   if (rovioNode.forceMarkersPublishing_)
137 |     std::cout << ", markers";
138 |   if (rovioNode.forcePatchPublishing_)
139 |     std::cout << ", patch data";
140 |   std::cout << std::endl;
141 | 
142 |   rosbag::Bag bagIn;
143 |   std::string rosbag_filename = "dataset.bag";
144 |   nh_private.param("rosbag_filename", rosbag_filename, rosbag_filename);
145 |   bagIn.open(rosbag_filename, rosbag::bagmode::Read);
146 | 
147 |   std::vector<std::string> topics;
148 |   std::string imu_topic_name = "/imu0";
149 |   nh_private.param("imu_topic_name", imu_topic_name, imu_topic_name);
150 |   std::string cam0_topic_name = "/cam0/image_raw";
151 |   nh_private.param("cam0_topic_name", cam0_topic_name, cam0_topic_name);
152 |   std::string cam1_topic_name = "/cam1/image_raw";
153 |   nh_private.param("cam1_topic_name", cam1_topic_name, cam1_topic_name);
154 |   std::string topic_vel = "";
155 |   nh_private.param("topic_vel", topic_vel, topic_vel);
156 | 
157 |   // Radar ego veloity
158 |   std::string topic_radar_trigger = "";
159 |   nh_private.param("topic_radar_trigger", topic_radar_trigger, topic_radar_trigger);
160 |   std::string topic_radar_scan = "";
161 |   nh_private.param("topic_radar_scan", topic_radar_scan, topic_radar_scan);
162 | 
163 |   double timeshift_cam_imu = 0.0;
164 |   nh_private.param("timeshift_cam_imu", timeshift_cam_imu, timeshift_cam_imu);
165 | 
166 |   double bag_start = 0.0;
167 |   nh_private.param("bag_start", bag_start, bag_start);
168 | 
169 |   double bag_duration = -1.;
170 |   nh_private.param("bag_duration", bag_duration, bag_duration);
171 | 
172 |   double sigma_v_b_x = 0.0;
173 |   nh_private.param("sigma_v_b_x", sigma_v_b_x, sigma_v_b_x);
174 | 
175 |   double sigma_v_b_y = 0.0;
176 |   nh_private.param("sigma_v_b_y", sigma_v_b_y, sigma_v_b_y);
177 | 
178 |   double sigma_v_b_z = 0.0;
179 |   nh_private.param("sigma_v_b_z", sigma_v_b_z, sigma_v_b_z);
180 | 
181 |   int max_frame_ctr = 1000000;
182 |   nh_private.param("max_frame_ctr", max_frame_ctr, max_frame_ctr);
183 | 
184 |   std::default_random_engine generator(ros::WallTime::now().nsec);
185 |   std::normal_distribution<double> nd_v_b_x(0, sigma_v_b_x);
186 |   std::normal_distribution<double> nd_v_b_y(0, sigma_v_b_y);
187 |   std::normal_distribution<double> nd_v_b_z(0, sigma_v_b_z);
188 | 
189 |   std::string odometry_topic_name  = rovioNode.pubOdometry_.getTopic();
190 |   std::string transform_topic_name = rovioNode.pubTransform_.getTopic();
191 |   std::string extrinsics_topic_name[mtFilter::mtState::nCam_];
192 |   for (int camID = 0; camID < mtFilter::mtState::nCam_; camID++)
193 |   {
194 |     extrinsics_topic_name[camID] = rovioNode.pubExtrinsics_[camID].getTopic();
195 |   }
196 |   std::string imu_bias_topic_name = rovioNode.pubImuBias_.getTopic();
197 |   std::string pcl_topic_name      = rovioNode.pubPcl_.getTopic();
198 |   std::string u_rays_topic_name   = rovioNode.pubMarkers_.getTopic();
199 |   std::string patch_topic_name    = rovioNode.pubPatch_.getTopic();
200 | 
201 |   topics.push_back(std::string(imu_topic_name));
202 |   topics.push_back(std::string(cam0_topic_name));
203 |   topics.push_back(std::string(cam1_topic_name));
204 |   topics.push_back(std::string(topic_vel));
205 |   topics.push_back(std::string(topic_radar_trigger));
206 |   topics.push_back(std::string(topic_radar_scan));
207 |   rosbag::View view(bagIn, rosbag::TopicQuery(topics));
208 | 
209 |   ROS_INFO_STREAM("[rovio]: Subscribing radar_trigger on: " << topic_radar_trigger);
210 |   ROS_INFO_STREAM("[rovio]: Subscribing radar_scan on: " << topic_radar_scan);
211 | 
212 |   bool isTriggerInitialized = false;
213 |   double lastTriggerTime    = 0.0;
214 |   ros::Time start           = ros::TIME_MIN;
215 |   uint frame_ctr            = 0;
216 | 
217 |   for (rosbag::View::iterator it = view.begin(); it != view.end() && ros::ok(); it++)
218 |   {
219 |     if (start == ros::TIME_MIN)
220 |       start = it->getTime();
221 | 
222 |     if ((it->getTime() - start).toSec() < bag_start)
223 |       continue;
224 | 
225 |     if (bag_duration > 0 && (it->getTime() - start).toSec() - bag_start - bag_duration > 0)
226 |       break;
227 | 
228 |     if (it->getTopic() == imu_topic_name)
229 |     {
230 |       sensor_msgs::Imu::ConstPtr imuMsg = it->instantiate<sensor_msgs::Imu>();
231 |       if (imuMsg != NULL)
232 |         rovioNode.imuCallback(imuMsg);
233 |     }
234 |     else if (it->getTopic() == cam0_topic_name && frame_ctr < max_frame_ctr)
235 |     {
236 |       sensor_msgs::ImagePtr imgMsg = it->instantiate<sensor_msgs::Image>();
237 |       if (imgMsg != NULL)
238 |       {
239 |         imgMsg->header.stamp = ros::Time().fromSec(imgMsg->header.stamp.toSec() + timeshift_cam_imu);
240 |         rovioNode.imgCallback0(imgMsg);
241 |         ++frame_ctr;
242 |       }
243 |       else
244 |       {
245 |         sensor_msgs::CompressedImageConstPtr compImgMsg = it->instantiate<sensor_msgs::CompressedImage>();
246 |         if (compImgMsg != NULL)
247 |         {
248 |           cv_bridge::CvImageConstPtr ptr;
249 |           cv::Mat cv_img = cv_bridge::toCvCopy(compImgMsg)->image;
250 |           cv::resize(cv_img, cv_img, cv::Size(), 0.5, 0.5);
251 |           sensor_msgs::ImagePtr imgMsg = cv_bridge::CvImage(compImgMsg->header, "mono8", cv_img).toImageMsg();
252 |           imgMsg->header.stamp         = ros::Time().fromSec(imgMsg->header.stamp.toSec() + timeshift_cam_imu);
253 |           imgMsg->header.frame_id      = compImgMsg->header.frame_id;
254 |           rovioNode.imgCallback0(imgMsg);
255 |           ++frame_ctr;
256 |         }
257 |       }
258 |     }
259 |     else if (it->getTopic() == topic_vel)
260 |     {
261 |       geometry_msgs::TwistStampedPtr velMsg = it->instantiate<geometry_msgs::TwistStamped>();
262 |       if (velMsg != NULL)
263 |       {
264 |         const auto noise_x = nd_v_b_x(generator);
265 |         const auto noise_y = nd_v_b_y(generator);
266 |         const auto noise_z = nd_v_b_z(generator);
267 |         velMsg->twist.linear.x += noise_x;
268 |         velMsg->twist.linear.y += noise_y;
269 |         velMsg->twist.linear.z += noise_z;
270 | 
271 |         rovioNode.velocityCallback(velMsg);
272 |       }
273 |     }
274 | 
275 |     else if (it->getTopic() == topic_radar_trigger)
276 |     {
277 |       std_msgs::HeaderConstPtr trigger_msg = it->instantiate<std_msgs::Header>();
278 |       if (trigger_msg != NULL)
279 |         rovioNode.radarTriggerCallback(trigger_msg);
280 |     }
281 |     else if (it->getTopic() == topic_radar_scan)
282 |     {
283 |       sensor_msgs::PointCloud2ConstPtr radar_scan_msg = it->instantiate<sensor_msgs::PointCloud2>();
284 |       if (radar_scan_msg != NULL)
285 |         rovioNode.radarScanCallback(radar_scan_msg);
286 |     }
287 | 
288 |     ros::spinOnce();
289 |   }
290 | 
291 |   bagIn.close();
292 | 
293 |   return 0;
294 | }
295 | 


--------------------------------------------------------------------------------
/rrxio/launch/configs/rrxio_iros_datasets_visual.info:
--------------------------------------------------------------------------------
  1 | ; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with:
  2 | ; #include "/home/user/workspace/rovio/cfg/rovio_custom.info"
  3 | Common
  4 | {
  5 | 	doVECalibration true;		Should the camera-IMU extrinsics be calibrated online
  6 | 	depthType 1;				Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic)
  7 | 	verbose false;				Is the verbose active
  8 | }
  9 | Camera0
 10 | {
 11 | 	CalibrationFile ;												Camera-Calibration file for intrinsics
 12 | 	qCM_x  0.508349;										X-entry of IMU to Camera quaterion (Hamilton)
 13 | 	qCM_y  -0.492986;										Y-entry of IMU to Camera quaterion (Hamilton)
 14 | 	qCM_z  0.493682;											Z-entry of IMU to Camera quaterion (Hamilton)
 15 | 	qCM_w   0.5048 ;											W-entry of IMU to Camera quaterion (Hamilton)
 16 | 	MrMC_x -0.032505;											X-entry of IMU to Camera vector (expressed in IMU CF) [m]
 17 | 	MrMC_y 0.056359;										Y-entry of IMU to Camera vector (expressed in IMU CF) [m]
 18 | 	MrMC_z 0.019612;											Z-entry of IMU to Camera vector (expressed in IMU CF) [m]
 19 | }
 20 | Init
 21 | {
 22 |     State
 23 |     {
 24 |         pos_0 0;   X-entry of initial position (world to IMU in world) [m]
 25 |         pos_1 0;   Y-entry of initial position (world to IMU in world) [m]
 26 |         pos_2 0;   Z-entry of initial position (world to IMU in world) [m]
 27 |         vel_0 0;   X-entry of initial velocity (robocentric, IMU) [m/s]
 28 |         vel_1 0;   Y-entry of initial velocity (robocentric, IMU) [m/s]
 29 |         vel_2 0;   Z-entry of initial velocity (robocentric, IMU) [m/s]
 30 |         acb_0 0;   X-entry of accelerometer bias [m/s^2]
 31 |         acb_1 0;   Y-entry of accelerometer bias [m/s^2]
 32 |         acb_2 0;   Z-entry of accelerometer bias [m/s^2]
 33 |         gyb_0 0;   X-entry of gyroscope bias [rad/s]
 34 |         gyb_1 0;   Y-entry of gyroscope bias [rad/s]
 35 |         gyb_2 0;   Z-entry of gyroscope bias [rad/s]
 36 |         att_x 0;   X-entry of initial attitude (IMU to world, Hamilton)
 37 |         att_y 0;   Y-entry of initial attitude (IMU to world, Hamilton)
 38 |         att_z 0;   Z-entry of initial attitude (IMU to world, Hamilton)
 39 |         att_w 1;   W-entry of initial attitude (IMU to world, Hamilton)
 40 |     }
 41 |     Covariance
 42 |     {
 43 |         pos_0 0.0001;  X-Covariance of initial position [m^2]
 44 |         pos_1 0.0001;  Y-Covariance of initial position [m^2]
 45 |         pos_2 0.0001;  Z-Covariance of initial position [m^2]
 46 |         vel_0 1.0;   X-Covariance of initial velocity [m^2/s^2]
 47 |         vel_1 1.0;   Y-Covariance of initial velocity [m^2/s^2]
 48 |         vel_2 1.0;   Z-Covariance of initial velocity [m^2/s^2]
 49 |         acb_0 4e-3;   X-Covariance of initial accelerometer bias [m^2/s^4]
 50 |         acb_1 4e-3;   Y-Covariance of initial accelerometer bias [m^2/s^4]
 51 |         acb_2 4e-3;   Z-Covariance of initial accelerometer bias [m^2/s^4]
 52 |         gyb_0 3e-4;   X-Covariance of initial gyroscope bias [rad^2/s^2]
 53 |         gyb_1 3e-4;   Y-Covariance of initial gyroscope bias [rad^2/s^2]
 54 |         gyb_2 3e-4;   Z-Covariance of initial gyroscope bias [rad^2/s^2]
 55 |         vep 0.0001;   Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2]
 56 |         att_0 0.1;   X-Covariance of initial attitude [rad^2]
 57 |         att_1 0.1;   Y-Covariance of initial attitude [rad^2]
 58 |         att_2 0.00001;   Z-Covariance of initial attitude [rad^2]
 59 |         vea 0.01;   Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2]
 60 |     }
 61 | }
 62 | ImgUpdate
 63 | {
 64 |  updateVecNormTermination 1e-4;
 65 |  maxNumIteration 20;
 66 |     doPatchWarping true;          Should the patches be warped
 67 |     doFrameVisualisation true;         Should the frames be visualized
 68 |     visualizePatches false;          Should the patches be visualized
 69 |     useDirectMethod true;          Should the EKF-innovation be based on direct intensity error (o.w. reprojection error)
 70 |     startLevel 1;            Highest patch level which is being employed (must be smaller than the hardcoded template parameter)
 71 |     endLevel 1;             Lowest patch level which is being employed
 72 |     nDetectionBuckets 100;          Number of discretization buckets used during the candidates selection
 73 |     MahalanobisThImg 9.21 ;3.0; 9.21;           Mahalanobis treshold for the update, 5.8858356
 74 |     UpdateNoise
 75 |     {
 76 |         pix 2;             Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000)
 77 |         int 400;            Covariance used for the photometric error [intensity^2]
 78 |     }
 79 |     initCovFeature_0 0.5;          Covariance for the initial distance (Relative to initialization depth [m^2/m^2])
 80 |     initCovFeature_1 1e-5;          Covariance for the initial bearing vector, x-component [rad^2]
 81 |     initCovFeature_2 1e-5;          Covariance for the initial bearing vector, y-component [rad^2]
 82 |     initDepth 0.5;            Initial value for the initial distance parameter
 83 |     startDetectionTh 0.8;          Threshold for detecting new features (min: 0, max: 1)
 84 |     scoreDetectionExponent 0.25;        Exponent used for weighting the distance between candidates
 85 |     penaltyDistance 100;          Maximal distance which gets penalized during bucketing [pix]
 86 |     zeroDistancePenalty 100;         Penalty for zero distance (max: nDetectionBuckets)
 87 |     statLocalQualityRange 10;         Number of frames for local quality evaluation
 88 |     statLocalVisibilityRange 100;        Number of frames for local visibility evaluation
 89 |     statMinGlobalQualityRange 100;        Minimum number of frames for obtaining maximal global quality
 90 |     trackingUpperBound 0.9;          Threshold for local quality for min overall global quality
 91 |     trackingLowerBound 0.8;          Threshold for local quality for max overall global quality
 92 |     minTrackedAndFreeFeatures 0.75;        Minimum of amount of feature which are either tracked or free
 93 |     removalFactor 1.1;           Factor for enforcing feature removal if not enough free
 94 |     minRelativeSTScore 0.75;         Minimum relative ST-score for extracting new feature patch
 95 |     minAbsoluteSTScore 5.0;          Minimum absolute ST-score for extracting new feature patch
 96 |     minTimeBetweenPatchUpdate 1.0;        Minimum time between new multilevel patch extrection [s]
 97 |     fastDetectionThreshold 5;         Fast corner detector treshold while adding new feature
 98 |     alignConvergencePixelRange 10;        Assumed convergence range for image alignment (gets scaled with the level) [pixels]
 99 |     alignCoverageRatio 2;          How many sigma of the uncertainty should be covered in the adaptive alignement
100 |     alignMaxUniSample 1;          Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging!
101 |     patchRejectionTh 50.0;          If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed
102 |     alignmentHuberNormThreshold 10;        Intensity error threshold for Huber norm (enabled if > 0)
103 |     alignmentGaussianWeightingSigma -1;       Width of Gaussian which is used for pixel error weighting (enabled if > 0)
104 |     alignmentGradientExponent 0.0;        Exponent used for gradient based weighting of residuals
105 |     useIntensityOffsetForAlignment true;      Should an intensity offset between the patches be considered
106 |     useIntensitySqewForAlignment true;       Should an intensity sqew between the patches be considered
107 |     removeNegativeFeatureAfterUpdate true;      Should feature with negative distance get removed
108 |     maxUncertaintyToDepthRatioForDepthInitialization 0.3;  If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame
109 |     useCrossCameraMeasurements true;       Should cross measurements between frame be used. Might be turned of in calibration phase.
110 |     doStereoInitialization true;        Should a stereo match be used for feature initialization.
111 |     noiseGainForOffCamera 10.0;         Factor added on update noise if not main camera
112 |     discriminativeSamplingDistance 0.02;      Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed).
113 |     discriminativeSamplingGain 1.1;        Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used).
114 |     MotionDetection
115 |     {
116 |      isEnabled 0;           Is the motion detection enabled
117 |      rateOfMovingFeaturesTh 0.5;        Amount of feature with motion for overall motion detection
118 |      pixelCoordinateMotionTh 1.0;       Threshold for motion detection for patched [pixels]
119 |      minFeatureCountForNoMotionDetection 5;     Min feature count in frame for motion detection
120 |  }
121 |     ZeroVelocityUpdate
122 |     {
123 |         UpdateNoise
124 |         {
125 |             vel_0 0.01;           X-Covariance of zero velocity update [m^2/s^2]
126 |             vel_1 0.01;           Y-Covariance of zero velocity update [m^2/s^2]
127 |             vel_2 0.01;           Z-Covariance of zero velocity update [m^2/s^2]
128 |         }
129 |         MahalanobisTh0 7.689997599999999;      Mahalanobid distance for zero velocity updates
130 |         minNoMotionTime 1.0;         Min duration of no-motion
131 |         isEnabled 0;           Should zero velocity update be applied, only works if MotionDetection.isEnabled is true
132 |     }
133 | }
134 | Prediction
135 | {
136 |     PredictionNoise
137 |     {
138 |         pos_0 1e-4;        X-covariance parameter of position prediction [m^2/s]
139 |         pos_1 1e-4;        Y-covariance parameter of position prediction [m^2/s]
140 |         pos_2 1e-4;        Z-covariance parameter of position prediction [m^2/s]
141 |         vel_0 4e-5;        X-covariance parameter of velocity prediction [m^2/s^3]
142 |         vel_1 4e-5;        Y-covariance parameter of velocity prediction [m^2/s^3]
143 |         vel_2 4e-5;        Z-covariance parameter of velocity prediction [m^2/s^3]
144 |         acb_0 1e-8;        X-covariance parameter of accelerometer bias prediction [m^2/s^5]
145 |         acb_1 1e-8;        Y-covariance parameter of accelerometer bias prediction [m^2/s^5]
146 |         acb_2 1e-8;        Z-covariance parameter of accelerometer bias prediction [m^2/s^5]
147 |         gyb_0 3.8e-8;       X-covariance parameter of gyroscope bias prediction [rad^2/s^3]
148 |         gyb_1 3.8e-8;       Y-covariance parameter of gyroscope bias prediction [rad^2/s^3]
149 |         gyb_2 3.8e-9;       Z-covariance parameter of gyroscope bias prediction [rad^2/s^3]
150 |         vep 1e-8;        Covariance parameter of linear extrinsics prediction [m^2/s]
151 |         att_0 7.6e-8;       X-Covariance parameter of attitude prediction [rad^2/s]
152 |         att_1 7.6e-8;       Y-Covariance parameter of attitude prediction [rad^2/s]
153 |         att_2 7.6e-9;       Z-Covariance parameter of attitude prediction [rad^2/s]
154 |         vea 1e-8;        Covariance parameter of rotational extrinsics prediction [rad^2/s]
155 |         dep 0.0001;       Covariance parameter of distance prediction [m^2/s]
156 |         nor 0.00001;        Covariance parameter of bearing vector prediction [rad^2/s]
157 |     }
158 |     MotionDetection
159 |     {
160 |      inertialMotionRorTh 0.1;    Treshold on rotational rate for motion detection [rad/s]
161 |      inertialMotionAccTh 0.5;    Treshold on acceleration for motion detection [m/s^2]
162 |  }
163 | }
164 | PoseUpdate
165 | {
166 |     UpdateNoise
167 |     {
168 |         pos_0 0.01;       X-Covariance of linear pose measurement update [m^2]
169 |         pos_1 0.01;       Y-Covariance of linear pose measurement update [m^2]
170 |         pos_2 0.01;       Z-Covariance of linear pose measurement update [m^2]
171 |         att_0 0.01;       X-Covariance of rotational pose measurement update [rad^2]
172 |         att_1 0.01;       Y-Covariance of rotational pose measurement update [rad^2]
173 |         att_2 0.01;       Z-Covariance of rotational pose measurement update [rad^2]
174 |     }
175 |  init_cov_IrIW 1;       Covariance of initial pose between inertial frames, linear part [m^2]
176 |  init_cov_qWI 1;        Covariance of initial pose between inertial frames, rotational part [rad^2]
177 |  init_cov_MrMV 1;       Covariance of initial pose between inertial frames, linear part [m^2]
178 |  init_cov_qVM 1;        Covariance of initial pose between inertial frames, rotational part [rad^2]
179 |     pre_cov_IrIW 1e-4;       Covariance parameter of pose between inertial frames, linear part [m^2/s]
180 |     pre_cov_qWI 1e-4;       Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
181 |     pre_cov_MrMV 1e-4;       Covariance parameter of pose between inertial frames, linear part [m^2/s]
182 |     pre_cov_qVM 1e-4;       Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
183 |     MahalanobisTh0 12.6511204;     Mahalonobis distance treshold of pose measurement
184 |  doVisualization false;      Should the measured poses be vizualized
185 |  enablePosition true;      Should the linear part be used during the update
186 |  enableAttitude true;      Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement)
187 |  noFeedbackToRovio true;      By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements.
188 |  doInertialAlignmentAtStart true;   Should the transformation between I and W be explicitly computed and set with the first pose measurement.
189 |  timeOffset 0.0;        Time offset added to the pose measurement timestamps
190 |     useOdometryCov false;                    Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message
191 |     qVM_x 0;         X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
192 |     qVM_y 0;         Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
193 |     qVM_z 0;         Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
194 |     qVM_w 1;         W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
195 |     MrMV_x 0;         X-entry of vector representing IMU to reference body coordinate frame of pose measurement
196 |     MrMV_y 0;         Y-entry of vector representing IMU to reference body coordinate frame of pose measurement
197 |     MrMV_z 0;         Z-entry of vector representing IMU to reference body coordinate frame of pose measurement
198 |     qWI_x 0;         X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
199 |     qWI_y 0;         Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
200 |     qWI_z 0;         Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
201 |     qWI_w 1;         W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
202 |     IrIW_x 0;         X-entry of vector representing World to reference inertial coordinate frame of pose measurement
203 |     IrIW_y 0;         Y-entry of vector representing World to reference inertial coordinate frame of pose measurement
204 |     IrIW_z 0;         Z-entry of vector representing World to reference inertial coordinate frame of pose measurement
205 | }
206 | VelocityUpdate
207 | {
208 |     UpdateNoise
209 |     {
210 |         vel_0 0.01;vr  0.01;
211 |         vel_1 0.01;vr  0.01;
212 |         vel_2 0.023;vr 0.023;
213 |     }
214 |     MahalanobisThVel 9.0; 0.9->0.58   0.75->1.21   0.5->2.3   0.25->4.1   0.05->7.69   0.01->11.34
215 |     qAM_x 0
216 |     qAM_y 0
217 |     qAM_z 0
218 |     qAM_w 1
219 | }


--------------------------------------------------------------------------------
/rrxio/launch/configs/rrxio_iros_datasets_thermal.info:
--------------------------------------------------------------------------------
  1 | ; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with:
  2 | ; #include "/home/user/workspace/rovio/cfg/rovio_custom.info"
  3 | Common
  4 | {
  5 | 	doVECalibration true;		Should the camera-IMU extrinsics be calibrated online
  6 | 	depthType 1;				Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic)
  7 | 	verbose false;				Is the verbose active
  8 | }
  9 | Camera0
 10 | {
 11 | 	CalibrationFile ;												Camera-Calibration file for intrinsics
 12 | 	qCM_x  0.511  ;0.508349;										X-entry of IMU to Camera quaterion (Hamilton)
 13 | 	qCM_y -0.494 ;-0.492986;										Y-entry of IMU to Camera quaterion (Hamilton)
 14 | 	qCM_z  0.49 ;0.493682;											Z-entry of IMU to Camera quaterion (Hamilton)
 15 | 	qCM_w  0.505 ;0.5048 ;											W-entry of IMU to Camera quaterion (Hamilton)
 16 | 	MrMC_x 0.03;											X-entry of IMU to Camera vector (expressed in IMU CF) [m]
 17 | 	MrMC_y 0.02;										Y-entry of IMU to Camera vector (expressed in IMU CF) [m]
 18 | 	MrMC_z 0.0;											Z-entry of IMU to Camera vector (expressed in IMU CF) [m]
 19 | }
 20 | 
 21 | Init
 22 | {
 23 |     State
 24 |     {
 25 |         pos_0 0;   X-entry of initial position (world to IMU in world) [m]
 26 |         pos_1 0;   Y-entry of initial position (world to IMU in world) [m]
 27 |         pos_2 0;   Z-entry of initial position (world to IMU in world) [m]
 28 |         vel_0 0;   X-entry of initial velocity (robocentric, IMU) [m/s]
 29 |         vel_1 0;   Y-entry of initial velocity (robocentric, IMU) [m/s]
 30 |         vel_2 0;   Z-entry of initial velocity (robocentric, IMU) [m/s]
 31 |         acb_0 0;   X-entry of accelerometer bias [m/s^2]
 32 |         acb_1 0;   Y-entry of accelerometer bias [m/s^2]
 33 |         acb_2 0;   Z-entry of accelerometer bias [m/s^2]
 34 |         gyb_0 0;   X-entry of gyroscope bias [rad/s]
 35 |         gyb_1 0;   Y-entry of gyroscope bias [rad/s]
 36 |         gyb_2 0;   Z-entry of gyroscope bias [rad/s]
 37 |         att_x 0;   X-entry of initial attitude (IMU to world, Hamilton)
 38 |         att_y 0;   Y-entry of initial attitude (IMU to world, Hamilton)
 39 |         att_z 0;   Z-entry of initial attitude (IMU to world, Hamilton)
 40 |         att_w 1;   W-entry of initial attitude (IMU to world, Hamilton)
 41 |     }
 42 |     Covariance
 43 |     {
 44 |         pos_0 0.0001;  X-Covariance of initial position [m^2]
 45 |         pos_1 0.0001;  Y-Covariance of initial position [m^2]
 46 |         pos_2 0.0001;  Z-Covariance of initial position [m^2]
 47 |         vel_0 1.0;   X-Covariance of initial velocity [m^2/s^2]
 48 |         vel_1 1.0;   Y-Covariance of initial velocity [m^2/s^2]
 49 |         vel_2 1.0;   Z-Covariance of initial velocity [m^2/s^2]
 50 |         acb_0 4e-3;   X-Covariance of initial accelerometer bias [m^2/s^4]
 51 |         acb_1 4e-3;   Y-Covariance of initial accelerometer bias [m^2/s^4]
 52 |         acb_2 4e-3;   Z-Covariance of initial accelerometer bias [m^2/s^4]
 53 |         gyb_0 3e-4;   X-Covariance of initial gyroscope bias [rad^2/s^2]
 54 |         gyb_1 3e-4;   Y-Covariance of initial gyroscope bias [rad^2/s^2]
 55 |         gyb_2 3e-4;   Z-Covariance of initial gyroscope bias [rad^2/s^2]
 56 |         vep 0.0000001;   Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2]
 57 |         att_0 0.1;   X-Covariance of initial attitude [rad^2]
 58 |         att_1 0.1;   Y-Covariance of initial attitude [rad^2]
 59 |         att_2 0.00001;   Z-Covariance of initial attitude [rad^2]
 60 |         vea 0.001;   Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2]
 61 |     }
 62 | }
 63 | ImgUpdate
 64 | {
 65 |  updateVecNormTermination 1e-4;
 66 |  maxNumIteration 20;
 67 |     doPatchWarping true;          Should the patches be warped
 68 |     doFrameVisualisation true;         Should the frames be visualized
 69 |     visualizePatches false;          Should the patches be visualized
 70 |     useDirectMethod true;          Should the EKF-innovation be based on direct intensity error (o.w. reprojection error)
 71 |     startLevel 2;            Highest patch level which is being employed (must be smaller than the hardcoded template parameter)
 72 |     endLevel 1;             Lowest patch level which is being employed
 73 |     nDetectionBuckets 100;          Number of discretization buckets used during the candidates selection
 74 |     MahalanobisThImg 3.0; 9.21;           Mahalanobis treshold for the update, 5.8858356
 75 |     UpdateNoise
 76 |     {
 77 |         pix 2;             Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000)
 78 |         int 2000;            Covariance used for the photometric error [intensity^2]
 79 |     }
 80 |     initCovFeature_0 0.5;          Covariance for the initial distance (Relative to initialization depth [m^2/m^2])
 81 |     initCovFeature_1 1e-5;          Covariance for the initial bearing vector, x-component [rad^2]
 82 |     initCovFeature_2 1e-5;          Covariance for the initial bearing vector, y-component [rad^2]
 83 |     initDepth 0.5;            Initial value for the initial distance parameter
 84 |     startDetectionTh 0.8;          Threshold for detecting new features (min: 0, max: 1)
 85 |     scoreDetectionExponent 0.25;        Exponent used for weighting the distance between candidates
 86 |     penaltyDistance 100;          Maximal distance which gets penalized during bucketing [pix]
 87 |     zeroDistancePenalty 100;         Penalty for zero distance (max: nDetectionBuckets)
 88 |     statLocalQualityRange 10;         Number of frames for local quality evaluation
 89 |     statLocalVisibilityRange 100;        Number of frames for local visibility evaluation
 90 |     statMinGlobalQualityRange 100;        Minimum number of frames for obtaining maximal global quality
 91 |     trackingUpperBound 0.9;          Threshold for local quality for min overall global quality
 92 |     trackingLowerBound 0.8;          Threshold for local quality for max overall global quality
 93 |     minTrackedAndFreeFeatures 0.75;        Minimum of amount of feature which are either tracked or free
 94 |     removalFactor 1.1;           Factor for enforcing feature removal if not enough free
 95 |     minRelativeSTScore 0.75;         Minimum relative ST-score for extracting new feature patch
 96 |     minAbsoluteSTScore 5.0;          Minimum absolute ST-score for extracting new feature patch
 97 |     minTimeBetweenPatchUpdate 1.0;        Minimum time between new multilevel patch extrection [s]
 98 |     fastDetectionThreshold 5;         Fast corner detector treshold while adding new feature
 99 |     alignConvergencePixelRange 10;        Assumed convergence range for image alignment (gets scaled with the level) [pixels]
100 |     alignCoverageRatio 2;          How many sigma of the uncertainty should be covered in the adaptive alignement
101 |     alignMaxUniSample 1;          Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging!
102 |     patchRejectionTh 50.0;          If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed
103 |     alignmentHuberNormThreshold 10;        Intensity error threshold for Huber norm (enabled if > 0)
104 |     alignmentGaussianWeightingSigma -1;       Width of Gaussian which is used for pixel error weighting (enabled if > 0)
105 |     alignmentGradientExponent 0.0;        Exponent used for gradient based weighting of residuals
106 |     useIntensityOffsetForAlignment true;      Should an intensity offset between the patches be considered
107 |     useIntensitySqewForAlignment true;       Should an intensity sqew between the patches be considered
108 |     removeNegativeFeatureAfterUpdate true;      Should feature with negative distance get removed
109 |     maxUncertaintyToDepthRatioForDepthInitialization 0.3;  If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame
110 |     useCrossCameraMeasurements true;       Should cross measurements between frame be used. Might be turned of in calibration phase.
111 |     doStereoInitialization true;        Should a stereo match be used for feature initialization.
112 |     noiseGainForOffCamera 10.0;         Factor added on update noise if not main camera
113 |     discriminativeSamplingDistance 0.02;      Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed).
114 |     discriminativeSamplingGain 1.1;        Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used).
115 |     MotionDetection
116 |     {
117 |      isEnabled 0;           Is the motion detection enabled
118 |      rateOfMovingFeaturesTh 0.5;        Amount of feature with motion for overall motion detection
119 |      pixelCoordinateMotionTh 1.0;       Threshold for motion detection for patched [pixels]
120 |      minFeatureCountForNoMotionDetection 5;     Min feature count in frame for motion detection
121 |  }
122 |     ZeroVelocityUpdate
123 |     {
124 |         UpdateNoise
125 |         {
126 |             vel_0 0.01;           X-Covariance of zero velocity update [m^2/s^2]
127 |             vel_1 0.01;           Y-Covariance of zero velocity update [m^2/s^2]
128 |             vel_2 0.01;           Z-Covariance of zero velocity update [m^2/s^2]
129 |         }
130 |         MahalanobisTh0 7.689997599999999;      Mahalanobid distance for zero velocity updates
131 |         minNoMotionTime 1.0;         Min duration of no-motion
132 |         isEnabled 0;           Should zero velocity update be applied, only works if MotionDetection.isEnabled is true
133 |     }
134 | }
135 | Prediction
136 | {
137 |     PredictionNoise
138 |     {
139 |         pos_0 1e-4;        X-covariance parameter of position prediction [m^2/s]
140 |         pos_1 1e-4;        Y-covariance parameter of position prediction [m^2/s]
141 |         pos_2 1e-4;        Z-covariance parameter of position prediction [m^2/s]
142 |         vel_0 4e-5;        X-covariance parameter of velocity prediction [m^2/s^3]
143 |         vel_1 4e-5;        Y-covariance parameter of velocity prediction [m^2/s^3]
144 |         vel_2 4e-5;        Z-covariance parameter of velocity prediction [m^2/s^3]
145 |         acb_0 1e-8;        X-covariance parameter of accelerometer bias prediction [m^2/s^5]
146 |         acb_1 1e-8;        Y-covariance parameter of accelerometer bias prediction [m^2/s^5]
147 |         acb_2 1e-8;        Z-covariance parameter of accelerometer bias prediction [m^2/s^5]
148 |         gyb_0 3.8e-7;       X-covariance parameter of gyroscope bias prediction [rad^2/s^3]
149 |         gyb_1 3.8e-7;       Y-covariance parameter of gyroscope bias prediction [rad^2/s^3]
150 |         gyb_2 3.8e-7;       Z-covariance parameter of gyroscope bias prediction [rad^2/s^3]
151 |         vep 1e-8;        Covariance parameter of linear extrinsics prediction [m^2/s]
152 |         att_0 7.6e-7;       X-Covariance parameter of attitude prediction [rad^2/s]
153 |         att_1 7.6e-7;       Y-Covariance parameter of attitude prediction [rad^2/s]
154 |         att_2 7.6e-7;       Z-Covariance parameter of attitude prediction [rad^2/s]
155 |         vea 1e-8;        Covariance parameter of rotational extrinsics prediction [rad^2/s]
156 |         dep 0.0001;       Covariance parameter of distance prediction [m^2/s]
157 |         nor 0.00001;        Covariance parameter of bearing vector prediction [rad^2/s]
158 |     }
159 |     MotionDetection
160 |     {
161 |      inertialMotionRorTh 0.1;    Treshold on rotational rate for motion detection [rad/s]
162 |      inertialMotionAccTh 0.5;    Treshold on acceleration for motion detection [m/s^2]
163 |  }
164 | }
165 | PoseUpdate
166 | {
167 |     UpdateNoise
168 |     {
169 |         pos_0 0.01;       X-Covariance of linear pose measurement update [m^2]
170 |         pos_1 0.01;       Y-Covariance of linear pose measurement update [m^2]
171 |         pos_2 0.01;       Z-Covariance of linear pose measurement update [m^2]
172 |         att_0 0.01;       X-Covariance of rotational pose measurement update [rad^2]
173 |         att_1 0.01;       Y-Covariance of rotational pose measurement update [rad^2]
174 |         att_2 0.01;       Z-Covariance of rotational pose measurement update [rad^2]
175 |     }
176 |  init_cov_IrIW 1;       Covariance of initial pose between inertial frames, linear part [m^2]
177 |  init_cov_qWI 1;        Covariance of initial pose between inertial frames, rotational part [rad^2]
178 |  init_cov_MrMV 1;       Covariance of initial pose between inertial frames, linear part [m^2]
179 |  init_cov_qVM 1;        Covariance of initial pose between inertial frames, rotational part [rad^2]
180 |     pre_cov_IrIW 1e-4;       Covariance parameter of pose between inertial frames, linear part [m^2/s]
181 |     pre_cov_qWI 1e-4;       Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
182 |     pre_cov_MrMV 1e-4;       Covariance parameter of pose between inertial frames, linear part [m^2/s]
183 |     pre_cov_qVM 1e-4;       Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
184 |     MahalanobisTh0 12.6511204;     Mahalonobis distance treshold of pose measurement
185 |  doVisualization false;      Should the measured poses be vizualized
186 |  enablePosition true;      Should the linear part be used during the update
187 |  enableAttitude true;      Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement)
188 |  noFeedbackToRovio true;      By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements.
189 |  doInertialAlignmentAtStart true;   Should the transformation between I and W be explicitly computed and set with the first pose measurement.
190 |  timeOffset 0.0;        Time offset added to the pose measurement timestamps
191 |     useOdometryCov false;                    Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message
192 |     qVM_x 0;         X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
193 |     qVM_y 0;         Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
194 |     qVM_z 0;         Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
195 |     qVM_w 1;         W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
196 |     MrMV_x 0;         X-entry of vector representing IMU to reference body coordinate frame of pose measurement
197 |     MrMV_y 0;         Y-entry of vector representing IMU to reference body coordinate frame of pose measurement
198 |     MrMV_z 0;         Z-entry of vector representing IMU to reference body coordinate frame of pose measurement
199 |     qWI_x 0;         X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
200 |     qWI_y 0;         Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
201 |     qWI_z 0;         Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
202 |     qWI_w 1;         W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
203 |     IrIW_x 0;         X-entry of vector representing World to reference inertial coordinate frame of pose measurement
204 |     IrIW_y 0;         Y-entry of vector representing World to reference inertial coordinate frame of pose measurement
205 |     IrIW_z 0;         Z-entry of vector representing World to reference inertial coordinate frame of pose measurement
206 | }
207 | VelocityUpdate
208 | {
209 |     UpdateNoise
210 |     {
211 |         vel_0 0.01;vr  0.01;
212 |         vel_1 0.01;vr  0.01;
213 |         vel_2 0.023;vr 0.023;
214 |     }
215 |     MahalanobisThVel 3.0 ;9; 0.9->0.58   0.75->1.21   0.5->2.3   0.25->4.1   0.05->7.69   0.01->11.34
216 |     qAM_x 0
217 |     qAM_y 0
218 |     qAM_z 0
219 |     qAM_w 1
220 | }


--------------------------------------------------------------------------------
/rrxio/include/rrxio/RRxIOFilter.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2021, Christopher Doer
  3 |  * Copyright (c) 2014, Autonomous Systems Lab
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  * * Redistributions of source code must retain the above copyright
  9 |  * notice, this list of conditions and the following disclaimer.
 10 |  * * Redistributions in binary form must reproduce the above copyright
 11 |  * notice, this list of conditions and the following disclaimer in the
 12 |  * documentation and/or other materials provided with the distribution.
 13 |  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 14 |  * names of its contributors may be used to endorse or promote products
 15 |  * derived from this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 18 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 19 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 20 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 21 |  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 22 |  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 23 |  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 24 |  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 25 |  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 26 |  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 27 |  *
 28 |  */
 29 | 
 30 | #ifndef ROVIO_ROVIO_FILTER_HPP_
 31 | #define ROVIO_ROVIO_FILTER_HPP_
 32 | 
 33 | #include "lightweight_filtering/common.hpp"
 34 | #include "lightweight_filtering/FilterBase.hpp"
 35 | #include "rovio/FilterStates.hpp"
 36 | #include "rovio/ImgUpdate.hpp"
 37 | #include "rovio/PoseUpdate.hpp"
 38 | 
 39 | #include "rovio/ImuPrediction.hpp"
 40 | #include "rovio/MultiCamera.hpp"
 41 | 
 42 | #include "rrxio/VelocityUpdate.hpp"
 43 | 
 44 | namespace rovio
 45 | {
 46 | /** \brief Class, defining the Rovio Filter.
 47 |  *
 48 |  *  @tparam FILTERSTATE - \ref rovio::FilterState
 49 |  */
 50 | template <typename FILTERSTATE>
 51 | class RovioFilter : public LWF::FilterBase<ImuPrediction<FILTERSTATE>,
 52 |                                            ImgUpdate<FILTERSTATE>,
 53 |                                            PoseUpdate<FILTERSTATE,
 54 |                                                       (int)(FILTERSTATE::mtState::nPose_ > 0) - 1,
 55 |                                                       (int)(FILTERSTATE::mtState::nPose_ > 1) * 2 - 1>,
 56 |                                            VelocityUpdate<FILTERSTATE>>
 57 | {
 58 | public:
 59 |   typedef LWF::FilterBase<ImuPrediction<FILTERSTATE>,
 60 |                           ImgUpdate<FILTERSTATE>,
 61 |                           PoseUpdate<FILTERSTATE,
 62 |                                      (int)(FILTERSTATE::mtState::nPose_ > 0) - 1,
 63 |                                      (int)(FILTERSTATE::mtState::nPose_ > 1) * 2 - 1>,
 64 |                           VelocityUpdate<FILTERSTATE>>
 65 |       Base;
 66 |   using Base::boolRegister_;
 67 |   using Base::doubleRegister_;
 68 |   using Base::front_;
 69 |   using Base::init_;
 70 |   using Base::intRegister_;
 71 |   using Base::mPrediction_;
 72 |   using Base::mUpdates_;
 73 |   using Base::predictionTimeline_;
 74 |   using Base::readFromInfo;
 75 |   using Base::reset;
 76 |   using Base::safe_;
 77 |   using Base::stringRegister_;
 78 |   using Base::subHandlers_;
 79 |   using Base::updateToUpdateMeasOnly_;
 80 |   typedef typename Base::mtFilterState mtFilterState;
 81 |   typedef typename Base::mtPrediction mtPrediction;
 82 |   typedef typename Base::mtState mtState;
 83 |   rovio::MultiCamera<mtState::nCam_> multiCamera_;
 84 |   std::string cameraCalibrationFile_[mtState::nCam_];
 85 |   int depthTypeInt_;
 86 | 
 87 |   /** \brief Constructor. Initializes the filter.
 88 |    */
 89 |   RovioFilter()
 90 |   {
 91 |     updateToUpdateMeasOnly_ = true;
 92 |     std::get<0>(mUpdates_).setCamera(&multiCamera_);
 93 |     init_.setCamera(&multiCamera_);
 94 |     depthTypeInt_ = 1;
 95 |     subHandlers_.erase("Update0");
 96 |     subHandlers_["ImgUpdate"] = &std::get<0>(mUpdates_);
 97 |     subHandlers_.erase("Update1");
 98 |     subHandlers_["PoseUpdate"] = &std::get<1>(mUpdates_);
 99 |     subHandlers_.erase("Update2");
100 |     subHandlers_["VelocityUpdate"] = &std::get<2>(mUpdates_);
101 |     boolRegister_.registerScalar("Common.doVECalibration", init_.state_.aux().doVECalibration_);
102 |     intRegister_.registerScalar("Common.depthType", depthTypeInt_);
103 |     for (int camID = 0; camID < mtState::nCam_; camID++)
104 |     {
105 |       cameraCalibrationFile_[camID] = "";
106 |       stringRegister_.registerScalar("Camera" + std::to_string(camID) + ".CalibrationFile",
107 |                                      cameraCalibrationFile_[camID]);
108 |       doubleRegister_.registerVector("Camera" + std::to_string(camID) + ".MrMC", init_.state_.aux().MrMC_[camID]);
109 |       doubleRegister_.registerQuaternion("Camera" + std::to_string(camID) + ".qCM", init_.state_.aux().qCM_[camID]);
110 |       doubleRegister_.removeScalarByVar(init_.state_.MrMC(camID)(0));
111 |       doubleRegister_.removeScalarByVar(init_.state_.MrMC(camID)(1));
112 |       doubleRegister_.removeScalarByVar(init_.state_.MrMC(camID)(2));
113 |       doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().w());
114 |       doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().x());
115 |       doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().y());
116 |       doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().z());
117 |       for (int j = 0; j < 3; j++)
118 |       {
119 |         doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId<mtState::_vep>(camID) + j,
120 |                                                      mtState::template getId<mtState::_vep>(camID) + j));
121 |         doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId<mtState::_vea>(camID) + j,
122 |                                                      mtState::template getId<mtState::_vea>(camID) + j));
123 |         doubleRegister_.registerScalar("Init.Covariance.vep",
124 |                                        init_.cov_(mtState::template getId<mtState::_vep>(camID) + j,
125 |                                                   mtState::template getId<mtState::_vep>(camID) + j));
126 |         doubleRegister_.registerScalar("Init.Covariance.vea",
127 |                                        init_.cov_(mtState::template getId<mtState::_vea>(camID) + j,
128 |                                                   mtState::template getId<mtState::_vea>(camID) + j));
129 |       }
130 |       doubleRegister_.registerVector("Camera" + std::to_string(camID) + ".MrMC", init_.state_.MrMC(camID));
131 |       doubleRegister_.registerQuaternion("Camera" + std::to_string(camID) + ".qCM", init_.state_.qCM(camID));
132 |     }
133 |     for (int i = 0; i < mtState::nPose_; i++)
134 |     {
135 |       doubleRegister_.removeScalarByVar(init_.state_.poseLin(i)(0));
136 |       doubleRegister_.removeScalarByVar(init_.state_.poseLin(i)(1));
137 |       doubleRegister_.removeScalarByVar(init_.state_.poseLin(i)(2));
138 |       doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().w());
139 |       doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().x());
140 |       doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().y());
141 |       doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().z());
142 |       for (int j = 0; j < 3; j++)
143 |       {
144 |         doubleRegister_.removeScalarByVar(
145 |             init_.cov_(mtState::template getId<mtState::_pop>(i) + j, mtState::template getId<mtState::_pop>(i) + j));
146 |         doubleRegister_.removeScalarByVar(
147 |             init_.cov_(mtState::template getId<mtState::_poa>(i) + j, mtState::template getId<mtState::_poa>(i) + j));
148 |       }
149 |     }
150 |     if (std::get<1>(mUpdates_).inertialPoseIndex_ >= 0)
151 |     {
152 |       std::get<1>(mUpdates_).doubleRegister_.registerVector(
153 |           "IrIW", init_.state_.poseLin(std::get<1>(mUpdates_).inertialPoseIndex_));
154 |       std::get<1>(mUpdates_).doubleRegister_.registerQuaternion(
155 |           "qWI", init_.state_.poseRot(std::get<1>(mUpdates_).inertialPoseIndex_));
156 |       for (int j = 0; j < 3; j++)
157 |       {
158 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
159 |             "init_cov_IrIW",
160 |             init_.cov_(mtState::template getId<mtState::_pop>(std::get<1>(mUpdates_).inertialPoseIndex_) + j,
161 |                        mtState::template getId<mtState::_pop>(std::get<1>(mUpdates_).inertialPoseIndex_) + j));
162 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
163 |             "init_cov_qWI",
164 |             init_.cov_(mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).inertialPoseIndex_) + j,
165 |                        mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).inertialPoseIndex_) + j));
166 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
167 |             "pre_cov_IrIW",
168 |             mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(
169 |                                       std::get<1>(mUpdates_).inertialPoseIndex_) +
170 |                                       j,
171 |                                   mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(
172 |                                       std::get<1>(mUpdates_).inertialPoseIndex_) +
173 |                                       j));
174 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
175 |             "pre_cov_qWI",
176 |             mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(
177 |                                       std::get<1>(mUpdates_).inertialPoseIndex_) +
178 |                                       j,
179 |                                   mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(
180 |                                       std::get<1>(mUpdates_).inertialPoseIndex_) +
181 |                                       j));
182 |       }
183 |     }
184 |     if (std::get<1>(mUpdates_).bodyPoseIndex_ >= 0)
185 |     {
186 |       std::get<1>(mUpdates_).doubleRegister_.registerVector(
187 |           "MrMV", init_.state_.poseLin(std::get<1>(mUpdates_).bodyPoseIndex_));
188 |       std::get<1>(mUpdates_).doubleRegister_.registerQuaternion(
189 |           "qVM", init_.state_.poseRot(std::get<1>(mUpdates_).bodyPoseIndex_));
190 |       for (int j = 0; j < 3; j++)
191 |       {
192 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
193 |             "init_cov_MrMV",
194 |             init_.cov_(mtState::template getId<mtState::_pop>(std::get<1>(mUpdates_).bodyPoseIndex_) + j,
195 |                        mtState::template getId<mtState::_pop>(std::get<1>(mUpdates_).bodyPoseIndex_) + j));
196 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
197 |             "init_cov_qVM",
198 |             init_.cov_(mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).bodyPoseIndex_) + j,
199 |                        mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).bodyPoseIndex_) + j));
200 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
201 |             "pre_cov_MrMV",
202 |             mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(
203 |                                       std::get<1>(mUpdates_).bodyPoseIndex_) +
204 |                                       j,
205 |                                   mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(
206 |                                       std::get<1>(mUpdates_).bodyPoseIndex_) +
207 |                                       j));
208 |         std::get<1>(mUpdates_).doubleRegister_.registerScalar(
209 |             "pre_cov_qVM",
210 |             mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(
211 |                                       std::get<1>(mUpdates_).bodyPoseIndex_) +
212 |                                       j,
213 |                                   mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(
214 |                                       std::get<1>(mUpdates_).bodyPoseIndex_) +
215 |                                       j));
216 |       }
217 |     }
218 |     int ind;
219 |     for (int i = 0; i < FILTERSTATE::mtState::nMax_; i++)
220 |     {
221 |       ind = mtState::template getId<mtState::_fea>(i);
222 |       doubleRegister_.removeScalarByVar(init_.cov_(ind, ind));
223 |       doubleRegister_.removeScalarByVar(init_.cov_(ind + 1, ind + 1));
224 |       ind = mtState::template getId<mtState::_fea>(i) + 2;
225 |       doubleRegister_.removeScalarByVar(init_.cov_(ind, ind));
226 |       doubleRegister_.removeScalarByVar(init_.state_.dep(i).p_);
227 |       doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().w());
228 |       doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().x());
229 |       doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().y());
230 |       doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().z());
231 |       std::get<0>(mUpdates_).intRegister_.registerScalar("statLocalQualityRange",
232 |                                                          init_.fsm_.features_[i].mpStatistics_->localQualityRange_);
233 |       std::get<0>(mUpdates_).intRegister_.registerScalar("statLocalVisibilityRange",
234 |                                                          init_.fsm_.features_[i].mpStatistics_->localVisibilityRange_);
235 |       std::get<0>(mUpdates_).intRegister_.registerScalar("statMinGlobalQualityRange",
236 |                                                          init_.fsm_.features_[i].mpStatistics_->minGlobalQualityRange_);
237 |       std::get<0>(mUpdates_).boolRegister_.registerScalar("doPatchWarping", init_.state_.CfP(i).trackWarping_);
238 |     }
239 |     std::get<0>(mUpdates_).doubleRegister_.removeScalarByVar(std::get<0>(mUpdates_).outlierDetection_.getMahalTh(0));
240 |     std::get<0>(mUpdates_).doubleRegister_.registerScalar("MahalanobisThImg",
241 |                                                           std::get<0>(mUpdates_).outlierDetection_.getMahalTh(0));
242 |     std::get<0>(mUpdates_).outlierDetection_.setEnabledAll(true);
243 |     std::get<1>(mUpdates_).outlierDetection_.setEnabledAll(true);
244 |     std::get<2>(mUpdates_).outlierDetection_.setEnabledAll(true);
245 |     std::get<2>(mUpdates_).doubleRegister_.removeScalarByVar(std::get<2>(mUpdates_).outlierDetection_.getMahalTh(0));
246 |     std::get<2>(mUpdates_).doubleRegister_.registerScalar("MahalanobisThVel",
247 |                                                           std::get<2>(mUpdates_).outlierDetection_.getMahalTh(0));
248 | 
249 |     boolRegister_.registerScalar("Common.verbose", std::get<0>(mUpdates_).verbose_);
250 |     mPrediction_.doubleRegister_.removeScalarByStr("alpha");
251 |     mPrediction_.doubleRegister_.removeScalarByStr("beta");
252 |     mPrediction_.doubleRegister_.removeScalarByStr("kappa");
253 |     boolRegister_.registerScalar("PoseUpdate.doVisualization", init_.plotPoseMeas_);
254 |     reset(0.0);
255 |   }
256 | 
257 |   /** \brief Reloads the camera calibration for all cameras and resets the depth map type.
258 |    */
259 |   void refreshProperties()
260 |   {
261 |     if (std::get<0>(mUpdates_).useDirectMethod_)
262 |     {
263 |       init_.mode_ = LWF::ModeIEKF;
264 |     }
265 |     else
266 |     {
267 |       init_.mode_ = LWF::ModeEKF;
268 |     }
269 |     for (int camID = 0; camID < mtState::nCam_; camID++)
270 |     {
271 |       if (!cameraCalibrationFile_[camID].empty())
272 |       {
273 |         multiCamera_.cameras_[camID].load(cameraCalibrationFile_[camID]);
274 |       }
275 |     }
276 |     for (int i = 0; i < FILTERSTATE::mtState::nMax_; i++)
277 |     {
278 |       init_.state_.dep(i).setType(depthTypeInt_);
279 |     }
280 |   };
281 | 
282 |   /** \brief Destructor
283 |    */
284 |   virtual ~RovioFilter(){};
285 |   //  void resetToImuPose(V3D WrWM, QPD qMW, double t = 0.0){
286 |   //    init_.state_.initWithImuPose(WrWM,qMW);
287 |   //    reset(t);
288 |   //  }
289 | 
290 |   /** \brief Resets the filter with an accelerometer measurement.
291 |    *
292 |    *  @param fMeasInit - Accelerometer measurement.
293 |    *  @param t         - Current time.
294 |    */
295 |   void resetWithAccelerometer(const V3D& fMeasInit, double t = 0.0)
296 |   {
297 |     init_.initWithAccelerometer(fMeasInit);
298 |     reset(t);
299 |   }
300 | 
301 |   /** \brief Resets the filter with an external pose.
302 |    *
303 |    *  @param WrWM - Position Vector, pointing from the World-Frame to the IMU-Frame, expressed in World-Coordinates.
304 |    *  @param qMW  - Quaternion, expressing World-Frame in IMU-Coordinates (World Coordinates->IMU Coordinates)
305 |    *  @param t    - Current time.
306 |    */
307 |   void resetWithPose(V3D WrWM, QPD qMW, double t = 0.0)
308 |   {
309 |     init_.initWithImuPose(WrWM, qMW);
310 |     reset(t);
311 |   }
312 | 
313 |   /** \brief Sets the transformation between IMU and Camera.
314 |    *
315 |    *  @param R_VM  -  Rotation matrix, expressing the orientation of the IMU  in Camera Cooridinates (IMU Coordinates ->
316 |    * Camera Coordinates).
317 |    *  @param VrVM  -  Vector, pointing from the camera frame to the IMU frame, expressed in IMU Coordinates.
318 |    *  @param camID -  ID of the considered camera.
319 |    */
320 |   void setExtrinsics(const Eigen::Matrix3d& R_CM, const Eigen::Vector3d& CrCM, const int camID = 0)
321 |   {
322 |     kindr::RotationMatrixD R(R_CM);
323 |     init_.state_.aux().qCM_[camID]  = QPD(R);
324 |     init_.state_.aux().MrMC_[camID] = -init_.state_.aux().qCM_[camID].inverseRotate(CrCM);
325 |   }
326 | };
327 | 
328 | }  // namespace rovio
329 | 
330 | #endif /* ROVIO_ROVIO_FILTER_HPP_ */
331 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 |                     GNU GENERAL PUBLIC LICENSE
  2 |                        Version 3, 29 June 2007
  3 | 
  4 |  Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
  5 |  Everyone is permitted to copy and distribute verbatim copies
  6 |  of this license document, but changing it is not allowed.
  7 | 
  8 |                             Preamble
  9 | 
 10 |   The GNU General Public License is a free, copyleft license for
 11 | software and other kinds of works.
 12 | 
 13 |   The licenses for most software and other practical works are designed
 14 | to take away your freedom to share and change the works.  By contrast,
 15 | the GNU General Public License is intended to guarantee your freedom to
 16 | share and change all versions of a program--to make sure it remains free
 17 | software for all its users.  We, the Free Software Foundation, use the
 18 | GNU General Public License for most of our software; it applies also to
 19 | any other work released this way by its authors.  You can apply it to
 20 | your programs, too.
 21 | 
 22 |   When we speak of free software, we are referring to freedom, not
 23 | price.  Our General Public Licenses are designed to make sure that you
 24 | have the freedom to distribute copies of free software (and charge for
 25 | them if you wish), that you receive source code or can get it if you
 26 | want it, that you can change the software or use pieces of it in new
 27 | free programs, and that you know you can do these things.
 28 | 
 29 |   To protect your rights, we need to prevent others from denying you
 30 | these rights or asking you to surrender the rights.  Therefore, you have
 31 | certain responsibilities if you distribute copies of the software, or if
 32 | you modify it: responsibilities to respect the freedom of others.
 33 | 
 34 |   For example, if you distribute copies of such a program, whether
 35 | gratis or for a fee, you must pass on to the recipients the same
 36 | freedoms that you received.  You must make sure that they, too, receive
 37 | or can get the source code.  And you must show them these terms so they
 38 | know their rights.
 39 | 
 40 |   Developers that use the GNU GPL protect your rights with two steps:
 41 | (1) assert copyright on the software, and (2) offer you this License
 42 | giving you legal permission to copy, distribute and/or modify it.
 43 | 
 44 |   For the developers' and authors' protection, the GPL clearly explains
 45 | that there is no warranty for this free software.  For both users' and
 46 | authors' sake, the GPL requires that modified versions be marked as
 47 | changed, so that their problems will not be attributed erroneously to
 48 | authors of previous versions.
 49 | 
 50 |   Some devices are designed to deny users access to install or run
 51 | modified versions of the software inside them, although the manufacturer
 52 | can do so.  This is fundamentally incompatible with the aim of
 53 | protecting users' freedom to change the software.  The systematic
 54 | pattern of such abuse occurs in the area of products for individuals to
 55 | use, which is precisely where it is most unacceptable.  Therefore, we
 56 | have designed this version of the GPL to prohibit the practice for those
 57 | products.  If such problems arise substantially in other domains, we
 58 | stand ready to extend this provision to those domains in future versions
 59 | of the GPL, as needed to protect the freedom of users.
 60 | 
 61 |   Finally, every program is threatened constantly by software patents.
 62 | States should not allow patents to restrict development and use of
 63 | software on general-purpose computers, but in those that do, we wish to
 64 | avoid the special danger that patents applied to a free program could
 65 | make it effectively proprietary.  To prevent this, the GPL assures that
 66 | patents cannot be used to render the program non-free.
 67 | 
 68 |   The precise terms and conditions for copying, distribution and
 69 | modification follow.
 70 | 
 71 |                        TERMS AND CONDITIONS
 72 | 
 73 |   0. Definitions.
 74 | 
 75 |   "This License" refers to version 3 of the GNU General Public License.
 76 | 
 77 |   "Copyright" also means copyright-like laws that apply to other kinds of
 78 | works, such as semiconductor masks.
 79 | 
 80 |   "The Program" refers to any copyrightable work licensed under this
 81 | License.  Each licensee is addressed as "you".  "Licensees" and
 82 | "recipients" may be individuals or organizations.
 83 | 
 84 |   To "modify" a work means to copy from or adapt all or part of the work
 85 | in a fashion requiring copyright permission, other than the making of an
 86 | exact copy.  The resulting work is called a "modified version" of the
 87 | earlier work or a work "based on" the earlier work.
 88 | 
 89 |   A "covered work" means either the unmodified Program or a work based
 90 | on the Program.
 91 | 
 92 |   To "propagate" a work means to do anything with it that, without
 93 | permission, would make you directly or secondarily liable for
 94 | infringement under applicable copyright law, except executing it on a
 95 | computer or modifying a private copy.  Propagation includes copying,
 96 | distribution (with or without modification), making available to the
 97 | public, and in some countries other activities as well.
 98 | 
 99 |   To "convey" a work means any kind of propagation that enables other
100 | parties to make or receive copies.  Mere interaction with a user through
101 | a computer network, with no transfer of a copy, is not conveying.
102 | 
103 |   An interactive user interface displays "Appropriate Legal Notices"
104 | to the extent that it includes a convenient and prominently visible
105 | feature that (1) displays an appropriate copyright notice, and (2)
106 | tells the user that there is no warranty for the work (except to the
107 | extent that warranties are provided), that licensees may convey the
108 | work under this License, and how to view a copy of this License.  If
109 | the interface presents a list of user commands or options, such as a
110 | menu, a prominent item in the list meets this criterion.
111 | 
112 |   1. Source Code.
113 | 
114 |   The "source code" for a work means the preferred form of the work
115 | for making modifications to it.  "Object code" means any non-source
116 | form of a work.
117 | 
118 |   A "Standard Interface" means an interface that either is an official
119 | standard defined by a recognized standards body, or, in the case of
120 | interfaces specified for a particular programming language, one that
121 | is widely used among developers working in that language.
122 | 
123 |   The "System Libraries" of an executable work include anything, other
124 | than the work as a whole, that (a) is included in the normal form of
125 | packaging a Major Component, but which is not part of that Major
126 | Component, and (b) serves only to enable use of the work with that
127 | Major Component, or to implement a Standard Interface for which an
128 | implementation is available to the public in source code form.  A
129 | "Major Component", in this context, means a major essential component
130 | (kernel, window system, and so on) of the specific operating system
131 | (if any) on which the executable work runs, or a compiler used to
132 | produce the work, or an object code interpreter used to run it.
133 | 
134 |   The "Corresponding Source" for a work in object code form means all
135 | the source code needed to generate, install, and (for an executable
136 | work) run the object code and to modify the work, including scripts to
137 | control those activities.  However, it does not include the work's
138 | System Libraries, or general-purpose tools or generally available free
139 | programs which are used unmodified in performing those activities but
140 | which are not part of the work.  For example, Corresponding Source
141 | includes interface definition files associated with source files for
142 | the work, and the source code for shared libraries and dynamically
143 | linked subprograms that the work is specifically designed to require,
144 | such as by intimate data communication or control flow between those
145 | subprograms and other parts of the work.
146 | 
147 |   The Corresponding Source need not include anything that users
148 | can regenerate automatically from other parts of the Corresponding
149 | Source.
150 | 
151 |   The Corresponding Source for a work in source code form is that
152 | same work.
153 | 
154 |   2. Basic Permissions.
155 | 
156 |   All rights granted under this License are granted for the term of
157 | copyright on the Program, and are irrevocable provided the stated
158 | conditions are met.  This License explicitly affirms your unlimited
159 | permission to run the unmodified Program.  The output from running a
160 | covered work is covered by this License only if the output, given its
161 | content, constitutes a covered work.  This License acknowledges your
162 | rights of fair use or other equivalent, as provided by copyright law.
163 | 
164 |   You may make, run and propagate covered works that you do not
165 | convey, without conditions so long as your license otherwise remains
166 | in force.  You may convey covered works to others for the sole purpose
167 | of having them make modifications exclusively for you, or provide you
168 | with facilities for running those works, provided that you comply with
169 | the terms of this License in conveying all material for which you do
170 | not control copyright.  Those thus making or running the covered works
171 | for you must do so exclusively on your behalf, under your direction
172 | and control, on terms that prohibit them from making any copies of
173 | your copyrighted material outside their relationship with you.
174 | 
175 |   Conveying under any other circumstances is permitted solely under
176 | the conditions stated below.  Sublicensing is not allowed; section 10
177 | makes it unnecessary.
178 | 
179 |   3. Protecting Users' Legal Rights From Anti-Circumvention Law.
180 | 
181 |   No covered work shall be deemed part of an effective technological
182 | measure under any applicable law fulfilling obligations under article
183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or
184 | similar laws prohibiting or restricting circumvention of such
185 | measures.
186 | 
187 |   When you convey a covered work, you waive any legal power to forbid
188 | circumvention of technological measures to the extent such circumvention
189 | is effected by exercising rights under this License with respect to
190 | the covered work, and you disclaim any intention to limit operation or
191 | modification of the work as a means of enforcing, against the work's
192 | users, your or third parties' legal rights to forbid circumvention of
193 | technological measures.
194 | 
195 |   4. Conveying Verbatim Copies.
196 | 
197 |   You may convey verbatim copies of the Program's source code as you
198 | receive it, in any medium, provided that you conspicuously and
199 | appropriately publish on each copy an appropriate copyright notice;
200 | keep intact all notices stating that this License and any
201 | non-permissive terms added in accord with section 7 apply to the code;
202 | keep intact all notices of the absence of any warranty; and give all
203 | recipients a copy of this License along with the Program.
204 | 
205 |   You may charge any price or no price for each copy that you convey,
206 | and you may offer support or warranty protection for a fee.
207 | 
208 |   5. Conveying Modified Source Versions.
209 | 
210 |   You may convey a work based on the Program, or the modifications to
211 | produce it from the Program, in the form of source code under the
212 | terms of section 4, provided that you also meet all of these conditions:
213 | 
214 |     a) The work must carry prominent notices stating that you modified
215 |     it, and giving a relevant date.
216 | 
217 |     b) The work must carry prominent notices stating that it is
218 |     released under this License and any conditions added under section
219 |     7.  This requirement modifies the requirement in section 4 to
220 |     "keep intact all notices".
221 | 
222 |     c) You must license the entire work, as a whole, under this
223 |     License to anyone who comes into possession of a copy.  This
224 |     License will therefore apply, along with any applicable section 7
225 |     additional terms, to the whole of the work, and all its parts,
226 |     regardless of how they are packaged.  This License gives no
227 |     permission to license the work in any other way, but it does not
228 |     invalidate such permission if you have separately received it.
229 | 
230 |     d) If the work has interactive user interfaces, each must display
231 |     Appropriate Legal Notices; however, if the Program has interactive
232 |     interfaces that do not display Appropriate Legal Notices, your
233 |     work need not make them do so.
234 | 
235 |   A compilation of a covered work with other separate and independent
236 | works, which are not by their nature extensions of the covered work,
237 | and which are not combined with it such as to form a larger program,
238 | in or on a volume of a storage or distribution medium, is called an
239 | "aggregate" if the compilation and its resulting copyright are not
240 | used to limit the access or legal rights of the compilation's users
241 | beyond what the individual works permit.  Inclusion of a covered work
242 | in an aggregate does not cause this License to apply to the other
243 | parts of the aggregate.
244 | 
245 |   6. Conveying Non-Source Forms.
246 | 
247 |   You may convey a covered work in object code form under the terms
248 | of sections 4 and 5, provided that you also convey the
249 | machine-readable Corresponding Source under the terms of this License,
250 | in one of these ways:
251 | 
252 |     a) Convey the object code in, or embodied in, a physical product
253 |     (including a physical distribution medium), accompanied by the
254 |     Corresponding Source fixed on a durable physical medium
255 |     customarily used for software interchange.
256 | 
257 |     b) Convey the object code in, or embodied in, a physical product
258 |     (including a physical distribution medium), accompanied by a
259 |     written offer, valid for at least three years and valid for as
260 |     long as you offer spare parts or customer support for that product
261 |     model, to give anyone who possesses the object code either (1) a
262 |     copy of the Corresponding Source for all the software in the
263 |     product that is covered by this License, on a durable physical
264 |     medium customarily used for software interchange, for a price no
265 |     more than your reasonable cost of physically performing this
266 |     conveying of source, or (2) access to copy the
267 |     Corresponding Source from a network server at no charge.
268 | 
269 |     c) Convey individual copies of the object code with a copy of the
270 |     written offer to provide the Corresponding Source.  This
271 |     alternative is allowed only occasionally and noncommercially, and
272 |     only if you received the object code with such an offer, in accord
273 |     with subsection 6b.
274 | 
275 |     d) Convey the object code by offering access from a designated
276 |     place (gratis or for a charge), and offer equivalent access to the
277 |     Corresponding Source in the same way through the same place at no
278 |     further charge.  You need not require recipients to copy the
279 |     Corresponding Source along with the object code.  If the place to
280 |     copy the object code is a network server, the Corresponding Source
281 |     may be on a different server (operated by you or a third party)
282 |     that supports equivalent copying facilities, provided you maintain
283 |     clear directions next to the object code saying where to find the
284 |     Corresponding Source.  Regardless of what server hosts the
285 |     Corresponding Source, you remain obligated to ensure that it is
286 |     available for as long as needed to satisfy these requirements.
287 | 
288 |     e) Convey the object code using peer-to-peer transmission, provided
289 |     you inform other peers where the object code and Corresponding
290 |     Source of the work are being offered to the general public at no
291 |     charge under subsection 6d.
292 | 
293 |   A separable portion of the object code, whose source code is excluded
294 | from the Corresponding Source as a System Library, need not be
295 | included in conveying the object code work.
296 | 
297 |   A "User Product" is either (1) a "consumer product", which means any
298 | tangible personal property which is normally used for personal, family,
299 | or household purposes, or (2) anything designed or sold for incorporation
300 | into a dwelling.  In determining whether a product is a consumer product,
301 | doubtful cases shall be resolved in favor of coverage.  For a particular
302 | product received by a particular user, "normally used" refers to a
303 | typical or common use of that class of product, regardless of the status
304 | of the particular user or of the way in which the particular user
305 | actually uses, or expects or is expected to use, the product.  A product
306 | is a consumer product regardless of whether the product has substantial
307 | commercial, industrial or non-consumer uses, unless such uses represent
308 | the only significant mode of use of the product.
309 | 
310 |   "Installation Information" for a User Product means any methods,
311 | procedures, authorization keys, or other information required to install
312 | and execute modified versions of a covered work in that User Product from
313 | a modified version of its Corresponding Source.  The information must
314 | suffice to ensure that the continued functioning of the modified object
315 | code is in no case prevented or interfered with solely because
316 | modification has been made.
317 | 
318 |   If you convey an object code work under this section in, or with, or
319 | specifically for use in, a User Product, and the conveying occurs as
320 | part of a transaction in which the right of possession and use of the
321 | User Product is transferred to the recipient in perpetuity or for a
322 | fixed term (regardless of how the transaction is characterized), the
323 | Corresponding Source conveyed under this section must be accompanied
324 | by the Installation Information.  But this requirement does not apply
325 | if neither you nor any third party retains the ability to install
326 | modified object code on the User Product (for example, the work has
327 | been installed in ROM).
328 | 
329 |   The requirement to provide Installation Information does not include a
330 | requirement to continue to provide support service, warranty, or updates
331 | for a work that has been modified or installed by the recipient, or for
332 | the User Product in which it has been modified or installed.  Access to a
333 | network may be denied when the modification itself materially and
334 | adversely affects the operation of the network or violates the rules and
335 | protocols for communication across the network.
336 | 
337 |   Corresponding Source conveyed, and Installation Information provided,
338 | in accord with this section must be in a format that is publicly
339 | documented (and with an implementation available to the public in
340 | source code form), and must require no special password or key for
341 | unpacking, reading or copying.
342 | 
343 |   7. Additional Terms.
344 | 
345 |   "Additional permissions" are terms that supplement the terms of this
346 | License by making exceptions from one or more of its conditions.
347 | Additional permissions that are applicable to the entire Program shall
348 | be treated as though they were included in this License, to the extent
349 | that they are valid under applicable law.  If additional permissions
350 | apply only to part of the Program, that part may be used separately
351 | under those permissions, but the entire Program remains governed by
352 | this License without regard to the additional permissions.
353 | 
354 |   When you convey a copy of a covered work, you may at your option
355 | remove any additional permissions from that copy, or from any part of
356 | it.  (Additional permissions may be written to require their own
357 | removal in certain cases when you modify the work.)  You may place
358 | additional permissions on material, added by you to a covered work,
359 | for which you have or can give appropriate copyright permission.
360 | 
361 |   Notwithstanding any other provision of this License, for material you
362 | add to a covered work, you may (if authorized by the copyright holders of
363 | that material) supplement the terms of this License with terms:
364 | 
365 |     a) Disclaiming warranty or limiting liability differently from the
366 |     terms of sections 15 and 16 of this License; or
367 | 
368 |     b) Requiring preservation of specified reasonable legal notices or
369 |     author attributions in that material or in the Appropriate Legal
370 |     Notices displayed by works containing it; or
371 | 
372 |     c) Prohibiting misrepresentation of the origin of that material, or
373 |     requiring that modified versions of such material be marked in
374 |     reasonable ways as different from the original version; or
375 | 
376 |     d) Limiting the use for publicity purposes of names of licensors or
377 |     authors of the material; or
378 | 
379 |     e) Declining to grant rights under trademark law for use of some
380 |     trade names, trademarks, or service marks; or
381 | 
382 |     f) Requiring indemnification of licensors and authors of that
383 |     material by anyone who conveys the material (or modified versions of
384 |     it) with contractual assumptions of liability to the recipient, for
385 |     any liability that these contractual assumptions directly impose on
386 |     those licensors and authors.
387 | 
388 |   All other non-permissive additional terms are considered "further
389 | restrictions" within the meaning of section 10.  If the Program as you
390 | received it, or any part of it, contains a notice stating that it is
391 | governed by this License along with a term that is a further
392 | restriction, you may remove that term.  If a license document contains
393 | a further restriction but permits relicensing or conveying under this
394 | License, you may add to a covered work material governed by the terms
395 | of that license document, provided that the further restriction does
396 | not survive such relicensing or conveying.
397 | 
398 |   If you add terms to a covered work in accord with this section, you
399 | must place, in the relevant source files, a statement of the
400 | additional terms that apply to those files, or a notice indicating
401 | where to find the applicable terms.
402 | 
403 |   Additional terms, permissive or non-permissive, may be stated in the
404 | form of a separately written license, or stated as exceptions;
405 | the above requirements apply either way.
406 | 
407 |   8. Termination.
408 | 
409 |   You may not propagate or modify a covered work except as expressly
410 | provided under this License.  Any attempt otherwise to propagate or
411 | modify it is void, and will automatically terminate your rights under
412 | this License (including any patent licenses granted under the third
413 | paragraph of section 11).
414 | 
415 |   However, if you cease all violation of this License, then your
416 | license from a particular copyright holder is reinstated (a)
417 | provisionally, unless and until the copyright holder explicitly and
418 | finally terminates your license, and (b) permanently, if the copyright
419 | holder fails to notify you of the violation by some reasonable means
420 | prior to 60 days after the cessation.
421 | 
422 |   Moreover, your license from a particular copyright holder is
423 | reinstated permanently if the copyright holder notifies you of the
424 | violation by some reasonable means, this is the first time you have
425 | received notice of violation of this License (for any work) from that
426 | copyright holder, and you cure the violation prior to 30 days after
427 | your receipt of the notice.
428 | 
429 |   Termination of your rights under this section does not terminate the
430 | licenses of parties who have received copies or rights from you under
431 | this License.  If your rights have been terminated and not permanently
432 | reinstated, you do not qualify to receive new licenses for the same
433 | material under section 10.
434 | 
435 |   9. Acceptance Not Required for Having Copies.
436 | 
437 |   You are not required to accept this License in order to receive or
438 | run a copy of the Program.  Ancillary propagation of a covered work
439 | occurring solely as a consequence of using peer-to-peer transmission
440 | to receive a copy likewise does not require acceptance.  However,
441 | nothing other than this License grants you permission to propagate or
442 | modify any covered work.  These actions infringe copyright if you do
443 | not accept this License.  Therefore, by modifying or propagating a
444 | covered work, you indicate your acceptance of this License to do so.
445 | 
446 |   10. Automatic Licensing of Downstream Recipients.
447 | 
448 |   Each time you convey a covered work, the recipient automatically
449 | receives a license from the original licensors, to run, modify and
450 | propagate that work, subject to this License.  You are not responsible
451 | for enforcing compliance by third parties with this License.
452 | 
453 |   An "entity transaction" is a transaction transferring control of an
454 | organization, or substantially all assets of one, or subdividing an
455 | organization, or merging organizations.  If propagation of a covered
456 | work results from an entity transaction, each party to that
457 | transaction who receives a copy of the work also receives whatever
458 | licenses to the work the party's predecessor in interest had or could
459 | give under the previous paragraph, plus a right to possession of the
460 | Corresponding Source of the work from the predecessor in interest, if
461 | the predecessor has it or can get it with reasonable efforts.
462 | 
463 |   You may not impose any further restrictions on the exercise of the
464 | rights granted or affirmed under this License.  For example, you may
465 | not impose a license fee, royalty, or other charge for exercise of
466 | rights granted under this License, and you may not initiate litigation
467 | (including a cross-claim or counterclaim in a lawsuit) alleging that
468 | any patent claim is infringed by making, using, selling, offering for
469 | sale, or importing the Program or any portion of it.
470 | 
471 |   11. Patents.
472 | 
473 |   A "contributor" is a copyright holder who authorizes use under this
474 | License of the Program or a work on which the Program is based.  The
475 | work thus licensed is called the contributor's "contributor version".
476 | 
477 |   A contributor's "essential patent claims" are all patent claims
478 | owned or controlled by the contributor, whether already acquired or
479 | hereafter acquired, that would be infringed by some manner, permitted
480 | by this License, of making, using, or selling its contributor version,
481 | but do not include claims that would be infringed only as a
482 | consequence of further modification of the contributor version.  For
483 | purposes of this definition, "control" includes the right to grant
484 | patent sublicenses in a manner consistent with the requirements of
485 | this License.
486 | 
487 |   Each contributor grants you a non-exclusive, worldwide, royalty-free
488 | patent license under the contributor's essential patent claims, to
489 | make, use, sell, offer for sale, import and otherwise run, modify and
490 | propagate the contents of its contributor version.
491 | 
492 |   In the following three paragraphs, a "patent license" is any express
493 | agreement or commitment, however denominated, not to enforce a patent
494 | (such as an express permission to practice a patent or covenant not to
495 | sue for patent infringement).  To "grant" such a patent license to a
496 | party means to make such an agreement or commitment not to enforce a
497 | patent against the party.
498 | 
499 |   If you convey a covered work, knowingly relying on a patent license,
500 | and the Corresponding Source of the work is not available for anyone
501 | to copy, free of charge and under the terms of this License, through a
502 | publicly available network server or other readily accessible means,
503 | then you must either (1) cause the Corresponding Source to be so
504 | available, or (2) arrange to deprive yourself of the benefit of the
505 | patent license for this particular work, or (3) arrange, in a manner
506 | consistent with the requirements of this License, to extend the patent
507 | license to downstream recipients.  "Knowingly relying" means you have
508 | actual knowledge that, but for the patent license, your conveying the
509 | covered work in a country, or your recipient's use of the covered work
510 | in a country, would infringe one or more identifiable patents in that
511 | country that you have reason to believe are valid.
512 | 
513 |   If, pursuant to or in connection with a single transaction or
514 | arrangement, you convey, or propagate by procuring conveyance of, a
515 | covered work, and grant a patent license to some of the parties
516 | receiving the covered work authorizing them to use, propagate, modify
517 | or convey a specific copy of the covered work, then the patent license
518 | you grant is automatically extended to all recipients of the covered
519 | work and works based on it.
520 | 
521 |   A patent license is "discriminatory" if it does not include within
522 | the scope of its coverage, prohibits the exercise of, or is
523 | conditioned on the non-exercise of one or more of the rights that are
524 | specifically granted under this License.  You may not convey a covered
525 | work if you are a party to an arrangement with a third party that is
526 | in the business of distributing software, under which you make payment
527 | to the third party based on the extent of your activity of conveying
528 | the work, and under which the third party grants, to any of the
529 | parties who would receive the covered work from you, a discriminatory
530 | patent license (a) in connection with copies of the covered work
531 | conveyed by you (or copies made from those copies), or (b) primarily
532 | for and in connection with specific products or compilations that
533 | contain the covered work, unless you entered into that arrangement,
534 | or that patent license was granted, prior to 28 March 2007.
535 | 
536 |   Nothing in this License shall be construed as excluding or limiting
537 | any implied license or other defenses to infringement that may
538 | otherwise be available to you under applicable patent law.
539 | 
540 |   12. No Surrender of Others' Freedom.
541 | 
542 |   If conditions are imposed on you (whether by court order, agreement or
543 | otherwise) that contradict the conditions of this License, they do not
544 | excuse you from the conditions of this License.  If you cannot convey a
545 | covered work so as to satisfy simultaneously your obligations under this
546 | License and any other pertinent obligations, then as a consequence you may
547 | not convey it at all.  For example, if you agree to terms that obligate you
548 | to collect a royalty for further conveying from those to whom you convey
549 | the Program, the only way you could satisfy both those terms and this
550 | License would be to refrain entirely from conveying the Program.
551 | 
552 |   13. Use with the GNU Affero General Public License.
553 | 
554 |   Notwithstanding any other provision of this License, you have
555 | permission to link or combine any covered work with a work licensed
556 | under version 3 of the GNU Affero General Public License into a single
557 | combined work, and to convey the resulting work.  The terms of this
558 | License will continue to apply to the part which is the covered work,
559 | but the special requirements of the GNU Affero General Public License,
560 | section 13, concerning interaction through a network will apply to the
561 | combination as such.
562 | 
563 |   14. Revised Versions of this License.
564 | 
565 |   The Free Software Foundation may publish revised and/or new versions of
566 | the GNU General Public License from time to time.  Such new versions will
567 | be similar in spirit to the present version, but may differ in detail to
568 | address new problems or concerns.
569 | 
570 |   Each version is given a distinguishing version number.  If the
571 | Program specifies that a certain numbered version of the GNU General
572 | Public License "or any later version" applies to it, you have the
573 | option of following the terms and conditions either of that numbered
574 | version or of any later version published by the Free Software
575 | Foundation.  If the Program does not specify a version number of the
576 | GNU General Public License, you may choose any version ever published
577 | by the Free Software Foundation.
578 | 
579 |   If the Program specifies that a proxy can decide which future
580 | versions of the GNU General Public License can be used, that proxy's
581 | public statement of acceptance of a version permanently authorizes you
582 | to choose that version for the Program.
583 | 
584 |   Later license versions may give you additional or different
585 | permissions.  However, no additional obligations are imposed on any
586 | author or copyright holder as a result of your choosing to follow a
587 | later version.
588 | 
589 |   15. Disclaimer of Warranty.
590 | 
591 |   THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
592 | APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
596 | PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
597 | IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
599 | 
600 |   16. Limitation of Liability.
601 | 
602 |   IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
610 | SUCH DAMAGES.
611 | 
612 |   17. Interpretation of Sections 15 and 16.
613 | 
614 |   If the disclaimer of warranty and limitation of liability provided
615 | above cannot be given local legal effect according to their terms,
616 | reviewing courts shall apply local law that most closely approximates
617 | an absolute waiver of all civil liability in connection with the
618 | Program, unless a warranty or assumption of liability accompanies a
619 | copy of the Program in return for a fee.
620 | 
621 |                      END OF TERMS AND CONDITIONS
622 | 
623 |             How to Apply These Terms to Your New Programs
624 | 
625 |   If you develop a new program, and you want it to be of the greatest
626 | possible use to the public, the best way to achieve this is to make it
627 | free software which everyone can redistribute and change under these terms.
628 | 
629 |   To do so, attach the following notices to the program.  It is safest
630 | to attach them to the start of each source file to most effectively
631 | state the exclusion of warranty; and each file should have at least
632 | the "copyright" line and a pointer to where the full notice is found.
633 | 
634 |     <one line to give the program's name and a brief idea of what it does.>
635 |     Copyright (C) <year>  <name of author>
636 | 
637 |     This program is free software: you can redistribute it and/or modify
638 |     it under the terms of the GNU General Public License as published by
639 |     the Free Software Foundation, either version 3 of the License, or
640 |     (at your option) any later version.
641 | 
642 |     This program is distributed in the hope that it will be useful,
643 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
644 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
645 |     GNU General Public License for more details.
646 | 
647 |     You should have received a copy of the GNU General Public License
648 |     along with this program.  If not, see <https://www.gnu.org/licenses/>.
649 | 
650 | Also add information on how to contact you by electronic and paper mail.
651 | 
652 |   If the program does terminal interaction, make it output a short
653 | notice like this when it starts in an interactive mode:
654 | 
655 |     <program>  Copyright (C) <year>  <name of author>
656 |     This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
657 |     This is free software, and you are welcome to redistribute it
658 |     under certain conditions; type `show c' for details.
659 | 
660 | The hypothetical commands `show w' and `show c' should show the appropriate
661 | parts of the General Public License.  Of course, your program's commands
662 | might be different; for a GUI interface, you would use an "about box".
663 | 
664 |   You should also get your employer (if you work as a programmer) or school,
665 | if any, to sign a "copyright disclaimer" for the program, if necessary.
666 | For more information on this, and how to apply and follow the GNU GPL, see
667 | <https://www.gnu.org/licenses/>.
668 | 
669 |   The GNU General Public License does not permit incorporating your program
670 | into proprietary programs.  If your program is a subroutine library, you
671 | may consider it more useful to permit linking proprietary applications with
672 | the library.  If this is what you want to do, use the GNU Lesser General
673 | Public License instead of this License.  But first, please read
674 | <https://www.gnu.org/licenses/why-not-lgpl.html>.
675 | 


--------------------------------------------------------------------------------