├── .gitignore ├── README.md ├── data ├── calib_results.txt └── rviz_config.rviz ├── libviso2 ├── CMakeLists.txt ├── libviso2 │ ├── CMakeLists.txt │ ├── img │ │ ├── I1_000000.png │ │ ├── I1_000001.png │ │ ├── I1_000002.png │ │ ├── I1_000003.png │ │ ├── I1_000004.png │ │ ├── I1_000005.png │ │ ├── I1_000006.png │ │ ├── I1c.png │ │ ├── I1p.png │ │ ├── I2c.png │ │ └── I2p.png │ ├── matlab │ │ ├── demo_matching_flow.m │ │ ├── demo_matching_quad.m │ │ ├── demo_matching_stereo.m │ │ ├── demo_matching_tracking.m │ │ ├── demo_structure_from_motion.m │ │ ├── demo_viso_mono.m │ │ ├── demo_viso_stereo.m │ │ ├── make.m │ │ ├── matcherMex.cpp │ │ ├── plotMatch.m │ │ ├── plotTrack.m │ │ ├── reconstructionMex.cpp │ │ ├── visualOdometryMonoMex.cpp │ │ └── visualOdometryStereoMex.cpp │ ├── readme.txt │ └── src │ │ ├── demo.cpp │ │ ├── filter.cpp │ │ ├── filter.h │ │ ├── matcher.cpp │ │ ├── matcher.h │ │ ├── matrix.cpp │ │ ├── matrix.h │ │ ├── reconstruction.cpp │ │ ├── reconstruction.h │ │ ├── sse_to_neon.hpp │ │ ├── timer.h │ │ ├── triangle.cpp │ │ ├── triangle.h │ │ ├── viso.cpp │ │ ├── viso.h │ │ ├── viso_mono.cpp │ │ ├── viso_mono.h │ │ ├── viso_mono_omnidirectional.cpp │ │ ├── viso_mono_omnidirectional.h │ │ ├── viso_stereo.cpp │ │ └── viso_stereo.h └── package.xml ├── viso2 ├── CMakeLists.txt └── package.xml └── viso2_ros ├── CMakeLists.txt ├── launch ├── demo.launch ├── omni_version.launch └── perspective_version.launch ├── mainpage.dox ├── msg └── VisoInfo.msg ├── package.xml ├── src ├── mono_odometer.cpp ├── mono_odometer_omnidirectional.cpp ├── odometer_base.h ├── odometry_params.h ├── stereo_odometer.cpp └── stereo_processor.h └── wikidoc.txt /.gitignore: -------------------------------------------------------------------------------- 1 | # directories created by ros 2 | libviso2/libviso2/build 3 | */lib/ 4 | */bin/ 5 | */build/ 6 | */cfg/cpp/ 7 | */cfg/*.cfgc 8 | */CMakeFiles/ 9 | */msg_gen/ 10 | */src/*_msgs/msg/_*.py 11 | */src/*_msgs/msg/_*.pyc 12 | */src/*_msgs/_*.py 13 | */src/*_msgs/_*.pyc 14 | */msg/lisp/ 15 | */srv_gen/ 16 | */src/*/srv/_*.py 17 | */src/*/_*_.py 18 | */src/*/_*_.pyc 19 | */src/*/srv 20 | *~ 21 | *.swp 22 | *.swo 23 | *.swk 24 | *.swj 25 | *.swh 26 | *.swg 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | viso2 2 | ========== 3 | ROS Stack containing a wrapper for libviso2, a visual odometry library. 4 | http://www.ros.org/wiki/viso2 for the list of contained packages. 5 | *** 6 | 7 | ## Omnidirectional version 8 | 9 | * This package contains an additional version of the original Libviso2 that works with a **monocular omnidirectional camera system**. 10 | 11 | ### Dependencies 12 | 13 | * The only additional dependency of this version is the camera calibration toolbox [Ocamcalib](https://sites.google.com/site/scarabotix/ocamcalib-toolbox) for Matlab. 14 | - Instead of using a normal pinhole camera calibration model, the method uses the unified camera model proposed by Davide Scaramuzza. 15 | - Download the toolbox from [here](https://sites.google.com/site/scarabotix/ocamcalib-toolbox/ocamcalib-toolbox-download-page), put it in your Matlab workspace and execute the `ocam_calib` command to run the application. 16 | - After calibrating the camera save the generated text file that will be used by the method. 17 | 18 | ### How to execute 19 | 20 | * To execute omnidirectional Libviso2 make sure you have a camera node publishing over ROS. 21 | * After that you can use the demo launch file that we provide in the following way: 22 | ``` 23 | roslaunch viso2_ros demo.launch 24 | ``` 25 | * Make sure that first you set the **file calibration path** correctly by changing the `calib_path` parameter. 26 | * Also you can edit/add the default parameters that Libviso2 use (don't forget that camera height and pitch are mandatory to scale calculation). 27 | 28 | ### Performance test 29 | 30 | * If you want to check the performance of the method before using it in your own system you can use the bag file that we provide [here](https://drive.google.com/file/d/1FfBOMPrdk-uqLeNm3NNGXJQxdkKqgUfI/view?usp=sharing). 31 | 32 | ##### Available topics on the bag file 33 | 1. `perspective_cam/image/compressed` contains a perspective camera sequence that can be used by the default monocular Libviso2 version to benchmark. 34 | 2. `fisheye_cam/image/compressed` contains the same sequence captured by a fisheye camera. 35 | 3. `slam_out_pose` contains the camera motion estimation computed by Hector SLAM method. 36 | 4. `/husky_velocity_controller/odom` constains conventional odometry motion estimation. 37 | 5. `/fix` constains GPS data. 38 | 6. `/map` contains a map of the environment designed using a laser. 39 | 7. (...) and others. 40 | 41 | ##### Running the test sequences 42 | 43 | * To run both test sequences you can execute the following commands after setting the proper paths for the bag file and the calibration file. 44 | ``` 45 | roslaunch viso2_ros perspective.launch 46 | roslaunch viso2_ros fisheye.launch 47 | ``` 48 | **NOTE:** We provide the **camera calibration file** for the fisheye camera ('data/calib_results.txt'). You have to set the correct path to the rosbag file in both launch files and the calibration text file in the fisheye.launch. 49 | We also provide a **rviz configuration file** to easy visualization of the results. 50 | 51 | ## Acknowledges 52 | 53 | This work is co-financed by the ERDF – European Regional Development Fund through the Operational Programme for Competitiveness and Internationalisation - COMPETE 2020 under the PORTUGAL 2020 Partnership Agreement, and through the Portuguese National Innovation Agency (ANI) as a part of project «ROMOVI: POCI-01-0247-FEDER-017945»". 54 | -------------------------------------------------------------------------------- /data/calib_results.txt: -------------------------------------------------------------------------------- 1 | #polynomial coefficients for the DIRECT mapping function (ocam_model.ss in MATLAB). These are used by cam2world 2 | 3 | 5 -3.055608e+02 0.000000e+00 1.292850e-03 -1.425431e-06 5.302177e-09 4 | 5 | #polynomial coefficients for the inverse mapping function (ocam_model.invpol in MATLAB). These are used by world2cam 6 | 7 | 8 429.030106 210.073626 -47.103376 -69.021889 -208.158118 -229.001725 -107.244699 -18.561481 8 | 9 | #center: "row" and "column", starting from 0 (C convention) 10 | 11 | 213.926560 347.584904 12 | 13 | #affine parameters "c", "d", "e" 14 | 15 | 1.002352 0.000113 -0.001526 16 | 17 | #image size: "height" and "width" 18 | 19 | 480 640 20 | 21 | -------------------------------------------------------------------------------- /data/rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 75 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Grid1/Offset1 9 | - /Odometry1/Shape1 10 | - /Odometry2/Shape1 11 | Splitter Ratio: 0.396995693 12 | Tree Height: 273 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Angle Tolerance: 0.100000001 54 | Class: rviz/Odometry 55 | Covariance: 56 | Orientation: 57 | Alpha: 0.5 58 | Color: 255; 255; 127 59 | Color Style: Unique 60 | Frame: Local 61 | Offset: 1 62 | Scale: 1 63 | Value: true 64 | Position: 65 | Alpha: 0.300000012 66 | Color: 204; 51; 204 67 | Scale: 1 68 | Value: true 69 | Value: true 70 | Enabled: true 71 | Keep: 1000000 72 | Name: Odometry 73 | Position Tolerance: 0.100000001 74 | Shape: 75 | Alpha: 1 76 | Axes Length: 1 77 | Axes Radius: 0.100000001 78 | Color: 76; 200; 144 79 | Head Length: 1 80 | Head Radius: 1 81 | Shaft Length: 1 82 | Shaft Radius: 1 83 | Value: Arrow 84 | Topic: /mono_odometer/odometry 85 | Unreliable: true 86 | Value: true 87 | - Angle Tolerance: 0.100000001 88 | Class: rviz/Odometry 89 | Covariance: 90 | Orientation: 91 | Alpha: 0.5 92 | Color: 255; 255; 127 93 | Color Style: Unique 94 | Frame: Local 95 | Offset: 1 96 | Scale: 1 97 | Value: true 98 | Position: 99 | Alpha: 0.300000012 100 | Color: 204; 51; 204 101 | Scale: 1 102 | Value: true 103 | Value: false 104 | Enabled: true 105 | Keep: 1000000 106 | Name: Odometry 107 | Position Tolerance: 0.100000001 108 | Shape: 109 | Alpha: 1 110 | Axes Length: 1 111 | Axes Radius: 0.100000001 112 | Color: 200; 106; 43 113 | Head Length: 1 114 | Head Radius: 1 115 | Shaft Length: 1 116 | Shaft Radius: 1 117 | Value: Arrow 118 | Topic: /husky_velocity_controller/odom 119 | Unreliable: true 120 | Value: true 121 | - Alpha: 1 122 | Axes Length: 1 123 | Axes Radius: 0.100000001 124 | Class: rviz/Pose 125 | Color: 255; 25; 0 126 | Enabled: true 127 | Head Length: 1 128 | Head Radius: 1 129 | Name: Pose 130 | Shaft Length: 1 131 | Shaft Radius: 1 132 | Shape: Arrow 133 | Topic: /slam_out_pose 134 | Unreliable: false 135 | Value: true 136 | - Alpha: 1 137 | Autocompute Intensity Bounds: true 138 | Autocompute Value Bounds: 139 | Max Value: 10 140 | Min Value: -10 141 | Value: true 142 | Axis: Z 143 | Channel Name: intensity 144 | Class: rviz/LaserScan 145 | Color: 255; 255; 255 146 | Color Transformer: Intensity 147 | Decay Time: 0 148 | Enabled: false 149 | Invert Rainbow: false 150 | Max Color: 255; 255; 255 151 | Max Intensity: 680 152 | Min Color: 0; 0; 0 153 | Min Intensity: 0 154 | Name: LaserScan 155 | Position Transformer: XYZ 156 | Queue Size: 10 157 | Selectable: true 158 | Size (Pixels): 3 159 | Size (m): 0.100000001 160 | Style: Flat Squares 161 | Topic: /scan 162 | Unreliable: false 163 | Use Fixed Frame: true 164 | Use rainbow: true 165 | Value: false 166 | - Alpha: 0.699999988 167 | Class: rviz/Map 168 | Color Scheme: map 169 | Draw Behind: false 170 | Enabled: true 171 | Name: Map 172 | Topic: /map 173 | Unreliable: false 174 | Use Timestamp: false 175 | Value: true 176 | - Class: rviz/Image 177 | Enabled: true 178 | Image Topic: /fisheye_cam/image 179 | Max Value: 1 180 | Median window: 5 181 | Min Value: 0 182 | Name: Image 183 | Normalize Range: true 184 | Queue Size: 2 185 | Transport Hint: compressed 186 | Unreliable: false 187 | Value: true 188 | Enabled: true 189 | Global Options: 190 | Background Color: 48; 48; 48 191 | Default Light: true 192 | Fixed Frame: map 193 | Frame Rate: 30 194 | Name: root 195 | Tools: 196 | - Class: rviz/Interact 197 | Hide Inactive Objects: true 198 | - Class: rviz/MoveCamera 199 | - Class: rviz/Select 200 | - Class: rviz/FocusCamera 201 | - Class: rviz/Measure 202 | - Class: rviz/SetInitialPose 203 | Topic: /initialpose 204 | - Class: rviz/SetGoal 205 | Topic: /move_base_simple/goal 206 | - Class: rviz/PublishPoint 207 | Single click: true 208 | Topic: /clicked_point 209 | Value: true 210 | Views: 211 | Current: 212 | Class: rviz/XYOrbit 213 | Distance: 482.839386 214 | Enable Stereo Rendering: 215 | Stereo Eye Separation: 0.0599999987 216 | Stereo Focal Distance: 1 217 | Swap Stereo Eyes: false 218 | Value: false 219 | Focal Point: 220 | X: 0 221 | Y: 0 222 | Z: 0 223 | Focal Shape Fixed Size: false 224 | Focal Shape Size: 0.0500000007 225 | Invert Z Axis: false 226 | Name: Current View 227 | Near Clip Distance: 0.00999999978 228 | Pitch: 1.22479665 229 | Target Frame: 230 | Value: XYOrbit (rviz) 231 | Yaw: 3.08725929 232 | Saved: ~ 233 | Window Geometry: 234 | Displays: 235 | collapsed: false 236 | Height: 744 237 | Hide Left Dock: false 238 | Hide Right Dock: false 239 | Image: 240 | collapsed: false 241 | QMainWindow State: 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 242 | Selection: 243 | collapsed: false 244 | Time: 245 | collapsed: false 246 | Tool Properties: 247 | collapsed: false 248 | Views: 249 | collapsed: false 250 | Width: 1301 251 | X: 65 252 | Y: 24 253 | -------------------------------------------------------------------------------- /libviso2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(libviso2) 3 | 4 | find_package(catkin REQUIRED ) 5 | 6 | option(ARM_CROSS_COMPILATION "ARM Cross Compilation" OFF) 7 | 8 | if(ARM_CROSS_COMPILATION) 9 | SET(CMAKE_SYSTEM_PROCESSOR arm) 10 | SET(CMAKE_CXX_FLAGS -mfpu=neon) 11 | else(ARM_CROSS_COMPILATION) 12 | add_definitions(-msse3) 13 | endif(ARM_CROSS_COMPILATION) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS libviso2/src 17 | LIBRARIES viso2 18 | ) 19 | 20 | include_directories(libviso2/src 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | add_library(viso2 25 | libviso2/src/filter.cpp 26 | libviso2/src/matcher.cpp 27 | libviso2/src/matrix.cpp 28 | libviso2/src/reconstruction.cpp 29 | libviso2/src/triangle.cpp 30 | libviso2/src/viso.cpp 31 | libviso2/src/viso_mono.cpp 32 | libviso2/src/viso_mono_omnidirectional.cpp 33 | libviso2/src/viso_stereo.cpp) 34 | 35 | install(TARGETS viso2 36 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 39 | ) 40 | 41 | install(DIRECTORY ${PROJECT_NAME}/src/ 42 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 43 | PATTERN "*.cpp" EXCLUDE 44 | ) 45 | 46 | -------------------------------------------------------------------------------- /libviso2/libviso2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # project 2 | cmake_minimum_required(VERSION 3.0.2) 3 | project (libviso2) 4 | 5 | # directories 6 | set (LIBVISO2_SRC_DIR src) 7 | 8 | # include directory 9 | include_directories("${LIBVISO2_SRC_DIR}") 10 | 11 | # use sse3 instruction set 12 | if(ARM_CROSS_COMPILATION) 13 | SET(CMAKE_SYSTEM_PROCESSOR arm) 14 | SET(CMAKE_CXX_FLAGS -mfpu=neon) 15 | else(ARM_CROSS_COMPILATION) 16 | add_definitions(-msse3) 17 | endif 18 | 19 | # sources 20 | FILE(GLOB LIBVISO2_SRC_FILES "src/*.cpp") 21 | 22 | # make release version 23 | set(CMAKE_BUILD_TYPE Release) 24 | 25 | # demo program 26 | add_executable(viso2 ${LIBVISO2_SRC_FILES}) 27 | target_link_libraries (viso2 png) 28 | 29 | -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1_000000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1_000000.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1_000001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1_000001.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1_000002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1_000002.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1_000003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1_000003.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1_000004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1_000004.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1_000005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1_000005.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1_000006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1_000006.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1c.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1c.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I1p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I1p.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I2c.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I2c.png -------------------------------------------------------------------------------- /libviso2/libviso2/img/I2p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/viso2/7bf713844eb344dcd4adb57bcbf489555b0ad731/libviso2/libviso2/img/I2p.png -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/demo_matching_flow.m: -------------------------------------------------------------------------------- 1 | % demonstrates sparse optical flow 2 | disp('==========================='); 3 | clear all; dbstop error; close all; 4 | 5 | % read images from file 6 | I1p = imread('../img/I1p.png'); 7 | I1c = imread('../img/I1c.png'); 8 | %I1p = rgb2gray(imread('/home/geiger/test_data/2013_04_29_drive_0018_extract/image_03/data/0000000336.png')); 9 | %I1c = rgb2gray(imread('/home/geiger/test_data/2013_04_29_drive_0018_extract/image_03/data/0000000337.png')); 10 | 11 | % matching parameters 12 | param.nms_n = 4; % non-max-suppression: min. distance between maxima (in pixels) 13 | param.nms_tau = 50; % non-max-suppression: interest point peakiness threshold 14 | param.match_binsize = 50; % matching bin width/height (affects efficiency only) 15 | param.match_radius = 200; % matching radius (du/dv in pixels) 16 | param.match_disp_tolerance = 1; % du tolerance for stereo matches (in pixels) 17 | param.outlier_disp_tolerance = 5; % outlier removal: disparity tolerance (in pixels) 18 | param.outlier_flow_tolerance = 5; % outlier removal: flow tolerance (in pixels) 19 | param.multi_stage = 1; % 0=disabled,1=multistage matching (denser and faster) 20 | param.half_resolution = 0; % 0=disabled,1=match at half resolution, refine at full resolution 21 | param.refinement = 2; % refinement (0=none,1=pixel,2=subpixel) 22 | 23 | % init matcher 24 | matcherMex('init',param); 25 | 26 | % push back images 27 | matcherMex('push',I1p); 28 | tic 29 | matcherMex('push',I1c); 30 | disp(['Feature detection: ' num2str(toc) ' seconds']); 31 | 32 | % match images 33 | tic; matcherMex('match',0); 34 | p_matched = matcherMex('get_matches',0); 35 | disp(['Feature matching: ' num2str(toc) ' seconds']); 36 | 37 | % close matcher 38 | matcherMex('close'); 39 | 40 | % show matching results 41 | disp(['Number of matched points: ' num2str(length(p_matched))]); 42 | disp('Plotting ...'); 43 | plotMatch(I1c,p_matched,0); 44 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/demo_matching_quad.m: -------------------------------------------------------------------------------- 1 | % demonstrates sparse scene flow (quad matching = 2 consecutive stereo pairs) 2 | disp('==========================='); 3 | clear all; dbstop error; close all; 4 | 5 | % read images from file 6 | I1p = imread('../img/I1p.png'); 7 | I2p = imread('../img/I2p.png'); 8 | I1c = imread('../img/I1c.png'); 9 | I2c = imread('../img/I2c.png'); 10 | 11 | % matching parameters 12 | param.nms_n = 2; % non-max-suppression: min. distance between maxima (in pixels) 13 | param.nms_tau = 50; % non-max-suppression: interest point peakiness threshold 14 | param.match_binsize = 50; % matching bin width/height (affects efficiency only) 15 | param.match_radius = 200; % matching radius (du/dv in pixels) 16 | param.match_disp_tolerance = 1; % du tolerance for stereo matches (in pixels) 17 | param.outlier_disp_tolerance = 5; % outlier removal: disparity tolerance (in pixels) 18 | param.outlier_flow_tolerance = 5; % outlier removal: flow tolerance (in pixels) 19 | param.multi_stage = 1; % 0=disabled,1=multistage matching (denser and faster) 20 | param.half_resolution = 0; % 0=disabled,1=match at half resolution, refine at full resolution 21 | param.refinement = 1; % refinement (0=none,1=pixel,2=subpixel) 22 | 23 | % init matcher 24 | matcherMex('init',param); 25 | 26 | % push back images 27 | matcherMex('push',I1p,I2p); tic; 28 | matcherMex('push',I1c,I2c); 29 | disp(['Feature detection: ' num2str(toc) ' seconds']); 30 | 31 | % match images 32 | tic; matcherMex('match',2); 33 | p_matched = matcherMex('get_matches',2); 34 | disp(['Feature matching: ' num2str(toc) ' seconds']); 35 | 36 | % close matcher 37 | matcherMex('close'); 38 | 39 | % show matching results 40 | disp(['Number of matched points: ' num2str(length(p_matched))]); 41 | disp('Plotting ...'); 42 | figure('Position',[1200 30 size(I1c,2) size(I1c,1)]); axes('Position',[0 0 1 1]); 43 | plotMatch(I1c,p_matched,2); 44 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/demo_matching_stereo.m: -------------------------------------------------------------------------------- 1 | % demonstrates sparse stereo 2 | disp('==========================='); 3 | clear all; dbstop error; close all; 4 | 5 | % read images from file 6 | I1c = imread('../img/I1c.png'); 7 | I2c = imread('../img/I2c.png'); 8 | 9 | % matching parameters 10 | param.nms_n = 3; % non-max-suppression: min. distance between maxima (in pixels) 11 | param.nms_tau = 50; % non-max-suppression: interest point peakiness threshold 12 | param.match_binsize = 50; % matching bin width/height (affects efficiency only) 13 | param.match_radius = 200; % matching radius (du/dv in pixels) 14 | param.match_disp_tolerance = 1; % du tolerance for stereo matches (in pixels) 15 | param.outlier_disp_tolerance = 5; % outlier removal: disparity tolerance (in pixels) 16 | param.outlier_flow_tolerance = 5; % outlier removal: flow tolerance (in pixels) 17 | param.multi_stage = 1; % 0=disabled,1=multistage matching (denser and faster) 18 | param.half_resolution = 0; % 0=disabled,1=match at half resolution, refine at full resolution 19 | param.refinement = 1; % refinement (0=none,1=pixel,2=subpixel) 20 | 21 | % init matcher 22 | matcherMex('init',param); 23 | 24 | % push back images 25 | tic; matcherMex('push',I1c,I2c); 26 | disp(['Feature detection: ' num2str(toc) ' seconds']); 27 | 28 | % match images 29 | tic; matcherMex('match',1); 30 | p_matched = matcherMex('get_matches',1); 31 | disp(['Feature matching: ' num2str(toc) ' seconds']); 32 | 33 | % close matcher 34 | matcherMex('close'); 35 | 36 | % show matching results 37 | disp(['Number of matched points: ' num2str(length(p_matched))]); 38 | disp('Plotting ...'); 39 | plotMatch(I1c,p_matched,1); 40 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/demo_matching_tracking.m: -------------------------------------------------------------------------------- 1 | % demonstrates monocular feature tracking (via feature indices) 2 | disp('==========================='); 3 | clear all; dbstop error; close all; 4 | 5 | % matching parameters 6 | param.nms_n = 2; % non-max-suppression: min. distance between maxima (in pixels) 7 | param.nms_tau = 50; % non-max-suppression: interest point peakiness threshold 8 | param.match_binsize = 50; % matching bin width/height (affects efficiency only) 9 | param.match_radius = 200; % matching radius (du/dv in pixels) 10 | param.match_disp_tolerance = 1; % du tolerance for stereo matches (in pixels) 11 | param.outlier_disp_tolerance = 5; % outlier removal: disparity tolerance (in pixels) 12 | param.outlier_flow_tolerance = 5; % outlier removal: flow tolerance (in pixels) 13 | param.multi_stage = 1; % 0=disabled,1=multistage matching (denser and faster) 14 | param.half_resolution = 1; % 0=disabled,1=match at half resolution, refine at full resolution 15 | param.refinement = 2; % refinement (0=none,1=pixel,2=subpixel) 16 | 17 | % init matcher 18 | matcherMex('init',param); 19 | 20 | % push back first image 21 | I = imread('../img/I1_000000.png'); 22 | matcherMex('push',I); 23 | 24 | % feature tracks 25 | tracks = {}; 26 | 27 | % start matching 28 | for i=1:6 29 | I = imread(['../img/I1_' num2str(i,'%06d') '.png']); 30 | tic; matcherMex('push',I); 31 | disp(['Feature detection: ' num2str(toc) ' seconds']); 32 | tic; matcherMex('match',0); 33 | p_matched{i} = matcherMex('get_matches',0); 34 | i_matched{i} = matcherMex('get_indices',0); 35 | disp(['Feature matching: ' num2str(toc) ' seconds']); 36 | end 37 | 38 | % close matcher 39 | matcherMex('close'); 40 | 41 | % show matching results 42 | disp('Plotting ...'); 43 | plotTrack(I,p_matched,i_matched); 44 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/demo_structure_from_motion.m: -------------------------------------------------------------------------------- 1 | % takes a monocular input sequence and computes 3d point clouds 2 | disp('==========================='); 3 | clear all; close all; dbstop error; 4 | 5 | % parameter settings (for an example, please download 6 | % sequence '2010_03_09_drive_0019' from www.cvlibs.net) 7 | %img_dir = '/home/geiger/5_Data/karlsruhe_dataset/2011_stereo/2010_03_09_drive_0019'; 8 | img_dir = 'C:\Users\geiger\Desktop\2010_03_09_drive_0019'; 9 | param.f = 645.2; 10 | param.cu = 635.9; 11 | param.cv = 194.1; 12 | param.height = 1.6; 13 | param.pitch = -0.08; 14 | param.max_features = 1000; % disable bucketing 15 | first_frame = 0; 16 | last_frame = 372; 17 | 18 | % init matcher + odometry objects 19 | visualOdometryMonoMex('init',param); 20 | reconstructionMex('init',param.f,param.cu,param.cv); 21 | 22 | % init transformation matrix array 23 | Tr_total{1} = eye(4); 24 | 25 | % create figure 26 | figure('Color',[1 1 1]),axes('Position',[0,0,1,1]); axis equal, axis off; 27 | 28 | % for all frames do 29 | replace = 0; 30 | for frame=first_frame:last_frame 31 | 32 | % 1-index 33 | k = frame-first_frame+1; 34 | 35 | % read current images 36 | I = imread([img_dir '/I1_' num2str(frame,'%06d') '.png']); 37 | 38 | % compute and accumulate egomotion 39 | Tr = visualOdometryMonoMex('process',I,replace); 40 | 41 | % from second frame on 42 | if k<=1 43 | continue; 44 | end 45 | 46 | % if something went wrong (e.g., small motion) 47 | if isempty(Tr) 48 | 49 | % copy pose 50 | Tr_total{k} = Tr_total{k-1}; 51 | 52 | % replace current frame in next iteration 53 | replace = 1; 54 | 55 | % otherwise 56 | else 57 | 58 | % update pose 59 | Tr_total{k} = Tr_total{k-1}*inv(Tr); 60 | 61 | % reconstruction (only elements above road plane) 62 | p_matched = visualOdometryMonoMex('get_matches'); 63 | i_matched = visualOdometryMonoMex('get_indices'); 64 | reconstructionMex('update',p_matched,i_matched,Tr,2,2,30,3); 65 | 66 | % cycle ring buffer in next iteration 67 | replace = 0; 68 | end 69 | 70 | % get reconstructed 3d points 71 | p = reconstructionMex('getpoints'); 72 | 73 | % update point cloud and draw trajectory 74 | cla; hold on; view(0,0); 75 | cla,plot3(p(1,:),p(2,:),p(3,:),'.b','MarkerSize',1),axis equal; 76 | for i=2:frame 77 | plot3([Tr_total{i-1}(1,4) Tr_total{i}(1,4)], ... 78 | [Tr_total{i-1}(2,4) Tr_total{i}(2,4)], ... 79 | [Tr_total{i-1}(3,4) Tr_total{i}(3,4)],'-xr','LineWidth',1); 80 | end 81 | pause(0.05); 82 | refresh; 83 | 84 | % output statistics 85 | num_matches = visualOdometryMonoMex('num_matches'); 86 | num_inliers = visualOdometryMonoMex('num_inliers'); 87 | disp(['Frame: ' num2str(frame) ... 88 | ', Matches: ' num2str(num_matches) ... 89 | ', Inliers: ' num2str(100*num_inliers/num_matches,'%.1f') ,' %']); 90 | end 91 | 92 | % release objects 93 | visualOdometryMex('close'); 94 | reconstructionMex('close'); 95 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/demo_viso_mono.m: -------------------------------------------------------------------------------- 1 | % demonstrates mono visual odometry on an image sequence 2 | disp('==========================='); 3 | clear all; close all; dbstop error; 4 | 5 | % parameter settings (for an example, please download 6 | % sequence '2010_03_09_drive_0019' from www.cvlibs.net) 7 | img_dir = '/home/geiger/5_Data/kitti/2011_stereo/2010_03_09_drive_0019'; 8 | %img_dir = 'C:\Users\geiger\Desktop\2010_03_09_drive_0019'; 9 | param.f = 645.2; 10 | param.cu = 635.9; 11 | param.cv = 194.1; 12 | param.height = 1.6; 13 | param.pitch = -0.08; 14 | first_frame = 0; 15 | last_frame = 372; 16 | 17 | % init visual odometry 18 | visualOdometryMonoMex('init',param); 19 | 20 | % init transformation matrix array 21 | Tr_total{1} = eye(4); 22 | 23 | % create figure 24 | figure('Color',[1 1 1]); 25 | ha1 = axes('Position',[0.05,0.7,0.9,0.25]); 26 | axis off; 27 | ha2 = axes('Position',[0.05,0.05,0.9,0.6]); 28 | set(gca,'XTick',-500:10:500); 29 | set(gca,'YTick',-500:10:500); 30 | axis equal, grid on, hold on; 31 | 32 | % for all frames do 33 | replace = 0; 34 | for frame=first_frame:last_frame 35 | 36 | % 1-based index 37 | k = frame-first_frame+1; 38 | 39 | % read current images 40 | I = imread([img_dir '/I1_' num2str(frame,'%06d') '.png']); 41 | 42 | % compute egomotion 43 | Tr = visualOdometryMonoMex('process',I,replace); 44 | 45 | % accumulate egomotion, starting with second frame 46 | if k>1 47 | 48 | % if motion estimate failed: set replace "current frame" to "yes" 49 | % this will cause the "last frame" in the ring buffer unchanged 50 | if isempty(Tr) 51 | replace = 1; 52 | Tr_total{k} = Tr_total{k-1}; 53 | 54 | % on success: update total motion (=pose) 55 | else 56 | replace = 0; 57 | Tr_total{k} = Tr_total{k-1}*inv(Tr); 58 | end 59 | end 60 | 61 | % update image 62 | axes(ha1); cla; 63 | imagesc(I); colormap(gray); 64 | axis off; 65 | 66 | % update trajectory 67 | axes(ha2); 68 | if k>1 69 | plot([Tr_total{k-1}(1,4) Tr_total{k}(1,4)], ... 70 | [Tr_total{k-1}(3,4) Tr_total{k}(3,4)],'-xb','LineWidth',1); 71 | end 72 | pause(0.05); refresh; 73 | 74 | % output statistics 75 | num_matches = visualOdometryMonoMex('num_matches'); 76 | num_inliers = visualOdometryMonoMex('num_inliers'); 77 | disp(['Frame: ' num2str(frame) ... 78 | ', Matches: ' num2str(num_matches) ... 79 | ', Inliers: ' num2str(100*num_inliers/num_matches,'%.1f') ,' %']); 80 | end 81 | 82 | % release visual odometry 83 | visualOdometryMonoMex('close'); 84 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/demo_viso_stereo.m: -------------------------------------------------------------------------------- 1 | % demonstrates stereo visual odometry on an image sequence 2 | disp('==========================='); 3 | clear all; close all; dbstop error; 4 | 5 | % parameter settings (for an example, please download 6 | % sequence '2010_03_09_drive_0019' from www.cvlibs.net) 7 | img_dir = '/home/geiger/5_Data/kitti/2011_stereo/2010_03_09_drive_0019'; 8 | %img_dir = 'C:\Users\geiger\Desktop\2010_03_09_drive_0019'; 9 | param.f = 645.2; 10 | param.cu = 635.9; 11 | param.cv = 194.1; 12 | param.base = 0.571; 13 | first_frame = 0; 14 | last_frame = 372; 15 | 16 | % init visual odometry 17 | visualOdometryStereoMex('init',param); 18 | 19 | % init transformation matrix array 20 | Tr_total{1} = eye(4); 21 | 22 | % create figure 23 | figure('Color',[1 1 1]); 24 | ha1 = axes('Position',[0.05,0.7,0.9,0.25]); 25 | axis off; 26 | ha2 = axes('Position',[0.05,0.05,0.9,0.6]); 27 | set(gca,'XTick',-500:10:500); 28 | set(gca,'YTick',-500:10:500); 29 | axis equal, grid on, hold on; 30 | 31 | % for all frames do 32 | for frame=first_frame:last_frame 33 | 34 | % 1-index 35 | k = frame-first_frame+1; 36 | 37 | % read current images 38 | I1 = imread([img_dir '/I1_' num2str(frame,'%06d') '.png']); 39 | I2 = imread([img_dir '/I2_' num2str(frame,'%06d') '.png']); 40 | 41 | % compute and accumulate egomotion 42 | Tr = visualOdometryStereoMex('process',I1,I2); 43 | if k>1 44 | Tr_total{k} = Tr_total{k-1}*inv(Tr); 45 | end 46 | 47 | % update image 48 | axes(ha1); cla; 49 | imagesc(I1); colormap(gray); 50 | axis off; 51 | 52 | % update trajectory 53 | axes(ha2); 54 | if k>1 55 | plot([Tr_total{k-1}(1,4) Tr_total{k}(1,4)], ... 56 | [Tr_total{k-1}(3,4) Tr_total{k}(3,4)],'-xb','LineWidth',1); 57 | end 58 | pause(0.05); refresh; 59 | 60 | % output statistics 61 | num_matches = visualOdometryStereoMex('num_matches'); 62 | num_inliers = visualOdometryStereoMex('num_inliers'); 63 | disp(['Frame: ' num2str(frame) ... 64 | ', Matches: ' num2str(num_matches) ... 65 | ', Inliers: ' num2str(100*num_inliers/num_matches,'%.1f') ,' %']); 66 | end 67 | 68 | % release visual odometry 69 | visualOdometryStereoMex('close'); 70 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/make.m: -------------------------------------------------------------------------------- 1 | dbclear all; 2 | 3 | % compile matlab wrappers 4 | disp('Building wrappers ...'); 5 | mex('matcherMex.cpp','../src/matcher.cpp','../src/filter.cpp','../src/triangle.cpp','../src/matrix.cpp','-I../src','CXXFLAGS=-msse3 -fPIC'); 6 | mex('visualOdometryStereoMex.cpp','../src/viso_stereo.cpp','../src/viso.cpp','../src/matcher.cpp','../src/filter.cpp','../src/triangle.cpp','../src/matrix.cpp','-I../src','CXXFLAGS=-msse3 -fPIC'); 7 | mex('visualOdometryMonoMex.cpp','../src/viso_mono.cpp','../src/viso.cpp','../src/matcher.cpp','../src/filter.cpp','../src/triangle.cpp','../src/matrix.cpp','-I../src','CXXFLAGS=-msse3 -fPIC'); 8 | mex('reconstructionMex.cpp','../src/reconstruction.cpp','../src/matrix.cpp','-I../src','CXXFLAGS=-msse3 -fPIC'); 9 | disp('...done!'); 10 | 11 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/matcherMex.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "mex.h" 23 | #include 24 | #include 25 | #include "matcher.h" 26 | 27 | using namespace std; 28 | 29 | static Matcher *M; 30 | 31 | template T* transpose(T* I,const int32_t* dims) { 32 | T* I_ = (T*)malloc(dims[0]*dims[1]*sizeof(T)); 33 | for (int32_t u=0; u(I1,dims1); 105 | int32_t dims1_[] = {dims1[1],dims1[0],dims1[1]}; 106 | 107 | // push back single image 108 | if (nrhs==1+1) { 109 | 110 | // compute features and put them to ring buffer 111 | M->pushBack(I1_,dims1_,!strcmp(command,"replace")); 112 | 113 | // push back stereo image pair 114 | } else { 115 | 116 | if (!mxIsUint8(prhs[2]) || mxGetNumberOfDimensions(prhs[2])!=2) 117 | mexErrMsgTxt("Input I2 (right image) must be a uint8_t matrix."); 118 | 119 | // get pointer to right image 120 | uint8_t* I2 = (uint8_t*)mxGetPr(prhs[2]); 121 | const int32_t *dims2 = mxGetDimensions(prhs[2]); 122 | 123 | // transpose 124 | uint8_t* I2_ = transpose(I2,dims2); 125 | int32_t dims2_[] = {dims2[1],dims2[0],dims2[1]}; 126 | 127 | // check image size 128 | if (dims1_[0]!=dims2_[0] || dims1_[1]!=dims2_[1]) 129 | mexErrMsgTxt("Input I1 and I2 must be images of same size."); 130 | 131 | // compute features and put them to ring buffer 132 | M->pushBack(I1_,I2_,dims1_,!strcmp(command,"replace")); 133 | 134 | // free temporary memory 135 | free(I2_); 136 | } 137 | 138 | // free temporary memory 139 | free(I1_); 140 | 141 | // match features 142 | } else if (!strcmp(command,"match")) { 143 | 144 | // check arguments 145 | if (nrhs!=1+1 && nrhs!=1) 146 | mexErrMsgTxt("1 input required (method)."); 147 | if (nlhs!=0) 148 | mexErrMsgTxt("No output required."); 149 | 150 | if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1) 151 | mexErrMsgTxt("Input method must be a double scalar."); 152 | 153 | // matching method: 0 = flow, 1 = stereo, 2 = quad matching 154 | int32_t method = (int32_t)*((double*)mxGetPr(prhs[1])); 155 | 156 | // do matching 157 | M->matchFeatures(method); 158 | 159 | // bucketing 160 | } else if (!strcmp(command,"bucketing")) { 161 | 162 | // check arguments 163 | if (nrhs!=1+3) 164 | mexErrMsgTxt("3 inputs required (max_features,bucket_width,bucket_height)."); 165 | if (nlhs!=0) 166 | mexErrMsgTxt("No output required."); 167 | 168 | // remove closeby features via bucketing 169 | if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1) 170 | mexErrMsgTxt("Input max_features must be a double scalar."); 171 | if (!mxIsDouble(prhs[2]) || mxGetM(prhs[2])*mxGetN(prhs[2])!=1) 172 | mexErrMsgTxt("Input bucket_width must be a double scalar."); 173 | if (!mxIsDouble(prhs[3]) || mxGetM(prhs[3])*mxGetN(prhs[3])!=1) 174 | mexErrMsgTxt("Input bucket_height must be a double scalar."); 175 | 176 | // get pointers to input data 177 | int32_t max_features = (int32_t)*((double*)mxGetPr(prhs[1])); 178 | float bucket_width = *((double*)mxGetPr(prhs[2])); 179 | float bucket_height = *((double*)mxGetPr(prhs[3])); 180 | 181 | // bucketing 182 | M->bucketFeatures(max_features,bucket_width,bucket_height); 183 | 184 | // get matches 185 | } else if (!strcmp(command,"get_matches")) { 186 | 187 | // check arguments 188 | if (nrhs!=1+1) 189 | mexErrMsgTxt("1 input required (method)."); 190 | if (nlhs!=1) 191 | mexErrMsgTxt("One output required (p_matched)."); 192 | 193 | if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1) 194 | mexErrMsgTxt("Input method must be a double scalar."); 195 | 196 | // matching method: 0 = flow, 1 = stereo, 2 = quad matching 197 | int32_t method = (int32_t)*((double*)mxGetPr(prhs[1])); 198 | 199 | // grab matches 200 | vector matches = M->getMatches(); 201 | 202 | // method: flow 203 | if (method==0) { 204 | 205 | // create output matrix with matches 206 | const int32_t p_matched_dims[] = {4,matches.size()}; 207 | plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL); 208 | double* p_matched_mex = (double*)mxGetPr(plhs[0]); 209 | 210 | // copy matches to mex array (convert indices: C++ => MATLAB) 211 | int32_t k=0; 212 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 213 | *(p_matched_mex+k++) = it->u1p+1.0; 214 | *(p_matched_mex+k++) = it->v1p+1.0; 215 | *(p_matched_mex+k++) = it->u1c+1.0; 216 | *(p_matched_mex+k++) = it->v1c+1.0; 217 | } 218 | 219 | // method: stereo 220 | } else if (method==1) { 221 | 222 | // create output matrix with matches 223 | const int32_t p_matched_dims[] = {4,matches.size()}; 224 | plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL); 225 | double* p_matched_mex = (double*)mxGetPr(plhs[0]); 226 | 227 | // copy matches to mex array (convert indices: C++ => MATLAB) 228 | int32_t k=0; 229 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 230 | *(p_matched_mex+k++) = it->u1c+1.0; 231 | *(p_matched_mex+k++) = it->v1c+1.0; 232 | *(p_matched_mex+k++) = it->u2c+1.0; 233 | *(p_matched_mex+k++) = it->v2c+1.0; 234 | } 235 | 236 | // method: quad matching 237 | } else { 238 | 239 | // create output matrix with matches 240 | const int32_t p_matched_dims[] = {8,matches.size()}; 241 | plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL); 242 | double* p_matched_mex = (double*)mxGetPr(plhs[0]); 243 | 244 | // copy matches to mex array (convert indices: C++ => MATLAB) 245 | int32_t k=0; 246 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 247 | *(p_matched_mex+k++) = it->u1p+1.0; 248 | *(p_matched_mex+k++) = it->v1p+1.0; 249 | *(p_matched_mex+k++) = it->u2p+1.0; 250 | *(p_matched_mex+k++) = it->v2p+1.0; 251 | *(p_matched_mex+k++) = it->u1c+1.0; 252 | *(p_matched_mex+k++) = it->v1c+1.0; 253 | *(p_matched_mex+k++) = it->u2c+1.0; 254 | *(p_matched_mex+k++) = it->v2c+1.0; 255 | } 256 | } 257 | 258 | // get matching indices 259 | } else if (!strcmp(command,"get_indices")) { 260 | 261 | // check arguments 262 | if (nrhs!=1+1) 263 | mexErrMsgTxt("1 input required (method)."); 264 | if (nlhs!=1) 265 | mexErrMsgTxt("One output required (i_matched)."); 266 | if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1) 267 | mexErrMsgTxt("Input method must be a double scalar."); 268 | 269 | // matching method: 0 = flow, 1 = stereo, 2 = quad matching 270 | int32_t method = (int32_t)*((double*)mxGetPr(prhs[1])); 271 | 272 | // grab matches 273 | vector matches = M->getMatches(); 274 | 275 | // method: flow 276 | if (method==0) { 277 | 278 | // create output matrix with matches 279 | const int32_t i_matched_dims[] = {2,matches.size()}; 280 | plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL); 281 | double* i_matched_mex = (double*)mxGetPr(plhs[0]); 282 | 283 | // copy matches to mex array 284 | int32_t k=0; 285 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 286 | *(i_matched_mex+k++) = it->i1p+1; 287 | *(i_matched_mex+k++) = it->i1c+1; 288 | } 289 | 290 | // method: stereo 291 | } else if (method==1) { 292 | 293 | // create output matrix with matches 294 | const int32_t i_matched_dims[] = {2,matches.size()}; 295 | plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL); 296 | double* i_matched_mex = (double*)mxGetPr(plhs[0]); 297 | 298 | // copy matches to mex array 299 | int32_t k=0; 300 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 301 | *(i_matched_mex+k++) = it->i1c+1; 302 | *(i_matched_mex+k++) = it->i2c+1; 303 | } 304 | 305 | // method: quad matching 306 | } else { 307 | 308 | // create output matrix with matches 309 | const int32_t i_matched_dims[] = {4,matches.size()}; 310 | plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL); 311 | double* i_matched_mex = (double*)mxGetPr(plhs[0]); 312 | 313 | // copy matches to mex array 314 | int32_t k=0; 315 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 316 | *(i_matched_mex+k++) = it->i1p+1; 317 | *(i_matched_mex+k++) = it->i2p+1; 318 | *(i_matched_mex+k++) = it->i1c+1; 319 | *(i_matched_mex+k++) = it->i2c+1; 320 | } 321 | } 322 | 323 | // unknown command 324 | } else { 325 | mexPrintf("Unknown command: %s\n",command); 326 | } 327 | } 328 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/plotMatch.m: -------------------------------------------------------------------------------- 1 | function plotMatch(I,p_matched,method,inliers) 2 | 3 | if nargin<3 4 | method = 2; 5 | end 6 | 7 | if nargin<4 8 | inliers = 1:size(p_matched,2); 9 | end 10 | 11 | p_matched = p_matched'; 12 | 13 | % show image 14 | cla,imshow(uint8(I)),hold on; 15 | 16 | % show matches 17 | if method==0 18 | 19 | for i=1:size(p_matched,1) 20 | col = [1 0 0]; 21 | if ~any(inliers==i) 22 | col = [0 0 1]; 23 | end 24 | line([p_matched(i,1) p_matched(i,3)], ... 25 | [p_matched(i,2) p_matched(i,4)], 'Color', col,'LineWidth',1); 26 | plot(p_matched(i,3),p_matched(i,4),'s', 'Color', col,'LineWidth',1,'MarkerSize',2); 27 | %text(p_matched(i,3),p_matched(i,4),sprintf('%d',i)); 28 | end 29 | 30 | elseif method==1 31 | 32 | disp = p_matched(:,1)-p_matched(:,3); 33 | disp = min(disp,50); 34 | max_disp = max(disp(inliers)); 35 | %max_disp = min(max_disp,50); 36 | 37 | for i=1:size(p_matched,1) 38 | c = abs(disp(i)/max_disp); 39 | col = [c 1-c 0]; 40 | if ~any(inliers==i) 41 | col = [0 0 1]; 42 | end 43 | line([p_matched(i,1) p_matched(i,1)], ... 44 | [p_matched(i,2) p_matched(i,4)], 'Color', col,'LineWidth',2); 45 | plot(p_matched(i,1),p_matched(i,2),'s', 'Color', col,'LineWidth',2,'MarkerSize',2); 46 | end 47 | 48 | else 49 | 50 | disp = p_matched(:,1)-p_matched(:,3); 51 | max_disp = max(disp(inliers)); 52 | %max_disp = 80; 53 | 54 | for i=1:size(p_matched,1) 55 | c = min(abs(disp(i)/(max_disp+0.1)),1); 56 | col = [c 1-c 0]; 57 | if ~any(inliers==i) 58 | col = [0 0 1]; 59 | end 60 | line([p_matched(i,1) p_matched(i,5)], ... 61 | [p_matched(i,2) p_matched(i,6)], 'Color', col,'LineWidth',2); 62 | plot(p_matched(i,5),p_matched(i,6),'s', 'Color', col,'LineWidth',2,'MarkerSize',3); 63 | end 64 | 65 | end 66 | 67 | 68 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/plotTrack.m: -------------------------------------------------------------------------------- 1 | function plotTrack(I,p_matched,i_matched) 2 | 3 | % show image 4 | figure;axes('Position',[0,0,1,1]); 5 | imshow(uint8(I)),hold on; 6 | 7 | % for all matches in the last frame do 8 | for i=1:size(i_matched{end},2) 9 | 10 | ind = i; 11 | 12 | % init track with latest matches 13 | p1 = p_matched{end}(3:4,ind); 14 | p2 = p_matched{end}(1:2,ind); 15 | p = [p1 p2]; 16 | 17 | % augment track into the past 18 | for j=length(p_matched)-1:-1:1 19 | 20 | % find backwards 21 | ind = find(i_matched{j}(2,:)==i_matched{j+1}(1,ind)); 22 | if isempty(ind) 23 | break; 24 | end 25 | 26 | p3 = p_matched{j}(1:2,ind); 27 | p = [p p3]; 28 | end 29 | 30 | track_length = length(p_matched)-j; 31 | if track_length<2 32 | continue; 33 | end 34 | 35 | % plot track (if long enough) 36 | v1 = p(:,1)-p(:,end); 37 | v2 = p(:,1)-p(:,end); 38 | if norm(v1) >= 8 39 | col = hsv2rgb([atan2(v2(2),v2(1))/(2*pi)+0.5 1 0.9]); 40 | plot(p(1,:),p(2,:),'-s','Color',col,'LineWidth',1,'MarkerSize',2); 41 | end 42 | end 43 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/reconstructionMex.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "mex.h" 23 | #include 24 | #include 25 | #include "reconstruction.h" 26 | 27 | using namespace std; 28 | 29 | static Reconstruction *R; 30 | 31 | void mexFunction (int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) { 32 | 33 | // read command 34 | char command[128]; 35 | mxGetString(prhs[0],command,128); 36 | 37 | // init 38 | if (!strcmp(command,"init")) { 39 | 40 | // check arguments 41 | if(nrhs!=1+3) 42 | mexErrMsgTxt("3 parameters required (f,cu,cv)."); 43 | if(!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1) 44 | mexErrMsgTxt("Input f must be a double scalar."); 45 | if(!mxIsDouble(prhs[2]) || mxGetM(prhs[2])*mxGetN(prhs[2])!=1) 46 | mexErrMsgTxt("Input cu must be a double scalar."); 47 | if(!mxIsDouble(prhs[3]) || mxGetM(prhs[3])*mxGetN(prhs[3])!=1) 48 | mexErrMsgTxt("Input cv must be a double scalar."); 49 | 50 | double f = *((double*)mxGetPr(prhs[1])); 51 | double cu = *((double*)mxGetPr(prhs[2])); 52 | double cv = *((double*)mxGetPr(prhs[3])); 53 | 54 | R = new Reconstruction(); 55 | R->setCalibration(f,cu,cv); 56 | 57 | // close 58 | } else if (!strcmp(command,"close")) { 59 | delete R; 60 | 61 | // update via observations 62 | } else if (!strcmp(command,"update")) { 63 | 64 | // check arguments (for parameter description see reconstruction.h) 65 | if(nrhs!=1+7) 66 | mexErrMsgTxt("7 inputs required (p_matched,i_matched,Tr,point_type,min_track_length,max_dist,min_angle)."); 67 | if(!mxIsDouble(prhs[1]) || mxGetM(prhs[1])!=4) 68 | mexErrMsgTxt("Input p_matched must be a 4xN double matrix."); 69 | if(!mxIsDouble(prhs[2]) || mxGetM(prhs[2])!=2) 70 | mexErrMsgTxt("Input i_matched must be a 2xN double matrix."); 71 | if(!mxIsDouble(prhs[3]) || mxGetM(prhs[3])!=4 || mxGetN(prhs[3])!=4) 72 | mexErrMsgTxt("Input Tr must be a 4x4 double matrix."); 73 | if(!mxIsDouble(prhs[4]) || mxGetM(prhs[4])*mxGetN(prhs[4])!=1) 74 | mexErrMsgTxt("Input point_type must be a double scalar."); 75 | if(!mxIsDouble(prhs[5]) || mxGetM(prhs[5])*mxGetN(prhs[5])!=1) 76 | mexErrMsgTxt("Input min_track_length must be a double scalar."); 77 | if(!mxIsDouble(prhs[6]) || mxGetM(prhs[6])*mxGetN(prhs[6])!=1) 78 | mexErrMsgTxt("Input max_dist must be a double scalar."); 79 | if(!mxIsDouble(prhs[7]) || mxGetM(prhs[7])*mxGetN(prhs[7])!=1) 80 | mexErrMsgTxt("Input min_angle must be a double scalar."); 81 | if(nlhs!=0) 82 | mexErrMsgTxt("No outputs required."); 83 | 84 | // get pointers 85 | double* p_matched_data = (double*)mxGetPr(prhs[1]); 86 | double* i_matched_data = (double*)mxGetPr(prhs[2]); 87 | double* Tr_data = (double*)mxGetPr(prhs[3]); 88 | int32_t point_type = (int32_t)*((double*)mxGetPr(prhs[4])); 89 | int32_t min_track_length = (int32_t)*((double*)mxGetPr(prhs[5])); 90 | double max_dist = *((double*)mxGetPr(prhs[6])); 91 | double min_angle = *((double*)mxGetPr(prhs[7])); 92 | int N = mxGetN(prhs[1]); 93 | 94 | // create match vector (convert indices: MATLAB => C++) 95 | vector p_matched; 96 | Matcher::p_match match; 97 | int32_t k1=0,k2=0; 98 | for (int i=0; iupdate(p_matched,Tr,point_type,min_track_length,max_dist,min_angle); 114 | 115 | // get points 116 | } else if (!strcmp(command,"getpoints")) { 117 | 118 | // check arguments 119 | if(nlhs!=1) 120 | mexErrMsgTxt("1 output required (points)."); 121 | 122 | // get inliers 123 | vector points = R->getPoints(); 124 | const int dims[] = {3,points.size()}; 125 | plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL); 126 | 127 | double *points_mex = (double*)mxGetPr(plhs[0]); 128 | int32_t k=0; 129 | for (int i=0; i 26 | #include 27 | 28 | using namespace std; 29 | 30 | static VisualOdometryMono *viso; 31 | 32 | template T* transpose(T* I,const int32_t* dims) { 33 | T* I_ = (T*)malloc(dims[0]*dims[1]*sizeof(T)); 34 | for (int32_t u=0; u(I,dims); 124 | int32_t dims_[] = {dims[1],dims[0],dims[1]}; 125 | 126 | // compute visual odometry, return motion estimate on success 127 | // (mapping from previous to current frame) 128 | if (viso->process(I_,dims_,replace)) { 129 | 130 | Matrix Tr_delta = ~(viso->getMotion()); 131 | const int tr_dims[] = {4,4}; 132 | plhs[0] = mxCreateNumericArray(2,tr_dims,mxDOUBLE_CLASS,mxREAL); 133 | Tr_delta.getData((double*)mxGetPr(plhs[0])); 134 | 135 | // return empty transformation on failure 136 | // (motion too small to estimate scale) 137 | } else { 138 | 139 | // return motion estimate (mapping from previous to current frame) 140 | const int tr_dims[] = {0,0}; 141 | plhs[0] = mxCreateNumericArray(2,tr_dims,mxDOUBLE_CLASS,mxREAL); 142 | } 143 | 144 | // release temporary memory 145 | free(I_); 146 | 147 | // query number of matches 148 | } else if (!strcmp(command,"num_matches")) { 149 | const int dims[] = {1,1}; 150 | plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL); 151 | *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfMatches(); 152 | 153 | // query number of inliers 154 | } else if (!strcmp(command,"num_inliers")) { 155 | const int dims[] = {1,1}; 156 | plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL); 157 | *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfInliers(); 158 | 159 | // get matches 160 | } else if (!strcmp(command,"get_matches")) { 161 | 162 | // check arguments 163 | if (nrhs!=1+0) 164 | mexErrMsgTxt("No input required."); 165 | if (nlhs!=1) 166 | mexErrMsgTxt("One output required (p_matched)."); 167 | 168 | // grab matches 169 | vector matches = viso->getMatches(); 170 | 171 | // create output matrix with matches 172 | const int32_t p_matched_dims[] = {4,matches.size()}; 173 | plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL); 174 | double* p_matched_mex = (double*)mxGetPr(plhs[0]); 175 | 176 | // copy matches to mex array (convert indices: C++ => MATLAB) 177 | int32_t k=0; 178 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 179 | *(p_matched_mex+k++) = it->u1p+1; 180 | *(p_matched_mex+k++) = it->v1p+1; 181 | *(p_matched_mex+k++) = it->u1c+1; 182 | *(p_matched_mex+k++) = it->v1c+1; 183 | } 184 | 185 | // get matching indices 186 | } else if (!strcmp(command,"get_indices")) { 187 | 188 | // check arguments 189 | if (nrhs!=1+0) 190 | mexErrMsgTxt("No input required."); 191 | if (nlhs!=1) 192 | mexErrMsgTxt("One output required (i_matched)."); 193 | 194 | // grab matches 195 | vector matches = viso->getMatches(); 196 | 197 | // create output matrix with matches 198 | const int32_t i_matched_dims[] = {2,matches.size()}; 199 | plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL); 200 | double* i_matched_mex = (double*)mxGetPr(plhs[0]); 201 | 202 | // copy matches to mex array 203 | int32_t k=0; 204 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 205 | *(i_matched_mex+k++) = it->i1p+1; 206 | *(i_matched_mex+k++) = it->i1c+1; 207 | } 208 | 209 | // close 210 | } else if (!strcmp(command,"close")) { 211 | delete viso; 212 | 213 | // unknown command 214 | } else { 215 | mexPrintf("Unknown command: %s\n",command); 216 | } 217 | } 218 | -------------------------------------------------------------------------------- /libviso2/libviso2/matlab/visualOdometryStereoMex.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "mex.h" 23 | #include "viso_stereo.h" 24 | 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | 30 | static VisualOdometryStereo *viso; 31 | 32 | template T* transpose(T* I,const int32_t* dims) { 33 | T* I_ = (T*)malloc(dims[0]*dims[1]*sizeof(T)); 34 | for (int32_t u=0; u(I1,dims); 128 | uint8_t* I2_ = transpose(I2,dims); 129 | int32_t dims_[] = {dims[1],dims[0],dims[1]}; 130 | 131 | // compute visual odometry, extrapolates linearly if matching failed 132 | viso->process(I1_,I2_,dims_,replace); 133 | 134 | // return motion estimate (mapping from previous to current frame) 135 | Matrix Tr_delta = ~(viso->getMotion()); 136 | const int tr_dims[] = {4,4}; 137 | plhs[0] = mxCreateNumericArray(2,tr_dims,mxDOUBLE_CLASS,mxREAL); 138 | Tr_delta.getData((double*)mxGetPr(plhs[0])); 139 | 140 | // release temporary memory 141 | free(I1_); 142 | free(I2_); 143 | 144 | // query number of matches 145 | } else if (!strcmp(command,"num_matches")) { 146 | const int dims[] = {1,1}; 147 | plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL); 148 | *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfMatches(); 149 | 150 | // get matches 151 | } else if (!strcmp(command,"get_matches")) { 152 | 153 | // check arguments 154 | if (nrhs!=1+0) 155 | mexErrMsgTxt("No input required."); 156 | if (nlhs!=1) 157 | mexErrMsgTxt("One output required (p_matched)."); 158 | 159 | // grab matches 160 | vector matches = viso->getMatches(); 161 | 162 | // create output matrix with matches 163 | const int32_t p_matched_dims[] = {8,matches.size()}; 164 | plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL); 165 | double* p_matched_mex = (double*)mxGetPr(plhs[0]); 166 | 167 | // copy matches to mex array (convert indices: C++ => MATLAB) 168 | int32_t k=0; 169 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 170 | *(p_matched_mex+k++) = it->u1p+1; 171 | *(p_matched_mex+k++) = it->v1p+1; 172 | *(p_matched_mex+k++) = it->u2p+1; 173 | *(p_matched_mex+k++) = it->v2p+1; 174 | *(p_matched_mex+k++) = it->u1c+1; 175 | *(p_matched_mex+k++) = it->v1c+1; 176 | *(p_matched_mex+k++) = it->u2c+1; 177 | *(p_matched_mex+k++) = it->v2c+1; 178 | } 179 | 180 | // query number of inliers 181 | } else if (!strcmp(command,"num_inliers")) { 182 | const int dims[] = {1,1}; 183 | plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL); 184 | *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfInliers(); 185 | 186 | // query number of inliers 187 | } else if (!strcmp(command,"get_inliers")) { 188 | 189 | vector inliers = viso->getInlierIndices(); 190 | const int dims[] = {1,inliers.size()}; 191 | plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL); 192 | double* inliers_mex = (double*)mxGetPr(plhs[0]); 193 | for (int32_t i=0; i matches = viso->getMatches(); 207 | 208 | // create output matrix with matches 209 | const int32_t i_matched_dims[] = {4,matches.size()}; 210 | plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL); 211 | double* i_matched_mex = (double*)mxGetPr(plhs[0]); 212 | 213 | // copy matches to mex array 214 | int32_t k=0; 215 | for (vector::iterator it=matches.begin(); it!=matches.end(); it++) { 216 | *(i_matched_mex+k++) = it->i1p+1; 217 | *(i_matched_mex+k++) = it->i2p+1; 218 | *(i_matched_mex+k++) = it->i1c+1; 219 | *(i_matched_mex+k++) = it->i2c+1; 220 | } 221 | 222 | // close 223 | } else if (!strcmp(command,"close")) { 224 | delete viso; 225 | 226 | // unknown command 227 | } else { 228 | mexPrintf("Unknown command: %s\n",command); 229 | } 230 | } 231 | -------------------------------------------------------------------------------- /libviso2/libviso2/readme.txt: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | # Copyright 2012. All rights reserved. # 3 | # Institute of Measurement and Control Systems # 4 | # Karlsruhe Institute of Technology, Germany # 5 | # # 6 | # This file is part of libviso2. # 7 | # Authors: Andreas Geiger # 8 | # Please send any bugreports to geiger@kit.edu # 9 | # # 10 | # libviso2 is free software; you can redistribute it and/or modify it under the # 11 | # terms of the GNU General Public License as published by the Free Software # 12 | # Foundation; either version 2 of the License, or any later version. # 13 | # # 14 | # libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY # 15 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A # 16 | # PARTICULAR PURPOSE. See the GNU General Public License for more details. # 17 | # # 18 | # You should have received a copy of the GNU General Public License along with # 19 | # libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin # 20 | # Street, Fifth Floor, Boston, MA 02110-1301, USA # 21 | #################################################################################### 22 | 23 | +++++++++++++++++++++++++++++++++++ 24 | + INTRODUCTION + 25 | +++++++++++++++++++++++++++++++++++ 26 | 27 | Libviso 2 (LIBrary for ViSual Odometry 2) is a cross-platfrom (Linux, Windows) C++ 28 | library with MATLAB wrappers for computing the 6 DOF motion of a moving stereo 29 | camera. Input is a sequence of rectified stereo images. Output is a 4x4 matrix 30 | which projects a point from the previous to the current camera coordinates. 31 | 32 | Version 2 now also supports visual odometry from monocular sequences, and the 33 | feature matching functions implement sparse stereo, scene flow and optical flow. 34 | For more details, please have a look at the corresponding publication. 35 | 36 | If you distribute a software that uses libviso, you have to distribute it under GPL 37 | with the source code. Another option is to contact us to purchase a commercial license. 38 | 39 | If you find this software useful or if you use this software for your research, 40 | we would be happy if you cite the following related publication: 41 | 42 | @INPROCEEDINGS{Geiger11, 43 | author = {Andreas Geiger and Julius Ziegler and Christoph Stiller}, 44 | title = {StereoScan: Dense 3d Reconstruction in Real-time}, 45 | booktitle = {IEEE Intelligent Vehicles Symposium}, 46 | year = {2011}, 47 | month = {June}, 48 | address = {Baden-Baden, Germany} 49 | } 50 | 51 | +++++++++++++++++++++++++++++++++++ 52 | + COMPILING MATLAB WRAPPERS + 53 | +++++++++++++++++++++++++++++++++++ 54 | 55 | Prerequisites needed for compiling libviso2: 56 | - Matlab (if you want to compile the matlab wrappers) 57 | - gcc or Visual Studio (if you want to include the code in your C++ project) 58 | 59 | If you want to use libviso directly from MATLAB you can easily do this by using 60 | the MATLAB wrappers provided. They also include some demo files for testing. 61 | 62 | In the MATLAB directory of libviso, simply run 'make.m' to generate the mex wrappers. 63 | (Run mex -setup before to choose your desired compiler) 64 | 65 | Now try to run the demo*.m files! 66 | For some of them you will need the Karlsruhe dataset from www.cvlibs.net. 67 | 68 | +++++++++++++++++++++++++++++++++++ 69 | + BUILDING A C++ LIBRARY + 70 | +++++++++++++++++++++++++++++++++++ 71 | 72 | Prerequisites needed for compiling and running the libviso2 demonstration 73 | tool via c++: 74 | 75 | - libpng (available at: http://www.libpng.org/pub/png/libpng.html) 76 | - libpng++ (available at: http://www.nongnu.org/pngpp/) 77 | - sequence 2010_03_09_drive_0019 (available at: http://www.cvlibs.net/) 78 | 79 | libpng and png++ are needed for reading the png input images. On a ubuntu 80 | box you can get them via apt: 81 | 82 | - sudo apt-get install libpng12-dev 83 | - sudo apt-get install libpng++-dev 84 | 85 | Linux: 86 | 87 | 1) Move to libviso2 root directory 88 | 2) Type 'cmake .' 89 | 3) Type 'make' 90 | 4) Run './viso2 path/to/sequence/2010_03_09_drive_0019' 91 | 92 | Windows: 93 | 94 | 1) Modify CMakefile according to your needs (libpng(++) must be found) 95 | 2) Start CMake GUI 96 | 3) Set directories to libviso2 root directory 97 | 4) Run configure, configure and generate 98 | 5) Open the resulting Visual Studio solution with Visual Studio 99 | 6) Switch to 'Release' mode and build all 100 | 9) Run 'viso2.exe path/to/sequence/2010_03_09_drive_0019' 101 | 102 | For more information on CMake, have a look at the CMake documentation. 103 | 104 | For more information on the usage of the library, have a look into the 105 | MATLAB wrappers, demo.cpp, viso.h, viso_stereo.h, viso_mono.h and 106 | matcher.h. 107 | 108 | Please send any feedback and bugreports to geiger@kit.edu 109 | Andreas Geiger 110 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | /* 23 | Documented C++ sample code of stereo visual odometry (modify to your needs) 24 | To run this demonstration, download the Karlsruhe dataset sequence 25 | '2010_03_09_drive_0019' from: www.cvlibs.net! 26 | Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019 27 | */ 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | using namespace std; 38 | 39 | int main (int argc, char** argv) { 40 | 41 | // we need the path name to 2010_03_09_drive_0019 as input argument 42 | if (argc<2) { 43 | cerr << "Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019" << endl; 44 | return 1; 45 | } 46 | 47 | // sequence directory 48 | string dir = argv[1]; 49 | 50 | // set most important visual odometry parameters 51 | // for a full parameter list, look at: viso_stereo.h 52 | VisualOdometryStereo::parameters param; 53 | 54 | // calibration parameters for sequence 2010_03_09_drive_0019 55 | param.calib.f = 645.24; // focal length in pixels 56 | param.calib.cu = 635.96; // principal point (u-coordinate) in pixels 57 | param.calib.cv = 194.13; // principal point (v-coordinate) in pixels 58 | param.base = 0.5707; // baseline in meters 59 | 60 | // init visual odometry 61 | VisualOdometryStereo viso(param); 62 | 63 | // current pose (this matrix transforms a point from the current 64 | // frame's camera coordinates to the first frame's camera coordinates) 65 | Matrix pose = Matrix::eye(4); 66 | 67 | // loop through all frames i=0:372 68 | for (int32_t i=0; i<373; i++) { 69 | 70 | // input file names 71 | char base_name[256]; sprintf(base_name,"%06d.png",i); 72 | string left_img_file_name = dir + "/I1_" + base_name; 73 | string right_img_file_name = dir + "/I2_" + base_name; 74 | 75 | // catch image read/write errors here 76 | try { 77 | 78 | // load left and right input image 79 | png::image< png::gray_pixel > left_img(left_img_file_name); 80 | png::image< png::gray_pixel > right_img(right_img_file_name); 81 | 82 | // image dimensions 83 | int32_t width = left_img.get_width(); 84 | int32_t height = left_img.get_height(); 85 | 86 | // convert input images to uint8_t buffer 87 | uint8_t* left_img_data = (uint8_t*)malloc(width*height*sizeof(uint8_t)); 88 | uint8_t* right_img_data = (uint8_t*)malloc(width*height*sizeof(uint8_t)); 89 | int32_t k=0; 90 | for (int32_t v=0; v 29 | #include 30 | #endif 31 | 32 | // define fixed-width datatypes for Visual Studio projects 33 | #ifndef _MSC_VER 34 | #include 35 | #else 36 | typedef __int8 int8_t; 37 | typedef __int16 int16_t; 38 | typedef __int32 int32_t; 39 | typedef __int64 int64_t; 40 | typedef unsigned __int8 uint8_t; 41 | typedef unsigned __int16 uint16_t; 42 | typedef unsigned __int32 uint32_t; 43 | typedef unsigned __int64 uint64_t; 44 | #endif 45 | 46 | // fast filters: implements 3x3 and 5x5 sobel filters and 47 | // 5x5 blob and corner filters based on SSE2/3 instructions 48 | namespace filter { 49 | 50 | // private namespace, public user functions at the bottom of this file 51 | namespace detail { 52 | void integral_image( const uint8_t* in, int32_t* out, int w, int h ); 53 | void unpack_8bit_to_16bit( const __m128i a, __m128i& b0, __m128i& b1 ); 54 | void pack_16bit_to_8bit_saturate( const __m128i a0, const __m128i a1, __m128i& b ); 55 | 56 | // convolve image with a (1,4,6,4,1) row vector. Result is accumulated into output. 57 | // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255]. 58 | void convolve_14641_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ); 59 | 60 | // convolve image with a (1,2,0,-2,-1) row vector. Result is accumulated into output. 61 | // This one works on 16bit input and 8bit output. 62 | // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255]. 63 | void convolve_12021_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ); 64 | 65 | // convolve image with a (1,2,1) row vector. Result is accumulated into output. 66 | // This one works on 16bit input and 8bit output. 67 | // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255]. 68 | void convolve_121_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ); 69 | 70 | // convolve image with a (1,0,-1) row vector. Result is accumulated into output. 71 | // This one works on 16bit input and 8bit output. 72 | // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255]. 73 | void convolve_101_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ); 74 | 75 | void convolve_cols_5x5( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ); 76 | 77 | void convolve_col_p1p1p0m1m1_5x5( const unsigned char* in, int16_t* out, int w, int h ); 78 | 79 | void convolve_row_p1p1p0m1m1_5x5( const int16_t* in, int16_t* out, int w, int h ); 80 | 81 | void convolve_cols_3x3( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ); 82 | } 83 | 84 | void sobel3x3( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ); 85 | 86 | void sobel5x5( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ); 87 | 88 | // -1 -1 0 1 1 89 | // -1 -1 0 1 1 90 | // 0 0 0 0 0 91 | // 1 1 0 -1 -1 92 | // 1 1 0 -1 -1 93 | void checkerboard5x5( const uint8_t* in, int16_t* out, int w, int h ); 94 | 95 | // -1 -1 -1 -1 -1 96 | // -1 1 1 1 -1 97 | // -1 1 8 1 -1 98 | // -1 1 1 1 -1 99 | // -1 -1 -1 -1 -1 100 | void blob5x5( const uint8_t* in, int16_t* out, int w, int h ); 101 | }; 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/matcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef __MATCHER_H__ 23 | #define __MATCHER_H__ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #if defined(__ARM_NEON__) 32 | #include "sse_to_neon.hpp" 33 | #else 34 | #include 35 | #endif 36 | #include 37 | #include 38 | 39 | #include "matrix.h" 40 | 41 | class Matcher { 42 | 43 | public: 44 | 45 | // parameter settings 46 | struct parameters { 47 | 48 | int32_t nms_n; // non-max-suppression: min. distance between maxima (in pixels) 49 | int32_t nms_tau; // non-max-suppression: interest point peakiness threshold 50 | int32_t match_binsize; // matching bin width/height (affects efficiency only) 51 | int32_t match_radius; // matching radius (du/dv in pixels) 52 | int32_t match_disp_tolerance; // dv tolerance for stereo matches (in pixels) 53 | int32_t outlier_disp_tolerance; // outlier removal: disparity tolerance (in pixels) 54 | int32_t outlier_flow_tolerance; // outlier removal: flow tolerance (in pixels) 55 | int32_t multi_stage; // 0=disabled,1=multistage matching (denser and faster) 56 | int32_t half_resolution; // 0=disabled,1=match at half resolution, refine at full resolution 57 | int32_t refinement; // refinement (0=none,1=pixel,2=subpixel) 58 | double f,cu,cv,base; // calibration (only for match prediction) 59 | 60 | // default settings 61 | parameters () { 62 | nms_n = 3; 63 | nms_tau = 50; 64 | match_binsize = 50; 65 | match_radius = 200; 66 | match_disp_tolerance = 2; 67 | outlier_disp_tolerance = 5; 68 | outlier_flow_tolerance = 5; 69 | multi_stage = 1; 70 | half_resolution = 1; 71 | refinement = 1; 72 | } 73 | }; 74 | 75 | // constructor (with default parameters) 76 | Matcher(parameters param); 77 | 78 | // deconstructor 79 | ~Matcher(); 80 | 81 | // intrinsics 82 | void setIntrinsics(double f,double cu,double cv,double base) { 83 | param.f = f; 84 | param.cu = cu; 85 | param.cv = cv; 86 | param.base = base; 87 | } 88 | 89 | // structure for storing matches 90 | struct p_match { 91 | float u1p,v1p; // u,v-coordinates in previous left image 92 | int32_t i1p; // feature index (for tracking) 93 | float u2p,v2p; // u,v-coordinates in previous right image 94 | int32_t i2p; // feature index (for tracking) 95 | float u1c,v1c; // u,v-coordinates in current left image 96 | int32_t i1c; // feature index (for tracking) 97 | float u2c,v2c; // u,v-coordinates in current right image 98 | int32_t i2c; // feature index (for tracking) 99 | p_match(){} 100 | p_match(float u1p,float v1p,int32_t i1p,float u2p,float v2p,int32_t i2p, 101 | float u1c,float v1c,int32_t i1c,float u2c,float v2c,int32_t i2c): 102 | u1p(u1p),v1p(v1p),i1p(i1p),u2p(u2p),v2p(v2p),i2p(i2p), 103 | u1c(u1c),v1c(v1c),i1c(i1c),u2c(u2c),v2c(v2c),i2c(i2c) {} 104 | }; 105 | 106 | // structure for storing 3d matches 107 | struct p_match_3d 108 | { 109 | float xp, yp, zp; 110 | float xc, yc, zc; 111 | p_match_3d() {} 112 | p_match_3d(float xp, float yp, float zp, float xc, float yc, float zc) : 113 | xp(xp), yp(yp), zp(zp), 114 | xc(xc), yc(yc), zc(zc) {} 115 | }; 116 | 117 | // computes features from left/right images and pushes them back to a ringbuffer, 118 | // which interally stores the features of the current and previous image pair 119 | // use this function for stereo or quad matching 120 | // input: I1,I2 .......... pointers to left and right image (row-aligned), range [0..255] 121 | // dims[0,1] ...... image width and height (both images must be rectified and of same size) 122 | // dims[2] ........ bytes per line (often equals width) 123 | // replace ........ if this flag is set, the current image is overwritten with 124 | // the input images, otherwise the current image is first copied 125 | // to the previous image (ring buffer functionality, descriptors need 126 | // to be computed only once) 127 | void pushBack (uint8_t *I1,uint8_t* I2,int32_t* dims,const bool replace); 128 | 129 | // computes features from a single image and pushes it back to a ringbuffer, 130 | // which interally stores the features of the current and previous image pair 131 | // use this function for flow computation 132 | // parameter description see above 133 | void pushBack (uint8_t *I1,int32_t* dims,const bool replace) { pushBack(I1,0,dims,replace); } 134 | 135 | // match features currently stored in ring buffer (current and previous frame) 136 | // input: method ... 0 = flow, 1 = stereo, 2 = quad matching 137 | // Tr_delta: uses motion from previous frame to better search for 138 | // matches, if specified 139 | void matchFeatures(int32_t method, Matrix *Tr_delta = 0); 140 | 141 | // feature bucketing: keeps only max_features per bucket, where the domain 142 | // is split into buckets of size (bucket_width,bucket_height) 143 | void bucketFeatures(int32_t max_features,float bucket_width,float bucket_height); 144 | 145 | // return vector with matched feature points and indices 146 | std::vector getMatches() { return p_matched_2; } 147 | 148 | // given a vector of inliers computes gain factor between the current and 149 | // the previous frame. this function is useful if you want to reconstruct 3d 150 | // and you want to cancel the change of (unknown) camera gain. 151 | float getGain (std::vector inliers); 152 | 153 | private: 154 | 155 | // structure for storing interest points 156 | struct maximum { 157 | int32_t u; // u-coordinate 158 | int32_t v; // v-coordinate 159 | int32_t val; // value 160 | int32_t c; // class 161 | int32_t d1,d2,d3,d4,d5,d6,d7,d8; // descriptor 162 | maximum() {} 163 | maximum(int32_t u,int32_t v,int32_t val,int32_t c):u(u),v(v),val(val),c(c) {} 164 | }; 165 | 166 | // u/v ranges for matching stage 0-3 167 | struct range { 168 | float u_min[4]; 169 | float u_max[4]; 170 | float v_min[4]; 171 | float v_max[4]; 172 | }; 173 | 174 | struct delta { 175 | float val[8]; 176 | delta () {} 177 | delta (float v) { 178 | for (int32_t i=0; i<8; i++) 179 | val[i] = v; 180 | } 181 | }; 182 | 183 | // computes the address offset for coordinates u,v of an image of given width 184 | inline int32_t getAddressOffsetImage (const int32_t& u,const int32_t& v,const int32_t& width) { 185 | return v*width+u; 186 | } 187 | 188 | // Alexander Neubeck and Luc Van Gool: Efficient Non-Maximum Suppression, ICPR'06, algorithm 4 189 | void nonMaximumSuppression (int16_t* I_f1,int16_t* I_f2,const int32_t* dims,std::vector &maxima,int32_t nms_n); 190 | 191 | // descriptor functions 192 | inline uint8_t saturate(int16_t in); 193 | void filterImageAll (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,int16_t* I_f1,int16_t* I_f2,const int* dims); 194 | void filterImageSobel (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,const int* dims); 195 | inline void computeDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr); 196 | inline void computeSmallDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr); 197 | void computeDescriptors (uint8_t* I_du,uint8_t* I_dv,const int32_t bpl,std::vector &maxima); 198 | 199 | void getHalfResolutionDimensions(const int32_t *dims,int32_t *dims_half); 200 | uint8_t* createHalfResolutionImage(uint8_t *I,const int32_t* dims); 201 | 202 | // compute sparse set of features from image 203 | // inputs: I ........ image 204 | // dims ..... image dimensions [width,height] 205 | // n ........ non-max neighborhood 206 | // tau ...... non-max threshold 207 | // outputs: max ...... vector with maxima [u,v,value,class,descriptor (128 bits)] 208 | // I_du ..... gradient in horizontal direction 209 | // I_dv ..... gradient in vertical direction 210 | // WARNING: max,I_du,I_dv has to be freed by yourself! 211 | void computeFeatures (uint8_t *I,const int32_t* dims,int32_t* &max1,int32_t &num1,int32_t* &max2,int32_t &num2,uint8_t* &I_du,uint8_t* &I_dv,uint8_t* &I_du_full,uint8_t* &I_dv_full); 212 | 213 | // matching functions 214 | void computePriorStatistics (std::vector &p_matched,int32_t method); 215 | void createIndexVector (int32_t* m,int32_t n,std::vector *k,const int32_t &u_bin_num,const int32_t &v_bin_num); 216 | inline void findMatch (int32_t* m1,const int32_t &i1,int32_t* m2,const int32_t &step_size, 217 | std::vector *k2,const int32_t &u_bin_num,const int32_t &v_bin_num,const int32_t &stat_bin, 218 | int32_t& min_ind,int32_t stage,bool flow,bool use_prior,double u_=-1,double v_=-1); 219 | void matching (int32_t *m1p,int32_t *m2p,int32_t *m1c,int32_t *m2c, 220 | int32_t n1p,int32_t n2p,int32_t n1c,int32_t n2c, 221 | std::vector &p_matched,int32_t method,bool use_prior,Matrix *Tr_delta = 0); 222 | 223 | // outlier removal 224 | void removeOutliers (std::vector &p_matched,int32_t method); 225 | 226 | // parabolic fitting 227 | bool parabolicFitting(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1, 228 | const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2, 229 | const float &u1,const float &v1, 230 | float &u2,float &v2, 231 | Matrix At,Matrix AtA, 232 | uint8_t* desc_buffer); 233 | void relocateMinimum(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1, 234 | const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2, 235 | const float &u1,const float &v1, 236 | float &u2,float &v2, 237 | uint8_t* desc_buffer); 238 | void refinement (std::vector &p_matched,int32_t method); 239 | 240 | // mean for gain computation 241 | inline float mean(const uint8_t* I,const int32_t &bpl,const int32_t &u_min,const int32_t &u_max,const int32_t &v_min,const int32_t &v_max); 242 | 243 | // parameters 244 | parameters param; 245 | int32_t margin; 246 | 247 | int32_t *m1p1,*m2p1,*m1c1,*m2c1; 248 | int32_t *m1p2,*m2p2,*m1c2,*m2c2; 249 | int32_t n1p1,n2p1,n1c1,n2c1; 250 | int32_t n1p2,n2p2,n1c2,n2c2; 251 | uint8_t *I1p,*I2p,*I1c,*I2c; 252 | uint8_t *I1p_du,*I2p_du,*I1c_du,*I2c_du; 253 | uint8_t *I1p_dv,*I2p_dv,*I1c_dv,*I2c_dv; 254 | uint8_t *I1p_du_full,*I2p_du_full,*I1c_du_full,*I2c_du_full; // only needed for 255 | uint8_t *I1p_dv_full,*I2p_dv_full,*I1c_dv_full,*I2c_dv_full; // half-res matching 256 | int32_t dims_p[3],dims_c[3]; 257 | 258 | std::vector p_matched_1; 259 | std::vector p_matched_2; 260 | std::vector ranges; 261 | }; 262 | 263 | #endif 264 | 265 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef MATRIX_H 23 | #define MATRIX_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #ifndef _MSC_VER 32 | #include 33 | #else 34 | typedef __int8 int8_t; 35 | typedef __int16 int16_t; 36 | typedef __int32 int32_t; 37 | typedef __int64 int64_t; 38 | typedef unsigned __int8 uint8_t; 39 | typedef unsigned __int16 uint16_t; 40 | typedef unsigned __int32 uint32_t; 41 | typedef unsigned __int64 uint64_t; 42 | #endif 43 | 44 | #define endll endl << endl // double end line definition 45 | 46 | typedef double FLOAT; // double precision 47 | //typedef float FLOAT; // single precision 48 | 49 | class Matrix { 50 | 51 | public: 52 | 53 | // constructor / deconstructor 54 | Matrix (); // init empty 0x0 matrix 55 | Matrix (const int32_t m,const int32_t n); // init empty mxn matrix 56 | Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val' 57 | Matrix (const Matrix &M); // creates deepcopy of M 58 | ~Matrix (); 59 | 60 | // assignment operator, copies contents of M 61 | Matrix& operator= (const Matrix &M); 62 | 63 | // copies submatrix of M into array 'val', default values copy whole row/column/matrix 64 | void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 65 | 66 | // set or get submatrices of current matrix 67 | Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1); 68 | void setMat(const Matrix &M,const int32_t i,const int32_t j); 69 | 70 | // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix 71 | void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 72 | 73 | // set (part of) diagonal to scalar, -1 as end replaces whole diagonal 74 | void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1); 75 | 76 | // clear matrix 77 | void zero(); 78 | 79 | // extract columns with given index 80 | Matrix extractCols (std::vector idx); 81 | 82 | // create identity matrix 83 | static Matrix eye (const int32_t m); 84 | void eye (); 85 | 86 | // create diagonal matrix with nx1 or 1xn matrix M as elements 87 | static Matrix diag(const Matrix &M); 88 | 89 | // returns the m-by-n matrix whose elements are taken column-wise from M 90 | static Matrix reshape(const Matrix &M,int32_t m,int32_t n); 91 | 92 | // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix) 93 | static Matrix rotMatX(const FLOAT &angle); 94 | static Matrix rotMatY(const FLOAT &angle); 95 | static Matrix rotMatZ(const FLOAT &angle); 96 | 97 | // simple arithmetic operations 98 | Matrix operator+ (const Matrix &M); // add matrix 99 | Matrix operator- (const Matrix &M); // subtract matrix 100 | Matrix operator* (const Matrix &M); // multiply with matrix 101 | Matrix operator* (const FLOAT &s); // multiply with scalar 102 | Matrix operator/ (const Matrix &M); // divide elementwise by matrix (or vector) 103 | Matrix operator/ (const FLOAT &s); // divide by scalar 104 | Matrix operator- (); // negative matrix 105 | Matrix operator~ (); // transpose 106 | FLOAT l2norm (); // euclidean norm (vectors) / frobenius norm (matrices) 107 | FLOAT mean (); // mean of all elements in matrix 108 | 109 | // complex arithmetic operations 110 | static Matrix cross (const Matrix &a, const Matrix &b); // cross product of two vectors 111 | static Matrix inv (const Matrix &M); // invert matrix M 112 | bool inv (); // invert this matrix 113 | FLOAT det (); // returns determinant of matrix 114 | bool solve (const Matrix &M,FLOAT eps=1e-20); // solve linear system M*x=B, replaces *this and M 115 | bool lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20); // replace *this by lower upper decomposition 116 | void svd(Matrix &U,Matrix &W,Matrix &V); // singular value decomposition *this = U*diag(W)*V^T 117 | 118 | // print matrix to stream 119 | friend std::ostream& operator<< (std::ostream& out,const Matrix& M); 120 | 121 | // direct data access 122 | FLOAT **val; 123 | int32_t m,n; 124 | 125 | private: 126 | 127 | void allocateMemory (const int32_t m_,const int32_t n_); 128 | void releaseMemory (); 129 | inline FLOAT pythag(FLOAT a,FLOAT b); 130 | 131 | }; 132 | 133 | #endif // MATRIX_H 134 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/reconstruction.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "reconstruction.h" 23 | #include 24 | 25 | using namespace std; 26 | 27 | Reconstruction::Reconstruction () { 28 | K = Matrix::eye(3); 29 | } 30 | 31 | Reconstruction::~Reconstruction () { 32 | } 33 | 34 | void Reconstruction::setCalibration (FLOAT f,FLOAT cu,FLOAT cv) { 35 | FLOAT K_data[9] = {f,0,cu,0,f,cv,0,0,1}; 36 | K = Matrix(3,3,K_data); 37 | FLOAT cam_pitch = -0.08; 38 | FLOAT cam_height = 1.6; 39 | Tr_cam_road = Matrix(4,4); 40 | Tr_cam_road.val[0][0] = 1; 41 | Tr_cam_road.val[1][1] = +cos(cam_pitch); 42 | Tr_cam_road.val[1][2] = -sin(cam_pitch); 43 | Tr_cam_road.val[2][1] = +sin(cam_pitch); 44 | Tr_cam_road.val[2][2] = +cos(cam_pitch); 45 | Tr_cam_road.val[0][3] = 0; 46 | Tr_cam_road.val[1][3] = -cam_height; 47 | Tr_cam_road.val[2][3] = 0; 48 | } 49 | 50 | void Reconstruction::update (vector p_matched,Matrix Tr,int32_t point_type,int32_t min_track_length,double max_dist,double min_angle) { 51 | 52 | // update transformation vector 53 | Matrix Tr_total_curr; 54 | if (Tr_total.size()==0) Tr_total_curr = Matrix::inv(Tr); 55 | else Tr_total_curr = Tr_total.back()*Matrix::inv(Tr); 56 | Tr_total.push_back(Tr_total_curr); 57 | Tr_inv_total.push_back(Matrix::inv(Tr_total_curr)); 58 | 59 | // update projection vector 60 | Matrix P_total_curr = K*Matrix::inv(Tr_total_curr).getMat(0,0,2,3); 61 | P_total.push_back(P_total_curr); 62 | 63 | // current frame 64 | int32_t current_frame = Tr_total.size(); 65 | 66 | // create index vector 67 | int32_t track_idx_max = 0; 68 | for (vector::iterator m=p_matched.begin(); m!=p_matched.end(); m++) 69 | if (m->i1p > track_idx_max) 70 | track_idx_max = m->i1p; 71 | for (vector::iterator t=tracks.begin(); t!=tracks.end(); t++) 72 | if (t->last_idx > track_idx_max) 73 | track_idx_max = t->last_idx; 74 | int32_t *track_idx = new int32_t[track_idx_max+1]; 75 | for (int32_t i=0; i<=track_idx_max; i++) 76 | track_idx[i] = -1; 77 | for (int32_t i=0; i::iterator m=p_matched.begin(); m!=p_matched.end(); m++) { 82 | 83 | // track index (-1 = no existing track) 84 | int32_t idx = track_idx[m->i1p]; 85 | 86 | // add to existing track 87 | if (idx>=0 && tracks[idx].last_frame==current_frame-1) { 88 | 89 | tracks[idx].pixels.push_back(point2d(m->u1c,m->v1c)); 90 | tracks[idx].last_frame = current_frame; 91 | tracks[idx].last_idx = m->i1c; 92 | 93 | // create new track 94 | } else { 95 | track t; 96 | t.pixels.push_back(point2d(m->u1p,m->v1p)); 97 | t.pixels.push_back(point2d(m->u1c,m->v1c)); 98 | t.first_frame = current_frame-1; 99 | t.last_frame = current_frame; 100 | t.last_idx = m->i1c; 101 | tracks.push_back(t); 102 | } 103 | } 104 | 105 | // copy tracks 106 | vector tracks_copy = tracks; 107 | tracks.clear(); 108 | 109 | // devise tracks into active or reconstruct 3d points 110 | for (vector::iterator t=tracks_copy.begin(); t!=tracks_copy.end(); t++) { 111 | 112 | // track has been extended 113 | if (t->last_frame==current_frame) { 114 | 115 | // push back to active tracks 116 | tracks.push_back(*t); 117 | 118 | // track lost 119 | } else { 120 | 121 | // add to 3d reconstruction 122 | if (t->pixels.size()>=min_track_length) { 123 | 124 | // 3d point 125 | point3d p; 126 | 127 | // try to init point from first and last track frame 128 | if (initPoint(*t,p)) { 129 | if (pointType(*t,p)>=point_type) 130 | if (refinePoint(*t,p)) 131 | if(pointDistance(*t,p)min_angle) 132 | points.push_back(p); 133 | } 134 | } 135 | } 136 | } 137 | 138 | //cout << "P: " << points.size() << endl; 139 | //testJacobian(); 140 | 141 | delete track_idx; 142 | } 143 | 144 | bool Reconstruction::initPoint(const track &t,point3d &p) { 145 | 146 | // projection matrices 147 | Matrix P1 = P_total[t.first_frame]; 148 | Matrix P2 = P_total[t.last_frame]; 149 | 150 | // observations 151 | point2d p1 = t.pixels.front(); 152 | point2d p2 = t.pixels.back(); 153 | 154 | // triangulation via orthogonal regression 155 | Matrix J(4,4); 156 | Matrix U,S,V; 157 | for (int32_t j=0; j<4; j++) { 158 | J.val[0][j] = P1.val[2][j]*p1.u - P1.val[0][j]; 159 | J.val[1][j] = P1.val[2][j]*p1.v - P1.val[1][j]; 160 | J.val[2][j] = P2.val[2][j]*p2.u - P2.val[0][j]; 161 | J.val[3][j] = P2.val[2][j]*p2.v - P2.val[1][j]; 162 | } 163 | J.svd(U,S,V); 164 | 165 | // return false if this point is at infinity 166 | float w = V.val[3][3]; 167 | if (fabs(w)<1e-10) 168 | return false; 169 | 170 | // return 3d point 171 | p = point3d(V.val[0][3]/w,V.val[1][3]/w,V.val[2][3]/w); 172 | return true; 173 | } 174 | 175 | bool Reconstruction::refinePoint(const track &t,point3d &p) { 176 | 177 | int32_t num_frames = t.pixels.size(); 178 | J = new FLOAT[6*num_frames]; 179 | p_observe = new FLOAT[2*num_frames]; 180 | p_predict = new FLOAT[2*num_frames]; 181 | 182 | int32_t iter=0; 183 | Reconstruction::result result = UPDATED; 184 | while (result==UPDATED) { 185 | result = updatePoint(t,p,1,1e-5); 186 | if (iter++ > 20 || result==CONVERGED) 187 | break; 188 | } 189 | 190 | delete J; 191 | delete p_observe; 192 | delete p_predict; 193 | 194 | if (result==CONVERGED) 195 | return true; 196 | else 197 | return false; 198 | } 199 | 200 | double Reconstruction::pointDistance(const track &t,point3d &p) { 201 | int32_t mid_frame = (t.first_frame+t.last_frame)/2; 202 | double dx = Tr_total[mid_frame].val[0][3]-p.x; 203 | double dy = Tr_total[mid_frame].val[1][3]-p.y; 204 | double dz = Tr_total[mid_frame].val[2][3]-p.z; 205 | return sqrt(dx*dx+dy*dy+dz*dz); 206 | } 207 | 208 | double Reconstruction::rayAngle(const track &t,point3d &p) { 209 | Matrix c1 = Tr_total[t.first_frame].getMat(0,3,2,3); 210 | Matrix c2 = Tr_total[t.last_frame].getMat(0,3,2,3); 211 | Matrix pt(3,1); 212 | pt.val[0][0] = p.x; 213 | pt.val[1][0] = p.y; 214 | pt.val[2][0] = p.z; 215 | Matrix v1 = c1-pt; 216 | Matrix v2 = c2-pt; 217 | FLOAT n1 = v1.l2norm(); 218 | FLOAT n2 = v2.l2norm(); 219 | if (n1<1e-10 || n2<1e-10) 220 | return 1000; 221 | v1 = v1/n1; 222 | v2 = v2/n2; 223 | return acos(fabs((~v1*v2).val[0][0]))*180.0/M_PI; 224 | } 225 | 226 | int32_t Reconstruction::pointType(const track &t,point3d &p) { 227 | 228 | // project point to first and last camera coordinates 229 | Matrix x(4,1); 230 | x.val[0][0] = p.x; 231 | x.val[1][0] = p.y; 232 | x.val[2][0] = p.z; 233 | x.val[3][0] = 1; 234 | Matrix x1c = Tr_inv_total[t.first_frame]*x; 235 | Matrix x2c = Tr_inv_total[t.last_frame]*x; 236 | Matrix x2r = Tr_cam_road*x2c; 237 | 238 | // point not visible 239 | if (x1c.val[2][0]<=1 || x2c.val[2][0]<=1) 240 | return -1; 241 | 242 | // below road 243 | if (x2r.val[1][0]>0.5) 244 | return 0; 245 | 246 | // road 247 | if (x2r.val[1][0]>-1) 248 | return 1; 249 | 250 | // obstacle 251 | return 2; 252 | } 253 | 254 | Reconstruction::result Reconstruction::updatePoint(const track &t,point3d &p,const FLOAT &step_size,const FLOAT &eps) { 255 | 256 | // number of frames 257 | int32_t num_frames = t.pixels.size(); 258 | 259 | // extract observations 260 | computeObservations(t.pixels); 261 | 262 | // compute predictions 263 | vector::iterator P_begin = P_total.begin()+t.first_frame; 264 | vector::iterator P_end = P_begin+num_frames-1; 265 | 266 | if (!computePredictionsAndJacobian(P_begin,P_end,p)) 267 | return FAILED; 268 | 269 | // init 270 | Matrix A(3,3); 271 | Matrix B(3,1); 272 | 273 | // fill matrices A and B 274 | for (int32_t m=0; m<3; m++) { 275 | for (int32_t n=0; n<3; n++) { 276 | FLOAT a = 0; 277 | for (int32_t i=0; i<2*num_frames; i++) 278 | a += J[i*3+m]*J[i*3+n]; 279 | A.val[m][n] = a; 280 | } 281 | FLOAT b = 0; 282 | for (int32_t i=0; i<2*num_frames; i++) 283 | b += J[i*3+m]*(p_observe[i]-p_predict[i]); 284 | B.val[m][0] = b; 285 | } 286 | 287 | // perform elimination 288 | if (B.solve(A)) { 289 | p.x += step_size*B.val[0][0]; 290 | p.y += step_size*B.val[1][0]; 291 | p.z += step_size*B.val[2][0]; 292 | if (fabs(B.val[0][0]) &pixels) { 301 | for (int32_t i=0; i::iterator &P_begin,const vector::iterator &P_end,point3d &p) { 308 | 309 | // for all frames do 310 | int32_t k=0; 311 | for (vector::iterator P=P_begin; P<=P_end; P++) { 312 | 313 | // precompute coefficients 314 | FLOAT a = P->val[0][0]*p.x+P->val[0][1]*p.y+P->val[0][2]*p.z+P->val[0][3]; 315 | FLOAT b = P->val[1][0]*p.x+P->val[1][1]*p.y+P->val[1][2]*p.z+P->val[1][3]; 316 | FLOAT c = P->val[2][0]*p.x+P->val[2][1]*p.y+P->val[2][2]*p.z+P->val[2][3]; 317 | FLOAT cc = c*c; 318 | 319 | // check singularities 320 | if (cc<1e-10) 321 | return false; 322 | 323 | // set jacobian entries 324 | J[k*6+0] = (P->val[0][0]*c-P->val[2][0]*a)/cc; 325 | J[k*6+1] = (P->val[0][1]*c-P->val[2][1]*a)/cc; 326 | J[k*6+2] = (P->val[0][2]*c-P->val[2][2]*a)/cc; 327 | J[k*6+3] = (P->val[1][0]*c-P->val[2][0]*b)/cc; 328 | J[k*6+4] = (P->val[1][1]*c-P->val[2][1]*b)/cc; 329 | J[k*6+5] = (P->val[1][2]*c-P->val[2][2]*b)/cc; 330 | 331 | // set prediction 332 | p_predict[k*2+0] = a/c; // u 333 | p_predict[k*2+1] = b/c; // v 334 | 335 | k++; 336 | } 337 | 338 | // success 339 | return true; 340 | } 341 | 342 | void Reconstruction::testJacobian() { 343 | cout << "=================================" << endl; 344 | cout << "TESTING JACOBIAN" << endl; 345 | FLOAT delta = 1e-5; 346 | vector P; 347 | Matrix A(3,4); 348 | A.setMat(K,0,0); 349 | P.push_back(A); 350 | A.setMat(Matrix::rotMatX(0.1)*Matrix::rotMatY(0.1)*Matrix::rotMatZ(0.1),0,0); 351 | A.val[1][3] = 1; 352 | A.val[1][3] = 0.1; 353 | A.val[1][3] = -1.5; 354 | P.push_back(K*A); 355 | cout << P[0] << endll; 356 | cout << P[1] << endll; 357 | J = new FLOAT[6*2]; 358 | p_observe = new FLOAT[2*2]; 359 | p_predict = new FLOAT[2*2]; 360 | 361 | point3d p_ref(0.1,0.2,0.3); 362 | 363 | FLOAT p_predict1[4]; 364 | FLOAT p_predict2[4]; 365 | point3d p1 = p_ref; 366 | 367 | for (int32_t i=0; i<3; i++) { 368 | point3d p2 = p_ref; 369 | if (i==0) p2.x += delta; 370 | else if (i==1) p2.y += delta; 371 | else p2.z += delta; 372 | cout << endl << "Checking parameter " << i << ":" << endl; 373 | cout << "param1: "; cout << p1.x << " " << p1.y << " " << p1.z << endl; 374 | cout << "param2: "; cout << p2.x << " " << p2.y << " " << p2.z << endl; 375 | computePredictionsAndJacobian(P.begin(),P.end(),p1); 376 | memcpy(p_predict1,p_predict,4*sizeof(FLOAT)); 377 | computePredictionsAndJacobian(P.begin(),P.end(),p2); 378 | memcpy(p_predict2,p_predict,4*sizeof(FLOAT)); 379 | for (int32_t j=0; j<4; j++) { 380 | cout << "num: " << (p_predict2[j]-p_predict1[j])/delta; 381 | cout << ", ana: " << J[j*3+i] << endl; 382 | } 383 | } 384 | 385 | delete J; 386 | delete p_observe; 387 | delete p_predict; 388 | cout << "=================================" << endl; 389 | } 390 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/reconstruction.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef RECONSTRUCTION_H 23 | #define RECONSTRUCTION_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #define _USE_MATH_DEFINES 30 | #include 31 | 32 | #include "matcher.h" 33 | #include "matrix.h" 34 | 35 | class Reconstruction { 36 | 37 | public: 38 | 39 | // constructor 40 | Reconstruction (); 41 | 42 | // deconstructor 43 | ~Reconstruction (); 44 | 45 | // a generic 3d point 46 | struct point3d { 47 | float x,y,z; 48 | point3d () {} 49 | point3d (float x,float y,float z) : x(x),y(y),z(z) {} 50 | }; 51 | 52 | // set calibration parameters (intrinsics), must be called at least once 53 | // input: f ....... focal length (assumes fu=fv) 54 | // cu,cv ... principal point 55 | void setCalibration (FLOAT f,FLOAT cu,FLOAT cv); 56 | 57 | // takes a set of monocular feature matches (flow method) and the egomotion 58 | // estimate between the 2 frames Tr, tries to associate the features with previous 59 | // frames (tracking) and computes 3d points once tracks gets lost. 60 | // point types: 0 ..... everything 61 | // 1 ..... road and above 62 | // 2 ..... only above road 63 | // min_track_length ... min number of frames a point needs to be tracked for reconstruction 64 | // max_dist ........... maximum point distance from camera 65 | // min_angle .......... avoid angles smaller than this for reconstruction (in degrees) 66 | void update (std::vector p_matched,Matrix Tr,int32_t point_type=1,int32_t min_track_length=2,double max_dist=30,double min_angle=2); 67 | 68 | // return currently computed 3d points (finished tracks) 69 | std::vector getPoints() { return points; } 70 | 71 | private: 72 | 73 | struct point2d { 74 | float u,v; 75 | point2d () {} 76 | point2d (float u,float v) : u(u),v(v) {} 77 | }; 78 | 79 | struct track { 80 | std::vector pixels; 81 | int32_t first_frame; 82 | int32_t last_frame; 83 | int32_t last_idx; 84 | }; 85 | 86 | enum result { UPDATED, FAILED, CONVERGED }; 87 | 88 | bool initPoint(const track &t,point3d &p); 89 | bool refinePoint(const track &t,point3d &p); 90 | double pointDistance(const track &t,point3d &p); 91 | double rayAngle(const track &t,point3d &p); 92 | int32_t pointType(const track &t,point3d &p); 93 | result updatePoint(const track &t,point3d &p,const FLOAT &step_size,const FLOAT &eps); 94 | void computeObservations(const std::vector &p); 95 | bool computePredictionsAndJacobian(const std::vector::iterator &P_begin,const std::vector::iterator &P_end,point3d &p); 96 | void testJacobian(); 97 | 98 | // calibration matrices 99 | Matrix K,Tr_cam_road; 100 | 101 | std::vector tracks; 102 | std::vector Tr_total; 103 | std::vector Tr_inv_total; 104 | std::vector P_total; 105 | std::vector points; 106 | 107 | FLOAT *J; // jacobian 108 | FLOAT *p_observe,*p_predict; // observed and predicted 2d points 109 | 110 | }; 111 | 112 | #endif // RECONSTRUCTION_H 113 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/sse_to_neon.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // sse_to_neon.hpp 3 | // neon_test 4 | // 5 | // Created by Tim Oberhauser on 11/16/13. 6 | // Copyright (c) 2013 Tim Oberhauser. All rights reserved. 7 | // 8 | 9 | #ifndef neon_test_sse_to_neon_hpp 10 | #define neon_test_sse_to_neon_hpp 11 | 12 | #include 13 | 14 | #if !defined(__MM_MALLOC_H) 15 | // copied from mm_malloc.h { 16 | #include 17 | 18 | /* We can't depend on since the prototype of posix_memalign 19 | may not be visible. */ 20 | #ifndef __cplusplus 21 | extern int posix_memalign (void **, size_t, size_t); 22 | #else 23 | extern "C" int posix_memalign (void **, size_t, size_t) throw (); 24 | #endif 25 | 26 | static __inline void * 27 | _mm_malloc (size_t size, size_t alignment) 28 | { 29 | void *ptr; 30 | if (alignment == 1) 31 | return malloc (size); 32 | if (alignment == 2 || (sizeof (void *) == 8 && alignment == 4)) 33 | alignment = sizeof (void *); 34 | if (posix_memalign (&ptr, alignment, size) == 0) 35 | return ptr; 36 | else 37 | return NULL; 38 | } 39 | 40 | static __inline void 41 | _mm_free (void * ptr) 42 | { 43 | free (ptr); 44 | } 45 | // } copied from mm_malloc.h 46 | #endif 47 | 48 | 49 | typedef int16x8_t __m128i; 50 | typedef float32x4_t __m128; 51 | 52 | 53 | // ADDITION 54 | inline __m128i _mm_add_epi16(const __m128i& a, const __m128i& b){ 55 | return vaddq_s16(reinterpret_cast(a),reinterpret_cast(b)); 56 | } 57 | 58 | inline __m128 _mm_add_ps(const __m128& a, const __m128& b){ 59 | return vaddq_f32(a,b); 60 | } 61 | 62 | 63 | // SUBTRACTION 64 | inline __m128i _mm_sub_epi16(const __m128i& a, const __m128i& b){ 65 | return vsubq_s16(reinterpret_cast(a),reinterpret_cast(b)); 66 | } 67 | 68 | inline __m128 _mm_sub_ps(const __m128& a, const __m128& b){ 69 | return vsubq_f32(a,b); 70 | } 71 | 72 | 73 | // MULTIPLICATION 74 | inline __m128i _mm_mullo_epi16(const __m128i& a, const __m128i& b){ 75 | return vqrdmulhq_s16(reinterpret_cast(a),reinterpret_cast(b)); 76 | } 77 | 78 | inline __m128 _mm_mul_ps(const __m128& a, const __m128& b){ 79 | return vmulq_f32(a,b); 80 | } 81 | 82 | 83 | // SET VALUE 84 | inline __m128i _mm_set1_epi16(const int16_t w){ 85 | return vmovq_n_s16(w); 86 | } 87 | 88 | inline __m128i _mm_setzero_si128(){ 89 | return vmovq_n_s16(0); 90 | } 91 | 92 | inline __m128 _mm_set1_ps(const float32_t& w){ 93 | return vmovq_n_f32(w); 94 | } 95 | 96 | 97 | // STORE 98 | inline void _mm_storeu_si128(__m128i* p, __m128i& a){ 99 | vst1q_s16(reinterpret_cast(p),reinterpret_cast(a)); 100 | } 101 | 102 | inline void _mm_store_ps(float32_t* p, __m128&a){ 103 | vst1q_f32(p,a); 104 | } 105 | 106 | 107 | // LOAD 108 | inline __m128i _mm_loadu_si128(__m128i* p){//For SSE address p does not need be 16-byte aligned 109 | return reinterpret_cast<__m128i>(vld1q_s16(reinterpret_cast(p))); 110 | } 111 | 112 | inline __m128i _mm_load_si128(__m128i* p){//For SSE address p must be 16-byte aligned 113 | return reinterpret_cast<__m128i>(vld1q_s16(reinterpret_cast(p))); 114 | } 115 | 116 | inline __m128 _mm_load_ps(const float32_t* p){ 117 | return reinterpret_cast<__m128>(vld1q_f32(p)); 118 | } 119 | 120 | 121 | // SHIFT OPERATIONS 122 | inline __m128i _mm_srai_epi16(const __m128i& a, const int count){ 123 | int16x8_t b = vmovq_n_s16(-count); 124 | return reinterpret_cast<__m128i>(vshlq_s16(a,b)); 125 | // return vrshrq_n_s16(a, count);// TODO Argument to '__builtin_neon_vrshrq_n_v' must be a constant integer 126 | } 127 | 128 | 129 | // MIN/MAX OPERATIONS 130 | inline __m128 _mm_max_ps(const __m128& a, const __m128& b){ 131 | return reinterpret_cast<__m128>(vmaxq_f32(reinterpret_cast(a),reinterpret_cast(b))); 132 | } 133 | 134 | 135 | // SINGLE ELEMENT ACCESS 136 | inline int16_t _mm_extract_epi16(__m128i& a, int index){ 137 | return (reinterpret_cast(&a))[index]; 138 | // return vgetq_lane_s16(a,index);// TODO Argument to '__builtin_neon_vgetq_lane_i16' must be a constant integer 139 | } 140 | 141 | 142 | // MISCELLANOUS 143 | inline __m128i _mm_sad_epu8 (__m128i a, __m128i b){ 144 | uint64x2_t sad = reinterpret_cast(vabdq_u8(reinterpret_cast(a),reinterpret_cast(b))); 145 | sad = reinterpret_cast(vpaddlq_u8(reinterpret_cast(sad))); 146 | sad = reinterpret_cast(vpaddlq_u16(reinterpret_cast(sad))); 147 | sad = vpaddlq_u32(reinterpret_cast(sad)); 148 | return reinterpret_cast<__m128i>(sad); 149 | } 150 | 151 | 152 | // LOGICAL OPERATIONS 153 | inline __m128 _mm_and_ps(__m128& a, __m128& b){ 154 | return reinterpret_cast<__m128>(vandq_u32(reinterpret_cast(a),reinterpret_cast(b))); 155 | } 156 | 157 | 158 | // CONVERSIONS 159 | inline __m128i _mm_packus_epi16 (const __m128i a, const __m128i b){ 160 | __m128i result = _mm_setzero_si128(); 161 | int8x8_t* a_narrow = reinterpret_cast(&result); 162 | int8x8_t* b_narrow = &a_narrow[1]; 163 | *a_narrow = reinterpret_cast(vqmovun_s16(a)); 164 | *b_narrow = reinterpret_cast(vqmovun_s16(b)); 165 | return result; 166 | } 167 | 168 | // In my case this function was only needed to convert 8 bit to 16 bit integers by extending with zeros, the general case is not implemented!!! 169 | inline __m128i _mm_unpacklo_epi8(__m128i a, const __m128i dummy_zero){ 170 | // dummy_zero is a dummy variable 171 | uint8x8_t* a_low = reinterpret_cast(&a); 172 | return reinterpret_cast<__m128i>(vmovl_u8(*a_low)); 173 | } 174 | 175 | // In my case this function was only needed to convert 8 bit to 16 bit integers by extending with zeros, the general case is not implemented!!! 176 | inline __m128i _mm_unpackhi_epi8(__m128i a, const __m128i dummy_zero){ 177 | // dummy_zero is a dummy variable 178 | uint8x8_t* a_low = reinterpret_cast(&a); 179 | return reinterpret_cast<__m128i>(vmovl_u8(a_low[1])); 180 | } 181 | 182 | 183 | 184 | 185 | #endif 186 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef __TIMER_H__ 23 | #define __TIMER_H__ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | class Timer { 36 | 37 | public: 38 | 39 | Timer() {} 40 | 41 | ~Timer() {} 42 | 43 | void start (std::string title) { 44 | desc.push_back(title); 45 | push_back_time(); 46 | } 47 | 48 | void stop () { 49 | if (time.size()<=desc.size()) 50 | push_back_time(); 51 | } 52 | 53 | void plot () { 54 | stop(); 55 | float total_time = 0; 56 | for (int32_t i=0; i<(int32_t)desc.size(); i++) { 57 | float curr_time = getTimeDifferenceMilliseconds(time[i],time[i+1]); 58 | total_time += curr_time; 59 | std::cout.width(30); 60 | std::cout << desc[i] << " "; 61 | std::cout << std::fixed << std::setprecision(1) << std::setw(6); 62 | std::cout << curr_time; 63 | std::cout << " ms" << std::endl; 64 | } 65 | std::cout << "========================================" << std::endl; 66 | std::cout << " Total time "; 67 | std::cout << std::fixed << std::setprecision(1) << std::setw(6); 68 | std::cout << total_time; 69 | std::cout << " ms" << std::endl << std::endl; 70 | } 71 | 72 | void reset () { 73 | desc.clear(); 74 | time.clear(); 75 | } 76 | 77 | private: 78 | std::vector desc; 79 | std::vector time; 80 | 81 | void push_back_time () { 82 | timeval curr_time; 83 | gettimeofday(&curr_time,0); 84 | time.push_back(curr_time); 85 | } 86 | 87 | float getTimeDifferenceMilliseconds(timeval a,timeval b) { 88 | return ((float)(b.tv_sec -a.tv_sec ))*1e+3 + 89 | ((float)(b.tv_usec-a.tv_usec))*1e-3; 90 | } 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/viso.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "viso.h" 23 | 24 | #include 25 | 26 | using namespace std; 27 | 28 | VisualOdometry::VisualOdometry (parameters param) : param(param) { 29 | J = 0; 30 | p_observe = 0; 31 | p_predict = 0; 32 | matcher = new Matcher(param.match); 33 | Tr_delta = Matrix::eye(4); 34 | Tr_valid = false; 35 | srand(0); 36 | } 37 | 38 | VisualOdometry::~VisualOdometry () { 39 | delete matcher; 40 | } 41 | 42 | bool VisualOdometry::updateMotion () { 43 | 44 | // estimate motion 45 | vector tr_delta = estimateMotion(p_matched); 46 | 47 | // on failure 48 | if (tr_delta.size()!=6) 49 | return false; 50 | 51 | // set transformation matrix (previous to current frame) 52 | Tr_delta = transformationVectorToMatrix(tr_delta); 53 | Tr_valid = true; 54 | 55 | // success 56 | return true; 57 | } 58 | 59 | Matrix VisualOdometry::transformationVectorToMatrix (vector tr) { 60 | 61 | // extract parameters 62 | double rx = tr[0]; 63 | double ry = tr[1]; 64 | double rz = tr[2]; 65 | double tx = tr[3]; 66 | double ty = tr[4]; 67 | double tz = tr[5]; 68 | 69 | // precompute sine/cosine 70 | double sx = sin(rx); 71 | double cx = cos(rx); 72 | double sy = sin(ry); 73 | double cy = cos(ry); 74 | double sz = sin(rz); 75 | double cz = cos(rz); 76 | 77 | // compute transformation 78 | Matrix Tr(4,4); 79 | Tr.val[0][0] = +cy*cz; Tr.val[0][1] = -cy*sz; Tr.val[0][2] = +sy; Tr.val[0][3] = tx; 80 | Tr.val[1][0] = +sx*sy*cz+cx*sz; Tr.val[1][1] = -sx*sy*sz+cx*cz; Tr.val[1][2] = -sx*cy; Tr.val[1][3] = ty; 81 | Tr.val[2][0] = -cx*sy*cz+sx*sz; Tr.val[2][1] = +cx*sy*sz+sx*cz; Tr.val[2][2] = +cx*cy; Tr.val[2][3] = tz; 82 | Tr.val[3][0] = 0; Tr.val[3][1] = 0; Tr.val[3][2] = 0; Tr.val[3][3] = 1; 83 | return Tr; 84 | } 85 | 86 | vector VisualOdometry::getRandomSample(int32_t N,int32_t num) { 87 | 88 | // init sample and totalset 89 | vector sample; 90 | vector totalset; 91 | 92 | // create vector containing all indices 93 | for (int32_t i=0; i p_matched_) { 95 | p_matched = p_matched_; 96 | return updateMotion(); 97 | } 98 | 99 | // returns transformation from previous to current coordinates as a 4x4 100 | // homogeneous transformation matrix Tr_delta, with the following semantics: 101 | // p_t = Tr_delta * p_ {t-1} takes a point in the camera coordinate system 102 | // at time t_1 and maps it to the camera coordinate system at time t. 103 | // note: getMotion() returns the last transformation even when process() 104 | // has failed. this is useful if you wish to linearly extrapolate occasional 105 | // frames for which no correspondences have been found 106 | Matrix getMotion () { return Tr_delta; } 107 | 108 | // returns previous to current feature matches from internal matcher 109 | std::vector getMatches () { return matcher->getMatches(); } 110 | 111 | // returns the number of successfully matched points, after bucketing 112 | int32_t getNumberOfMatches () { return p_matched.size(); } 113 | 114 | // returns the number of inliers: num_inliers <= num_matched 115 | int32_t getNumberOfInliers () { return inliers.size(); } 116 | 117 | // returns the indices of all inliers 118 | std::vector getInlierIndices () { return inliers; } 119 | 120 | // given a vector of inliers computes gain factor between the current and 121 | // the previous frame. this function is useful if you want to reconstruct 3d 122 | // and you want to cancel the change of (unknown) camera gain. 123 | float getGain (std::vector inliers_) { return matcher->getGain(inliers_); } 124 | 125 | // streams out the current transformation matrix Tr_delta 126 | friend std::ostream& operator<< (std::ostream &os,VisualOdometry &viso) { 127 | Matrix p = viso.getMotion(); 128 | os << p.val[0][0] << " " << p.val[0][1] << " " << p.val[0][2] << " " << p.val[0][3] << " "; 129 | os << p.val[1][0] << " " << p.val[1][1] << " " << p.val[1][2] << " " << p.val[1][3] << " "; 130 | os << p.val[2][0] << " " << p.val[2][1] << " " << p.val[2][2] << " " << p.val[2][3]; 131 | return os; 132 | } 133 | 134 | protected: 135 | 136 | // calls bucketing and motion estimation 137 | bool updateMotion (); 138 | 139 | // compute transformation matrix from transformation vector 140 | Matrix transformationVectorToMatrix (std::vector tr); 141 | 142 | // compute motion from previous to current coordinate system 143 | // if motion could not be computed, resulting vector will be of size 0 144 | virtual std::vector estimateMotion (std::vector p_matched) = 0; 145 | 146 | // get random and unique sample of num numbers from 1:N 147 | std::vector getRandomSample (int32_t N,int32_t num); 148 | 149 | Matrix Tr_delta; // transformation (previous -> current frame) 150 | bool Tr_valid; // motion estimate exists? 151 | Matcher *matcher; // feature matcher 152 | std::vector inliers; // inlier set 153 | double *J; // jacobian 154 | double *p_observe; // observed 2d points 155 | double *p_predict; // predicted 2d points 156 | std::vector p_matched; // feature point matches 157 | 158 | private: 159 | 160 | parameters param; // common parameters 161 | }; 162 | 163 | #endif // VISO_H 164 | 165 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/viso_mono.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef VISO_MONO_H 23 | #define VISO_MONO_H 24 | 25 | #include "viso.h" 26 | 27 | class VisualOdometryMono : public VisualOdometry { 28 | 29 | public: 30 | 31 | // monocular-specific parameters (mandatory: height,pitch) 32 | struct parameters : public VisualOdometry::parameters { 33 | double height; // camera height above ground (meters) 34 | double pitch; // camera pitch (rad, negative=pointing down) 35 | int32_t ransac_iters; // number of RANSAC iterations 36 | double inlier_threshold; // fundamental matrix inlier threshold 37 | double motion_threshold; // directly return false on small motions 38 | parameters () { 39 | height = 1.0; 40 | pitch = 0.0; 41 | ransac_iters = 2000; 42 | inlier_threshold = 0.00001; 43 | motion_threshold = 50.0; 44 | } 45 | }; 46 | 47 | // constructor, takes as inpute a parameter structure 48 | VisualOdometryMono (parameters param); 49 | 50 | // deconstructor 51 | ~VisualOdometryMono (); 52 | 53 | // process a new image, pushs the image back to an internal ring buffer. 54 | // valid motion estimates are available after calling process for two times. 55 | // inputs: I ......... pointer to rectified image (uint8, row-aligned) 56 | // dims[0] ... width of I 57 | // dims[1] ... height of I 58 | // dims[2] ... bytes per line (often equal to width) 59 | // replace ... replace current image with I, without copying last current 60 | // image to previous image internally. this option can be used 61 | // when small/no motions are observed to obtain Tr_delta wrt 62 | // an older coordinate system / time step than the previous one. 63 | // output: returns false if motion too small or an error occured 64 | bool process (uint8_t *I,int32_t* dims,bool replace=false); 65 | 66 | private: 67 | 68 | template struct idx_cmp { 69 | idx_cmp(const T arr) : arr(arr) {} 70 | bool operator()(const size_t a, const size_t b) const { return arr[a] < arr[b]; } 71 | const T arr; 72 | }; 73 | 74 | std::vector estimateMotion (std::vector p_matched); 75 | Matrix smallerThanMedian (Matrix &X,double &median); 76 | bool normalizeFeaturePoints (std::vector &p_matched,Matrix &Tp,Matrix &Tc); 77 | void fundamentalMatrix (const std::vector &p_matched,const std::vector &active,Matrix &F); 78 | void EtoRt(Matrix &E,Matrix &K,std::vector &p_matched,Matrix &X,Matrix &R,Matrix &t); 79 | int32_t triangulateChieral (std::vector &p_matched,Matrix &K,Matrix &R,Matrix &t,Matrix &X); 80 | std::vector getInlier (std::vector &p_matched,Matrix &F); 81 | 82 | // parameters 83 | parameters param; 84 | }; 85 | 86 | #endif // VISO_MONO_H 87 | 88 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/viso_mono_omnidirectional.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef VISO_MONO_omnidirectional_H 3 | #define VISO_MONO_omnidirectional_H 4 | 5 | #include "viso.h" 6 | 7 | class VisualOdometryMonoOmnidirectional : public VisualOdometry 8 | { 9 | public: 10 | 11 | // monocular-specific parameters (mandatory: height,pitch) 12 | struct parameters : public VisualOdometry::parameters { 13 | double height; // camera height above ground (meters) 14 | double pitch; // camera pitch (rad, negative=pointing down) 15 | int32_t ransac_iters; // number of RANSAC iterations 16 | double inlier_threshold; // fundamental matrix inlier threshold 17 | double motion_threshold; // directly return false on small motions 18 | parameters () { 19 | height = 1.0; 20 | pitch = 0.0; 21 | ransac_iters = 2000; 22 | inlier_threshold = 0.00001; 23 | motion_threshold = 100.0; 24 | } 25 | }; 26 | 27 | // constructor, takes as input a parameter structure 28 | VisualOdometryMonoOmnidirectional (parameters param); 29 | 30 | // deconstructor 31 | ~VisualOdometryMonoOmnidirectional (); 32 | 33 | // process a new image, pushs the image back to an internal ring buffer. 34 | // valid motion estimates are available after calling process for two times. 35 | // inputs: I ......... pointer to rectified image (uint8, row-aligned) 36 | // dims[0] ... width of I 37 | // dims[1] ... height of I 38 | // dims[2] ... bytes per line (often equal to width) 39 | // replace ... replace current image with I, without copying last current 40 | // image to previous image internally. this option can be used 41 | // when small/no motions are observed to obtain Tr_delta wrt 42 | // an older coordinate system / time step than the previous one. 43 | // output: returns false if motion too small or an error occured 44 | bool process (uint8_t *I, int32_t* dims, bool replace = false); 45 | 46 | 47 | private: 48 | template struct idx_cmp { 49 | idx_cmp(const T arr) : arr(arr) {} 50 | bool operator()(const size_t a, const size_t b) const { return arr[a] < arr[b]; } 51 | const T arr; 52 | }; 53 | 54 | std::vector estimateMotion (std::vector p_matched); 55 | Matrix smallerThanMedian (Matrix &X,double &median); 56 | void fundamentalMatrix (const std::vector &p_matched_3d,const std::vector &active,Matrix &F); 57 | void EtoRt(Matrix &E,std::vector &p_matched_3d,Matrix &X,Matrix &R,Matrix &t); 58 | int32_t triangulateChieral (std::vector &p_matched_3d,Matrix &R,Matrix &t,Matrix &X); 59 | std::vector getInlier (std::vector &p_matched_3d,Matrix &F); 60 | 61 | // functions that project a pixel point into a 3D unit sphere point 62 | // and a 3D world point into a pixel point 63 | void world2cam(double point2D[2], double point3D[3]); 64 | void cam2world(double point3D[3], double point2D[2]); 65 | Matcher::p_match_3d projectIntoUnitSphere(Matcher::p_match); 66 | Matcher::p_match projectIntoImage(Matrix X, Matrix P1, Matrix P2); 67 | std::vector projectMatches(std::vector matches); 68 | std::vector reprojectMatches(Matrix X, Matrix P1, Matrix P2); 69 | 70 | // parameters 71 | parameters param; 72 | }; 73 | 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/viso_stereo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "viso_stereo.h" 23 | 24 | using namespace std; 25 | 26 | VisualOdometryStereo::VisualOdometryStereo (parameters param) : param(param), VisualOdometry(param) { 27 | matcher->setIntrinsics(param.calib.f,param.calib.cu,param.calib.cv,param.base); 28 | } 29 | 30 | VisualOdometryStereo::~VisualOdometryStereo() { 31 | } 32 | 33 | bool VisualOdometryStereo::process (uint8_t *I1,uint8_t *I2,int32_t* dims,bool replace) { 34 | matcher->pushBack(I1,I2,dims,replace); 35 | if (Tr_valid) matcher->matchFeatures(2,&Tr_delta); 36 | else matcher->matchFeatures(2); 37 | matcher->bucketFeatures(param.bucket.max_features,param.bucket.bucket_width,param.bucket.bucket_height); 38 | p_matched = matcher->getMatches(); 39 | return updateMotion(); 40 | } 41 | 42 | vector VisualOdometryStereo::estimateMotion (vector p_matched) { 43 | 44 | // return value 45 | bool success = true; 46 | 47 | // compute minimum distance for RANSAC samples 48 | double width=0,height=0; 49 | for (vector::iterator it=p_matched.begin(); it!=p_matched.end(); it++) { 50 | if (it->u1c>width) width = it->u1c; 51 | if (it->v1c>height) height = it->v1c; 52 | } 53 | double min_dist = min(width,height)/3.0; 54 | 55 | // get number of matches 56 | int32_t N = p_matched.size(); 57 | if (N<6) 58 | return vector(); 59 | 60 | // allocate dynamic memory 61 | X = new double[N]; 62 | Y = new double[N]; 63 | Z = new double[N]; 64 | J = new double[4*N*6]; 65 | p_predict = new double[4*N]; 66 | p_observe = new double[4*N]; 67 | p_residual = new double[4*N]; 68 | 69 | // project matches of previous image into 3d 70 | for (int32_t i=0; i tr_delta; 79 | vector tr_delta_curr; 80 | tr_delta_curr.resize(6); 81 | 82 | // clear parameter vector 83 | inliers.clear(); 84 | 85 | // initial RANSAC estimate 86 | for (int32_t k=0;k active = getRandomSample(N,3); 90 | 91 | // clear parameter vector 92 | for (int32_t i=0; i<6; i++) 93 | tr_delta_curr[i] = 0; 94 | 95 | // minimize reprojection errors 96 | VisualOdometryStereo::result result = UPDATED; 97 | int32_t iter=0; 98 | while (result==UPDATED) { 99 | result = updateParameters(p_matched,active,tr_delta_curr,1,1e-6); 100 | if (iter++ > 20 || result==CONVERGED) 101 | break; 102 | } 103 | 104 | // overwrite best parameters if we have more inliers 105 | if (result!=FAILED) { 106 | vector inliers_curr = getInlier(p_matched,tr_delta_curr); 107 | if (inliers_curr.size()>inliers.size()) { 108 | inliers = inliers_curr; 109 | tr_delta = tr_delta_curr; 110 | } 111 | } 112 | } 113 | 114 | // final optimization (refinement) 115 | if (inliers.size()>=6) { 116 | int32_t iter=0; 117 | VisualOdometryStereo::result result = UPDATED; 118 | while (result==UPDATED) { 119 | result = updateParameters(p_matched,inliers,tr_delta,1,1e-8); 120 | if (iter++ > 100 || result==CONVERGED) 121 | break; 122 | } 123 | 124 | // not converged 125 | if (result!=CONVERGED) 126 | success = false; 127 | 128 | // not enough inliers 129 | } else { 130 | success = false; 131 | } 132 | 133 | // release dynamic memory 134 | delete[] X; 135 | delete[] Y; 136 | delete[] Z; 137 | delete[] J; 138 | delete[] p_predict; 139 | delete[] p_observe; 140 | delete[] p_residual; 141 | 142 | // parameter estimate succeeded? 143 | if (success) return tr_delta; 144 | else return vector(); 145 | } 146 | 147 | vector VisualOdometryStereo::getInlier(vector &p_matched,vector &tr) { 148 | 149 | // mark all observations active 150 | vector active; 151 | for (int32_t i=0; i<(int32_t)p_matched.size(); i++) 152 | active.push_back(i); 153 | 154 | // extract observations and compute predictions 155 | computeObservations(p_matched,active); 156 | computeResidualsAndJacobian(tr,active); 157 | 158 | // compute inliers 159 | vector inliers; 160 | for (int32_t i=0; i<(int32_t)p_matched.size(); i++) 161 | if (pow(p_observe[4*i+0]-p_predict[4*i+0],2)+pow(p_observe[4*i+1]-p_predict[4*i+1],2) + 162 | pow(p_observe[4*i+2]-p_predict[4*i+2],2)+pow(p_observe[4*i+3]-p_predict[4*i+3],2) < param.inlier_threshold*param.inlier_threshold) 163 | inliers.push_back(i); 164 | return inliers; 165 | } 166 | 167 | VisualOdometryStereo::result VisualOdometryStereo::updateParameters(vector &p_matched,vector &active,vector &tr,double step_size,double eps) { 168 | 169 | // we need at least 3 observations 170 | if (active.size()<3) 171 | return FAILED; 172 | 173 | // extract observations and compute predictions 174 | computeObservations(p_matched,active); 175 | computeResidualsAndJacobian(tr,active); 176 | 177 | // init 178 | Matrix A(6,6); 179 | Matrix B(6,1); 180 | 181 | // fill matrices A and B 182 | for (int32_t m=0; m<6; m++) { 183 | for (int32_t n=0; n<6; n++) { 184 | double a = 0; 185 | for (int32_t i=0; i<4*(int32_t)active.size(); i++) { 186 | a += J[i*6+m]*J[i*6+n]; 187 | } 188 | A.val[m][n] = a; 189 | } 190 | double b = 0; 191 | for (int32_t i=0; i<4*(int32_t)active.size(); i++) { 192 | b += J[i*6+m]*(p_residual[i]); 193 | } 194 | B.val[m][0] = b; 195 | } 196 | 197 | // perform elimination 198 | if (B.solve(A)) { 199 | bool converged = true; 200 | for (int32_t m=0; m<6; m++) { 201 | tr[m] += step_size*B.val[m][0]; 202 | if (fabs(B.val[m][0])>eps) 203 | converged = false; 204 | } 205 | if (converged) 206 | return CONVERGED; 207 | else 208 | return UPDATED; 209 | } else { 210 | return FAILED; 211 | } 212 | } 213 | 214 | void VisualOdometryStereo::computeObservations(vector &p_matched,vector &active) { 215 | 216 | // set all observations 217 | for (int32_t i=0; i<(int32_t)active.size(); i++) { 218 | p_observe[4*i+0] = p_matched[active[i]].u1c; // u1 219 | p_observe[4*i+1] = p_matched[active[i]].v1c; // v1 220 | p_observe[4*i+2] = p_matched[active[i]].u2c; // u2 221 | p_observe[4*i+3] = p_matched[active[i]].v2c; // v2 222 | } 223 | } 224 | 225 | void VisualOdometryStereo::computeResidualsAndJacobian(vector &tr,vector &active) { 226 | 227 | // extract motion parameters 228 | double rx = tr[0]; double ry = tr[1]; double rz = tr[2]; 229 | double tx = tr[3]; double ty = tr[4]; double tz = tr[5]; 230 | 231 | // precompute sine/cosine 232 | double sx = sin(rx); double cx = cos(rx); double sy = sin(ry); 233 | double cy = cos(ry); double sz = sin(rz); double cz = cos(rz); 234 | 235 | // compute rotation matrix and derivatives 236 | double r00 = +cy*cz; double r01 = -cy*sz; double r02 = +sy; 237 | double r10 = +sx*sy*cz+cx*sz; double r11 = -sx*sy*sz+cx*cz; double r12 = -sx*cy; 238 | double r20 = -cx*sy*cz+sx*sz; double r21 = +cx*sy*sz+sx*cz; double r22 = +cx*cy; 239 | double rdrx10 = +cx*sy*cz-sx*sz; double rdrx11 = -cx*sy*sz-sx*cz; double rdrx12 = -cx*cy; 240 | double rdrx20 = +sx*sy*cz+cx*sz; double rdrx21 = -sx*sy*sz+cx*cz; double rdrx22 = -sx*cy; 241 | double rdry00 = -sy*cz; double rdry01 = +sy*sz; double rdry02 = +cy; 242 | double rdry10 = +sx*cy*cz; double rdry11 = -sx*cy*sz; double rdry12 = +sx*sy; 243 | double rdry20 = -cx*cy*cz; double rdry21 = +cx*cy*sz; double rdry22 = -cx*sy; 244 | double rdrz00 = -cy*sz; double rdrz01 = -cy*cz; 245 | double rdrz10 = -sx*sy*sz+cx*cz; double rdrz11 = -sx*sy*cz-cx*sz; 246 | double rdrz20 = +cx*sy*sz+sx*cz; double rdrz21 = +cx*sy*cz-sx*sz; 247 | 248 | // loop variables 249 | double X1p,Y1p,Z1p; 250 | double X1c,Y1c,Z1c,X2c; 251 | double X1cd,Y1cd,Z1cd; 252 | 253 | // for all observations do 254 | for (int32_t i=0; i<(int32_t)active.size(); i++) { 255 | 256 | // get 3d point in previous coordinate system 257 | X1p = X[active[i]]; 258 | Y1p = Y[active[i]]; 259 | Z1p = Z[active[i]]; 260 | 261 | // compute 3d point in current left coordinate system 262 | X1c = r00*X1p+r01*Y1p+r02*Z1p+tx; 263 | Y1c = r10*X1p+r11*Y1p+r12*Z1p+ty; 264 | Z1c = r20*X1p+r21*Y1p+r22*Z1p+tz; 265 | 266 | // weighting 267 | double weight = 1.0; 268 | if (param.reweighting) 269 | weight = 1.0/(fabs(p_observe[4*i+0]-param.calib.cu)/fabs(param.calib.cu) + 0.05); 270 | 271 | // compute 3d point in current right coordinate system 272 | X2c = X1c-param.base; 273 | 274 | // for all paramters do 275 | for (int32_t j=0; j<6; j++) { 276 | 277 | // derivatives of 3d pt. in curr. left coordinates wrt. param j 278 | switch (j) { 279 | case 0: X1cd = 0; 280 | Y1cd = rdrx10*X1p+rdrx11*Y1p+rdrx12*Z1p; 281 | Z1cd = rdrx20*X1p+rdrx21*Y1p+rdrx22*Z1p; 282 | break; 283 | case 1: X1cd = rdry00*X1p+rdry01*Y1p+rdry02*Z1p; 284 | Y1cd = rdry10*X1p+rdry11*Y1p+rdry12*Z1p; 285 | Z1cd = rdry20*X1p+rdry21*Y1p+rdry22*Z1p; 286 | break; 287 | case 2: X1cd = rdrz00*X1p+rdrz01*Y1p; 288 | Y1cd = rdrz10*X1p+rdrz11*Y1p; 289 | Z1cd = rdrz20*X1p+rdrz21*Y1p; 290 | break; 291 | case 3: X1cd = 1; Y1cd = 0; Z1cd = 0; break; 292 | case 4: X1cd = 0; Y1cd = 1; Z1cd = 0; break; 293 | case 5: X1cd = 0; Y1cd = 0; Z1cd = 1; break; 294 | } 295 | 296 | // set jacobian entries (project via K) 297 | J[(4*i+0)*6+j] = weight*param.calib.f*(X1cd*Z1c-X1c*Z1cd)/(Z1c*Z1c); // left u' 298 | J[(4*i+1)*6+j] = weight*param.calib.f*(Y1cd*Z1c-Y1c*Z1cd)/(Z1c*Z1c); // left v' 299 | J[(4*i+2)*6+j] = weight*param.calib.f*(X1cd*Z1c-X2c*Z1cd)/(Z1c*Z1c); // right u' 300 | J[(4*i+3)*6+j] = weight*param.calib.f*(Y1cd*Z1c-Y1c*Z1cd)/(Z1c*Z1c); // right v' 301 | } 302 | 303 | // set prediction (project via K) 304 | p_predict[4*i+0] = param.calib.f*X1c/Z1c+param.calib.cu; // left u 305 | p_predict[4*i+1] = param.calib.f*Y1c/Z1c+param.calib.cv; // left v 306 | p_predict[4*i+2] = param.calib.f*X2c/Z1c+param.calib.cu; // right u 307 | p_predict[4*i+3] = param.calib.f*Y1c/Z1c+param.calib.cv; // right v 308 | 309 | // set residuals 310 | p_residual[4*i+0] = weight*(p_observe[4*i+0]-p_predict[4*i+0]); 311 | p_residual[4*i+1] = weight*(p_observe[4*i+1]-p_predict[4*i+1]); 312 | p_residual[4*i+2] = weight*(p_observe[4*i+2]-p_predict[4*i+2]); 313 | p_residual[4*i+3] = weight*(p_observe[4*i+3]-p_predict[4*i+3]); 314 | } 315 | } 316 | 317 | -------------------------------------------------------------------------------- /libviso2/libviso2/src/viso_stereo.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef VISO_STEREO_H 23 | #define VISO_STEREO_H 24 | 25 | #include "viso.h" 26 | 27 | class VisualOdometryStereo : public VisualOdometry { 28 | 29 | public: 30 | 31 | // stereo-specific parameters (mandatory: base) 32 | struct parameters : public VisualOdometry::parameters { 33 | double base; // baseline (meters) 34 | int32_t ransac_iters; // number of RANSAC iterations 35 | double inlier_threshold; // fundamental matrix inlier threshold 36 | bool reweighting; // lower border weights (more robust to calibration errors) 37 | parameters () { 38 | base = 1.0; 39 | ransac_iters = 200; 40 | inlier_threshold = 2.0; 41 | reweighting = true; 42 | } 43 | }; 44 | 45 | // constructor, takes as inpute a parameter structure 46 | VisualOdometryStereo (parameters param); 47 | 48 | // deconstructor 49 | ~VisualOdometryStereo (); 50 | 51 | // process a new images, push the images back to an internal ring buffer. 52 | // valid motion estimates are available after calling process for two times. 53 | // inputs: I1 ........ pointer to rectified left image (uint8, row-aligned) 54 | // I2 ........ pointer to rectified right image (uint8, row-aligned) 55 | // dims[0] ... width of I1 and I2 (both must be of same size) 56 | // dims[1] ... height of I1 and I2 (both must be of same size) 57 | // dims[2] ... bytes per line (often equal to width) 58 | // replace ... replace current images with I1 and I2, without copying last current 59 | // images to previous images internally. this option can be used 60 | // when small/no motions are observed to obtain Tr_delta wrt 61 | // an older coordinate system / time step than the previous one. 62 | // output: returns false if an error occured 63 | bool process (uint8_t *I1,uint8_t *I2,int32_t* dims,bool replace=false); 64 | 65 | using VisualOdometry::process; 66 | 67 | 68 | 69 | private: 70 | 71 | std::vector estimateMotion (std::vector p_matched); 72 | enum result { UPDATED, FAILED, CONVERGED }; 73 | result updateParameters(std::vector &p_matched,std::vector &active,std::vector &tr,double step_size,double eps); 74 | void computeObservations(std::vector &p_matched,std::vector &active); 75 | void computeResidualsAndJacobian(std::vector &tr,std::vector &active); 76 | std::vector getInlier(std::vector &p_matched,std::vector &tr); 77 | 78 | double *X,*Y,*Z; // 3d points 79 | double *p_residual; // residuals (p_residual=p_observe-p_predict) 80 | 81 | // parameters 82 | parameters param; 83 | }; 84 | 85 | #endif // VISO_STEREO_H 86 | 87 | -------------------------------------------------------------------------------- /libviso2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | libviso2 3 | 0.0.1 4 | This is a ROS-Package for libviso2, a library for visual odometry created by Andeas Geiger 5 | from the Institute of Measurement and Control Systems at Karlsruhe Institute of Technology. 6 | 7 | Apart from the original libviso2 sources, this package contains 8 | a CMakeFile for easier integration into the ROS build system. 9 | 10 | Please note that this code is licensed under GPL. For a commercial usage, please 11 | contact Andreas Geiger directly (see 12 | http://www.cvlibs.net/software/libviso2.html). 13 | 14 | 15 | maintained by: Stephan Wirth 16 | 17 | GPL 18 | 19 | http://ros.org/wiki/libviso2 20 | https://github.com/srv/viso2/issues 21 | 22 | Andreas Geiger 23 | Stephan Wirth 24 | 25 | catkin 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /viso2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(viso2) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /viso2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | viso2 3 | 0.0.1 4 | 5 | A ROS-Wrapper for libviso2, a library for visual odometry, 6 | maintained by the Systems, Robotics and Vision group of the 7 | University of the Balearic Islands, Spain. http://srv.uib.es 8 | 9 | 10 | GPL 11 | 12 | http://ros.org/wiki/viso2 13 | https://github.com/srv/viso2/issues 14 | 15 | Stephan Wirth 16 | Stephan Wirth 17 | 18 | catkin 19 | libviso2 20 | viso2_ros 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /viso2_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(viso2_ros) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | libviso2 6 | roscpp 7 | sensor_msgs 8 | nav_msgs 9 | message_filters 10 | image_transport 11 | cv_bridge 12 | image_geometry 13 | tf 14 | pcl_ros 15 | std_srvs 16 | message_generation 17 | ) 18 | 19 | find_package(OpenCV REQUIRED) 20 | find_package(PCL REQUIRED) 21 | 22 | option(ARM_CROSS_COMPILATION "ARM Cross Compilation" OFF) 23 | 24 | add_message_files( DIRECTORY msg FILES VisoInfo.msg) 25 | 26 | generate_messages( 27 | DEPENDENCIES 28 | std_msgs 29 | ) 30 | 31 | catkin_package() 32 | 33 | if(ARM_CROSS_COMPILATION) 34 | SET(CMAKE_SYSTEM_PROCESSOR arm) 35 | SET(CMAKE_CXX_FLAGS -mfpu=neon) 36 | else(ARM_CROSS_COMPILATION) 37 | add_definitions(-msse3) 38 | endif(ARM_CROSS_COMPILATION) 39 | 40 | include_directories(src ${libviso2_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 41 | 42 | add_executable(stereo_odometer 43 | src/stereo_odometer.cpp) 44 | add_dependencies(stereo_odometer ${catkin_EXPORTED_TARGETS}) 45 | add_dependencies(stereo_odometer ${PROJECT_NAME}_gencpp) 46 | 47 | add_executable(mono_odometer 48 | src/mono_odometer.cpp) 49 | add_dependencies(mono_odometer ${catkin_EXPORTED_TARGETS}) 50 | add_dependencies(mono_odometer ${PROJECT_NAME}_gencpp) 51 | 52 | add_executable(mono_odometer_omnidirectional 53 | src/mono_odometer_omnidirectional.cpp) 54 | add_dependencies(mono_odometer_omnidirectional ${catkin_EXPORTED_TARGETS}) 55 | add_dependencies(mono_odometer_omnidirectional ${PROJECT_NAME}_gencpp) 56 | 57 | target_link_libraries(stereo_odometer ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 58 | find_package(Boost REQUIRED COMPONENTS thread) 59 | include_directories(${Boost_INCLUDE_DIRS}) 60 | target_link_libraries(stereo_odometer ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 61 | target_link_libraries(mono_odometer ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 62 | target_link_libraries(mono_odometer_omnidirectional ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 63 | -------------------------------------------------------------------------------- /viso2_ros/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /viso2_ros/launch/omni_version.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /viso2_ros/launch/perspective_version.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /viso2_ros/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b viso2 is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /viso2_ros/msg/VisoInfo.msg: -------------------------------------------------------------------------------- 1 | # Internal information on the 2 | # viso2 algorithm parameters 3 | # and results 4 | 5 | Header header 6 | 7 | # True if the previous iteration of viso2 8 | # was not able to complete the matching process 9 | # therefore the visual odometer is re-started. 10 | bool got_lost 11 | 12 | # True if in the next run the reference 13 | # frame will be changed. This is the case 14 | # when the number of inliers drops below 15 | # a threshold or the previous motion estimate 16 | # failed in last motion estimation. 17 | bool change_reference_frame 18 | 19 | # info from motion estimator 20 | bool motion_estimate_valid 21 | int32 num_matches 22 | int32 num_inliers 23 | 24 | # runtime of last iteration in seconds 25 | float64 runtime 26 | -------------------------------------------------------------------------------- /viso2_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | viso2_ros 3 | 0.0.1 4 | This is the ROS wrapper for libviso2, library for visual odometry (see package libviso2). 5 | 6 | GPL 7 | 8 | http://ros.org/wiki/viso2_ros 9 | https://github.com/srv/viso2/issues 10 | 11 | Stephan Wirth 12 | Bo Miquel Nordfeldt-Fiol 13 | 14 | catkin 15 | 16 | libviso2 17 | roscpp 18 | sensor_msgs 19 | nav_msgs 20 | message_filters 21 | image_transport 22 | cv_bridge 23 | image_geometry 24 | tf 25 | libpcl-all-dev 26 | pcl_ros 27 | std_srvs 28 | message_generation 29 | 30 | libviso2 31 | roscpp 32 | sensor_msgs 33 | nav_msgs 34 | message_filters 35 | image_transport 36 | cv_bridge 37 | image_geometry 38 | tf 39 | libpcl-all-dev 40 | pcl_ros 41 | std_srvs 42 | message_runtime 43 | 44 | 45 | -------------------------------------------------------------------------------- /viso2_ros/src/mono_odometer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include "odometer_base.h" 13 | #include "odometry_params.h" 14 | 15 | namespace viso2_ros 16 | { 17 | 18 | class MonoOdometer : public OdometerBase 19 | { 20 | 21 | private: 22 | 23 | boost::shared_ptr visual_odometer_; 24 | VisualOdometryMono::parameters visual_odometer_params_; 25 | 26 | image_transport::CameraSubscriber camera_sub_; 27 | 28 | ros::Publisher info_pub_; 29 | 30 | bool replace_; 31 | 32 | public: 33 | 34 | MonoOdometer(const std::string& transport) : OdometerBase(), replace_(false) 35 | { 36 | // Read local parameters 37 | ros::NodeHandle local_nh("~"); 38 | odometry_params::loadParams(local_nh, visual_odometer_params_); 39 | 40 | ros::NodeHandle nh; 41 | image_transport::ImageTransport it(nh); 42 | camera_sub_ = it.subscribeCamera("image", 1, &MonoOdometer::imageCallback, this, transport); 43 | 44 | info_pub_ = local_nh.advertise("info", 1); 45 | } 46 | 47 | protected: 48 | 49 | void imageCallback( 50 | const sensor_msgs::ImageConstPtr& image_msg, 51 | const sensor_msgs::CameraInfoConstPtr& info_msg) 52 | { 53 | ros::WallTime start_time = ros::WallTime::now(); 54 | 55 | bool first_run = false; 56 | // create odometer if not exists 57 | if (!visual_odometer_) 58 | { 59 | first_run = true; 60 | // read calibration info from camera info message 61 | // to fill remaining odometer parameters 62 | image_geometry::PinholeCameraModel model; 63 | model.fromCameraInfo(info_msg); 64 | visual_odometer_params_.calib.f = model.fx(); 65 | visual_odometer_params_.calib.cu = model.cx(); 66 | visual_odometer_params_.calib.cv = model.cy(); 67 | visual_odometer_.reset(new VisualOdometryMono(visual_odometer_params_)); 68 | if (image_msg->header.frame_id != "") setSensorFrameId(image_msg->header.frame_id); 69 | ROS_INFO_STREAM("Initialized libviso2 mono odometry " 70 | "with the following parameters:" << std::endl << 71 | visual_odometer_params_); 72 | } 73 | 74 | // convert image if necessary 75 | uint8_t *image_data; 76 | int step; 77 | cv_bridge::CvImageConstPtr cv_ptr; 78 | if (image_msg->encoding == sensor_msgs::image_encodings::MONO8) 79 | { 80 | image_data = const_cast(&(image_msg->data[0])); 81 | step = image_msg->step; 82 | } 83 | else 84 | { 85 | cv_ptr = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); 86 | image_data = cv_ptr->image.data; 87 | step = cv_ptr->image.step[0]; 88 | } 89 | 90 | // run the odometer 91 | int32_t dims[] = {(int32_t)image_msg->width, (int32_t)image_msg->height, step}; 92 | // on first run, only feed the odometer with first image pair without 93 | // retrieving data 94 | if (first_run) 95 | { 96 | visual_odometer_->process(image_data, dims); 97 | tf::Transform delta_transform; 98 | delta_transform.setIdentity(); 99 | integrateAndPublish(delta_transform, image_msg->header.stamp); 100 | } 101 | else 102 | { 103 | bool success = visual_odometer_->process(image_data, dims); 104 | if(success) 105 | { 106 | replace_ = false; 107 | Matrix camera_motion = Matrix::inv(visual_odometer_->getMotion()); 108 | ROS_DEBUG("Found %i matches with %i inliers.", 109 | visual_odometer_->getNumberOfMatches(), 110 | visual_odometer_->getNumberOfInliers()); 111 | ROS_DEBUG_STREAM("libviso2 returned the following motion:\n" << camera_motion); 112 | 113 | tf::Matrix3x3 rot_mat( 114 | camera_motion.val[0][0], camera_motion.val[0][1], camera_motion.val[0][2], 115 | camera_motion.val[1][0], camera_motion.val[1][1], camera_motion.val[1][2], 116 | camera_motion.val[2][0], camera_motion.val[2][1], camera_motion.val[2][2]); 117 | tf::Vector3 t(camera_motion.val[0][3], camera_motion.val[1][3], camera_motion.val[2][3]); 118 | tf::Transform delta_transform(rot_mat, t); 119 | 120 | integrateAndPublish(delta_transform, image_msg->header.stamp); 121 | } 122 | else 123 | { 124 | ROS_DEBUG("Call to VisualOdometryMono::process() failed. Assuming motion too small."); 125 | replace_ = true; 126 | tf::Transform delta_transform; 127 | delta_transform.setIdentity(); 128 | integrateAndPublish(delta_transform, image_msg->header.stamp); 129 | } 130 | 131 | // create and publish viso2 info msg 132 | VisoInfo info_msg; 133 | info_msg.header.stamp = image_msg->header.stamp; 134 | info_msg.got_lost = !success; 135 | info_msg.change_reference_frame = false; 136 | info_msg.num_matches = visual_odometer_->getNumberOfMatches(); 137 | info_msg.num_inliers = visual_odometer_->getNumberOfInliers(); 138 | ros::WallDuration time_elapsed = ros::WallTime::now() - start_time; 139 | info_msg.runtime = time_elapsed.toSec(); 140 | info_pub_.publish(info_msg); 141 | } 142 | } 143 | }; 144 | 145 | } // end of namespace 146 | 147 | 148 | int main(int argc, char **argv) 149 | { 150 | ros::init(argc, argv, "mono_odometer"); 151 | if (ros::names::remap("image").find("rect") == std::string::npos) { 152 | ROS_WARN("mono_odometer needs rectified input images. The used image " 153 | "topic is '%s'. Are you sure the images are rectified?", 154 | ros::names::remap("image").c_str()); 155 | } 156 | 157 | std::string transport = argc > 1 ? argv[1] : "raw"; 158 | viso2_ros::MonoOdometer odometer(transport); 159 | 160 | ros::spin(); 161 | return 0; 162 | } 163 | 164 | -------------------------------------------------------------------------------- /viso2_ros/src/mono_odometer_omnidirectional.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include "odometer_base.h" 13 | #include "odometry_params.h" 14 | 15 | 16 | namespace viso2_ros 17 | { 18 | 19 | class MonoOdometerOmnidirectional : public OdometerBase 20 | { 21 | 22 | private: 23 | 24 | boost::shared_ptr visual_odometer_; 25 | VisualOdometryMonoOmnidirectional::parameters visual_odometer_params_; 26 | 27 | image_transport::Subscriber camera_sub_; 28 | 29 | ros::Publisher info_pub_; 30 | 31 | bool replace_; 32 | 33 | public: 34 | 35 | MonoOdometerOmnidirectional(const std::string& transport) : OdometerBase(), replace_(false) 36 | { 37 | // Read local parameters 38 | ros::NodeHandle local_nh("~"); 39 | odometry_params::loadParams(local_nh, visual_odometer_params_); 40 | 41 | ros::NodeHandle nh; 42 | std::string image_topic; 43 | nh.param("/mono_odometer/image", image_topic, "/image"); 44 | 45 | image_transport::ImageTransport it(nh); 46 | camera_sub_ = it.subscribe(image_topic, 1, &MonoOdometerOmnidirectional::imageCallback, this); 47 | 48 | info_pub_ = local_nh.advertise("info", 1); 49 | } 50 | 51 | protected: 52 | 53 | void imageCallback(const sensor_msgs::ImageConstPtr& image_msg) 54 | { 55 | ros::WallTime start_time = ros::WallTime::now(); 56 | 57 | bool first_run = false; 58 | // create odometer if not exists 59 | if (!visual_odometer_) 60 | { 61 | first_run = true; 62 | visual_odometer_.reset(new VisualOdometryMonoOmnidirectional(visual_odometer_params_)); 63 | if (image_msg->header.frame_id != "") setSensorFrameId(image_msg->header.frame_id); 64 | ROS_INFO_STREAM("Initialized libviso2 mono odometry " 65 | "with the following parameters:" << std::endl << 66 | visual_odometer_params_); 67 | } 68 | 69 | // convert image if necessary 70 | uint8_t *image_data; 71 | int step; 72 | cv_bridge::CvImageConstPtr cv_ptr; 73 | if (image_msg->encoding == sensor_msgs::image_encodings::MONO8) 74 | { 75 | image_data = const_cast(&(image_msg->data[0])); 76 | step = image_msg->step; 77 | } 78 | else 79 | { 80 | cv_ptr = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); 81 | image_data = cv_ptr->image.data; 82 | step = cv_ptr->image.step[0]; 83 | } 84 | 85 | // run the odometer 86 | int32_t dims[] = {(int32_t)image_msg->width, (int32_t)image_msg->height, step}; 87 | // on first run, only feed the odometer with first image pair without 88 | // retrieving data 89 | if (first_run) 90 | { 91 | visual_odometer_->process(image_data, dims); 92 | tf::Transform delta_transform; 93 | delta_transform.setIdentity(); 94 | integrateAndPublish(delta_transform, image_msg->header.stamp); 95 | } 96 | else 97 | { 98 | bool success = visual_odometer_->process(image_data, dims); 99 | if(success) 100 | { 101 | replace_ = false; 102 | Matrix camera_motion = Matrix::inv(visual_odometer_->getMotion()); 103 | ROS_DEBUG("Found %i matches with %i inliers.", 104 | visual_odometer_->getNumberOfMatches(), 105 | visual_odometer_->getNumberOfInliers()); 106 | ROS_DEBUG_STREAM("libviso2 returned the following motion:\n" << camera_motion); 107 | 108 | tf::Matrix3x3 rot_mat( 109 | camera_motion.val[0][0], camera_motion.val[0][1], camera_motion.val[0][2], 110 | camera_motion.val[1][0], camera_motion.val[1][1], camera_motion.val[1][2], 111 | camera_motion.val[2][0], camera_motion.val[2][1], camera_motion.val[2][2]); 112 | tf::Vector3 t(camera_motion.val[0][3], camera_motion.val[1][3], camera_motion.val[2][3]); 113 | tf::Transform delta_transform(rot_mat, t); 114 | 115 | integrateAndPublish(delta_transform, image_msg->header.stamp); 116 | } 117 | else 118 | { 119 | ROS_DEBUG("Call to VisualOdometryMono::process() failed. Assuming motion too small."); 120 | replace_ = true; 121 | tf::Transform delta_transform; 122 | delta_transform.setIdentity(); 123 | integrateAndPublish(delta_transform, image_msg->header.stamp); 124 | } 125 | 126 | // create and publish viso2 info msg 127 | VisoInfo info_msg; 128 | info_msg.header.stamp = image_msg->header.stamp; 129 | info_msg.got_lost = !success; 130 | info_msg.change_reference_frame = false; 131 | info_msg.num_matches = visual_odometer_->getNumberOfMatches(); 132 | info_msg.num_inliers = visual_odometer_->getNumberOfInliers(); 133 | ros::WallDuration time_elapsed = ros::WallTime::now() - start_time; 134 | info_msg.runtime = time_elapsed.toSec(); 135 | info_pub_.publish(info_msg); 136 | } 137 | } 138 | }; 139 | 140 | } // end of namespace 141 | 142 | 143 | int main(int argc, char **argv) 144 | { 145 | ros::init(argc, argv, "mono_odometer"); 146 | if (ros::names::remap("image").find("rect") == std::string::npos) { 147 | ROS_WARN("mono_odometer needs rectified input images. The used image " 148 | "topic is '%s'. Are you sure the images are rectified?", 149 | ros::names::remap("image").c_str()); 150 | } 151 | 152 | std::string transport = argc > 1 ? argv[1] : "raw"; 153 | viso2_ros::MonoOdometerOmnidirectional odometer(transport); 154 | 155 | ros::spin(); 156 | return 0; 157 | } 158 | -------------------------------------------------------------------------------- /viso2_ros/src/odometer_base.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef ODOMETER_BASE_H_ 3 | #define ODOMETER_BASE_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace viso2_ros 13 | { 14 | 15 | /** 16 | * Base class for odometers, handles tf's, odometry and pose 17 | * publishing. This can be used as base for any incremental pose estimating 18 | * sensor. Sensors that measure velocities cannot be used. 19 | */ 20 | class OdometerBase 21 | { 22 | 23 | private: 24 | 25 | // publisher 26 | ros::Publisher odom_pub_; 27 | ros::Publisher pose_pub_; 28 | 29 | ros::ServiceServer reset_service_; 30 | 31 | // tf related 32 | std::string sensor_frame_id_; 33 | std::string odom_frame_id_; 34 | std::string base_link_frame_id_; 35 | tf::TransformListener tf_listener_; 36 | tf::TransformBroadcaster tf_broadcaster_; 37 | bool publish_tf_; 38 | bool invert_tf_; 39 | 40 | // the current integrated camera pose 41 | tf::Transform integrated_pose_; 42 | // timestamp of the last update 43 | ros::Time last_update_time_; 44 | 45 | // covariances 46 | boost::array pose_covariance_; 47 | boost::array twist_covariance_; 48 | 49 | public: 50 | 51 | OdometerBase() 52 | { 53 | // Read local parameters 54 | ros::NodeHandle local_nh("~"); 55 | 56 | local_nh.param("odom_frame_id", odom_frame_id_, std::string("/odom")); 57 | local_nh.param("base_link_frame_id", base_link_frame_id_, std::string("/base_link")); 58 | local_nh.param("sensor_frame_id", sensor_frame_id_, std::string("/camera")); 59 | local_nh.param("publish_tf", publish_tf_, true); 60 | local_nh.param("invert_tf", invert_tf_, false); 61 | 62 | ROS_INFO_STREAM("Basic Odometer Settings:" << std::endl << 63 | " odom_frame_id = " << odom_frame_id_ << std::endl << 64 | " base_link_frame_id = " << base_link_frame_id_ << std::endl << 65 | " publish_tf = " << (publish_tf_?"true":"false") << std::endl << 66 | " invert_tf = " << (invert_tf_?"true":"false")); 67 | 68 | // advertise 69 | odom_pub_ = local_nh.advertise("odometry", 1); 70 | pose_pub_ = local_nh.advertise("pose", 1); 71 | 72 | reset_service_ = local_nh.advertiseService("reset_pose", &OdometerBase::resetPose, this); 73 | 74 | integrated_pose_.setIdentity(); 75 | 76 | pose_covariance_.assign(0.0); 77 | twist_covariance_.assign(0.0); 78 | } 79 | 80 | protected: 81 | 82 | void setSensorFrameId(const std::string& frame_id) 83 | { 84 | sensor_frame_id_ = frame_id; 85 | } 86 | 87 | std::string getSensorFrameId() const 88 | { 89 | return sensor_frame_id_; 90 | } 91 | 92 | void setPoseCovariance(const boost::array& pose_covariance) 93 | { 94 | pose_covariance_ = pose_covariance; 95 | } 96 | 97 | void setTwistCovariance(const boost::array& twist_covariance) 98 | { 99 | twist_covariance_ = twist_covariance; 100 | } 101 | 102 | void integrateAndPublish(const tf::Transform& delta_transform, const ros::Time& timestamp) 103 | { 104 | if (sensor_frame_id_.empty()) 105 | { 106 | ROS_ERROR("[odometer] update called with unknown sensor frame id!"); 107 | return; 108 | } 109 | if (timestamp < last_update_time_) 110 | { 111 | ROS_WARN("[odometer] saw negative time change in incoming sensor data, resetting pose."); 112 | integrated_pose_.setIdentity(); 113 | tf_listener_.clear(); 114 | } 115 | integrated_pose_ *= delta_transform; 116 | 117 | // transform integrated pose to base frame 118 | tf::StampedTransform base_to_sensor; 119 | std::string error_msg; 120 | if (tf_listener_.canTransform(base_link_frame_id_, sensor_frame_id_, timestamp, &error_msg)) 121 | { 122 | tf_listener_.lookupTransform( 123 | base_link_frame_id_, 124 | sensor_frame_id_, 125 | timestamp, base_to_sensor); 126 | } 127 | else 128 | { 129 | ROS_WARN_THROTTLE(10.0, "The tf from '%s' to '%s' does not seem to be available, " 130 | "will assume it as identity!", 131 | base_link_frame_id_.c_str(), 132 | sensor_frame_id_.c_str()); 133 | ROS_DEBUG("Transform error: %s", error_msg.c_str()); 134 | base_to_sensor.setIdentity(); 135 | } 136 | 137 | tf::Transform base_transform = base_to_sensor * integrated_pose_ * base_to_sensor.inverse(); 138 | 139 | nav_msgs::Odometry odometry_msg; 140 | odometry_msg.header.stamp = timestamp; 141 | odometry_msg.header.frame_id = odom_frame_id_; 142 | odometry_msg.child_frame_id = base_link_frame_id_; 143 | tf::poseTFToMsg(base_transform, odometry_msg.pose.pose); 144 | 145 | // calculate twist (not possible for first run as no delta_t can be computed) 146 | tf::Transform delta_base_transform = base_to_sensor * delta_transform * base_to_sensor.inverse(); 147 | if (!last_update_time_.isZero()) 148 | { 149 | double delta_t = (timestamp - last_update_time_).toSec(); 150 | if (delta_t) 151 | { 152 | odometry_msg.twist.twist.linear.x = delta_base_transform.getOrigin().getX() / delta_t; 153 | odometry_msg.twist.twist.linear.y = delta_base_transform.getOrigin().getY() / delta_t; 154 | odometry_msg.twist.twist.linear.z = delta_base_transform.getOrigin().getZ() / delta_t; 155 | tf::Quaternion delta_rot = delta_base_transform.getRotation(); 156 | tfScalar angle = delta_rot.getAngle(); 157 | tf::Vector3 axis = delta_rot.getAxis(); 158 | tf::Vector3 angular_twist = axis * angle / delta_t; 159 | odometry_msg.twist.twist.angular.x = angular_twist.x(); 160 | odometry_msg.twist.twist.angular.y = angular_twist.y(); 161 | odometry_msg.twist.twist.angular.z = angular_twist.z(); 162 | } 163 | } 164 | 165 | odometry_msg.pose.covariance = pose_covariance_; 166 | odometry_msg.twist.covariance = twist_covariance_; 167 | odom_pub_.publish(odometry_msg); 168 | 169 | geometry_msgs::PoseStamped pose_msg; 170 | pose_msg.header.stamp = odometry_msg.header.stamp; 171 | pose_msg.header.frame_id = odometry_msg.header.frame_id; 172 | pose_msg.pose = odometry_msg.pose.pose; 173 | 174 | pose_pub_.publish(pose_msg); 175 | 176 | if (publish_tf_) 177 | { 178 | if (invert_tf_) 179 | { 180 | tf_broadcaster_.sendTransform( 181 | tf::StampedTransform(base_transform.inverse(), timestamp, 182 | base_link_frame_id_, odom_frame_id_)); 183 | } 184 | else 185 | { 186 | tf_broadcaster_.sendTransform( 187 | tf::StampedTransform(base_transform, timestamp, 188 | odom_frame_id_, base_link_frame_id_)); 189 | } 190 | } 191 | 192 | last_update_time_ = timestamp; 193 | } 194 | 195 | 196 | bool resetPose(std_srvs::Empty::Request&, std_srvs::Empty::Response&) 197 | { 198 | integrated_pose_.setIdentity(); 199 | return true; 200 | } 201 | 202 | }; 203 | 204 | } // end of namespace 205 | 206 | #endif 207 | 208 | -------------------------------------------------------------------------------- /viso2_ros/src/odometry_params.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace viso2_ros 8 | { 9 | 10 | namespace odometry_params 11 | { 12 | 13 | /// loads matcher params 14 | void loadParams(const ros::NodeHandle& local_nh, Matcher::parameters& params) 15 | { 16 | local_nh.getParam("nms_n", params.nms_n); 17 | local_nh.getParam("nms_tau", params.nms_tau); 18 | local_nh.getParam("match_binsize", params.match_binsize); 19 | local_nh.getParam("match_radius", params.match_radius); 20 | local_nh.getParam("match_disp_tolerance", params.match_disp_tolerance); 21 | local_nh.getParam("outlier_disp_tolerance", params.outlier_disp_tolerance); 22 | local_nh.getParam("outlier_flow_tolerance", params.outlier_flow_tolerance); 23 | local_nh.getParam("multi_stage", params.multi_stage); 24 | local_nh.getParam("half_resolution", params.half_resolution); 25 | local_nh.getParam("refinement", params.refinement); 26 | } 27 | 28 | /// loads bucketing params 29 | void loadParams(const ros::NodeHandle& local_nh, VisualOdometry::bucketing& bucketing) 30 | { 31 | local_nh.getParam("max_features", bucketing.max_features); 32 | local_nh.getParam("bucket_width", bucketing.bucket_width); 33 | local_nh.getParam("bucket_height", bucketing.bucket_height); 34 | } 35 | 36 | /// loads common odometry params 37 | void loadCommonParams(const ros::NodeHandle& local_nh, VisualOdometry::parameters& params) 38 | { 39 | loadParams(local_nh, params.match); 40 | loadParams(local_nh, params.bucket); 41 | } 42 | 43 | /// loads common & stereo specific params 44 | void loadParams(const ros::NodeHandle& local_nh, VisualOdometryStereo::parameters& params) 45 | { 46 | loadCommonParams(local_nh, params); 47 | local_nh.getParam("ransac_iters", params.ransac_iters); 48 | local_nh.getParam("inlier_threshold", params.inlier_threshold); 49 | local_nh.getParam("reweighting", params.reweighting); 50 | } 51 | 52 | /// loads common & mono specific params 53 | void loadParams(const ros::NodeHandle& local_nh, VisualOdometryMono::parameters& params) 54 | { 55 | loadCommonParams(local_nh, params); 56 | if (!local_nh.getParam("camera_height", params.height)) 57 | { 58 | ROS_WARN("Parameter 'camera_height' is required but not set. Using default: %f", params.height); 59 | } 60 | if (!local_nh.getParam("camera_pitch", params.pitch)) 61 | { 62 | ROS_WARN("Paramter 'camera_pitch' is required but not set. Using default: %f", params.pitch); 63 | } 64 | local_nh.getParam("ransac_iters", params.ransac_iters); 65 | local_nh.getParam("inlier_threshold", params.inlier_threshold); 66 | local_nh.getParam("motion_threshold", params.motion_threshold); 67 | } 68 | 69 | /// loads common & omnidirectional mono specific params 70 | void loadParams(const ros::NodeHandle& local_nh, VisualOdometryMonoOmnidirectional::parameters& params) 71 | { 72 | loadCommonParams(local_nh, params); 73 | local_nh.getParam("ransac_iters", params.ransac_iters); 74 | local_nh.getParam("inlier_threshold", params.inlier_threshold); 75 | local_nh.getParam("motion_threshold", params.motion_threshold); 76 | 77 | std::string path; 78 | local_nh.getParam("calib_path", path); 79 | std::ifstream file(path.c_str()); 80 | if(file.is_open()) 81 | { 82 | std::string s; 83 | 84 | //Read polynomial coefficients 85 | std::getline(file, s); 86 | file >> params.omnidirectional_calib.length_pol; 87 | for (int i = 0; i < params.omnidirectional_calib.length_pol; i++) 88 | { 89 | file >> params.omnidirectional_calib.pol[i]; 90 | } 91 | 92 | //Read inverse polynomial coefficients 93 | std::getline(file, s); 94 | std::getline(file, s); 95 | std::getline(file, s); 96 | file >> params.omnidirectional_calib.length_invpol; 97 | for (int i = 0; i < params.omnidirectional_calib.length_invpol; i++) 98 | { 99 | file >> params.omnidirectional_calib.invpol[i]; 100 | } 101 | 102 | //Read center coordinates 103 | std::getline(file, s); 104 | std::getline(file, s); 105 | std::getline(file, s); 106 | file >> params.omnidirectional_calib.xc >> 107 | params.omnidirectional_calib.yc; 108 | 109 | //Read affine coefficients 110 | std::getline(file, s); 111 | std::getline(file, s); 112 | std::getline(file, s); 113 | file >> params.omnidirectional_calib.c >> 114 | params.omnidirectional_calib.d >> 115 | params.omnidirectional_calib.e; 116 | 117 | //Read image size 118 | std::getline(file, s); 119 | std::getline(file, s); 120 | std::getline(file, s); 121 | file >> params.omnidirectional_calib.height >> 122 | params.omnidirectional_calib.width; 123 | 124 | file.close(); 125 | } 126 | else 127 | { 128 | std::cout << "File not found or path not provided" << std::endl; 129 | std::cout << "Using default parameters" << std::endl; 130 | params.omnidirectional_calib.fx = 1; 131 | params.omnidirectional_calib.fy = 1; 132 | params.omnidirectional_calib.cx = 0; 133 | params.omnidirectional_calib.cy = 0; 134 | params.omnidirectional_calib.xc = 1; 135 | params.omnidirectional_calib.yc = 1; 136 | params.omnidirectional_calib.c = 1; 137 | params.omnidirectional_calib.d = 1; 138 | params.omnidirectional_calib.e = 1; 139 | 140 | params.omnidirectional_calib.length_pol = 1; 141 | params.omnidirectional_calib.length_invpol = 1; 142 | params.omnidirectional_calib.width = 2; 143 | params.omnidirectional_calib.height = 2; 144 | 145 | params.omnidirectional_calib.pol[0] = 1; 146 | params.omnidirectional_calib.invpol[0] = 1; 147 | } 148 | 149 | } 150 | 151 | } // end of namespace 152 | 153 | std::ostream& operator<<(std::ostream& out, const Matcher::parameters& params) 154 | { 155 | out << " nms_n = " << params.nms_n << std::endl; 156 | out << " nms_tau = " << params.nms_tau << std::endl; 157 | out << " match_binsize = " << params.match_binsize << std::endl; 158 | out << " match_radius = " << params.match_radius << std::endl; 159 | out << " match_disp_tolerance = " << params.match_disp_tolerance << std::endl; 160 | out << " outlier_disp_tolerance = " << params.outlier_disp_tolerance << std::endl; 161 | out << " outlier_flow_tolerance = " << params.outlier_flow_tolerance << std::endl; 162 | out << " multi_stage = " << params.multi_stage << std::endl; 163 | out << " half_resolution = " << params.half_resolution << std::endl; 164 | out << " refinement = " << params.refinement << std::endl; 165 | return out; 166 | } 167 | 168 | std::ostream& operator<<(std::ostream& out, const VisualOdometry::calibration& calibration) 169 | { 170 | out << " f = " << calibration.f << std::endl; 171 | out << " cu = " << calibration.cu << std::endl; 172 | out << " cv = " << calibration.cv << std::endl; 173 | return out; 174 | } 175 | 176 | std::ostream& operator<<(std::ostream& out, const VisualOdometry::omnidirectional_calibration& params) 177 | { 178 | out << " poly = "; 179 | for (int i = 0; i < params.length_pol; i++) { 180 | out << params.pol[i] << " ; "; 181 | } 182 | out << std::endl; 183 | 184 | out << " inverse poly = "; 185 | for (int i = 0; i < params.length_invpol; i++) { 186 | out << params.invpol[i] << " ; "; 187 | } 188 | out << std::endl; 189 | 190 | out << " xc = " << params.xc << std::endl; 191 | out << " yc = " << params.yc << std::endl; 192 | out << " width = " << params.width << std::endl; 193 | out << " height = " << params.height << std::endl; 194 | out << " fx = " << params.fx << std::endl; 195 | out << " fy = " << params.fy << std::endl; 196 | out << " cx = " << params.cx << std::endl; 197 | out << " cy = " << params.cy << std::endl; 198 | return out; 199 | } 200 | 201 | std::ostream& operator<<(std::ostream& out, const VisualOdometry::bucketing& bucketing) 202 | { 203 | out << " max_features = " << bucketing.max_features << std::endl; 204 | out << " bucket_width = " << bucketing.bucket_width << std::endl; 205 | out << " bucket_height = " << bucketing.bucket_height << std::endl; 206 | return out; 207 | } 208 | 209 | std::ostream& operator<<(std::ostream& out, const VisualOdometry::parameters& params) 210 | { 211 | out << "Calibration parameters:" << std::endl << params.calib; 212 | out << "Matcher parameters:" << std::endl << params.match; 213 | out << "Bucketing parameters:" << std::endl << params.bucket; 214 | return out; 215 | } 216 | 217 | std::ostream& operator<<(std::ostream& out, const VisualOdometryStereo::parameters& params) 218 | { 219 | out << static_cast(params); 220 | out << "Stereo odometry parameters:" << std::endl; 221 | out << " base = " << params.base << std::endl; 222 | out << " ransac_iters = " << params.ransac_iters << std::endl; 223 | out << " inlier_threshold = " << params.inlier_threshold << std::endl; 224 | out << " reweighting = " << params.reweighting << std::endl; 225 | return out; 226 | } 227 | 228 | std::ostream& operator<<(std::ostream& out, const VisualOdometryMono::parameters& params) 229 | { 230 | out << static_cast(params); 231 | out << "Mono odometry parameters:" << std::endl; 232 | out << " camera_height = " << params.height << std::endl; 233 | out << " camera_pitch = " << params.pitch << std::endl; 234 | out << " ransac_iters = " << params.ransac_iters << std::endl; 235 | out << " inlier_threshold = " << params.inlier_threshold << std::endl; 236 | out << " motion_threshold = " << params.motion_threshold << std::endl; 237 | return out; 238 | } 239 | 240 | 241 | } // end of namespace 242 | 243 | -------------------------------------------------------------------------------- /viso2_ros/src/stereo_processor.h: -------------------------------------------------------------------------------- 1 | #ifndef STEREO_PROCESSOR_H_ 2 | #define STEREO_PROCESSOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace viso2_ros 15 | { 16 | 17 | /** 18 | * This is an abstract base class for stereo image processing nodes. 19 | * It handles synchronization of input topics (approximate or exact) 20 | * and checks for sync errors. 21 | * To use this class, subclass it and implement the imageCallback() method. 22 | */ 23 | class StereoProcessor 24 | { 25 | 26 | private: 27 | 28 | // subscriber 29 | image_transport::SubscriberFilter left_sub_, right_sub_; 30 | message_filters::Subscriber left_info_sub_, right_info_sub_; 31 | typedef message_filters::sync_policies::ExactTime ExactPolicy; 32 | typedef message_filters::sync_policies::ApproximateTime ApproximatePolicy; 33 | typedef message_filters::Synchronizer ExactSync; 34 | typedef message_filters::Synchronizer ApproximateSync; 35 | boost::shared_ptr exact_sync_; 36 | boost::shared_ptr approximate_sync_; 37 | int queue_size_; 38 | 39 | // for sync checking 40 | ros::WallTimer check_synced_timer_; 41 | int left_received_, right_received_, left_info_received_, right_info_received_, all_received_; 42 | 43 | // for sync checking 44 | static void increment(int* value) 45 | { 46 | ++(*value); 47 | } 48 | 49 | void dataCb(const sensor_msgs::ImageConstPtr& l_image_msg, 50 | const sensor_msgs::ImageConstPtr& r_image_msg, 51 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 52 | const sensor_msgs::CameraInfoConstPtr& r_info_msg) 53 | { 54 | 55 | // For sync error checking 56 | ++all_received_; 57 | 58 | // call implementation 59 | imageCallback(l_image_msg, r_image_msg, l_info_msg, r_info_msg); 60 | } 61 | 62 | void checkInputsSynchronized() 63 | { 64 | int threshold = 3 * all_received_; 65 | if (left_received_ >= threshold || right_received_ >= threshold || 66 | left_info_received_ >= threshold || right_info_received_ >= threshold) { 67 | ROS_WARN("[stereo_processor] Low number of synchronized left/right/left_info/right_info tuples received.\n" 68 | "Left images received: %d (topic '%s')\n" 69 | "Right images received: %d (topic '%s')\n" 70 | "Left camera info received: %d (topic '%s')\n" 71 | "Right camera info received: %d (topic '%s')\n" 72 | "Synchronized tuples: %d\n" 73 | "Possible issues:\n" 74 | "\t* stereo_image_proc is not running.\n" 75 | "\t Does `rosnode info %s` show any connections?\n" 76 | "\t* The cameras are not synchronized.\n" 77 | "\t Try restarting the node with parameter _approximate_sync:=True\n" 78 | "\t* The network is too slow. One or more images are dropped from each tuple.\n" 79 | "\t Try restarting the node, increasing parameter 'queue_size' (currently %d)", 80 | left_received_, left_sub_.getTopic().c_str(), 81 | right_received_, right_sub_.getTopic().c_str(), 82 | left_info_received_, left_info_sub_.getTopic().c_str(), 83 | right_info_received_, right_info_sub_.getTopic().c_str(), 84 | all_received_, ros::this_node::getName().c_str(), queue_size_); 85 | } 86 | } 87 | 88 | 89 | protected: 90 | 91 | /** 92 | * Constructor, subscribes to input topics using image transport and registers 93 | * callbacks. 94 | * \param transport The image transport to use 95 | */ 96 | StereoProcessor(const std::string& transport) : 97 | left_received_(0), right_received_(0), left_info_received_(0), right_info_received_(0), all_received_(0) 98 | { 99 | // Read local parameters 100 | ros::NodeHandle local_nh("~"); 101 | 102 | // Resolve topic names 103 | ros::NodeHandle nh; 104 | std::string stereo_ns = nh.resolveName("stereo"); 105 | std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image")); 106 | std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image")); 107 | 108 | std::string left_info_topic = stereo_ns + "/left/camera_info"; 109 | std::string right_info_topic = stereo_ns + "/right/camera_info"; 110 | 111 | // Subscribe to four input topics. 112 | ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s", 113 | left_topic.c_str(), right_topic.c_str(), 114 | left_info_topic.c_str(), right_info_topic.c_str()); 115 | 116 | image_transport::ImageTransport it(nh); 117 | left_sub_.subscribe(it, left_topic, 3, transport); 118 | right_sub_.subscribe(it, right_topic, 3, transport); 119 | left_info_sub_.subscribe(nh, left_info_topic, 3); 120 | right_info_sub_.subscribe(nh, right_info_topic, 3); 121 | 122 | // Complain every 15s if the topics appear unsynchronized 123 | left_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_received_)); 124 | right_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_received_)); 125 | left_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_info_received_)); 126 | right_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_info_received_)); 127 | check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0), 128 | boost::bind(&StereoProcessor::checkInputsSynchronized, this)); 129 | 130 | // Synchronize input topics. Optionally do approximate synchronization. 131 | local_nh.param("queue_size", queue_size_, 5); 132 | bool approx; 133 | local_nh.param("approximate_sync", approx, false); 134 | if (approx) 135 | { 136 | approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_), 137 | left_sub_, right_sub_, left_info_sub_, right_info_sub_) ); 138 | approximate_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4)); 139 | } 140 | else 141 | { 142 | exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_), 143 | left_sub_, right_sub_, left_info_sub_, right_info_sub_) ); 144 | exact_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4)); 145 | } 146 | } 147 | 148 | /** 149 | * Implement this method in sub-classes 150 | */ 151 | virtual void imageCallback(const sensor_msgs::ImageConstPtr& l_image_msg, 152 | const sensor_msgs::ImageConstPtr& r_image_msg, 153 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 154 | const sensor_msgs::CameraInfoConstPtr& r_info_msg) = 0; 155 | 156 | }; 157 | 158 | } // end of namespace 159 | 160 | #endif 161 | 162 | -------------------------------------------------------------------------------- /viso2_ros/wikidoc.txt: -------------------------------------------------------------------------------- 1 | <> 2 | <> 3 | 4 | == Overview == 5 | This package contains two nodes that talk to [[http://www.cvlibs.net/software/libviso2.html|libviso2]] (which is included in the [[libviso2]] package): `mono_odometer` and `stereo_odometer`. Both estimate camera motion based on incoming rectified images from calibrated cameras. To estimate the scale of the motion, the mono odometer uses the ground plane and therefore needs information about the camera's z-coordinate and its pitch. The stereo odometer needs no additional parameters and works - if provided with images of good quality - out of the box. 6 | 7 | The video below shows an online 3D reconstruction of a 3D scene shot by a Micro AUV using dense stereo point clouds coming from [[stereo_image_proc]] concatenated in [[rviz]] using the stereo odometer of this package. 8 | 9 | <> 10 | 11 | == Used tfs == 12 | Please read [[http://www.ros.org/reps/rep-0105.html|REP 105]] for an explanation of odometry frame ids. 13 | 14 | The chain of transforms relevant for visual odometry is as follows: 15 | `world` → `odom` → `base_link` → `camera` 16 | 17 | Visual odometry algorithms generally calculate ''camera motion''. To be able to calculate ''robot motion'' based on ''camera motion'', the transformation from the camera frame to the robot frame has to be known. 18 | Therefore this implementation needs to know the tf `base_link` → `camera` to be able to publish `odom` → `base_link`. 19 | 20 | The name of the camera frame is taken from the incoming images, so be sure your camera driver publishes it correctly. If your camera driver does not set frame ids, you can use the fallback parameter `sensor_frame_id` (see below). 21 | 22 | ''NOTE'': The coordinate frame of the camera is expected to be the ''optical'' frame, which means `x` is pointing right, `y` downwards and `z` from the camera into the scene. The origin is where the camera's principle axis hits the image plane (as given in <>). 23 | 24 | To learn how to publish the required tf `base_link` → `camera`, please refer to the [[tf/Tutorials|tf tutorials]]. 25 | If the required tf is not available, the odometer assumes it as the identity matrix which means the robot frame and the camera frame are identical. 26 | 27 | == Limitations == 28 | libviso2 was designed to estimate the motion of a car using wide angle cameras. Cameras with large focal lengths have less overlap between consecutive images, especially on rotations and are therefore not recommended. 29 | 30 | === Monocular Odometry === 31 | Monocular odometry and SLAM systems cannot estimate motion or position on a metric scale. All estimates are relative to some unknown scaling factor. libviso2 overcomes this by assuming a fixed transformation from the ground plane to the camera (parameters `camera_height` and `camera_pitch`). To introduce these values in each iteration the ground plane has to be estimated. That is why features on the ground are mandatory for the mono odometer to work. 32 | 33 | 34 | Roughly the steps are the following: 35 | 36 | 1. Find F matrix from point correspondences using RANSAC and 8-point algorithm 37 | 2. Compute E matrix using the camera calibration 38 | 3. Compute 3D points and R|t up to scale 39 | 4. Estimate the ground plane in the 3D points 40 | 5. Use `camera_height` and `camera_pitch` to scale points and R|t 41 | Unfortunately libviso2 does not provide sufficient introspection to signal if one of these steps fails. 42 | 43 | Another problem occurs when the camera performs just pure rotation: even if there are enough features, the linear system to calculate the F matrix degenerates. 44 | 45 | === Stereo Odometry === 46 | In a properly calibrated stereo system 3D points can be calculated from a single image pair. The linear system to calculate camera motion is therefore based on 3D-3D point correspondences. There are no limitations for the camera movement or the feature distribution. 47 | 48 | == Nodes == 49 | {{{ 50 | #!clearsilver CS/NodeAPI 51 | name = Common for mono_odometer and stereo_odometer 52 | pub { 53 | 0.name = ~pose 54 | 0.type = geometry_msgs/PoseStamped 55 | 0.desc = The robot's current pose according to the odometer. 56 | 1.name = ~odometry 57 | 1.type = nav_msgs/Odometry 58 | 1.desc = Odometry information that was calculated, contains pose, twist and covariances. ''NOTE:'' pose and twist covariance is not published yet. 59 | 2.name = ~info 60 | 2.type = viso2_ros/VisoInfo 61 | 2.desc = Message containing internal information on the libviso2 process regarding the current iteration. 62 | } 63 | param { 64 | group.0 { 65 | name = tf related 66 | 0.name = ~odom_frame_id 67 | 0.type = string 68 | 0.desc = Name of the world-fixed frame where the odometer lives. 69 | 0.default = `/odom` 70 | 1.name = ~base_link_frame_id 71 | 1.type = string 72 | 1.desc = Name of the moving frame whose pose the odometer should report. 73 | 1.default = `/base_link` 74 | 2.name = ~publish_tf 75 | 2.type = bool 76 | 2.desc = If true, the odometer publishes tf's (see above). 77 | 2.default = true 78 | 3.name = ~sensor_frame_id 79 | 3.type = string 80 | 3.desc = Fallback sensor frame id. If the incoming camera info topic does not carry a frame id, this frame id will be used. 81 | 3.default = "/camera" 82 | } 83 | group.1 { 84 | name = Bucketing parameters 85 | 0.name = ~max_features 86 | 0.type = int 87 | 0.desc = Maximum number of features per bucket. 88 | 0.default = 2 89 | 1.name = ~bucket_width 90 | 1.type = double 91 | 1.desc = Width of the bucket. 92 | 1.default = 50.0 93 | 2.name = ~bucket_height 94 | 2.type = double 95 | 2.desc = Height of the bucket. 96 | 2.default = 50.0 97 | } 98 | group.2 { 99 | name = Matcher parameters 100 | 0.name = ~nms_n 101 | 0.type = int 102 | 0.desc = Minimum distance between maxima in pixels for non-maxima-suppression. 103 | 0.default = 3 104 | 1.name = ~nms_tau 105 | 1.type = int 106 | 1.desc = Interest point peakiness threshold. 107 | 1.default = 50 108 | 2.name = ~match_binsize 109 | 2.type = int 110 | 2.desc = Matching width/height (affects efficiency only) 111 | 2.default = 50 112 | 3.name = ~match_radius 113 | 3.type = int 114 | 3.desc = Matching radius (du/dv in pixels) 115 | 3.default = 200 116 | 4.name = ~match_disp_tolerance 117 | 4.type = int 118 | 4.desc = dv tolerance for stereo matches (in pixels). 119 | 4.default = 2 120 | 5.name = ~outlier_disp_tolerance 121 | 5.type = int 122 | 5.desc = Disparity tolerance for outlier removal (in pixels). 123 | 5.default = 5 124 | 6.name = ~outlier_flow_tolerance 125 | 6.type = int 126 | 6.desc = Flow tolerance for outlier removal (in pixels). 127 | 6.default = 5 128 | 7.name = ~multi_stage 129 | 7.type = int 130 | 7.desc = 0=disabled, 1=multistage matching (denser and faster). 131 | 7.default = 1 132 | 8.name = ~half_resolution 133 | 8.type = int 134 | 8.desc = 0=disabled, 1=match at half resolution, refine at full resolution. 135 | 8.default = 1 136 | 9.name = ~refinement 137 | 9.type = int 138 | 9.desc = 0=none, 1=pixel, 2=subpixel. 139 | 9.default = 1 140 | } 141 | } 142 | req_tf { 143 | 0.from = ~base_link_frame_id 144 | 0.to = 145 | 0.desc = Transformation from the robot's reference point (`base_link` in most cases) to the camera's optical frame. 146 | } 147 | prov_tf { 148 | 0.from = ~odom_frame_id 149 | 0.to = ~base_link_frame_id 150 | 0.desc = Transformation from the odometry's origin (e.g. `odom`) to the robot's reference point (e.g. `base_link`) 151 | } 152 | }}} 153 | 154 | {{{ 155 | #!clearsilver CS/NodeAPI 156 | name = mono_odometer 157 | sub { 158 | 0.name = image 159 | 0.type = sensor_msgs/Image 160 | 0.desc = The rectified input image. There must be a corresponding `camera_info` topic as well. Internally, `image_transport::CameraSubscriber` is used to synchronize between both. 161 | } 162 | param { 163 | 0.name = ~camera_height 164 | 0.type = double 165 | 0.desc = Height of the camera above the ground in meters. 166 | 0.default = 1.0 167 | 1.name = ~camera_pitch 168 | 1.type = double 169 | 1.desc = Pitch of the camera in radiants, negative pitch means looking downwards. 170 | 1.default = 0.0 171 | 2.name = ~ransac_iters 172 | 2.type = int 173 | 2.desc = Number of RANSAC iterations. 174 | 2.default = 2000 175 | 3.name = ~inlier_threshold 176 | 3.type = double 177 | 3.desc = Fundamental matrix inlier threshold. 178 | 3.default = 0.00001 179 | 4.name = ~motion_threshold 180 | 4.type = double 181 | 4.desc = Threshold for stable fundamental matrix estimation. 182 | 4.default = 100.0 183 | } 184 | }}} 185 | 186 | {{{ 187 | #!clearsilver CS/NodeAPI 188 | name = stereo_odometer 189 | sub { 190 | 0.name = /left/ 191 | 0.type = sensor_msgs/Image 192 | 0.desc = Left rectified input image. 193 | 1.name = /right/ 194 | 1.type = sensor_msgs/Image 195 | 1.desc = Right rectified input image. 196 | 2.name = /left/camera_info 197 | 2.type = sensor_msgs/CameraInfo 198 | 2.desc = Camera info for left image. 199 | 3.name = /right/camera_info 200 | 3.type = sensor_msgs/CameraInfo 201 | 3.desc = Camera info for right image. 202 | } 203 | pub { 204 | 0.name = ~point_cloud 205 | 0.type = sensor_msgs/PointCloud2 206 | 0.desc = Point cloud formed by the matched features. 207 | } 208 | param { 209 | 0.name = ~queue_size 210 | 0.type = int 211 | 0.desc = Length of the input queues for left and right camera synchronization. 212 | 0.default = 5 213 | 1.name = ~approximate_sync 214 | 1.type = bool 215 | 1.desc = Approximate synchronization of incoming messages, set to true if cameras do not have synchronized timestamps. 216 | 1.default = false 217 | 2.name = ~ransac_iters 218 | 2.type = int 219 | 2.desc = Number of RANSAC iterations. 220 | 2.default = 200 221 | 3.name = ~inlier_threshold 222 | 3.type = double 223 | 3.desc = Fundamental matrix inlier threshold. 224 | 3.default = 1.5 225 | 4.name = ~reweighting 226 | 4.type = bool 227 | 4.desc = Lower border weights (more robust to calibration errors). 228 | 4.default = true 229 | 5.name = ~motion_threshold 230 | 5.type = double 231 | 5.desc = Threshold for drift compensation. If the mean movement in pixels of all features lies below this threshold, the reference image inside the odometer will not be changed. 232 | 5.default = 5.0 233 | } 234 | }}} 235 | 236 | == Troubleshoting == 237 | If you have a problem, please look if it is stated here or on ROS Answers (FAQ link above) and you can solve it on your own. 238 | === I run mono_odometer but I get no messages on the output topics === 239 | * Check if incoming image and camera_info messages are synchronized. 240 | * Move the camera. To estimate motion the mono odometer actually needs some motion (else the estimation of the F-matrix is degenerating). The system needs the camera to perform a translation, pure rotation will not work. 241 | * Set the log level of mono_odometer to DEBUG (e.g. using [[rxconsole]]) and look if you can find something. 242 | 243 | == Feedback == 244 | Please use the stack's issue tracker at Github to submit bug reports and feature requests regarding the ROS wrapper of libviso2: https://github.com/srv/viso2/issues/new. 245 | 246 | 247 | ## AUTOGENERATED DON'T DELETE 248 | ## CategoryPackage 249 | 250 | --------------------------------------------------------------------------------