├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── ROS └── jpp │ ├── CMakeLists.txt │ ├── cfg │ ├── CamToRobotCalibParams.cfg │ └── Parameters.cfg │ ├── manifest.xml │ └── src │ ├── naive_navigation.cpp │ ├── navigation.cpp │ └── path_follower.cpp ├── build.sh ├── build_ros.sh ├── calibration ├── amrl_jackal_webcam_stereo.yml └── kitti_2011_09_26.yml ├── cfg ├── amrl.cfg └── kitti.cfg ├── dumps ├── astar126-path.jpg ├── astar126-vis.jpg ├── astar7-path.jpg ├── astar7-vis.jpg ├── rrt73-path.jpg └── rrt73-vis.jpg ├── include ├── astarplanner.h ├── jpp.h ├── popt_pp.h ├── rrt.h ├── stereo.h └── util.h ├── scripts ├── plot_epipolar_costs.py └── profile └── src ├── analysis.cc ├── astarplanner.cc ├── daisy_descriptor ├── include │ ├── daisy │ │ └── daisy.h │ └── kutility │ │ ├── convolution.h │ │ ├── convolution_default.h │ │ ├── convolution_opencv.h │ │ ├── corecv.h │ │ ├── fileio.h │ │ ├── fileio.tcc │ │ ├── general.h │ │ ├── image.h │ │ ├── image_io.h │ │ ├── image_io_bmp.h │ │ ├── image_io_jpeg.h │ │ ├── image_io_png.h │ │ ├── image_io_pnm.h │ │ ├── image_manipulation.h │ │ ├── interaction.h │ │ ├── kutility.def │ │ ├── math.h │ │ └── progress_bar.h └── src │ ├── corecv.cpp │ ├── daisy.cpp │ ├── general.cpp │ ├── image_io_bmp.cpp │ ├── image_io_jpeg.cpp │ ├── image_io_png.cpp │ ├── image_io_pnm.cpp │ ├── image_manipulation.cpp │ ├── interaction.cpp │ └── progress_bar.cpp ├── jpp.cc ├── main.cc ├── rrt.cc └── stereo.cc /.gitignore: -------------------------------------------------------------------------------- 1 | *.kdev* 2 | ROS/jpp/*.kdev* 3 | build/ 4 | bin/ 5 | lib/ 6 | 7 | ROS/jpp/build/ 8 | ROS/jpp/bin/ 9 | ROS/jpp/lib/ 10 | 11 | # Generated by dynamic reconfigure 12 | ROS/jpp/cfg/*.cfgc 13 | ROS/jpp/cfg/cpp/ 14 | ROS/jpp/cfg/*.py 15 | ROS/jpp/src/jpp/ 16 | 17 | # Ignore generated docs 18 | ROS/jpp/docs/ 19 | *.dox 20 | *.wikidoc 21 | 22 | scripts/profile_results 23 | .ycm_extra_conf.py 24 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(libjpp) 3 | 4 | #set(DAISY_DESC_INC_DIR /usr/local/lib/libdaisy.so) 5 | set(DAISY_DESC_INC_DIR ./src/daisy_descriptor/include) 6 | set(JPP_INC_DIR ./include) 7 | set(Boost_USE_STATIC_LIBS OFF) 8 | set(Boost_USE_MULTITHREADED ON) 9 | set(Boost_USE_STATIC_RUNTIME OFF) 10 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 11 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 12 | 13 | if(NOT CMAKE_BUILD_TYPE) 14 | set(CMAKE_BUILD_TYPE Release) 15 | endif() 16 | 17 | if (CMAKE_VERSION VERSION_LESS "3.1") 18 | if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 19 | set (CMAKE_CXX_FLAGS "-std=gnu++11 ${CMAKE_CXX_FLAGS}") 20 | endif () 21 | else () 22 | set (CMAKE_CXX_STANDARD 11) 23 | endif () 24 | 25 | set(CMAKE_CXX_FLAGS "-march=native -O3 -Wall ${CMAKE_CXX_FLAGS}") 26 | 27 | find_package(OpenCV REQUIRED) 28 | find_package(Eigen3 REQUIRED) 29 | find_package(OpenMP) 30 | if (OPENMP_FOUND) 31 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 33 | endif() 34 | find_package(Boost 1.54.0 COMPONENTS system filesystem program_options REQUIRED) 35 | 36 | include_directories( 37 | ${DAISY_DESC_INC_DIR} 38 | ${OpenCV_INCLUDE_DIRS} 39 | ${OpenMP_INCLUDE_DIRS} 40 | ${Boost_INCLUDE_DIRS} 41 | ${EIGEN3_INCLUDE_DIR} 42 | ${JPP_INC_DIR} 43 | ) 44 | 45 | file(GLOB DAISY_DESC_SRC_FILES "src/daisy_descriptor/src/*") 46 | file(GLOB JPP_LIB_FILES 47 | "src/stereo.cc" 48 | "src/jpp.cc" 49 | "src/astarplanner.cc" 50 | "src/rrt.cc" 51 | ) 52 | file(GLOB JPP_BIN_FILES 53 | "src/main.cc" 54 | ) 55 | 56 | add_library(libjpp SHARED ${DAISY_DESC_SRC_FILES} ${JPP_LIB_FILES}) 57 | set_target_properties(libjpp PROPERTIES OUTPUT_NAME "jpp") 58 | target_link_libraries(libjpp ${OpenCV_LIBS} ${OpenMP_LIBS} config) 59 | 60 | add_executable(jpp ${JPP_BIN_FILES}) 61 | target_link_libraries(jpp popt config libjpp) 62 | 63 | add_executable(analysis "src/analysis.cc") 64 | target_link_libraries(analysis popt config libjpp) 65 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Autonomous Mobile Robotics Lab, University of Massachusetts Amherst 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Joint Perception and Planning For Efficient Obstacle Avoidance Using Stereo Vision 2 | 3 | This repository contains a C++ implementation of JPP for local obstacle avoidance using stereo cameras. A ROS wrapper for JPP is also included in the `ROS/` 4 | folder. JPP uses disparity confidence checks on-demand instead of the traditional dense 3D reconstruction approach while performing obstacle avoidance during 5 | navigation planning, thus significantly saving computational cost. 6 | 7 | - Author: [Sourish Ghosh](http://sourishghosh.com/) 8 | 9 | ## Publication 10 | 11 | If you use this software in an academic work or find it relevant to your research, kindly cite: 12 | 13 | ``` 14 | @inproceedings{ghosh2017joint, 15 | doi = { 10.1109/IROS.2017.8202271 }, 16 | url = { https://www.joydeepb.com/Publications/jpp.pdf }, 17 | pages = { 1026--1031 }, 18 | organization = { IEEE }, 19 | year = { 2017 }, 20 | booktitle = { Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on }, 21 | author = { Sourish Ghosh and Joydeep Biswas }, 22 | title = { Joint Perception And Planning For Efficient Obstacle Avoidance Using Stereo Vision }, 23 | } 24 | ``` 25 | 26 | Link to paper: [https://www.joydeepb.com/Publications/jpp.pdf](https://www.joydeepb.com/Publications/jpp.pdf) 27 | 28 | ## Dependencies 29 | 30 | - A C++ compiler (*e.g.*, [GCC](http://gcc.gnu.org/)) 31 | - [cmake](http://www.cmake.org/cmake/resources/software.html) 32 | - [popt](http://freecode.com/projects/popt) 33 | - [libconfig](http://www.hyperrealm.com/libconfig/libconfig.html) 34 | - [Boost](http://www.boost.org/) 35 | - [OpenCV](https://github.com/opencv/opencv) 36 | - [OpenMP](http://www.openmp.org/) 37 | 38 | Use the following command to install dependencies: 39 | 40 | ```bash 41 | $ sudo apt-get install g++ cmake libpopt-dev libconfig-dev libboost-all-dev libopencv-dev python-opencv gcc-multilib 42 | ``` 43 | 44 | For compiling and running the ROS wrapper, install [ROS Indigo](http://wiki.ros.org/indigo/Installation/Ubuntu). 45 | 46 | ## Compiling 47 | 48 | ### 1. Building JPP libraries and binaries 49 | 50 | Clone the repository: 51 | 52 | ```bash 53 | $ git clone https://github.com/umass-amrl/jpp 54 | ``` 55 | 56 | The script `build.sh` compiles the JPP library: 57 | 58 | ```bash 59 | $ cd jpp 60 | $ chmod +x build.sh 61 | $ ./build.sh 62 | ``` 63 | 64 | ### 2. Building JPP ROS 65 | 66 | For compiling the ROS wrapper, `rosbuild` is used. Add the path of the ROS wrapper to `ROS_PACKAGE_PATH` and put the following line in your `.bashrc` file. 67 | Replace `PATH` by the actual path where you have cloned the repository: 68 | 69 | ```bash 70 | $ export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/PATH/jpp/ROS 71 | ``` 72 | 73 | Execute the `build_ros.sh` script: 74 | 75 | ```bash 76 | $ chmod +x build_ros.sh 77 | $ ./build_ros.sh 78 | ``` 79 | 80 | ## Running JPP on AMRL and KITTI Datasets 81 | 82 | ### 1. Download Datasets 83 | 84 | The complete example data (AMRL and KITTI) along with calibration files can be found 85 | [here](https://greyhound.cs.umass.edu/owncloud/index.php/s/3g9AwCSkGi6LznK). 86 | 87 | ### 2. Running JPP 88 | 89 | After compilation, the `jpp` binary file is store inside the `bin/` folder. For processing a single pair of stereo images, use: 90 | 91 | ```bash 92 | $ ./bin/jpp -l [path/to/left/image] -r [path/to/right/image] -c [path/to/stereo/calibration/file] -j [path/to/jpp/config/file] -o [output_mode] 93 | ``` 94 | 95 | For processing multiple stereo pairs stored in a directory, use: 96 | 97 | ```bash 98 | $ ./bin/jpp -n [number of pairs] -d [path/to/directory] -c [path/to/stereo/calibration/file] -j [path/to/jpp/config/file] -o [output_mode] 99 | ``` 100 | 101 | **Note:** stereo image pairs inside the directory must be named like this: `left1.jpg`, `left2.jpg`, ... , `right1.jpg`, `right2.jpg`, ... 102 | 103 | For the example datasets, calibration files are stored in the `calibration/` folder and JPP configurations are stored in the `cfg/` folder. JPP operates on 104 | 3 output modes (set by the `-o` flag) as of now: `astar`, `rrt`, and `debug` mode. Set the flag `-v 1` for generating visualizations. 105 | 106 | ```bash 107 | Usage: jpp [OPTION...] 108 | -n, --num_imgs=NUM Number of images to be processed 109 | -d, --img_dir=STR Directory containing image pairs (set if n > 0) 110 | -l, --left_img=STR Left image file name 111 | -r, --right_img=STR Right image file name 112 | -c, --calib_file=STR Stereo calibration file name 113 | -j, --jpp_config_file=STR JPP config file name 114 | -o, --output=STR Output - astar, rrt, debug 115 | -v, --visualize=NUM Set v=1 for displaying visualizations 116 | -w, --write_files=NUM Set w=1 for writing visualizations to files 117 | ``` 118 | 119 | For example, running JPP on the KITTI dataset in `astar` mode: 120 | 121 | ```bash 122 | $ ./bin/jpp -n 33 -d KITTI/ -c calibration/kitti_2011_09_26.yml -j cfg/kitti.cfg -o astar -v 1 123 | ``` 124 | 125 | |Confidence match visualizations | Path visualization | 126 | |:------------------------------:|:-------------------------:| 127 | |![](dumps/astar7-vis.jpg) | ![](dumps/astar7-path.jpg)| 128 | 129 | Running JPP on the AMRL dataset in `rrt` mode: 130 | 131 | ```bash 132 | $ ./bin/jpp -n 158 -d AMRL/ -c calibration/amrl_jackal_webcam_stereo.yml -j cfg/amrl.cfg -o rrt -v 1 133 | ``` 134 | 135 | |Confidence match visualizations | Path visualization | 136 | |:------------------------------:|:-------------------------:| 137 | |![](dumps/rrt73-vis.jpg) | ![](dumps/rrt73-path.jpg) | 138 | 139 | **Note:** Press any key to move on to the next image pair. 140 | 141 | ### 3. Running JPP ROS 142 | 143 | Run the ROS node `navigation`: 144 | 145 | ```bash 146 | $ rosrun jpp navigation -l [left/image/topic] -r [right/image/topic] -c [path/to/stereo/calibration/file] -j [path/to/jpp/config/file] -o [output_mode] 147 | ``` 148 | 149 | The same flags for displaying/writing visualizations can be used for the ROS node as well. 150 | 151 | ```bash 152 | Usage: navigation [OPTION...] 153 | -l, --left_topic=STR Left image topic name 154 | -r, --right_topic=STR Right image topic name 155 | -c, --calib_file=STR Stereo calibration file name 156 | -j, --jpp_config_file=STR JPP config file name 157 | -o, --output=STR Output - astar, rrt, debug 158 | -v, --visualize=NUM Set v=1 for displaying visualizations 159 | -w, --write_files=NUM Set w=1 for writing visualizations to files 160 | -d, --dynamic_reconfigure=NUM Set d=1 for enabling dynamic reconfigure 161 | ``` 162 | 163 | JPP configuration parameters can be changed realtime by using `rqt_reconfigure`: 164 | 165 | ```bash 166 | $ rosrun rqt_reconfigure rqt_reconfigure 167 | ``` 168 | 169 | Make sure you set the flag `-d 1` while using dynamic reconfigure. 170 | 171 | ## Running JPP on your Datasets 172 | 173 | ### 1. Stereo Calibration 174 | 175 | To run JPP on your own data, you need to have a pair of calibrated stereo cameras. For stereo calibration it is recommended to use 176 | [this tool](https://github.com/sourishg/stereo-calibration). The `XR` and `XT` matrices in the calibration file are the transformation matrices from the left 177 | camera reference frame to the robot reference frame. These matrices depends on how the stereo camera is mounted on the robot. Initially after stereo 178 | calibration (using the tool mentioned) you will not have the `XR` and `XT` matrices in your calibration file. You need to manually calibrate them and add them 179 | to the calibration file. Also, you only need the following matrices in your calibration file: `K1`, `K2`, `D1`, `D2`, `R`, `T`, `XR`, and `XT`. An example 180 | calibration file can be found inside the `calibration/` folder. 181 | 182 | If you cannot calibrate for `XR` and `XT` then just set them to the identity and zero matrices respectively. Then use this [stereo dense 183 | reconstruction](https://github.com/umass-amrl/stereo_dense_reconstruction) tool to visualize how the point cloud looks in the robot reference frame and 184 | visually align the ground plane with `z=0`. 185 | 186 | ### 2. Running JPP 187 | 188 | JPP can be run in the same way as explained for the exmaple AMRL and KITTI datasets. 189 | 190 | ## License 191 | 192 | This software is released under the [MIT license](LICENSE). 193 | -------------------------------------------------------------------------------- /ROS/jpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(jpp) 3 | 4 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 5 | 6 | rosbuild_init() 7 | 8 | IF(NOT ROS_BUILD_TYPE) 9 | SET(ROS_BUILD_TYPE Release) 10 | ENDIF() 11 | 12 | MESSAGE("Build type: " ${ROS_BUILD_TYPE}) 13 | 14 | if (CMAKE_VERSION VERSION_LESS "3.1") 15 | if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 16 | set (CMAKE_CXX_FLAGS "-std=gnu++11 ${CMAKE_CXX_FLAGS}") 17 | endif () 18 | else () 19 | set (CMAKE_CXX_STANDARD 11) 20 | endif () 21 | set(CMAKE_CXX_FLAGS "-march=native -O3 -Wall ${CMAKE_CXX_FLAGS}") 22 | 23 | rosbuild_find_ros_package(dynamic_reconfigure) 24 | include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) 25 | gencfg() 26 | 27 | set(JPP_LIBS ${PROJECT_SOURCE_DIR}/../../lib/libjpp.so) 28 | 29 | find_package(Eigen3 REQUIRED) 30 | find_package(OpenCV REQUIRED) 31 | set(DAISY_DESC_INC_DIR ${CMAKE_SOURCE_DIR}/../../src/daisy_descriptor/include) 32 | set(LIBJPP_INC_DIR ${CMAKE_SOURCE_DIR}/../../include) 33 | find_package(OpenMP) 34 | if (OPENMP_FOUND) 35 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 36 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 37 | endif() 38 | set(Boost_USE_STATIC_LIBS OFF) 39 | set(Boost_USE_MULTITHREADED ON) 40 | set(Boost_USE_STATIC_RUNTIME OFF) 41 | find_package(Boost 1.54.0 COMPONENTS system filesystem program_options REQUIRED) 42 | 43 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 44 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 45 | 46 | include_directories( 47 | ${DAISY_DESC_INC_DIR} 48 | ${OpenCV_INCLUDE_DIRS} 49 | ${OpenMP_INCLUDE_DIRS} 50 | ${Boost_INCLUDE_DIRS} 51 | ${EIGEN3_INCLUDE_DIR} 52 | ${LIBJPP_INC_DIR} 53 | ) 54 | 55 | rosbuild_add_executable(navigation 56 | src/navigation.cpp 57 | ) 58 | target_link_libraries(navigation ${JPP_LIBS} config popt) 59 | 60 | rosbuild_add_executable(path_follower 61 | src/path_follower.cpp 62 | ) 63 | target_link_libraries(path_follower ${JPP_LIBS} "-lconfig -lpopt") 64 | -------------------------------------------------------------------------------- /ROS/jpp/cfg/CamToRobotCalibParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "jpp" 3 | 4 | import roslib;roslib.load_manifest(PACKAGE) 5 | 6 | from dynamic_reconfigure.parameter_generator import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("PHI_X", double_t, 0, "Euler angle X", 1.3, -6.28, 6.28) 11 | gen.add("PHI_Y", double_t, 0, "Euler angle Y", -3.14, -6.28, 6.28) 12 | gen.add("PHI_Z", double_t, 0, "Euler angle Z", 1.57, -6.28, 6.28) 13 | gen.add("TRANS_X", double_t, 0, "Translation in X-axis", 0, -100, 100) 14 | gen.add("TRANS_Y", double_t, 0, "Translation in Y-axis", 0, -100, 100) 15 | gen.add("TRANS_Z", double_t, 0, "Translation in Z-axis", 0.28, -100, 100) 16 | 17 | exit(gen.generate(PACKAGE, "jpp", "CamToRobotCalibParams")) -------------------------------------------------------------------------------- /ROS/jpp/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "jpp" 3 | 4 | import roslib;roslib.load_manifest(PACKAGE) 5 | 6 | from dynamic_reconfigure.parameter_generator import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("DR", double_t, 0, "Descriptor radius", 5.0, 0, 100) 11 | gen.add("DQ", int_t, 0, "Radius quantization", 2, 0, 100) 12 | gen.add("DT", int_t, 0, "Angular quantization", 1, 0, 100) 13 | gen.add("DH", int_t, 0, "Histogram quantization", 1, 0, 100) 14 | gen.add("LENGTH", int_t, 0, "Robot length (mm)", 400, 0, 5000) 15 | gen.add("WIDTH", int_t, 0, "Robot width (mm)", 400, 0, 5000) 16 | gen.add("HEIGHT", int_t, 0, "Robot height (mm)", 200, 0, 1000) 17 | gen.add("GRID_SIZE", int_t, 0, "Grid size (mm)", 50, 10, 1000) 18 | gen.add("CONF_POS_THRESH", double_t, 0, "Confident positive threshold", 0.75, 0, 19 | 500) 20 | gen.add("CONF_NEG_THRESH", double_t, 0, "Confident negative threshold", 1.2, 0, 21 | 500) 22 | gen.add("SAD_WINDOW_SIZE", int_t, 0, "SAD window size", 5, 1, 10) 23 | gen.add("SPATIAL_FILTER_WINDOW", int_t, 0, "Spatial filter window size (mm)", 24 | 100, 10, 5000) 25 | gen.add("SPATIAL_FILTER_INC", int_t, 0, "Spatial filter grid size (mm)", 100, 26 | 20, 5000) 27 | gen.add("SPATIAL_FILTER_RATIO", double_t, 0, "Spatial filter ratio (s)", 0.95, 28 | 0, 1) 29 | gen.add("CONF_NEG_INC", int_t, 0, "Column inc size", 20, 10, 500) 30 | gen.add("CONF_NEG_FILTER_RATIO", double_t, 0, "Conf neg filter ratio", 0.75, 0, 31 | 1) 32 | gen.add("START_X", int_t, 0, "Start x-coordinate", 500, 0, 10000) 33 | gen.add("MAX_X", int_t, 0, "Max x-coordinate", 2000, 0, 10000) 34 | gen.add("MAX_Y", int_t, 0, "Max y-coordinate", 300, 0, 10000) 35 | gen.add("CONVEX_WORLD", bool_t, 0, "Convex world assumption", True) 36 | 37 | exit(gen.generate(PACKAGE, "jpp", "Params")) -------------------------------------------------------------------------------- /ROS/jpp/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | jpp 5 | 1.0.0 6 | The JPP ROS package 7 | 8 | Sourish Ghosh 9 | 10 | MIT 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ROS/jpp/src/naive_navigation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "disparitydaisy.h" 17 | 18 | using namespace cv; 19 | using namespace std; 20 | 21 | Mat img_left, img_right; 22 | Mat XR, XT, Q, P1, P2; 23 | Mat R1, R2, K1, K2, D1, D2, R; 24 | Mat lmapx, lmapy, rmapx, rmapy; 25 | Vec3d T; 26 | jpp::ParamsConfig config; 27 | FileStorage calib_file; 28 | Size out_img_size(320, 180); //kitti - 489x180, 1392x512 29 | Size calib_img_size(640, 360); 30 | 31 | ros::Publisher vel_pub; 32 | double forward_vel = 0., rot_vel = 0.; 33 | double trans_accel = 0.025; // forward acceleration 34 | double trans_decel = 0.1; // forward deceleration 35 | double rot_accel = 0.05; // rotational acceleration 36 | float max_forward_vel = 0.6; // maximum forward velocity 37 | double max_rot_vel = 1.3; // maximum rotational velocity 38 | bool is_obstacle = false; 39 | 40 | void undistortRectifyImage(Mat& src, Mat& dst, FileStorage& calib_file, int 41 | left = 1) { 42 | if (left == 1) { 43 | remap(src, dst, lmapx, lmapy, cv::INTER_LINEAR); 44 | } else { 45 | remap(src, dst, rmapx, rmapy, cv::INTER_LINEAR); 46 | } 47 | } 48 | 49 | void blend_images(Mat& src1, Mat& src2, float alpha, Mat& dst) { 50 | float beta = ( 1.0 - alpha ); 51 | addWeighted( src1, alpha, src2, beta, 0.0, dst); 52 | } 53 | 54 | void visualizeConfPos(Mat& img, DisparityDaisy *daisy) { 55 | clock_t begin = clock(); 56 | Point start(config.START_X, -config.MAX_Y); 57 | Point end(config.MAX_X, config.MAX_Y); 58 | int inc = config.GRID_SIZE; 59 | float safe_radius = (float)max(config.LENGTH/2, config.WIDTH/2)/1000.; 60 | #pragma omp parallel for collapse(2)\ 61 | num_threads(omp_get_max_threads()) 62 | for (int x = start.x; x <= end.x; x += inc) { 63 | for (int y = start.y; y <= end.y; y += inc) { 64 | Point3f p((float)x/1000.,(float)y/1000.,0.0); 65 | Point ptl = daisy->projectPointCam(p, 0); 66 | //if (daisy->confPositive(p)) { 67 | //if (daisy->isEmptySpace(p)) { 68 | //if (daisy->isObstacleFreeRegion(p)) { 69 | if (daisy->isBotClearOfObstacle(p, safe_radius, 70 | (float)config.GRID_SIZE/1000., false)) { 71 | Scalar green = Scalar(0,255,0); 72 | circle(img, ptl, 3, green, -1, 8, 0); 73 | } 74 | } 75 | } 76 | clock_t stop = clock(); 77 | double elapsed_secs = double(stop - begin) / CLOCKS_PER_SEC; 78 | cout << "conf pos time: " << elapsed_secs << endl; 79 | } 80 | 81 | void visualizeConfNeg(Mat& img, DisparityDaisy *daisy) { 82 | clock_t begin = clock(); 83 | Point start(config.START_X, -config.MAX_Y); 84 | Point end(config.MAX_X, config.MAX_Y); 85 | int inc = config.GRID_SIZE; 86 | float safe_radius = (float)max(config.LENGTH/2, config.WIDTH/2)/1000.; 87 | #pragma omp parallel for collapse(3)\ 88 | num_threads(omp_get_max_threads()) 89 | for (int x = start.x; x <= end.x; x += inc) { 90 | for (int y = start.y; y <= end.y; y += inc) { 91 | for (int z = config.CONF_NEG_INC; z <= config.HEIGHT; z += 92 | config.CONF_NEG_INC) { 93 | Point3f q((float)x/1000.,(float)y/1000.,(float)z/1000.); 94 | Point qtl = daisy->projectPointCam(q, 0); 95 | if (!daisy->confNegative(q)) { 96 | circle(img, qtl, 1, Scalar(0,255,0), -1, 8, 0); 97 | } else { 98 | circle(img, qtl, 1, Scalar(0,0,255), -1, 8, 0); 99 | } 100 | } 101 | } 102 | } 103 | clock_t stop = clock(); 104 | double elapsed_secs = double(stop - begin) / CLOCKS_PER_SEC; 105 | cout << "conf neg time: " << elapsed_secs << endl; 106 | } 107 | 108 | void imgCallback(const sensor_msgs::CompressedImageConstPtr& msg_left, const 109 | sensor_msgs::CompressedImageConstPtr& msg_right) 110 | { 111 | Mat tmp1 = cv::imdecode(cv::Mat(msg_left->data), CV_LOAD_IMAGE_GRAYSCALE); 112 | Mat tmp2 = cv::imdecode(cv::Mat(msg_right->data), 113 | CV_LOAD_IMAGE_GRAYSCALE); 114 | undistortRectifyImage(tmp1, img_left, calib_file, 1); 115 | undistortRectifyImage(tmp2, img_right, calib_file, 0); 116 | if (img_left.empty() || img_right.empty()) 117 | return; 118 | 119 | DisparityDaisy *daisy = new DisparityDaisy(config); 120 | daisy->loadImages(img_left, img_right); 121 | daisy->initDaisyDesc(config.DR, config.DQ, config.DT, config.DH, NRM_FULL); 122 | daisy->setCamParams(XR, XT, Q, P1, P2); 123 | Mat img_left_disp; 124 | cvtColor(img_left, img_left_disp, CV_GRAY2BGR); 125 | Point start(config.START_X, -config.MAX_Y); 126 | Point end(config.MAX_X, config.MAX_Y); 127 | int inc = config.GRID_SIZE; 128 | float safe_radius = (float)max(config.LENGTH/2, config.WIDTH/2)/1000.; 129 | double safe_points = 0; 130 | double total_points = 0; 131 | for (int x = start.x; x <= end.x; x += inc) { 132 | for (int y = start.y; y <= end.y; y += inc) { 133 | Point3f p((float)x/1000.,(float)y/1000.,0.0); 134 | Point ptl = daisy->projectPointCam(p, 0); 135 | //if (daisy->confPositive(p)) { 136 | //if (daisy->isEmptySpace(p)) { 137 | //if (daisy->isObstacleFreeRegion(p)) { 138 | if (daisy->isBotClearOfObstacle(p, safe_radius, 139 | (float)config.GRID_SIZE/1000., false)) { 140 | Scalar green = Scalar(0,255,0); 141 | circle(img_left_disp, ptl, 3, green, -1, 8, 0); 142 | safe_points += 1.0; 143 | } 144 | total_points += 1.0; 145 | } 146 | } 147 | double ratio = safe_points / total_points; 148 | if (ratio > 0.6) { 149 | is_obstacle = false; 150 | } else { 151 | is_obstacle = true; 152 | cout << ratio << " obstacle!" << endl; 153 | } 154 | imshow("VIS", img_left_disp); 155 | waitKey(30); 156 | 157 | delete daisy; 158 | } 159 | 160 | void findRectificationMap(FileStorage& calib_file, Size finalSize) { 161 | Rect validRoi[2]; 162 | cout << "starting rectification" << endl; 163 | stereoRectify(K1, D1, K2, D2, calib_img_size, R, Mat(T), R1, R2, P1, P2, Q, 164 | CV_CALIB_ZERO_DISPARITY, 0, finalSize, &validRoi[0], 165 | &validRoi[1]); 166 | cv::initUndistortRectifyMap(K1, D1, R1, P1, finalSize, CV_32F, lmapx, 167 | lmapy); 168 | cv::initUndistortRectifyMap(K2, D2, R2, P2, finalSize, CV_32F, rmapx, 169 | rmapy); 170 | cout << "done rectification" << endl; 171 | } 172 | 173 | void paramsCallback(jpp::ParamsConfig &conf, uint32_t level) { 174 | config = conf; 175 | } 176 | 177 | pair< double, double > stopInFrontMode(double side, double front) { 178 | double desired_forward_vel = max_forward_vel * front; 179 | double desired_rot_vel = max_rot_vel * side; 180 | if (is_obstacle) { 181 | // if there's an obstacle stop or allow the jackal to move back 182 | desired_forward_vel = min(desired_forward_vel, 0.); 183 | } 184 | return make_pair(desired_forward_vel, desired_rot_vel); 185 | } 186 | 187 | void safeNavigate(const sensor_msgs::JoyConstPtr& msg) { 188 | // read joystick input 189 | int R2 = msg->buttons[9]; 190 | int R1 = msg->buttons[11]; 191 | int X = msg->buttons[14]; 192 | int O = msg->buttons[13]; 193 | int triangle = msg->buttons[12]; 194 | double side = msg->axes[0]; 195 | double front = msg->axes[1]; 196 | double desired_forward_vel, desired_rot_vel; 197 | pair< double, double > desired_vel; 198 | // run the different modes 199 | if (R1 && R2) { 200 | desired_vel = stopInFrontMode(side, front); 201 | } 202 | // accelerate or decelerate accordingly 203 | desired_forward_vel = desired_vel.first; 204 | desired_rot_vel = desired_vel.second; 205 | if (desired_forward_vel < forward_vel) { 206 | forward_vel = max(desired_forward_vel, forward_vel - trans_decel); 207 | } else { 208 | forward_vel = min(desired_forward_vel, forward_vel + trans_accel); 209 | } 210 | if (desired_rot_vel < rot_vel) { 211 | rot_vel = max(desired_rot_vel, rot_vel - rot_accel); 212 | } else { 213 | rot_vel = min(desired_rot_vel, rot_vel + rot_accel); 214 | } 215 | geometry_msgs::Twist vel_msg; 216 | vel_msg.linear.x = forward_vel; 217 | vel_msg.angular.z = rot_vel; 218 | vel_pub.publish(vel_msg); 219 | } 220 | 221 | int main(int argc, char** argv) { 222 | ros::init(argc, argv, "jpp_naive_navigation"); 223 | ros::NodeHandle nh; 224 | image_transport::ImageTransport it(nh); 225 | 226 | calib_file = FileStorage(argv[3], FileStorage::READ); 227 | calib_file["K1"] >> K1; 228 | calib_file["K2"] >> K2; 229 | calib_file["D1"] >> D1; 230 | calib_file["D2"] >> D2; 231 | calib_file["R"] >> R; 232 | calib_file["T"] >> T; 233 | calib_file["XR"] >> XR; 234 | calib_file["XT"] >> XT; 235 | findRectificationMap(calib_file, out_img_size); 236 | 237 | message_filters::Subscriber sub_img_left(nh, 238 | argv[1], 1); 239 | message_filters::Subscriber sub_img_right(nh, 240 | argv[2], 1); 241 | 242 | ros::Subscriber sub_safe_drive = nh.subscribe("/bluetooth_teleop/joy", 1, 243 | safeNavigate); 244 | vel_pub = 245 | nh.advertise("/jackal_velocity_controller/cmd_vel", 1); 246 | 247 | typedef 248 | message_filters::sync_policies::ApproximateTime SyncPolicy; 250 | message_filters::Synchronizer sync(SyncPolicy(10), 251 | sub_img_left, sub_img_right); 252 | sync.registerCallback(boost::bind(&imgCallback, _1, _2)); 253 | 254 | dynamic_reconfigure::Server server; 255 | dynamic_reconfigure::Server::CallbackType 256 | f; 257 | 258 | f = boost::bind(¶msCallback, _1, _2); 259 | server.setCallback(f); 260 | 261 | ros::spin(); 262 | return 0; 263 | } -------------------------------------------------------------------------------- /ROS/jpp/src/navigation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "jpp.h" 19 | #include "popt_pp.h" 20 | 21 | JPP *jpp_obj; 22 | JPP_Config jpp_config; 23 | config_t cfg, *cf; 24 | FileStorage calib_file; 25 | const char* output = "astar"; 26 | int w = 0; 27 | int v = 0; 28 | int d = 0; 29 | int counter = 0; 30 | geometry_msgs::Pose global_waypoint; 31 | 32 | float prev_y_total = 0; 33 | 34 | //publishers not in main 35 | ros::Publisher pub_path; 36 | 37 | void update_planned_path(vector< Point > path){ 38 | //get the jpp generated path 39 | 40 | //define a path message from path to send to path_follower 41 | nav_msgs::Path real_path; 42 | real_path.header.frame_id = "jackal"; 43 | real_path.header.stamp = ros::Time::now(); 44 | 45 | bool path_invalid = true; 46 | float y_total = 0; 47 | for(uint32_t i = 0; i < path.size(); i++) 48 | { 49 | if (path[i].x > jpp_config.START_X) 50 | path_invalid = false; 51 | //need to divide by 1000 to convert from mm to m 52 | geometry_msgs::PoseStamped s_pose; 53 | s_pose.header = real_path.header; 54 | s_pose.pose.position.x = path[i].x / 1000.0; 55 | s_pose.pose.position.y = path[i].y / 1000.0; 56 | s_pose.pose.position.z = 0; 57 | s_pose.pose.orientation.x = 0; 58 | s_pose.pose.orientation.y = 0; 59 | s_pose.pose.orientation.z = 0; 60 | s_pose.pose.orientation.w = 1; 61 | 62 | y_total += s_pose.pose.position.y; 63 | 64 | real_path.poses.push_back(s_pose); 65 | } 66 | 67 | if (path_invalid)//if the path is not valid stop, doing this by making the path just the 0 pose 68 | {//if invalid turn left or right 69 | geometry_msgs::PoseStamped s_pose; 70 | s_pose.header = real_path.header; 71 | s_pose.pose.position.x = 0.0; 72 | //s_pose.pose.position.y = 0; 73 | s_pose.pose.position.z = 0.0; 74 | s_pose.pose.orientation.x = 0.0; 75 | s_pose.pose.orientation.y = 0.0; 76 | s_pose.pose.orientation.z = 0.0; 77 | s_pose.pose.orientation.w = 1.0; 78 | 79 | if (prev_y_total > 0.0)//real_path.poses[0].pose.position.y > 0.0) 80 | { 81 | ROS_INFO("stop and turn left"); 82 | s_pose.pose.position.y = 1.0;//turn left 83 | } 84 | else if (prev_y_total < 0.0)//real_path.poses[0].pose.position.y < 0.0) 85 | { 86 | ROS_INFO("stop and turn right"); 87 | s_pose.pose.position.y = -1.0;//turn right 88 | } 89 | else 90 | { 91 | ROS_INFO("stop"); 92 | s_pose.pose.position.y = 0.0;//stop 93 | } 94 | 95 | real_path.poses.clear(); 96 | real_path.poses.push_back(s_pose); 97 | ROS_INFO("real_path.poses[real_path.size()]: %f", real_path.poses[real_path.poses.size() - 1].pose.position.x); 98 | } 99 | else 100 | { 101 | prev_y_total = y_total; 102 | } 103 | 104 | pub_path.publish(real_path); 105 | } 106 | 107 | void imgCallback(const sensor_msgs::ImageConstPtr& msg_left, const sensor_msgs::ImageConstPtr& msg_right) 108 | { 109 | counter++; 110 | Mat img_left = cv_bridge::toCvShare(msg_left, "bgr8")->image; 111 | Mat img_right = cv_bridge::toCvShare(msg_right, "bgr8")->image; 112 | if (img_left.empty() || img_right.empty()) 113 | return; 114 | 115 | char rrt_file_prefix[100], astar_file_prefix[100]; 116 | sprintf(astar_file_prefix, "%s%d", "astar", counter); 117 | sprintf(rrt_file_prefix, "%s%d", "rrt", counter); 118 | 119 | if (d == 1) 120 | jpp_obj->update_jpp_config(jpp_config); 121 | 122 | jpp_obj->load_images(img_left, img_right); 123 | 124 | if (strcmp(output, "astar") == 0) { 125 | vector< Point > path = jpp_obj->plan_astar(global_waypoint.position.x, global_waypoint.position.y); 126 | update_planned_path(jpp_obj->getPath()); 127 | if (v == 1) { 128 | pair< Mat, Mat > vis; 129 | if (w == 1) 130 | vis = jpp_obj->visualize_jpp(astar_file_prefix); 131 | else 132 | vis = jpp_obj->visualize_jpp(); 133 | imshow("PATH", vis.first); 134 | imshow("CONFIDENCE", vis.second); 135 | } 136 | } else if (strcmp(output, "rrt") == 0) { 137 | vector< Point > path = jpp_obj->plan_rrt(); 138 | update_planned_path(jpp_obj->getPath()); 139 | if (v == 1) { 140 | pair< Mat, Mat > vis; 141 | if (w == 1) 142 | vis = jpp_obj->visualize_jpp(rrt_file_prefix); 143 | else 144 | vis = jpp_obj->visualize_jpp(); 145 | imshow("PATH", vis.first); 146 | imshow("CONFIDENCE", vis.second); 147 | } 148 | } else if (strcmp(output, "debug") == 0) { 149 | Mat conf_pos = jpp_obj->visualize_conf_pos(); 150 | Mat conf_neg = jpp_obj->visualize_conf_neg(); 151 | imshow("CONF POS", conf_pos); 152 | imshow("CONF NEG", conf_neg); 153 | } 154 | waitKey(30); 155 | } 156 | 157 | void waypointCallback(const geometry_msgs::Pose& pose) 158 | { 159 | global_waypoint = pose; 160 | } 161 | 162 | void paramsCallback(jpp::ParamsConfig &conf, uint32_t level) { 163 | jpp_config.DR = conf.DR; 164 | jpp_config.DQ = conf.DQ; 165 | jpp_config.DT = conf.DT; 166 | jpp_config.DH = conf.DH; 167 | jpp_config.BOT_LENGTH = conf.LENGTH; 168 | jpp_config.BOT_WIDTH = conf.WIDTH; 169 | jpp_config.BOT_HEIGHT = conf.HEIGHT; 170 | jpp_config.GRID_SIZE = conf.GRID_SIZE; 171 | jpp_config.CONF_POS_THRESH = conf.CONF_POS_THRESH; 172 | jpp_config.CONF_NEG_THRESH = conf.CONF_NEG_THRESH; 173 | jpp_config.SAD_WINDOW_SIZE = conf.SAD_WINDOW_SIZE; 174 | jpp_config.SPATIAL_FILTER_WINDOW = conf.SPATIAL_FILTER_WINDOW; 175 | jpp_config.SPATIAL_FILTER_INC = conf.SPATIAL_FILTER_INC; 176 | jpp_config.SPATIAL_FILTER_RATIO = conf.SPATIAL_FILTER_RATIO; 177 | jpp_config.CONF_NEG_INC = conf.CONF_NEG_INC; 178 | jpp_config.CONF_NEG_FILTER_RATIO = conf.CONF_NEG_FILTER_RATIO; 179 | jpp_config.START_X = conf.START_X; 180 | jpp_config.MAX_X = conf.MAX_X; 181 | jpp_config.MAX_Y = conf.MAX_Y; 182 | jpp_config.CONVEX_WORLD = conf.CONVEX_WORLD; 183 | } 184 | 185 | int main(int argc, char** argv) { 186 | ros::init(argc, argv, "jpp_navigation"); 187 | ros::NodeHandle nh; 188 | image_transport::ImageTransport it(nh); 189 | 190 | pub_path = nh.advertise("/jackal/planned_path", 1); 191 | ros::Subscriber way_point_sub = nh.subscribe("/jpp/waypoint", 1, waypointCallback); 192 | 193 | const char* left_img_topic; 194 | const char* right_img_topic; 195 | const char* calib_file_name; 196 | const char* jpp_config_file; 197 | 198 | static struct poptOption options[] = { 199 | { "left_topic",'l',POPT_ARG_STRING,&left_img_topic,0,"Left image topic name","STR" }, 200 | { "right_topic",'r',POPT_ARG_STRING,&right_img_topic,0,"Right image topic name","STR" }, 201 | { "calib_file",'c',POPT_ARG_STRING,&calib_file_name,0,"Stereo calibration file name","STR" }, 202 | { "jpp_config_file",'j',POPT_ARG_STRING,&jpp_config_file,0,"JPP config file name","STR" }, 203 | { "output",'o',POPT_ARG_STRING,&output,0,"Output - astar, rrt, debug","STR" }, 204 | { "visualize",'v',POPT_ARG_INT,&v,0,"Set v=1 for displaying visualizations","NUM" }, 205 | { "write_files",'w',POPT_ARG_INT,&w,0,"Set w=1 for writing visualizations to files","NUM" }, 206 | { "dynamic_reconfigure",'d',POPT_ARG_INT,&d,0,"Set d=1 for enabling dynamic reconfigure","NUM" }, 207 | POPT_AUTOHELP 208 | { NULL, 0, 0, NULL, 0, NULL, NULL } 209 | }; 210 | 211 | POpt popt(NULL, argc, argv, options, 0); 212 | int c; 213 | while((c = popt.getNextOpt()) >= 0) {} 214 | 215 | calib_file = FileStorage(calib_file_name, FileStorage::READ); 216 | 217 | // Read JPP config 218 | cf = &cfg; 219 | config_init(cf); 220 | if (!config_read_file(cf, jpp_config_file)) { 221 | cout << "Could not read config file!" << endl; 222 | config_destroy(cf); 223 | return(EXIT_FAILURE); 224 | } 225 | jpp_config = JPP_Config(cf); 226 | 227 | jpp_obj = new JPP(calib_file, cf); 228 | 229 | //intitulize global way point 230 | global_waypoint.position.x = ((float)jpp_config.MAX_X)/1000.0; 231 | global_waypoint.position.y = 0; 232 | global_waypoint.position.z = 0; 233 | global_waypoint.orientation.x = 0; 234 | global_waypoint.orientation.y = 0; 235 | global_waypoint.orientation.z = 0; 236 | global_waypoint.orientation.w = 1; 237 | 238 | message_filters::Subscriber sub_img_left(nh, left_img_topic, 1); 239 | message_filters::Subscriber sub_img_right(nh, right_img_topic, 1); 240 | 241 | typedef message_filters::sync_policies::ApproximateTime SyncPolicy; 242 | message_filters::Synchronizer sync(SyncPolicy(10), sub_img_left, sub_img_right); 243 | sync.registerCallback(boost::bind(&imgCallback, _1, _2)); 244 | 245 | dynamic_reconfigure::Server server; 246 | dynamic_reconfigure::Server::CallbackType f; 247 | f = boost::bind(¶msCallback, _1, _2); 248 | server.setCallback(f); 249 | 250 | ros::spin(); 251 | return 0; 252 | } -------------------------------------------------------------------------------- /ROS/jpp/src/path_follower.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include //there is a more specific lib 9 | #include 10 | #include "jpp.h" 11 | #include "popt_pp.h" 12 | 13 | JPP_Config jpp_config; 14 | config_t cfg, *cf; 15 | FileStorage calib_file; 16 | const char* output = "astar"; 17 | int w = 0; 18 | int counter = 0; 19 | 20 | typedef nav_msgs::Path Path; 21 | typedef geometry_msgs::Point GPoint; 22 | 23 | Path path; 24 | int path_index = -1; 25 | geometry_msgs::Pose path_init_pose; 26 | geometry_msgs::Pose current_pose; 27 | tf::Pose tf_path_init_pose; 28 | tf::Pose tf_current_pose; 29 | geometry_msgs::Twist current_twist; 30 | 31 | //defining path for rviz 32 | vector< GPoint > rvizPath; 33 | 34 | //publishers not in main 35 | ros::Publisher vel_pub; 36 | ros::Publisher path_pub; 37 | 38 | //constants 39 | double forward_vel = 0., rot_vel = 0.; 40 | double trans_accel = 0.025; // forward acceleration 41 | double trans_decel = 0.1; // forward deceleration 42 | double rot_accel = 0.05; // rotational acceleration 43 | float max_forward_vel = 0.2;//0.6; // maximum forward velocity 44 | double max_rot_vel = 0.5;//1.3; // maximum rotational velocity 45 | float dist_threshold = 0.045;//0.045;//near enough distance 46 | 47 | void newPathCallBack(const nav_msgs::Path::ConstPtr& p){ 48 | //get the jpp generated path 49 | 50 | path.header = p->header; 51 | path.poses = p->poses; 52 | path_index = path.poses.size() - 1; 53 | ROS_INFO("path pose: %d", path.poses.size()); 54 | path_init_pose = current_pose; 55 | tf_path_init_pose = tf_current_pose; 56 | 57 | 58 | //initulizing ros path marker 59 | visualization_msgs::Marker line_strip; 60 | line_strip.header.frame_id = "jackal"; 61 | line_strip.header.stamp = ros::Time::now(); 62 | line_strip.ns = "ros_path"; 63 | line_strip.action = visualization_msgs::Marker::ADD; 64 | line_strip.pose.orientation.w = 1.0; 65 | line_strip.id = 0; 66 | line_strip.type = visualization_msgs::Marker::LINE_STRIP; 67 | line_strip.scale.x = 0.1; 68 | line_strip.color.b = 1.0; 69 | line_strip.color.a = 1.0; 70 | 71 | //fill in line_strip from path 72 | for(uint32_t i = 0; i < path.poses.size(); i++) 73 | { 74 | line_strip.points.push_back(path.poses[i].pose.position); 75 | } 76 | 77 | //if (path.poses.size() == 1) 78 | //{ 79 | //ROS_INFO("x val = %f", path.poses[0].pose.position.x); 80 | //} 81 | 82 | //publish 83 | path_pub.publish(line_strip); 84 | } 85 | 86 | //transforms a pose into the reference frame of the robot from odometry 87 | geometry_msgs::Pose transform_pose(geometry_msgs::Pose pose) 88 | { 89 | tf::Transform trans = tf_current_pose.inverseTimes(tf_path_init_pose); 90 | 91 | tf::Pose tf_pose; 92 | tf::poseMsgToTF(pose, tf_pose); 93 | 94 | tf_pose = trans*tf_pose; 95 | 96 | geometry_msgs::Pose new_pose; 97 | tf::poseTFToMsg(tf_pose, new_pose);//getting warnings 98 | 99 | 100 | return new_pose; 101 | } 102 | 103 | void odometry_call_back(const nav_msgs::Odometry& odom) 104 | { 105 | current_twist = odom.twist.twist; 106 | current_pose = odom.pose.pose; 107 | tf::poseMsgToTF(odom.pose.pose, tf_current_pose); 108 | 109 | //initulizing ros path marker 110 | visualization_msgs::Marker line_strip; 111 | line_strip.header.frame_id = "jackal"; 112 | line_strip.header.stamp = ros::Time::now(); 113 | line_strip.ns = "ros_path"; 114 | line_strip.action = visualization_msgs::Marker::ADD; 115 | line_strip.pose.orientation.w = 1.0; 116 | line_strip.id = 0; 117 | line_strip.type = visualization_msgs::Marker::LINE_STRIP; 118 | line_strip.scale.x = 0.1; 119 | line_strip.color.b = 1.0; 120 | line_strip.color.a = 1.0; 121 | 122 | //fill in line_strip from path 123 | for(uint32_t i = 0; i < path.poses.size(); i++) 124 | { 125 | line_strip.points.push_back(transform_pose(path.poses[i].pose).position); 126 | } 127 | 128 | //publish 129 | path_pub.publish(line_strip); 130 | } 131 | 132 | float distance_to_point(geometry_msgs::Point p){ 133 | return sqrt(p.x*p.x + p.y*p.y); 134 | } 135 | 136 | float angle_to_point(geometry_msgs::Point p){ 137 | return atan(p.y/p.x); 138 | } 139 | 140 | pair< double, double > bestVelocity(float radius, int sign) 141 | { 142 | pair< double, double > velocity (0,0); 143 | if (radius == -1) 144 | { 145 | velocity.first = max_forward_vel; 146 | velocity.second = 0; 147 | } 148 | else if (radius != 0) 149 | { 150 | velocity.first = max_forward_vel; 151 | velocity.second = (max_forward_vel/radius) * sign; 152 | } 153 | return velocity; 154 | } 155 | 156 | pair< double, double > goto_pose(geometry_msgs::Pose pose) 157 | { 158 | //for now orientation is egnored 159 | pair< double, double > velocity(0,0); 160 | 161 | //ROS_INFO("pose.position.x: %f", pose.position.x); 162 | if (pose.position.x <= 0.01) 163 | { 164 | //ROS_INFO("stop!"); 165 | velocity.first = 0.0; 166 | } 167 | else 168 | { 169 | //ROS_INFO("go as you please"); 170 | velocity.first = max_forward_vel; 171 | } 172 | 173 | geometry_msgs::Pose gPose = transform_pose(pose); 174 | 175 | velocity.second = (0.8 * angle_to_point(gPose.position)); 176 | if (velocity.second > 0) 177 | velocity.second = min(velocity.second, max_rot_vel); 178 | else 179 | velocity.second = max(velocity.second, -max_rot_vel); 180 | 181 | return velocity; 182 | 183 | } 184 | 185 | pair< double, double > follow_path() 186 | { 187 | //ROS_INFO("dist to point: %f\n", distance_to_point(path.poses[path_index].pose.position)); 188 | //^causes segmentation fault if no path 189 | if (path_index < 0 || path.poses.size() <= 0) 190 | { 191 | return(pair< double, double > (0,0)); 192 | } 193 | else if (distance_to_point(path.poses[path_index].pose.position) < dist_threshold) 194 | { 195 | path_index--; 196 | return follow_path(); 197 | } 198 | else 199 | { 200 | //ROS_INFO("path.poses.size(): %d", path.poses.size()); 201 | //ROS_INFO("index: %d", path_index); 202 | return goto_pose(path.poses[path_index].pose);//transform_pose(path.poses[path_index].pose)); 203 | } 204 | return(pair< double, double > (0,0)); 205 | } 206 | 207 | void safeNavigate(const sensor_msgs::JoyConstPtr& msg) { 208 | // read joystick input 209 | int R2 = msg->buttons[9]; 210 | int R1 = msg->buttons[11]; 211 | int X = msg->buttons[14]; 212 | int O = msg->buttons[13]; 213 | int triangle = msg->buttons[12]; 214 | //double side = msg->axes[0]; 215 | //double front = msg->axes[1]; 216 | double desired_forward_vel, desired_rot_vel; 217 | pair< double, double > desired_vel; 218 | // run the different modes 219 | if (R1 && R2) { 220 | //desired_vel = stopInFrontMode(side, front); 221 | } else if (triangle) { 222 | desired_vel = follow_path(); 223 | ROS_INFO("wanted V: %f, W: %f", forward_vel, forward_vel); 224 | //desired_vel = autoNavigateMode(front); // navigation doesn't work yet 225 | } else if (X) { 226 | //desired_vel = obstacleAvoidMode(front); 227 | } else if (O) { 228 | //desired_vel = stopInFrontMode(); 229 | } else { 230 | return; 231 | } 232 | // accelerate or decelerate accordingly 233 | desired_forward_vel = desired_vel.first; 234 | desired_rot_vel = desired_vel.second; 235 | if (desired_forward_vel < forward_vel) { 236 | forward_vel = max(desired_forward_vel, forward_vel - trans_decel); 237 | } else { 238 | forward_vel = min(desired_forward_vel, forward_vel + trans_accel); 239 | } 240 | if (desired_rot_vel < rot_vel) { 241 | rot_vel = max(desired_rot_vel, rot_vel - rot_accel); 242 | } else { 243 | rot_vel = min(desired_rot_vel, rot_vel + rot_accel); 244 | } 245 | ROS_INFO("real V: %f, W: %f", forward_vel, rot_vel); 246 | geometry_msgs::Twist vel_msg; 247 | vel_msg.linear.x = forward_vel; 248 | vel_msg.angular.z = rot_vel; 249 | vel_pub.publish(vel_msg); 250 | } 251 | 252 | int main(int argc, char** argv) { 253 | ros::init(argc, argv, "path_follower"); 254 | ros::NodeHandle nh; 255 | 256 | path_pub = nh.advertise("path_marker", 10); 257 | vel_pub = nh.advertise("/jackal_velocity_controller/cmd_vel", 1); 258 | // /jackal_velocity_controller/cmd_vel 259 | 260 | ros::Subscriber sub_safe_drive = nh.subscribe("/bluetooth_teleop/joy", 1, safeNavigate); 261 | ros::Subscriber sub_path = nh.subscribe("/jackal/planned_path", 1, newPathCallBack); 262 | ros::Subscriber sub_odometry = nh.subscribe("/odometry/filtered", 1, odometry_call_back); 263 | 264 | ros::spin(); 265 | 266 | return 0; 267 | } -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building JPP..." 2 | 3 | if [ ! -d "build" ]; then 4 | mkdir build 5 | fi 6 | cd build 7 | cmake .. -DCMAKE_BUILD_TYPE=Release 8 | make -j -------------------------------------------------------------------------------- /build_ros.sh: -------------------------------------------------------------------------------- 1 | echo "Building ROS nodes" 2 | 3 | cd ROS/jpp 4 | if [ ! -d "build" ]; then 5 | mkdir build 6 | fi 7 | cd build 8 | cmake .. -DROS_BUILD_TYPE=Release 9 | make -j -------------------------------------------------------------------------------- /calibration/amrl_jackal_webcam_stereo.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | K1: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 4.6417933392659904e+02, 0., 3.2479711799310849e+02, 0., 7 | 4.6611716361740059e+02, 1.8685472713963392e+02, 0., 0., 1. ] 8 | K2: !!opencv-matrix 9 | rows: 3 10 | cols: 3 11 | dt: d 12 | data: [ 4.6394860327263103e+02, 0., 3.3106360678338558e+02, 0., 13 | 4.6375869272018139e+02, 1.7571346440013161e+02, 0., 0., 1. ] 14 | D1: !!opencv-matrix 15 | rows: 1 16 | cols: 5 17 | dt: d 18 | data: [ 1.4885193925432560e-01, -3.8454604770748702e-01, 19 | -1.8950854861753609e-03, 7.8121300147955593e-03, 20 | 2.7294034259258465e-01 ] 21 | D2: !!opencv-matrix 22 | rows: 1 23 | cols: 5 24 | dt: d 25 | data: [ 1.2249914632632175e-01, -2.1440080513600884e-01, 26 | -2.8013224434709164e-03, 4.6375383671683921e-03, 27 | 4.3812259920217027e-02 ] 28 | R: !!opencv-matrix 29 | rows: 3 30 | cols: 3 31 | dt: d 32 | data: [ 9.9942653697036332e-01, -2.9629020698892981e-02, 33 | -1.6392630412826015e-02, 2.9331318104542686e-02, 34 | 9.9940562799161392e-01, -1.8112551364675371e-02, 35 | 1.6919544251458529e-02, 1.7621347028886541e-02, 36 | 9.9970156404359523e-01 ] 37 | T: [ -9.4052586442980660e-02, -1.2149101400467301e-03, 38 | -7.2235718228952177e-04 ] 39 | XR: !!opencv-matrix 40 | rows: 3 41 | cols: 3 42 | dt: d 43 | data: [ -0.0007962732853436516, -0.2675000227968607, 0.9635575706420958, 44 | -0.9999984502796089, -0.001321509725770019, -0.00119326128710218, 45 | 0.001592547981999815, -0.9635569909380592, -0.2674985457970802 ] 46 | XT: !!opencv-matrix 47 | rows: 3 48 | cols: 1 49 | dt: d 50 | data: [ 0.0, 51 | 0.0, 52 | 0.28 ] 53 | -------------------------------------------------------------------------------- /calibration/kitti_2011_09_26.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | K1: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 9.842439e+02, 0.000000e+00, 6.900000e+02, 7 | 0.000000e+00, 9.808141e+02, 2.331966e+02, 8 | 0.000000e+00, 0.000000e+00, 1.000000e+00 ] 9 | K2: !!opencv-matrix 10 | rows: 3 11 | cols: 3 12 | dt: d 13 | data: [ 9.895267e+02, 0.000000e+00, 7.020000e+02, 14 | 0.000000e+00, 9.878386e+02, 2.455590e+02, 15 | 0.000000e+00, 0.000000e+00, 1.000000e+00 ] 16 | D1: !!opencv-matrix 17 | rows: 1 18 | cols: 5 19 | dt: d 20 | data: [ -3.728755e-01, 2.037299e-01, 2.219027e-03, 1.383707e-03, -7.233722e-02 ] 21 | D2: !!opencv-matrix 22 | rows: 1 23 | cols: 5 24 | dt: d 25 | data: [ -3.644661e-01, 1.790019e-01, 1.148107e-03, -6.298563e-04, -5.314062e-02 ] 26 | R: !!opencv-matrix 27 | rows: 3 28 | cols: 3 29 | dt: d 30 | data: [ 9.993513e-01, 1.860866e-02, -3.083487e-02, 31 | -1.887662e-02, 9.997863e-01, -8.421873e-03, 32 | 3.067156e-02, 8.998467e-03, 9.994890e-01 ] 33 | T: [ -5.370000e-01, 4.822061e-03, -1.252488e-02 ] 34 | XR: !!opencv-matrix 35 | rows: 3 36 | cols: 3 37 | dt: d 38 | data: [ 0.00080, 0.00080, 1.00000, 39 | -1.00000, 0.00000, 0.00080, 40 | 0.00000, -1.00000, 0.00080 ] 41 | XT: !!opencv-matrix 42 | rows: 3 43 | cols: 1 44 | dt: d 45 | data: [ -4.0, 46 | 0.0, 47 | 1.7 ] -------------------------------------------------------------------------------- /cfg/amrl.cfg: -------------------------------------------------------------------------------- 1 | # JPP configurations for AMRL dataset 2 | # Units in mm 3 | 4 | calib_img_width = 640; // calibration img width 5 | calib_img_height = 360; // calibration img height 6 | 7 | rect_img_width = 320; // rectified (final) img width 8 | rect_img_height = 180; // rectified (final) img height 9 | 10 | daisy_params = { 11 | R = 5.0; // Descriptor radius 12 | Q = 2; // Radius quantization 13 | T = 1; // Angular quantization 14 | H = 1; // Histogram quantization 15 | }; 16 | 17 | bot_length = 400; 18 | bot_width = 400; 19 | bot_height = 200; 20 | 21 | grid_size = 50; // planning grid cell size 22 | 23 | conf_pos_thresh = 0.75; 24 | conf_neg_thresh = 1.2; 25 | 26 | SAD_window_size = 5; 27 | 28 | spatial_filter_window = 100; 29 | spatial_filter_inc = 20; 30 | spatial_filter_ratio = 0.95; 31 | 32 | conf_neg_inc = 20; 33 | conf_neg_filter_ratio = 0.75; 34 | 35 | start_x = 500; 36 | max_x = 2000; 37 | max_y = 300; 38 | 39 | convex_world = false; 40 | -------------------------------------------------------------------------------- /cfg/kitti.cfg: -------------------------------------------------------------------------------- 1 | # JPP configurations for KITTI dataset 2 | # Units in mm 3 | 4 | calib_img_width = 1392; // calibration img width 5 | calib_img_height = 512; // calibration img height 6 | 7 | rect_img_width = 489; // rectified (final) img width 8 | rect_img_height = 180; // rectified (final) img height 9 | 10 | daisy_params = { 11 | R = 5.0; // Descriptor radius 12 | Q = 2; // Radius quantization 13 | T = 1; // Angular quantization 14 | H = 1; // Histogram quantization 15 | }; 16 | 17 | bot_length = 2000; 18 | bot_width = 300; 19 | bot_height = 1000; 20 | 21 | grid_size = 500; // planning grid cell size 22 | 23 | conf_pos_thresh = 0.75; 24 | conf_neg_thresh = 1.1; 25 | 26 | SAD_window_size = 5; 27 | 28 | spatial_filter_window = 200; 29 | spatial_filter_inc = 100; 30 | spatial_filter_ratio = 0.95; 31 | 32 | conf_neg_inc = 100; 33 | conf_neg_filter_ratio = 0.6; 34 | 35 | start_x = 2500; 36 | max_x = 8000; 37 | max_y = 3000; 38 | 39 | convex_world = false; 40 | -------------------------------------------------------------------------------- /dumps/astar126-path.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ut-amrl/jpp/5285bb9dffa8f7333d5df736f7c5c9a5b43b4b6b/dumps/astar126-path.jpg -------------------------------------------------------------------------------- /dumps/astar126-vis.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ut-amrl/jpp/5285bb9dffa8f7333d5df736f7c5c9a5b43b4b6b/dumps/astar126-vis.jpg -------------------------------------------------------------------------------- /dumps/astar7-path.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ut-amrl/jpp/5285bb9dffa8f7333d5df736f7c5c9a5b43b4b6b/dumps/astar7-path.jpg -------------------------------------------------------------------------------- /dumps/astar7-vis.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ut-amrl/jpp/5285bb9dffa8f7333d5df736f7c5c9a5b43b4b6b/dumps/astar7-vis.jpg -------------------------------------------------------------------------------- /dumps/rrt73-path.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ut-amrl/jpp/5285bb9dffa8f7333d5df736f7c5c9a5b43b4b6b/dumps/rrt73-path.jpg -------------------------------------------------------------------------------- /dumps/rrt73-vis.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ut-amrl/jpp/5285bb9dffa8f7333d5df736f7c5c9a5b43b4b6b/dumps/rrt73-vis.jpg -------------------------------------------------------------------------------- /include/astarplanner.h: -------------------------------------------------------------------------------- 1 | #ifndef ASTARPLANNER_H 2 | #define ASTARPLANNER_H 3 | 4 | #include "stereo.h" 5 | 6 | class AStarPlanner 7 | { 8 | private: 9 | struct node { 10 | int id; 11 | Point p; 12 | float f, g, h; 13 | bool operator < (const node& rhs) const { 14 | return f <= rhs.f; 15 | } 16 | bool operator == (const node& rhs) const { 17 | return (p.x == rhs.p.x && p.y == rhs.p.y); 18 | } 19 | }; 20 | node start, end; 21 | int max_x, max_y, inc; 22 | bool inGrid(Point p); 23 | float L1Dist(Point p, Point q); 24 | float L2Dist(Point p, Point q); 25 | multiset< node > open_list; 26 | vector< Point > path; 27 | int safe_radius; 28 | int bot_height; 29 | bool convex_world; 30 | Mat closed_list; 31 | JPP_Config _config; 32 | public: 33 | int parent[100000]; 34 | map< int, Point > grid_id; 35 | vector< node > graph; 36 | AStarPlanner(JPP_Config& conf); 37 | void setParams(Point s, Point e, int my, int i, int r, int bh, bool cw); 38 | void findPath(Stereo* stereo); 39 | void setPath(node n); 40 | vector< Point > getPath(); 41 | Point clCoord(Point p); 42 | int getPathLength(); 43 | }; 44 | 45 | #endif // LOCALPLANNER_H 46 | -------------------------------------------------------------------------------- /include/jpp.h: -------------------------------------------------------------------------------- 1 | #ifndef JPP_H 2 | #define JPP_H 3 | 4 | #include "astarplanner.h" 5 | #include "rrt.h" 6 | 7 | class JPP { 8 | private: 9 | JPP_Config _jpp_config; 10 | Stereo *_stereo; 11 | 12 | void _reset(); 13 | void _get_jpp_config(config_t *cf); 14 | vector< Point > _path; 15 | public: 16 | JPP(); 17 | JPP(FileStorage& fs, config_t *cf); 18 | ~JPP(); 19 | void load_images(const Mat& left, const Mat& right); 20 | Mat get_disparity_map(const char* method, int max_disp, const char* outfile = NULL); 21 | Mat visualize_conf_pos(); 22 | Mat visualize_conf_neg(); 23 | Mat visualize_empty_cols(); 24 | vector< Point > plan_astar(); 25 | vector< Point > plan_astar(float x, float y); 26 | vector< Point > plan_rrt(); 27 | pair< Mat, Mat > visualize_jpp(const char* outfile = NULL); 28 | vector< Point > getPath(); 29 | void update_jpp_config(JPP_Config& config); 30 | vector< double > get_epipolar_costs(Point p, int max_disp); 31 | Mat get_img_left(); 32 | Mat get_img_right(); 33 | Mat get_Q_matrix(); 34 | }; 35 | 36 | #endif // JPP_H -------------------------------------------------------------------------------- /include/popt_pp.h: -------------------------------------------------------------------------------- 1 | #ifndef _INCLUDED_POPT_PP_H_ 2 | #define _INCLUDED_POPT_PP_H_ 3 | 4 | #include 5 | 6 | class POpt{ 7 | protected: 8 | poptContext con; 9 | public: 10 | // creation and deletion 11 | POpt(const char *name, int argc, const char **argv, 12 | const poptOption *options, int flags) 13 | {con = poptGetContext(name,argc,argv,options,flags);} 14 | POpt(const char *name, int argc, char **argv, 15 | const poptOption *options, int flags) 16 | {con = poptGetContext(name,argc,(const char **)argv,options,flags);} 17 | ~POpt() 18 | {poptFreeContext(con);} 19 | 20 | // functions for processing options 21 | int getNextOpt() 22 | {return(poptGetNextOpt(con));} 23 | void ignoreOptions() 24 | {while(getNextOpt() >= 0);} 25 | const char *getOptArg() 26 | {return(poptGetOptArg(con));} 27 | const char *strError(int error) 28 | {return(poptStrerror(error));} 29 | const char *badOption(int flags = POPT_BADOPTION_NOALIAS) 30 | {return(poptBadOption(con,flags));} 31 | 32 | // processing other arguments 33 | const char *getArg() 34 | {return(poptGetArg(con));} 35 | void ignoreArgs() 36 | {while(getArg());} 37 | }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /include/rrt.h: -------------------------------------------------------------------------------- 1 | #ifndef RRT_H 2 | #define RRT_H 3 | 4 | #include "stereo.h" 5 | 6 | class RRT 7 | { 8 | private: 9 | struct Node { 10 | Node *parent; 11 | vector children; 12 | Point position; 13 | }; 14 | Node *root; 15 | Point start, end; 16 | int step_size, max_iter; 17 | int obstacle_step_size; 18 | float goal_bias; 19 | int safe_radius; 20 | int bot_height; 21 | bool convex; 22 | JPP_Config _config; 23 | public: 24 | vector< Node* > nodes; 25 | vector< Node* > path; 26 | int nSAD, nSADConvex, nSADCache, nSADConvexCache; 27 | 28 | RRT(); 29 | void initRRT(Point s, Point e, int step, int iter, float bias, int r, int bh, bool c, JPP_Config conf); 30 | Point getRandomStateSpacePoint(int range_x, int range_y); 31 | Node* nearest(Point p); 32 | Point extend(Node *n, Point to, Stereo* stereo); 33 | void add(Node *q, Node *qNew); 34 | vector< Point > getPointsInLine(Point p, Point q); 35 | bool isBotClearOfObstacle(Point p, Stereo* stereo); 36 | void findPath(Stereo* stereo); 37 | vector< Point > getPath(); 38 | int getPathLength(); 39 | void freeMemory(); 40 | }; 41 | 42 | #endif // RRT_H 43 | -------------------------------------------------------------------------------- /include/stereo.h: -------------------------------------------------------------------------------- 1 | #ifndef STEREO_H 2 | #define STEREO_H 3 | 4 | #include "util.h" 5 | 6 | using namespace std; 7 | using namespace cv; 8 | using namespace kutility; 9 | 10 | class Stereo { 11 | private: 12 | JPP_Config _jpp_config; 13 | Mat _img_left, _img_right; 14 | Mat _XR, _XRINV, _XT, _Q, _P1, _P2; 15 | Mat _R1, _R2, _K1, _K2, _D1, _D2, _R; 16 | Mat _lmapx, _lmapy, _rmapx, _rmapy; 17 | Vec3d _T; 18 | Eigen::Matrix3f _cam2robot_R; 19 | Eigen::Vector3f _cam2robot_T; 20 | Eigen::Matrix< float, 3, 4 > _eP1, _eP2; 21 | daisy *_desc_left, *_desc_right; 22 | Mat _disparityMap; 23 | vector< int > _obstacleCache; 24 | vector< int > _obstacleRangeCache; 25 | vector< int > _colCache; 26 | vector< int > _confNegCache; 27 | vector< int > _descLeftSet, _descRightSet; 28 | vector< float* > _descLeftCache, _descRightCache; 29 | Mat _cacheVis; 30 | 31 | void _init_rectification_map(const FileStorage& fs); 32 | void _rectify_images(const Mat& left, const Mat& right); 33 | void _compute_dense_descriptors(); 34 | void _reallocate_cache(); 35 | public: 36 | Stereo(); 37 | Stereo(FileStorage& fs, JPP_Config& config); 38 | Stereo& operator=(Stereo& s); 39 | ~Stereo(); 40 | void load_images(const Mat& left, const Mat& right); 41 | bool in_img(int x, int y); 42 | Point project_point_cam(const Point3f p, int cam); 43 | void init_daisy_descriptors(double rad, int radq, int thq, int histq, int nrm_type); 44 | double desc_cost(Point left, Point right, int w); 45 | double desc_cost_SAD(Point left, Point right, int w); 46 | bool conf_positive(const Point3f p); 47 | //bool conf_positive(const Point3f p, float z_start, float z_end); 48 | bool conf_negative(const Point3f p); 49 | bool is_obstacle_free_region(const Point3f p); 50 | bool is_empty_col(const Point3f p); 51 | bool is_bot_clear(const Point3f p, float safe_radius, float inc, bool col_check); 52 | bool is_bot_clear_blind_ground(const Point3f p, float safe_radius, float inc, bool col_check); 53 | int compute_disparity(Point p, int ndisp, int w); 54 | void compute_disparity_map(int ndisp, int w); 55 | void jpp_visualizations(Mat& confPos, Mat& confNeg); 56 | void blend_images(Mat& src1, Mat& src2, float alpha, Mat& dst); 57 | void update_jpp_config(JPP_Config& config); 58 | Mat get_img_left(); 59 | Mat get_img_right(); 60 | Mat get_Q_matrix(); 61 | Mat get_disparity_map(); 62 | }; 63 | 64 | #endif // STEREO_H -------------------------------------------------------------------------------- /include/util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | #define UTIL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "daisy/daisy.h" 10 | 11 | struct JPP_Config { 12 | int CALIB_IMG_WIDTH; 13 | int CALIB_IMG_HEIGHT; 14 | int RECT_IMG_WIDTH; 15 | int RECT_IMG_HEIGHT; 16 | double DR; 17 | int DQ; 18 | int DT; 19 | int DH; 20 | int BOT_LENGTH; 21 | int BOT_WIDTH; 22 | int BOT_HEIGHT; 23 | int GRID_SIZE; 24 | double CONF_POS_THRESH; 25 | double CONF_NEG_THRESH; 26 | int SAD_WINDOW_SIZE; 27 | int SPATIAL_FILTER_WINDOW; 28 | int SPATIAL_FILTER_INC; 29 | double SPATIAL_FILTER_RATIO; 30 | int CONF_NEG_INC; 31 | double CONF_NEG_FILTER_RATIO; 32 | int START_X; 33 | int MAX_X; 34 | int MAX_Y; 35 | int CONVEX_WORLD; 36 | 37 | JPP_Config() {} 38 | JPP_Config(config_t *cf) { 39 | config_lookup_int(cf, "calib_img_width", &CALIB_IMG_WIDTH); 40 | config_lookup_int(cf, "calib_img_height", &CALIB_IMG_HEIGHT); 41 | config_lookup_int(cf, "rect_img_width", &RECT_IMG_WIDTH); 42 | config_lookup_int(cf, "rect_img_height", &RECT_IMG_HEIGHT); 43 | config_lookup_float(cf, "daisy_params.R", &DR); 44 | config_lookup_int(cf, "daisy_params.Q", &DQ); 45 | config_lookup_int(cf, "daisy_params.T", &DT); 46 | config_lookup_int(cf, "daisy_params.H", &DH); 47 | config_lookup_int(cf, "bot_length", &BOT_LENGTH); 48 | config_lookup_int(cf, "bot_width", &BOT_WIDTH); 49 | config_lookup_int(cf, "bot_height", &BOT_HEIGHT); 50 | config_lookup_int(cf, "grid_size", &GRID_SIZE); 51 | config_lookup_float(cf, "conf_pos_thresh", &CONF_POS_THRESH); 52 | config_lookup_float(cf, "conf_neg_thresh", &CONF_NEG_THRESH); 53 | config_lookup_int(cf, "SAD_window_size", &SAD_WINDOW_SIZE); 54 | config_lookup_int(cf, "spatial_filter_window", &SPATIAL_FILTER_WINDOW); 55 | config_lookup_int(cf, "spatial_filter_inc", &SPATIAL_FILTER_INC); 56 | config_lookup_float(cf, "spatial_filter_ratio", &SPATIAL_FILTER_RATIO); 57 | config_lookup_int(cf, "conf_neg_inc", &CONF_NEG_INC); 58 | config_lookup_float(cf, "conf_neg_filter_ratio", &CONF_NEG_FILTER_RATIO); 59 | config_lookup_int(cf, "start_x", &START_X); 60 | config_lookup_int(cf, "max_x", &MAX_X); 61 | config_lookup_int(cf, "max_y", &MAX_Y); 62 | config_lookup_bool(cf, "convex_world", &CONVEX_WORLD); 63 | //diagnostic 64 | /* 65 | printf("CALIB_IMG_WIDTH: %d\n", CALIB_IMG_WIDTH); 66 | printf("CALIB_IMG_HEIGHT: %d\n", CALIB_IMG_HEIGHT); 67 | printf("RECT_IMG_WIDTH: %d\n", RECT_IMG_WIDTH); 68 | printf("RECT_IMG_HEIGHT: %d\n", RECT_IMG_HEIGHT); 69 | printf("DR: %lf\n", DR); 70 | printf("DQ: %d\n", DQ); 71 | printf("DT: %d\n", DT); 72 | printf("DH: %d\n", DH); 73 | printf("BOT_LENGTH: %d\n", BOT_LENGTH); 74 | printf("BOT_WIDTH: %d\n", BOT_WIDTH); 75 | printf("BOT_HEIGHT: %d\n", BOT_HEIGHT); 76 | printf("GRID_SIZE: %d\n", GRID_SIZE); 77 | printf("CONF_POS_THRESH: %f\n", CONF_POS_THRESH); 78 | printf("CONF_NEG_THRESH: %f\n", CONF_NEG_THRESH); 79 | printf("SAD_WINDOW_SIZE: %d\n", SAD_WINDOW_SIZE); 80 | printf("SPATIAL_FILTER_WINDOW: %d\n", SPATIAL_FILTER_WINDOW); 81 | printf("SPATIAL_FILTER_INC: %d\n", SPATIAL_FILTER_INC); 82 | printf("SPATIAL_FILTER_RATIO: %f\n", SPATIAL_FILTER_RATIO); 83 | printf("CONF_NEG_INC: %d\n", CONF_NEG_INC); 84 | printf("CONF_NEG_FILTER_RATIO: %f\n", CONF_NEG_FILTER_RATIO); 85 | printf("START_X: %d\n", START_X); 86 | printf("MAX_X: %d\n", MAX_X); 87 | printf("MAX_Y: %d\n", MAX_Y); 88 | printf("CONVEX_WORLD: %d\n", CONVEX_WORLD); 89 | */ 90 | } 91 | }; 92 | 93 | #endif // UTIL_H -------------------------------------------------------------------------------- /scripts/plot_epipolar_costs.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.mlab as mlab 4 | import sys 5 | import math 6 | 7 | def normalize_costs(costs): 8 | c = np.array(costs) 9 | norm_costs = c / np.max(c) 10 | norm_costs[norm_costs < 1e-9] = 1.0 11 | return norm_costs 12 | 13 | 14 | costs_file = open(sys.argv[1], "r") 15 | lines = costs_file.readlines() 16 | 17 | ndisp = 0 18 | count = 0 19 | costs = [] 20 | gt_disp = [] 21 | 22 | for line in lines: 23 | parts = line.split(" ") 24 | if count == 0: 25 | ndisp = int(parts[0]) 26 | else: 27 | c = [] 28 | for i in range(ndisp+1): 29 | c.append(float(parts[i])) 30 | costs.append(c) 31 | gt_disp.append(int(parts[ndisp+1])) 32 | count = count + 1 33 | 34 | plt.figure(figsize=(9,6)) 35 | plt.xlim(-20, 20) 36 | 37 | for i in range(count-1): 38 | x = [(k-gt_disp[i]) for k in range(ndisp+1)] 39 | plt.plot(x, costs[i], color='blue', alpha=0.1) 40 | if i > 2000: 41 | break 42 | 43 | 44 | plt.xlabel('Disparity (Ground truth at x = 0)') 45 | plt.ylabel('Normalized Cost') 46 | plt.tight_layout() 47 | plt.draw() 48 | plt.pause(1) 49 | raw_input() -------------------------------------------------------------------------------- /scripts/profile: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # (C) Joydeep Biswas, 2010, 2017 3 | # A simple Bash script to profile a command and 4 | # display the results on termination. 5 | # You'll need to install the following packages: valgrind, kcachegrind 6 | if [ ! -d "profile_results" ]; then 7 | mkdir profile_results 8 | fi 9 | echo Profiling \"$@\" 10 | valgrind --tool=callgrind --dump-instr=yes --trace-jump=yes --callgrind-out-file="profile_results/callgrind.out.%p" $@ 11 | kcachegrind `ls -t1 profile_results/callgrind.out.*|head -n 1` 12 | -------------------------------------------------------------------------------- /src/analysis.cc: -------------------------------------------------------------------------------- 1 | #include "jpp.h" 2 | #include "popt_pp.h" 3 | 4 | JPP *jpp; 5 | 6 | void compute_epipolar_costs() { 7 | int max_disp = 50; 8 | ofstream data_file; 9 | data_file.open("epipolar_costs.txt"); 10 | data_file << max_disp << endl; 11 | Mat ground_truth_dmap = jpp->get_disparity_map("spp", max_disp); 12 | int start_x = ground_truth_dmap.cols/2 - 50; 13 | int end_x = ground_truth_dmap.cols/2 + 50; 14 | int start_y = ground_truth_dmap.rows/2 - 50; 15 | int end_y = ground_truth_dmap.rows/2 + 50; 16 | for (int i = start_x; i < end_x; i++) { 17 | for (int j = start_y; j < end_y; j++) { 18 | cout << i << ", " << j << endl; 19 | vector< double > costs = jpp->get_epipolar_costs(Point(i, j), max_disp); 20 | for (int k = 0; k < costs.size(); k++) { 21 | data_file << costs[k] << " "; 22 | } 23 | data_file << ((int)ground_truth_dmap.at(j, i)) << endl; 24 | } 25 | } 26 | } 27 | 28 | int main(int argc, const char** argv) { 29 | const char* left_img_file; 30 | const char* right_img_file; 31 | const char* calib_file; 32 | const char* jpp_config_file; 33 | const char* img_dir; 34 | const char* output = "astar"; 35 | int w = 0; 36 | int num_imgs = 0; 37 | 38 | static struct poptOption options[] = { 39 | { "num_imgs",'n',POPT_ARG_INT,&num_imgs,0,"Number of images to be processed","NUM" }, 40 | { "img_dir",'d',POPT_ARG_STRING,&img_dir,0,"Directory containing image pairs (set if n > 0)","STR" }, 41 | { "left_img",'l',POPT_ARG_STRING,&left_img_file,0,"Left image file name","STR" }, 42 | { "right_img",'r',POPT_ARG_STRING,&right_img_file,0,"Right image file name","STR" }, 43 | { "calib_file",'c',POPT_ARG_STRING,&calib_file,0,"Stereo calibration file name","STR" }, 44 | { "jpp_config_file",'j',POPT_ARG_STRING,&jpp_config_file,0,"JPP config file name","STR" }, 45 | { "output",'o',POPT_ARG_STRING,&output,0,"Output - astar, rrt, debug","STR" }, 46 | { "write_files",'w',POPT_ARG_INT,&w,0,"Set w=1 for writing visualizations to files","NUM" }, 47 | POPT_AUTOHELP 48 | { NULL, 0, 0, NULL, 0, NULL, NULL } 49 | }; 50 | 51 | POpt popt(NULL, argc, argv, options, 0); 52 | int c; 53 | while((c = popt.getNextOpt()) >= 0) {} 54 | 55 | // Read JPP config 56 | config_t cfg, *cf; 57 | cf = &cfg; 58 | config_init(cf); 59 | if (!config_read_file(cf, jpp_config_file)) { 60 | cout << "Could not read config file!" << endl; 61 | config_destroy(cf); 62 | return(EXIT_FAILURE); 63 | } 64 | 65 | // Read stereo calibration info 66 | FileStorage fs = FileStorage(calib_file, FileStorage::READ); 67 | 68 | JPP jpp(fs, cf); 69 | 70 | if (num_imgs == 0) { 71 | Mat img_left = imread(left_img_file, CV_LOAD_IMAGE_COLOR); 72 | Mat img_right = imread(right_img_file, CV_LOAD_IMAGE_COLOR); 73 | 74 | char dmap_file[100], rrt_file_prefix[100], astar_file_prefix[100]; 75 | sprintf(astar_file_prefix, "%s%d", "astar", 0); 76 | sprintf(rrt_file_prefix, "%s%d", "rrt", 0); 77 | sprintf(dmap_file, "%s%d.jpg", "dmap", 0); 78 | 79 | jpp.load_images(img_left, img_right); 80 | /* 81 | int max_disp = 50; 82 | Mat dmap = jpp->get_disparity_map("spp", max_disp); 83 | for (int i = 0; i < dmap.cols; i++) { 84 | for (int j = 0; j < dmap.rows; j++) { 85 | int d = (int)dmap.at(j, i); 86 | dmap.at(j, i) = (int)(((double)d/(double)max_disp)*255.0); 87 | } 88 | } 89 | while (1) { 90 | imshow("DMAP", dmap); 91 | imshow("LEFT", jpp->get_img_left()); 92 | if (waitKey(30) > 0) { 93 | break; 94 | } 95 | } 96 | */ 97 | compute_epipolar_costs(); 98 | 99 | } else if (num_imgs > 0) { 100 | for (int i = 1; i <= num_imgs; i++) { 101 | cout << "Processing pair " << i << endl; 102 | char left_img_file[100], right_img_file[100]; 103 | char dmap_file[100], rrt_file_prefix[100], astar_file_prefix[100]; 104 | sprintf(left_img_file, "%s%s%d.jpg", img_dir, "left", i); 105 | sprintf(right_img_file, "%s%s%d.jpg", img_dir, "right", i); 106 | sprintf(dmap_file, "%s%d.jpg", "dmap", i); 107 | sprintf(astar_file_prefix, "%s%d", "astar", i); 108 | sprintf(rrt_file_prefix, "%s%d", "rrt", i); 109 | 110 | Mat img_left = imread(left_img_file, CV_LOAD_IMAGE_COLOR); 111 | Mat img_right = imread(right_img_file, CV_LOAD_IMAGE_COLOR); 112 | 113 | jpp.load_images(img_left, img_right); 114 | } 115 | } 116 | 117 | config_destroy(cf); 118 | return 0; 119 | } -------------------------------------------------------------------------------- /src/astarplanner.cc: -------------------------------------------------------------------------------- 1 | #include "astarplanner.h" 2 | 3 | AStarPlanner::AStarPlanner(JPP_Config& conf) 4 | { 5 | closed_list = Mat(1000, 1000, CV_8UC1, Scalar(0)); 6 | _config = conf; 7 | } 8 | 9 | bool AStarPlanner::inGrid(Point p) 10 | { 11 | //return (p.x >= start.p.x && p.x <= max_x && p.y >= -max_y && p.y <= max_y); 12 | return (p.x >= 0 && p.x <= max_x && p.y >= -max_y && p.y <= max_y); 13 | } 14 | 15 | float AStarPlanner::L1Dist(Point p, Point q) 16 | { 17 | float s = fabs(p.x - q.x) + fabs(p.y - q.y); 18 | return s; 19 | } 20 | 21 | float AStarPlanner::L2Dist(Point p, Point q) 22 | { 23 | float s = sqrt((p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y)); 24 | return s; 25 | } 26 | 27 | void AStarPlanner::setParams(Point s, Point e, int my, int i, int r, int bh, 28 | bool cw) 29 | { 30 | max_x = e.x; 31 | max_y = my; 32 | inc = i; 33 | start.p = s; 34 | start.g = start.h = 0; 35 | start.f = 0; 36 | start.id = 0; 37 | 38 | //pach 39 | Point realStart; 40 | realStart.x = 0; 41 | realStart.y = 0; 42 | 43 | grid_id[0] = realStart; //s; 44 | parent[0] = -1; 45 | end.p = e; 46 | safe_radius = r; 47 | convex_world = cw; 48 | bot_height = bh; 49 | 50 | Point goal = e; 51 | if (goal.x > _config.MAX_X) //when past grid 52 | { 53 | printf("goal.x: %d, goal.y: %d\n", goal.x, goal.y); 54 | //project goal to edge of planner grid 55 | int y_intercept = (int)round(((float)goal.y*(float)_config.MAX_X)/(float)goal.x); 56 | printf("y_intercept: %d\n", y_intercept); 57 | if (y_intercept > -_config.MAX_Y && y_intercept < _config.MAX_Y) 58 | { 59 | end.p.x = _config.MAX_X; 60 | end.p.y = y_intercept; 61 | } 62 | else 63 | { 64 | printf("bad\n"); 65 | if (goal.y > 0) 66 | { 67 | path.push_back(Point(0, _config.MAX_X)); 68 | } 69 | else 70 | { 71 | path.push_back(Point(0, -_config.MAX_X)); 72 | } 73 | } 74 | } 75 | else if (goal.x >= _config.START_X && goal.x <= _config.MAX_X && // when in grid 76 | goal.y >= -_config.MAX_Y && goal.y <= _config.MAX_Y) 77 | { 78 | //do nothing 79 | printf("good\n"); 80 | } 81 | else 82 | { 83 | printf("duno\n"); 84 | if (goal.y > 0) 85 | { 86 | path.push_back(Point(0, _config.MAX_X)); 87 | } 88 | else 89 | { 90 | path.push_back(Point(0, -_config.MAX_X)); 91 | } 92 | } 93 | } 94 | 95 | Point AStarPlanner::clCoord(Point p) 96 | { 97 | Point q = Point(-p.y + 3000, 9000 - p.x); 98 | Point r = Point(q.x * 0.1, q.y * 0.1); 99 | return r; 100 | } 101 | 102 | void AStarPlanner::findPath(Stereo* stereo) 103 | { 104 | if (path.size() > 0)//end point was not valid see constructor 105 | { 106 | return; 107 | } 108 | 109 | node robotCenter; 110 | robotCenter.p.x = 0; 111 | robotCenter.p.y = 0; 112 | robotCenter.g = 0; 113 | robotCenter.f = 0; 114 | robotCenter.id = 0; 115 | 116 | open_list.insert(robotCenter);//start 117 | node closestnode = robotCenter;//start 118 | closestnode.f = 1e9; 119 | float mindist = 1e9; 120 | multiset< node >::iterator ito; 121 | int x = 0; 122 | bool stop = false; 123 | for (;!open_list.empty() && !stop;) { 124 | ito = open_list.begin(); 125 | node q = *ito; 126 | //cout << q.p << " - " << open_list.size() << endl; 127 | open_list.erase(ito); 128 | Point qcl = clCoord(q.p); 129 | /* 130 | if ((int)closed_list.at(qcl) == 255) { 131 | break; 132 | } 133 | */ 134 | //world->img_world.at(q_sim_coord) = Vec3b(0,0,255); 135 | //circle(closed_list, qcl, 1, Scalar(255), -1, 8, 0); 136 | closed_list.at(qcl) = 255; 137 | graph.push_back(q); 138 | //cout << "add close list: " << q.p << " ------ " << endl; 139 | vector< Point > neighbours; 140 | neighbours.push_back(Point(inc,0)); 141 | neighbours.push_back(Point(inc,inc)); 142 | neighbours.push_back(Point(inc,-inc)); 143 | neighbours.push_back(Point(0,inc)); 144 | neighbours.push_back(Point(0,-inc)); 145 | //neighbours.push_back(Point(-inc,inc)); 146 | //neighbours.push_back(Point(-inc,0)); 147 | //neighbours.push_back(Point(-inc,-inc)); 148 | for (int i = 0; i < neighbours.size(); i++) { 149 | Point cur_pt = q.p + neighbours[i]; 150 | if (!inGrid(cur_pt)) { 151 | continue; 152 | } 153 | Point curpt_cl = clCoord(cur_pt); 154 | if ((int)closed_list.at(curpt_cl) == 255) { 155 | //cout << "closed " << cur_pt << endl; 156 | continue; 157 | } 158 | // obstacle check 159 | Point3f pt3d = Point3f((float)cur_pt.x/1000.,(float)cur_pt.y/1000.,0); 160 | 161 | //obstical check within grid 162 | if (cur_pt.x >= start.p.x && !stereo->is_bot_clear(pt3d, (float)safe_radius/1000., (float)inc/1000., !convex_world)) 163 | continue; 164 | //obstical check blind to ground area before grid 165 | //else if (!stereo->is_bot_clear_blind_ground(pt3d, (float)safe_radius/1000., (float)inc/1000., !convex_world)) 166 | //continue; 167 | 168 | // create successor node 169 | node suc; 170 | suc.p = cur_pt; 171 | // if goal is reached terminate 172 | if (L2Dist(cur_pt, end.p) < 2 * inc) { 173 | end = suc; 174 | end.id = ++x; 175 | parent[end.id] = q.id; 176 | grid_id[end.id] = cur_pt; 177 | //cout << "path found" << endl; 178 | setPath(end); 179 | stop = true; 180 | break; 181 | } 182 | // update scores of successor node 183 | suc.g = q.g + L2Dist(suc.p, q.p); 184 | suc.h = L1Dist(end.p, cur_pt); 185 | suc.f = suc.g + suc.h; 186 | 187 | /* 188 | x++; 189 | suc.id = x; 190 | x++; 191 | grid_id[x] = cur_pt; 192 | suc.id = x; 193 | */ 194 | 195 | //ito = find(open_list.begin(), open_list.end(), suc); 196 | ito = open_list.find(suc); 197 | float eps = 1e-5; 198 | if (ito != open_list.end()) { 199 | node n = *ito; 200 | if (suc.g < n.g) { 201 | //cout << "update - " << suc.p << " " << suc.g << " " << n.g << endl; 202 | suc.id = n.id; 203 | parent[suc.id] = q.id; 204 | grid_id[suc.id] = cur_pt; 205 | open_list.erase(ito); 206 | open_list.insert(suc); 207 | } 208 | } else { 209 | x++; 210 | suc.id = x; 211 | grid_id[suc.id] = cur_pt; 212 | parent[suc.id] = q.id; 213 | /* 214 | cout << suc.p << " f: " << suc.f << " g: " << suc.g << " h: " << 215 | suc.h << " " << parent[suc.id] << endl; 216 | */ 217 | open_list.insert(suc); 218 | } 219 | 220 | if (norm(suc.p - end.p) < mindist) { 221 | mindist = norm(suc.p - end.p); 222 | closestnode = suc; 223 | } 224 | } 225 | } 226 | //cout << "path not found" << endl; 227 | if (!stop) 228 | setPath(closestnode); 229 | } 230 | 231 | void AStarPlanner::setPath(AStarPlanner::node n) 232 | { 233 | int id = n.id; 234 | if (id == -1) return; 235 | while (id != -1) { 236 | path.push_back(grid_id[id]); 237 | id = parent[id]; 238 | } 239 | } 240 | 241 | vector< Point > AStarPlanner::getPath() 242 | { 243 | return path; 244 | } 245 | 246 | int AStarPlanner::getPathLength() 247 | { 248 | int length = 0; 249 | for (int i = 0; i < path.size()-1; i++) { 250 | Point p = path[i]; 251 | Point q = path[i+1]; 252 | length += norm(p-q); 253 | } 254 | return length; 255 | } -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/convolution.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_CONVOLUTION_H 42 | #define KUTILITY_CONVOLUTION_H 43 | 44 | #if defined(WITH_OPENCV) && defined(WITH_OPENCV_EXTRAS) 45 | #include "kutility/convolution_opencv.h" 46 | #else 47 | #include "kutility/convolution_default.h" 48 | #endif 49 | 50 | namespace kutility 51 | { 52 | inline void convolve_sym( float* image, int h, int w, float* kernel, int ksize, float* out=NULL ) 53 | { 54 | if( out == NULL ) out = image; 55 | else memcpy( out, image, sizeof(float)*h*w ); 56 | 57 | if( h == 240 && w == 320 ) { convolve_sym_(out, 240, 320, kernel, ksize); return; } 58 | if( h == 480 && w == 640 ) { convolve_sym_(out, 480, 640, kernel, ksize); return; } 59 | if( h == 512 && w == 512 ) { convolve_sym_(out, 512, 512, kernel, ksize); return; } 60 | if( h == 512 && w == 768 ) { convolve_sym_(out, 512, 768, kernel, ksize); return; } 61 | if( h == 600 && w == 800 ) { convolve_sym_(out, 600, 800, kernel, ksize); return; } 62 | if( h == 768 && w == 1024 ) { convolve_sym_(out, 768, 1024, kernel, ksize); return; } 63 | if( h == 1024 && w == 768 ) { convolve_sym_(out, 1024, 768, kernel, ksize); return; } 64 | if( h == 256 && w == 256 ) { convolve_sym_(out, 256, 256, kernel, ksize); return; } 65 | if( h == 128 && w == 128 ) { convolve_sym_(out, 128, 128, kernel, ksize); return; } 66 | if( h == 128 && w == 192 ) { convolve_sym_(out, 128, 192, kernel, ksize); return; } 67 | //cout<<"[convolve_sym] insert this h,w to unrolling list: "< inline 49 | void conv_buffer_(T1* buffer, T2* kernel, int rsize, int ksize) 50 | { 51 | for ( int i=0; i inline 63 | void conv_horizontal(T1* image, int h, int w, T2 *kernel, int ksize) 64 | { 65 | int halfsize = ksize / 2; 66 | assert(w + ksize < 4096); 67 | 68 | T1 buffer[4096]; 69 | for( int r=0; r inline 90 | void conv_vertical(T1* image, int h, int w, T2 *kernel, int ksize) 91 | { 92 | T1 buffer[4096]; 93 | 94 | int halfsize = ksize / 2; 95 | assert(h + ksize < 4096); 96 | 97 | int h_1w = (h-1)*w; 98 | 99 | for( int c=0; c inline 120 | void convolve_sym_( T* image, int h, int w, T* kernel, int ksize ) 121 | { 122 | conv_horizontal( image, h, w, kernel, ksize ); 123 | conv_vertical ( image, h, w, kernel, ksize ); 124 | } 125 | } 126 | 127 | #endif 128 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/convolution_opencv.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_CONVOLUTION_OPENCV_TCC 42 | #define KUTILITY_CONVOLUTION_OPENCV_TCC 43 | 44 | #if defined(WITH_OPENCV) && defined(WITH_OPENCV_EXTRAS) 45 | 46 | #include "cv.h" 47 | #include "highgui.h" 48 | 49 | namespace kutility 50 | { 51 | inline void conv_horizontal( float* image, int h, int w, float* kernel, int ksize ) 52 | { 53 | CvMat cvI; cvInitMatHeader(&cvI, h, w, CV_32FC1, (float*)image); 54 | CvMat cvK; cvInitMatHeader(&cvK, 1, ksize, CV_32FC1, (float*)kernel ); 55 | cvFilter2D( &cvI, &cvI, &cvK ); 56 | } 57 | inline void conv_horizontal( double* image, int h, int w, double* kernel, int ksize ) 58 | { 59 | CvMat cvI; cvInitMatHeader(&cvI, h, w, CV_64FC1, (double*)image); 60 | CvMat cvK; cvInitMatHeader(&cvK, 1, ksize, CV_64FC1, (double*)kernel ); 61 | cvFilter2D( &cvI, &cvI, &cvK ); 62 | } 63 | 64 | inline void conv_vertical( float* image, int h, int w, float* kernel, int ksize ) 65 | { 66 | CvMat cvI; cvInitMatHeader(&cvI, h, w, CV_32FC1, (float*)image); 67 | CvMat cvK; cvInitMatHeader(&cvK, ksize, 1, CV_32FC1, (float*)kernel ); 68 | cvFilter2D( &cvI, &cvI, &cvK ); 69 | } 70 | 71 | inline void conv_vertical( double* image, int h, int w, double* kernel, int ksize ) 72 | { 73 | CvMat cvI; cvInitMatHeader(&cvI, h, w, CV_64FC1, (double*)image); 74 | CvMat cvK; cvInitMatHeader(&cvK, ksize, 1, CV_64FC1, (double*)kernel ); 75 | cvFilter2D( &cvI, &cvI, &cvK ); 76 | } 77 | 78 | template inline 79 | void convolve_sym_( T* image, int h, int w, T* kernel, int ksize ) 80 | { 81 | conv_horizontal( image, h, w, kernel, ksize ); 82 | conv_vertical ( image, h, w, kernel, ksize ); 83 | } 84 | } 85 | 86 | #endif 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/corecv.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_CORECV_H 42 | #define KUTILITY_CORECV_H 43 | 44 | #include "math.h" 45 | 46 | namespace kutility 47 | { 48 | void point_transform_via_homography( double* H, double x, double y, double &u, double &v ); 49 | 50 | double epipolar_line_slope( double y, double x, double* F ); 51 | 52 | 53 | } 54 | 55 | 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/fileio.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_FILEIO_H 42 | #define KUTILITY_FILEIO_H 43 | 44 | #include "kutility/general.h" 45 | 46 | namespace kutility 47 | { 48 | enum data_types {TYPE_CHAR, TYPE_FLOAT, TYPE_DOUBLE, TYPE_INT}; 49 | 50 | // ascii 51 | template inline void save_ascii( ofstream& fout, T* data, int h, int w, int nb, int type ); 52 | template inline void save_ascii( string filename, T* data, int h, int w, int nb, int type ); 53 | template inline void load_ascii( ifstream& fin, T* &data, int &h, int &w, int &nb ); 54 | template inline void load_ascii( string filename, T* &data, int &h, int &w, int &nb ); 55 | 56 | // binary 57 | template inline void save_binary(ofstream& fout, T* data, int h, int w, int nb, int type ); 58 | template inline int save_binary(string filename, T* data, int h, int w, int nb, int type ); 59 | inline int load_binary(ifstream &fin, float* &data, int &h, int &w, int &nb ); 60 | inline int load_binary(ifstream &fin, int* &data, int &h, int &w, int &nb ); 61 | inline int load_binary(ifstream &fin, double* &data, int &h, int &w, int &nb ); 62 | inline int load_binary(ifstream &fin, char* &data, int &h, int &w, int &nb ); 63 | template inline int load_binary(string filename, T* &data, int &h, int &w, int &nb ); 64 | 65 | template inline void save_plain(ofstream& fout, T* data, int sz ); 66 | template inline void save_plain(ofstream& fout, T* data, int rs, int cs ); 67 | template inline void save(string filename, T* data, int sz ); 68 | template inline void save(string filename, T* data, int rs, int cs); 69 | 70 | template inline int load( ifstream& fin, T* &out, int size=-1 ); 71 | template inline int load( string filename, T* &out, int size=-1 ); 72 | inline void* load_array( string filename, int size, int type=1 ); 73 | 74 | #include "fileio.tcc" 75 | } 76 | 77 | #endif 78 | 79 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/image.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_IMAGE_H 42 | #define KUTILITY_IMAGE_H 43 | 44 | #include "kutility/image_io_bmp.h" 45 | #include "kutility/image_io_pnm.h" 46 | 47 | #ifdef WITH_JPEG 48 | #include "kutility/image_io_jpeg.h" 49 | #endif 50 | 51 | #ifdef WITH_PNG 52 | #include "kutility/image_io_png.h" 53 | #endif 54 | 55 | #include "kutility/image_manipulation.h" 56 | 57 | 58 | //inline int load_image(string filename, uchar* &body, int &h, int &w, int &ch ); 59 | //inline int load_gray_image( string filename, uchar* &body, int &h, int &w ); 60 | //template inline int tload_image(string filename, T* &body, int& h, int& w, int& ch); 61 | //template inline int tload_gray_image(string filename, T* &body, int& h, int& w); 62 | //inline void save_image( string filename, uchar* body, int h, int w, int ch ); 63 | //template inline void tsave_image(string filename, T* body, int h, int w, int d); 64 | 65 | namespace kutility 66 | { 67 | inline int load_image(string filename, uchar* &body, int &h, int &w, int &ch ) 68 | { 69 | string format = get_file_format( filename ); 70 | if( !format.compare( "jpg" ) || !format.compare( "jpeg" ) ) 71 | { 72 | #ifdef WITH_JPEG 73 | return load_jpg(filename.c_str(), body, h, w, ch); 74 | #else 75 | cout<<"cannot load jpeg file: "< inline int tload_image(string filename, T* &body, int& h, int& w, int& ch) 122 | { 123 | uchar* data = NULL; 124 | if( !load_image(filename, data, h, w, ch ) ) 125 | { 126 | body = type_cast(data, h*w*ch); 127 | delete []data; 128 | return 0; 129 | } 130 | cout<<"could not load: tload_image"< inline int tload_gray_image(string filename, T* &body, int& h, int& w) 134 | { 135 | uchar* data = NULL; 136 | if( !load_gray_image( filename, data, h, w ) ) 137 | { 138 | body = type_cast(data, h*w); 139 | delete []data; 140 | return 0; 141 | } 142 | cout<<"could not load: tload_gray_image"< inline void tsave_image(string filename, T* body, int h, int w, int d) 203 | { 204 | uchar* tdata = type_cast(body, h*w*d); 205 | save_image(filename, tdata, h, w, d); 206 | delete []tdata; 207 | } 208 | 209 | } 210 | 211 | #endif 212 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/image_io.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_IMAGE_IO_H 42 | #define KUTILITY_IMAGE_IO_H 43 | 44 | #include "kutility/image_io_bmp.h" 45 | #include "kutility/image_io_pnm.h" 46 | 47 | #ifdef WITH_JPEG 48 | #include "kutility/image_io_jpeg.h" 49 | #endif 50 | 51 | #ifdef WITH_PNG 52 | #include "kutility/image_io_png.h" 53 | #endif 54 | 55 | #include "kutility/image_manipulation.h" 56 | 57 | 58 | //inline int load_image(string filename, uchar* &body, int &h, int &w, int &ch ); 59 | //inline int load_gray_image( string filename, uchar* &body, int &h, int &w ); 60 | //template inline int tload_image(string filename, T* &body, int& h, int& w, int& ch); 61 | //template inline int tload_gray_image(string filename, T* &body, int& h, int& w); 62 | //inline void save_image( string filename, uchar* body, int h, int w, int ch ); 63 | //template inline void tsave_image(string filename, T* body, int h, int w, int d); 64 | 65 | namespace kutility 66 | { 67 | inline int load_image(string filename, uchar* &body, int &h, int &w, int &ch ) 68 | { 69 | string format = get_file_format( filename ); 70 | if( !format.compare( "jpg" ) || !format.compare( "jpeg" ) ) 71 | { 72 | #ifdef WITH_JPEG 73 | return load_jpg(filename.c_str(), body, h, w, ch); 74 | #else 75 | cout<<"cannot load jpeg file: "< inline int tload_image(string filename, T* &body, int& h, int& w, int& ch) 122 | { 123 | uchar* data = NULL; 124 | if( !load_image(filename, data, h, w, ch ) ) 125 | { 126 | body = type_cast(data, h*w*ch); 127 | delete []data; 128 | return 0; 129 | } 130 | cout<<"could not load: tload_image"< inline int tload_gray_image(string filename, T* &body, int& h, int& w) 134 | { 135 | uchar* data = NULL; 136 | if( !load_gray_image( filename, data, h, w ) ) 137 | { 138 | body = type_cast(data, h*w); 139 | delete []data; 140 | return 0; 141 | } 142 | cout<<"could not load: tload_gray_image"< inline void tsave_image(string filename, T* body, int h, int w, int d) 203 | { 204 | uchar* tdata = type_cast(body, h*w*d); 205 | save_image(filename, tdata, h, w, d); 206 | delete []tdata; 207 | } 208 | 209 | } 210 | 211 | #endif 212 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/image_io_bmp.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_IMAGE_IO_BMP_H 42 | #define KUTILITY_IMAGE_IO_BMP_H 43 | 44 | #if defined(WIN32) 45 | #pragma warning( disable : 4996 ) 46 | #endif 47 | 48 | #include 49 | 50 | using std::string; 51 | 52 | #ifndef uchar 53 | typedef unsigned char uchar; 54 | #endif 55 | 56 | namespace kutility 57 | { 58 | /// converts an integer number to a hex string. 59 | inline void convert_hex(int number, int* hex_array) 60 | { 61 | for(int i=0; i<4; i++) 62 | { 63 | hex_array[i] = number%256; 64 | number = number/256; 65 | } 66 | } 67 | 68 | // void savebmp(string str, uchar* body, int h, int w, int channel); 69 | void save_bmp(const char* str, uchar* body, int h, int w, int channel); 70 | 71 | } 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/image_io_jpeg.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifdef WITH_JPEG 42 | 43 | #ifndef KUTILITY_IMAGE_IO_JPEG 44 | #define KUTILITY_IMAGE_IO_JPEG 45 | 46 | #include 47 | extern "C" { 48 | #include "jpeglib.h" 49 | } 50 | #include 51 | #include 52 | 53 | #ifndef uchar 54 | typedef unsigned char uchar; 55 | #endif 56 | 57 | using std::string; 58 | 59 | void save_jpg(const char* filename, uchar* body, int h, int w, int ch, int quality); 60 | int load_jpg(const char* filename, uchar* &body, int &h, int &w, int &ch); 61 | 62 | #endif 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/image_io_png.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifdef WITH_PNG 42 | 43 | #ifndef KUTILITY_IMAGE_IO_PNG_H 44 | #define KUTILITY_IMAGE_IO_PNG_H 45 | 46 | extern "C" { 47 | #include "png.h" 48 | } 49 | 50 | #include "kutility/kutility.def" 51 | 52 | namespace kutility 53 | { 54 | int load_png(const char* file_name, uchar* &body, int &h, int &w, int &ch); 55 | void save_png(const char* file_name, uchar* body, int height, int width, int chl); 56 | } 57 | 58 | typedef struct _write_png_info 59 | { 60 | double gamma; 61 | long width; 62 | long height; 63 | time_t modtime; 64 | FILE *infile; 65 | FILE *outfile; 66 | void *png_ptr; 67 | void *info_ptr; 68 | uchar *image_data; 69 | uchar **row_pointers; 70 | char *title; 71 | char *author; 72 | char *desc; 73 | char *copyright; 74 | char *email; 75 | char *url; 76 | int channel_no; 77 | int filter; /* command-line-filter flag, not PNG row filter! */ 78 | // int pnmtype; 79 | int sample_depth; 80 | int interlaced; 81 | int have_time; 82 | jmp_buf jmpbuf; 83 | uchar bg_red; 84 | uchar bg_green; 85 | uchar bg_blue; 86 | } write_png_info; 87 | 88 | void wpng_cleanup(write_png_info* a); 89 | 90 | void writepng_version_info (); 91 | int writepng_init (write_png_info *png_ptr); 92 | int writepng_encode_image (write_png_info *png_ptr); 93 | int writepng_encode_row (write_png_info *png_ptr); 94 | int writepng_encode_finish(write_png_info *png_ptr); 95 | void writepng_cleanup (write_png_info *png_ptr); 96 | 97 | #endif 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/image_io_pnm.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_IMAGE_IO_PNM_H 42 | #define KUTILITY_IMAGE_IO_PNM_H 43 | 44 | #include 45 | #include "string.h" 46 | #include 47 | #include "limits.h" 48 | 49 | #ifndef uchar 50 | typedef unsigned char uchar; 51 | #endif 52 | 53 | namespace kutility 54 | { 55 | void load_pbm(const char* name, uchar* &data, int &height, int &width); 56 | void load_pgm(const char* name, uchar* &data, int &height, int &width); 57 | void load_ppm(const char* name, uchar* &data, int &height, int &width); 58 | 59 | void save_pbm(const char* name, uchar *im, int height, int width); 60 | 61 | template 62 | void save_pgm(const char* name, T *im, int height, int width) 63 | { 64 | std::ofstream file(name, std::ios::out | std::ios::binary); 65 | 66 | file << "P5\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; 67 | 68 | for( int k=0; k 76 | void save_ppm(const char* name, T *im, int height, int width) 77 | { 78 | std::ofstream file(name, std::ios::out | std::ios::binary); 79 | 80 | file << "P6\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; 81 | for( int k=0; k<3*width*height; k++ ) 82 | { 83 | file <<(uchar)im[k]; 84 | } 85 | file.close(); 86 | } 87 | 88 | void get_size_ppm(const char* name, int &height, int &width); 89 | } 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/image_manipulation.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_IMAGE_MANIPULATION_H 42 | #define KUTILITY_IMAGE_MANIPULATION_H 43 | 44 | #include "kutility/kutility.def" 45 | #include "kutility/general.h" 46 | 47 | namespace kutility 48 | { 49 | template 50 | void scale( T1* src, int h, int w, float sc, T2* dst, int dh, int dw ) 51 | { 52 | int nh = int( h*sc ); 53 | int nw = int( w*sc ); 54 | 55 | assert( dst != NULL ); 56 | assert( nh == dh ); 57 | assert( nw == dw ); 58 | 59 | if( sc == 1 ) 60 | { 61 | for( int i=0; i= h-1 ) continue; 73 | for( int nx=0; nx= w-1 ) continue; 77 | dst[ny*nw+nx] = (T2)bilinear_interpolation(src, w, x, y); 78 | } 79 | } 80 | } 81 | 82 | template inline 83 | void rgb_to_y(T* cim, int h, int w, T* gim ) 84 | { 85 | assert( (gim!=NULL) && (cim!=NULL) ); 86 | 87 | for( int y=0; y inline 103 | void y_to_rgb(T* yim, int h, int w, T* rgbim ) 104 | { 105 | assert( rgbim != NULL ); 106 | 107 | int wh = w*h; 108 | 109 | for( int k=0; k inline 118 | void rgb_to_bgr(T* rgb, int h, int w, T* bgr ) 119 | { 120 | assert( bgr != NULL ); 121 | int wh3 = w*h*3; 122 | 123 | for( int k=0; k inline 133 | void bgr_to_rgb(T* bgr, int h, int w, T* rgb ) 134 | { 135 | rgb_to_bgr(bgr,h,w,rgb); 136 | } 137 | 138 | template inline void rgba_to_y(T* cim, int h, int w, T* gim ) 139 | { 140 | assert( (gim!=NULL) && (cim!=NULL) ); 141 | 142 | for( int y=0; y inline void rgba_to_rgb(T* rgbaim, int h, int w, T* rgbim ) 157 | { 158 | assert( (rgbim!=NULL) && (rgbaim!=NULL) ); 159 | int wh = w*h; 160 | for( int k=0; k 181 | void decompose_channels( T* image, int h, int w, T* &ch_0, T* &ch_1, T* &ch_2) 182 | { 183 | int image_size = h*w; 184 | 185 | ch_0 = kutility::allocate(image_size); 186 | ch_1 = kutility::allocate(image_size); 187 | ch_2 = kutility::allocate(image_size); 188 | 189 | #if defined(WITH_OPENMP) 190 | #pragma omp parallel for 191 | #endif 192 | for( int y=0; y inline 209 | T* gamma_correction( T* im, int h, int w, double gamma, bool in_place=false) 210 | { 211 | int sz = w*h; 212 | T* out; 213 | 214 | if( !in_place ) 215 | out = kutility::allocate(sz); 216 | else 217 | out = im; 218 | 219 | double val; 220 | 221 | for( int i=0; i 255 ) 225 | out[i] = (T)255; 226 | else 227 | out[i] = (T)val; 228 | } 229 | return out; 230 | } 231 | 232 | /// adds some noise to the pixels 233 | template inline 234 | T* add_noise( T* im, int h, int w, int noise_level, bool in_place=false) 235 | { 236 | int sz = w*h; 237 | T* out; 238 | 239 | if( !in_place ) 240 | out = kutility::allocate(sz); 241 | else 242 | out = im; 243 | 244 | 245 | for( int i=0; i 71 | void message( string str, T num ) 72 | { 73 | std::cout< inline 79 | void progress(T1 state, T2 range, int freq, time_t elapsed=-1) 80 | { 81 | if( ((int)(state)) % freq == 0 ) 82 | { 83 | std::cout.width(5); 84 | std::cout.precision(4); 85 | double percent = ((double)(state))/((double)(range)); 86 | std::cout<<"completed: "<<100*percent<<"%"; 87 | 88 | double eta; 89 | if( elapsed != -1 ) 90 | { 91 | eta = ((double)elapsed)/percent; 92 | std::cout<<"\tremaining: "<<(double)(eta-elapsed)<<"s\t total: "< inline 106 | void display( T* data, int r, int c=1, bool no_zero=false, bool legend=false, int precision=3, int width=4, string sep="\t") 107 | { 108 | cout.width(width); 109 | cout.fill(' '); 110 | cout.precision(precision); 111 | 112 | int i,j; 113 | if( legend ) 114 | { 115 | cout<<"\t"<<" "; 116 | cout.setf( ios_base::right); 117 | for(j=0; j inline 164 | void display( T** data, int r, int c=1, bool no_zero=false, bool legend=false, int precision=3, int width=4, char* sep="\t") 165 | { 166 | cout.width(width); 167 | cout.fill(' '); 168 | cout.precision(precision); 169 | 170 | int i,j; 171 | if( legend ) 172 | { 173 | cout<<"\t"<<" "; 174 | cout.setf( ios_base::right); 175 | for(j=0; j 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include "assert.h" 52 | #include 53 | #include "float.h" 54 | 55 | #if !defined(PI) 56 | #define PI 3.141592653589793 57 | #endif 58 | #if !defined(RADIAN) 59 | #define RADIAN 0.017453292519943 // pi/180 60 | #endif 61 | #if !defined(DEGREE) 62 | #define DEGREE 57.29577951308232 // 180/pi 63 | #endif 64 | 65 | #ifndef UNCHAR 66 | #define UNCHAR 67 | typedef unsigned char uchar; 68 | #endif 69 | 70 | #ifndef U_INT 71 | #define U_INT 72 | typedef unsigned int uint; 73 | #endif 74 | 75 | #ifdef WIN32 76 | #include "omp.h" 77 | #define isnan(x) ((x) != (x)) 78 | #pragma warning( disable : 4996 ) 79 | #ifndef NOMINMAX 80 | #define NOMINMAX 81 | #endif 82 | #endif 83 | 84 | using std::string; 85 | using std::ostream; 86 | using std::ofstream; 87 | using std::ifstream; 88 | using std::cout; 89 | using std::cin; 90 | using std::endl; 91 | using std::ios_base; 92 | using std::flush; 93 | using std::vector; 94 | 95 | 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /src/daisy_descriptor/include/kutility/progress_bar.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #ifndef KUTILITY_PROGRESS_BAR_H 42 | #define KUTILITY_PROGRESS_BAR_H 43 | 44 | #include 45 | #include 46 | 47 | class progress_bar 48 | { 49 | public: 50 | explicit inline progress_bar(int start, int end, int divisions); 51 | 52 | void reset(); 53 | 54 | void reset(int start, int end, int divisions); 55 | 56 | std::ostream& operator>>(std::ostream& os) const; 57 | 58 | const progress_bar& operator()(int current); 59 | 60 | void set_text(const std::string& text); 61 | 62 | void set_end_text( const std::string& text); 63 | 64 | void set_format(const std::string& formatString); 65 | 66 | private: 67 | int m_start; 68 | int m_current; 69 | int m_end; 70 | int m_divisions; 71 | mutable int m_progress; 72 | time_t m_starting_time; 73 | 74 | std::string m_message; 75 | std::string m_end_message; 76 | std::string m_done; 77 | std::string m_processing; 78 | std::string m_notDone; 79 | std::string m_limit; 80 | }; 81 | 82 | 83 | inline progress_bar::progress_bar(int start, int end, int divisions) 84 | : m_start(start), 85 | m_current(start), 86 | m_end(end), 87 | m_divisions(divisions), 88 | m_progress(0), 89 | m_message("Progress: "), 90 | m_end_message(" "), 91 | m_done("-"), 92 | m_processing(">"), 93 | m_notDone(" "), 94 | m_limit("|") 95 | { 96 | time(&m_starting_time); 97 | } 98 | 99 | inline std::ostream& operator<<(std::ostream& os, const progress_bar& pb) 100 | { 101 | return pb >> os; 102 | } 103 | 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /src/daisy_descriptor/src/corecv.cpp: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #include "kutility/corecv.h" 42 | 43 | namespace kutility 44 | { 45 | /// transform a point via the homography 46 | void point_transform_via_homography( double* H, double x, double y, double &u, double &v ) 47 | { 48 | double kxp = H[0]*x + H[1]*y + H[2]; 49 | double kyp = H[3]*x + H[4]*y + H[5]; 50 | double kp = H[6]*x + H[7]*y + H[8]; 51 | u = kxp / kp; 52 | v = kyp / kp; 53 | } 54 | 55 | double epipolar_line_slope( double y, double x, double* F ) 56 | { 57 | double line[3]; 58 | line[0] = F[0]*x + F[1]*y + F[2]; 59 | line[1] = F[3]*x + F[4]*y + F[5]; 60 | line[2] = F[6]*x + F[7]*y + F[8]; 61 | 62 | double m = -line[0]/line[1]; 63 | double slope = atan( m )*DEGREE; 64 | 65 | if( slope < 0 ) slope += 360; 66 | if( slope >= 360 ) slope -= 360; 67 | 68 | return slope; 69 | } 70 | 71 | } 72 | -------------------------------------------------------------------------------- /src/daisy_descriptor/src/general.cpp: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #include "kutility/general.h" 42 | 43 | namespace kutility 44 | { 45 | char* itoa(int value, char* str, int radix) 46 | { 47 | int rem = 0; 48 | int pos = 0; 49 | char ch = '!' ; 50 | do 51 | { 52 | rem = value % radix ; 53 | value /= radix; 54 | if ( 16 == radix ) 55 | { 56 | if( rem >= 10 && rem <= 15 ) 57 | { 58 | switch( rem ) 59 | { 60 | case 10: 61 | ch = 'a' ; 62 | break; 63 | case 11: 64 | ch ='b' ; 65 | break; 66 | case 12: 67 | ch = 'c' ; 68 | break; 69 | case 13: 70 | ch ='d' ; 71 | break; 72 | case 14: 73 | ch = 'e' ; 74 | break; 75 | case 15: 76 | ch ='f' ; 77 | break; 78 | } 79 | } 80 | } 81 | if( '!' == ch ) 82 | { 83 | str[pos++] = (char) ( rem + 0x30 ); 84 | } 85 | else 86 | { 87 | str[pos++] = ch ; 88 | } 89 | }while( value != 0 ); 90 | str[pos] = '\0' ; 91 | return strrev(str); 92 | } 93 | 94 | //strrev the standard way 95 | // the following directives to make the code portable 96 | // between windows and Linux. 97 | char* strrev(char* szT) 98 | { 99 | if ( !szT ) // handle null passed strings. 100 | return NULL; 101 | int i = strlen(szT); 102 | int t = !(i%2)? 1 : 0; // check the length of the string . 103 | for(int j = i-1 , k = 0 ; j > (i/2 -t) ; j-- ) 104 | { 105 | char ch = szT[j]; 106 | szT[j] = szT[k]; 107 | szT[k++] = ch; 108 | } 109 | return szT; 110 | } 111 | 112 | } 113 | 114 | -------------------------------------------------------------------------------- /src/daisy_descriptor/src/image_io_bmp.cpp: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #include "kutility/image_io_bmp.h" 42 | 43 | namespace kutility 44 | { 45 | void save_bmp(const char* str, uchar* body, int h, int w, int channel) 46 | { 47 | int image_data_size = w * h; 48 | 49 | // if( channel == 3 ) 50 | image_data_size *= 4; 51 | 52 | int hexWidth[4]; 53 | int hexHeight[4]; 54 | int hexFileSize[4]; 55 | int hexIdent[4]; 56 | 57 | convert_hex(w, hexWidth); 58 | convert_hex(h, hexHeight); 59 | convert_hex(image_data_size+54,hexFileSize); 60 | convert_hex(image_data_size,hexIdent); 61 | 62 | FILE * maskFile = fopen( str , "w+b"); 63 | 64 | char headerArray[54]; 65 | headerArray[0] =(char)0x42 ; 66 | headerArray[1] =(char)0x4D ; 67 | headerArray[2] =(char)hexFileSize[0] ; 68 | headerArray[3] =(char)hexFileSize[1] ; 69 | headerArray[4] =(char)hexFileSize[2] ; 70 | headerArray[5] =(char)hexFileSize[3] ; 71 | headerArray[6] = (char)0x0; 72 | headerArray[7] = (char)0x0; 73 | headerArray[8] = (char)0x0; 74 | headerArray[9] = (char)0x0; 75 | headerArray[10] = (char)0x36; 76 | headerArray[11] = (char)0x0; 77 | headerArray[12] = (char)0x0; 78 | headerArray[13] = (char)0x0; 79 | headerArray[14] = (char)0x28; 80 | headerArray[15] = (char)0x0; 81 | headerArray[16] = (char)0x0; 82 | headerArray[17] = (char)0x0; 83 | headerArray[18] = (char)hexWidth[0]; 84 | headerArray[19] = (char)hexWidth[1]; 85 | headerArray[20] = (char)hexWidth[2]; 86 | headerArray[21] = (char)hexWidth[3]; 87 | headerArray[22] = (char)hexHeight[0]; 88 | headerArray[23] = (char)hexHeight[1]; 89 | headerArray[24] = (char)hexHeight[2]; 90 | headerArray[25] = (char)hexHeight[3]; 91 | headerArray[26] = (char)0x01; 92 | headerArray[27] = (char)0x0; 93 | headerArray[28] = (char)0x20; 94 | headerArray[29] = (char)0x0; 95 | headerArray[30] = (char)0x0; 96 | headerArray[31] = (char)0x0; 97 | headerArray[32] = (char)0x0; 98 | headerArray[33] = (char)0x0; 99 | headerArray[34] = (char)hexIdent[0]; 100 | headerArray[35] = (char)hexIdent[1]; 101 | headerArray[36] = (char)hexIdent[2]; 102 | headerArray[37] = (char)hexIdent[3]; 103 | headerArray[38] = (char)0xC4; 104 | headerArray[39] = (char)0x0E; 105 | headerArray[40] = (char)0x0; 106 | headerArray[41] = (char)0x0; 107 | headerArray[42] = (char)0xC4; 108 | headerArray[43] = (char)0x0E; 109 | headerArray[44] = (char)0x0; 110 | headerArray[45] = (char)0x0; 111 | headerArray[46] = (char)0x0; 112 | headerArray[47] = (char)0x0; 113 | headerArray[48] = (char)0x0; 114 | headerArray[49] = (char)0x0; 115 | headerArray[50] = (char)0x0; 116 | headerArray[51] = (char)0x0; 117 | headerArray[52] = (char)0x0; 118 | headerArray[53] = (char)0x0; 119 | 120 | fwrite(headerArray, sizeof(char), 54, maskFile); 121 | fclose(maskFile); 122 | maskFile = fopen( str , "a+b"); 123 | 124 | uchar* data = new uchar[image_data_size]; 125 | 126 | int index=0; 127 | //create bitmap data// 128 | for(int m=0; m> bitshift) & 1; 58 | bitshift--; 59 | } 60 | } 61 | void write_packed(unsigned char *data, int size, std::ofstream &f) 62 | { 63 | unsigned char c = 0; 64 | 65 | int bitshift = 7; 66 | for (int pos = 0; pos < size; pos++) { 67 | c = c | (data[pos] << bitshift); 68 | bitshift--; 69 | if ((bitshift == -1) || (pos == size-1)) { 70 | f.put(c); 71 | bitshift = 7; 72 | c = 0; 73 | } 74 | } 75 | } 76 | void pnm_read(std::ifstream &file, char *buf) 77 | { 78 | char doc[PNM_BUFFER_SIZE]; 79 | char c; 80 | 81 | file >> c; 82 | while (c == '#') { 83 | file.getline(doc, PNM_BUFFER_SIZE); 84 | file >> c; 85 | } 86 | file.putback(c); 87 | 88 | file.width(PNM_BUFFER_SIZE); 89 | file >> buf; 90 | file.ignore(); 91 | } 92 | void get_size_ppm(const char *name, int &height, int &width) 93 | { 94 | char buf[PNM_BUFFER_SIZE]; 95 | //char doc[PNM_BUFFER_SIZE] 96 | // read header 97 | std::ifstream file(name, std::ios::in | std::ios::binary); 98 | pnm_read(file, buf); 99 | if (strncmp(buf, "P6", 2)) 100 | { 101 | printf("type mismatch\n"); 102 | exit(1); 103 | } 104 | 105 | pnm_read(file, buf); 106 | width = atoi(buf); 107 | 108 | pnm_read(file, buf); 109 | height = atoi(buf); 110 | 111 | file.close(); 112 | return; 113 | } 114 | 115 | void load_pbm(const char* name, uchar* &im, int &height, int &width) 116 | { 117 | char buf[PNM_BUFFER_SIZE]; 118 | 119 | /* read header */ 120 | std::ifstream file(name, std::ios::in | std::ios::binary); 121 | pnm_read(file, buf); 122 | if (strncmp(buf, "P4", 2)) 123 | { 124 | printf("type mismatch\n"); 125 | exit(1); 126 | } 127 | 128 | pnm_read(file, buf); 129 | width = atoi(buf); 130 | 131 | pnm_read(file, buf); 132 | height = atoi(buf); 133 | 134 | /* read data */ 135 | if( im != NULL) delete[]im; 136 | im = new uchar[width*height]; 137 | for (int i = 0; i < height; i++) 138 | read_packed(im+(width*i), width, file); 139 | } 140 | void load_pgm(const char* name, uchar* &im, int &height, int& width) 141 | { 142 | char buf[PNM_BUFFER_SIZE]; 143 | 144 | /* read header */ 145 | std::ifstream file(name, std::ios::in | std::ios::binary); 146 | pnm_read(file, buf); 147 | if (strncmp(buf, "P5", 2)) 148 | { 149 | printf("type mismatch\n"); 150 | exit(1); 151 | } 152 | 153 | pnm_read(file, buf); 154 | width = atoi(buf); 155 | 156 | pnm_read(file, buf); 157 | height = atoi(buf); 158 | 159 | pnm_read(file, buf); 160 | if (atoi(buf) > UCHAR_MAX) 161 | { 162 | printf("type mismatch\n"); 163 | exit(1); 164 | } 165 | 166 | /* read data */ 167 | if( im != NULL ) delete[] im; 168 | im = new uchar[width*height]; 169 | file.read( (char *)im, width * height * sizeof(uchar)); 170 | } 171 | 172 | void load_ppm(const char* name, uchar* &im, int &height, int &width) 173 | { 174 | char buf[PNM_BUFFER_SIZE]; 175 | //char doc[PNM_BUFFER_SIZE] 176 | std::ifstream file(name, std::ios::in | std::ios::binary); 177 | pnm_read(file, buf); 178 | if (strncmp(buf, "P6", 2)) 179 | { 180 | printf("type mismatch\n");; 181 | exit(1); 182 | } 183 | pnm_read(file, buf); 184 | width = atoi(buf); 185 | 186 | pnm_read(file, buf); 187 | height = atoi(buf); 188 | 189 | pnm_read(file, buf); 190 | if (atoi(buf) > UCHAR_MAX) 191 | { 192 | printf("type mismatch\n");; 193 | exit(1); 194 | } 195 | 196 | /* read data */ 197 | if( im != NULL ) delete[] im; 198 | im = new uchar[width*height*3]; 199 | file.read((char *)im, width * height * 3 * sizeof(uchar)); 200 | } 201 | 202 | void save_pbm(const char* name, uchar* im, int height, int width ) 203 | { 204 | std::ofstream file(name, std::ios::out | std::ios::binary); 205 | 206 | file << "P4\n" << width << " " << height << "\n"; 207 | for (int i = 0; i < height; i++) 208 | write_packed(im+(width*i), width, file); 209 | } 210 | 211 | 212 | } 213 | -------------------------------------------------------------------------------- /src/daisy_descriptor/src/image_manipulation.cpp: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #include 42 | #include 43 | 44 | namespace kutility 45 | { 46 | double* scale_intensity( uchar* image, int sz, double il, double iu) 47 | { 48 | double* out = kutility::allocate(sz); 49 | 50 | double mult = (iu-il)/255.0; 51 | 52 | for( int i=0; i( nh*nw ); 64 | 65 | double ratioy = h / (double)nh; 66 | double ratiox = w / (double)nw; 67 | 68 | int y, x, ynw; 69 | double ny, nx; 70 | 71 | #pragma omp parallel for private( y, x, ny, nx, ynw ) 72 | for( y=0; y>1; 200 | int h_s = h>>1; 201 | 202 | int tmpIndex1, tmpIndex2, tmpIndex3, tmpIndex4, tmpIndex5; 203 | 204 | uchar * out = new uchar[3*w_s*h_s]; 205 | int i; 206 | 207 | for( i=0; i> 2; 219 | out[tmpIndex1+1] = ( image[tmpIndex2+1] + image[tmpIndex3+1] + image[tmpIndex4+1] + image[tmpIndex5+1] ) >> 2; 220 | out[tmpIndex1+2] = ( image[tmpIndex2+2] + image[tmpIndex3+2] + image[tmpIndex4+2] + image[tmpIndex5+2] ) >> 2; 221 | } 222 | } 223 | return out; 224 | } 225 | 226 | int threshold_yen( double *array, int sz) 227 | { 228 | int i,c,c2; //counters 229 | double rho=0.15,threshold=0; 230 | double h=0; 231 | double hf,hb; //total objective fn, foreground and background parts 232 | double currentMaxH=0; //threshold and current max total entropy 233 | double scale=1.0/(1-rho); //used in calculation of entropic correlation 234 | 235 | double pC=0; //cumulative probabilities 236 | double pmf[256]; //probability mass function 237 | 238 | // for( i=0; i<256; i++ ) pmf[i]=0; 239 | memset(pmf, 0, sizeof(double)*256 ); 240 | 241 | //calculation of pmf 242 | for(i=0; i<256; i++) pmf[i]=array[i]/sz; 243 | 244 | for( c=0; c<256; c++ ) 245 | { 246 | 247 | if( pmf[c] != 0 ) 248 | { 249 | pC += pmf[c]; //calculate cumulative probabilities 250 | 251 | //initialization 252 | hf=0; 253 | hb=0; 254 | 255 | //foreground part 256 | for( c2=0; c2= 0 ) hf += pow( pmf[c2] , rho ); 257 | 258 | //background part 259 | for( c2=c; c2<256; c2++ ) if( pmf[c2] > 0 ) hb += pow( pmf[c2] , rho ); 260 | 261 | //total objective function 262 | if( pC < 0.99999999999 ) h = scale*(log(hf*hb)-rho*log(pC*(1-pC))); 263 | 264 | //check if max 265 | if( h>currentMaxH ) { threshold = c; currentMaxH = h; } 266 | } 267 | } 268 | 269 | return (int)threshold; 270 | } 271 | 272 | 273 | } 274 | 275 | -------------------------------------------------------------------------------- /src/daisy_descriptor/src/interaction.cpp: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////////////// 2 | // Software License Agreement (BSD License) // 3 | // // 4 | // Copyright (c) 2009 // 5 | // Engin Tola // 6 | // web : http://cvlab.epfl.ch/~tola // 7 | // email : engin.tola@epfl.ch // 8 | // // 9 | // All rights reserved. // 10 | // // 11 | // Redistribution and use in source and binary forms, with or without // 12 | // modification, are permitted provided that the following conditions // 13 | // are met: // 14 | // // 15 | // * Redistributions of source code must retain the above copyright // 16 | // notice, this list of conditions and the following disclaimer. // 17 | // * Redistributions in binary form must reproduce the above // 18 | // copyright notice, this list of conditions and the following // 19 | // disclaimer in the documentation and/or other materials provided // 20 | // with the distribution. // 21 | // * Neither the name of the EPFL nor the names of its // 22 | // contributors may be used to endorse or promote products derived // 23 | // from this software without specific prior written permission. // 24 | // // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // 28 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // 29 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // 30 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // 31 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // 32 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // 34 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // 35 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // 36 | // POSSIBILITY OF SUCH DAMAGE. // 37 | // // 38 | // See licence.txt file for more details // 39 | ////////////////////////////////////////////////////////////////////////// 40 | 41 | #include "kutility/interaction.h" 42 | 43 | using std::string; 44 | 45 | namespace kutility 46 | { 47 | void error( string str1, int code ) 48 | { 49 | std::cout<<"ERROR: " 50 | < 140 ) length = 140; 94 | 95 | for( int i=0; i 43 | 44 | void progress_bar::reset() 45 | { 46 | m_current = m_start; 47 | m_progress = 0; 48 | time(&m_starting_time); 49 | } 50 | 51 | void progress_bar::reset(int start, int end, int divisions) 52 | { 53 | m_start = start; 54 | m_current = start; 55 | m_end = end; 56 | m_divisions = divisions; 57 | m_progress = 0; 58 | time(&m_starting_time); 59 | } 60 | 61 | std::ostream& progress_bar::operator>>(std::ostream& os) const 62 | { 63 | if(m_current > (m_progress * (m_end - m_start) / m_divisions) || m_current == m_end) 64 | { 65 | ++m_progress; 66 | os << m_message << m_limit; 67 | for(int c = 1; c <= m_divisions; ++c) 68 | { 69 | if(c < m_progress || m_current == m_end) { 70 | os << m_done; 71 | } 72 | else if(c > m_progress) { 73 | os << m_notDone; 74 | } 75 | else { 76 | os << m_processing; 77 | } 78 | } 79 | os << m_limit; 80 | 81 | time_t now; time(&now); 82 | double percent = double(m_current-m_start)/double(m_end-m_start); 83 | double elapsed = difftime( now, m_starting_time ); 84 | double eta = elapsed / percent; 85 | 86 | os<<" "; 87 | os.width(5); 88 | os.fill(' '); 89 | os.precision(3); 90 | os.setf( std::ios_base::right ); 91 | os<= 4) 131 | { 132 | m_limit = formatString[0]; 133 | m_done = formatString[1]; 134 | m_processing = formatString[2]; 135 | m_notDone = formatString[3]; 136 | } 137 | } 138 | -------------------------------------------------------------------------------- /src/jpp.cc: -------------------------------------------------------------------------------- 1 | #include "jpp.h" 2 | 3 | JPP::JPP() 4 | { 5 | 6 | } 7 | 8 | JPP::JPP(FileStorage& fs, config_t* cf) 9 | { 10 | _get_jpp_config(cf); 11 | _stereo = new Stereo(fs, _jpp_config); 12 | } 13 | 14 | void JPP::load_images(const Mat& left, const Mat& right) 15 | { 16 | _stereo->load_images(left, right); 17 | } 18 | 19 | Mat JPP::get_disparity_map(const char* method, int max_disp, const char* outfile) 20 | { 21 | _reset(); 22 | Mat dmap; 23 | _stereo->compute_disparity_map(max_disp, _jpp_config.SAD_WINDOW_SIZE); 24 | dmap = _stereo->get_disparity_map(); 25 | if (outfile != NULL) { 26 | imwrite(outfile, dmap); 27 | } 28 | return dmap; 29 | } 30 | 31 | JPP::~JPP() 32 | { 33 | if (_stereo != NULL) 34 | delete _stereo; 35 | } 36 | 37 | Mat JPP::visualize_conf_pos() 38 | { 39 | _reset(); 40 | Mat vis_img = _stereo->get_img_left(); 41 | cvtColor(vis_img, vis_img, CV_GRAY2BGR); 42 | Point start(_jpp_config.START_X, -_jpp_config.MAX_Y); 43 | Point end(_jpp_config.MAX_X, _jpp_config.MAX_Y); 44 | int inc = _jpp_config.GRID_SIZE; 45 | float safe_radius = (float)max(_jpp_config.BOT_LENGTH/2, _jpp_config.BOT_WIDTH/2)/1000.; 46 | //#pragma omp parallel for collapse(2)\ 47 | //num_threads(omp_get_max_threads()) 48 | for (int x = start.x; x <= end.x; x += inc) { 49 | for (int y = start.y; y <= end.y; y += inc) { 50 | Point3f p((float)x/1000.,(float)y/1000.,0.0); 51 | Point ptl = _stereo->project_point_cam(p, 0); 52 | //if (_stereo->conf_positive(p)) { 53 | if (_stereo->is_bot_clear(p, safe_radius, (float)_jpp_config.GRID_SIZE/1000., false)) { 54 | Scalar green = Scalar(0, 255, 0); 55 | circle(vis_img, ptl, 3, green, -1, 8, 0); 56 | } 57 | } 58 | } 59 | return vis_img; 60 | } 61 | 62 | Mat JPP::visualize_conf_neg() 63 | { 64 | _reset(); 65 | Mat vis_img = _stereo->get_img_left(); 66 | cvtColor(vis_img, vis_img, CV_GRAY2BGR); 67 | Point start(_jpp_config.START_X, -_jpp_config.MAX_Y); 68 | Point end(_jpp_config.MAX_X, _jpp_config.MAX_Y); 69 | int inc = _jpp_config.GRID_SIZE; 70 | //#pragma omp parallel for collapse(3)\ 71 | //num_threads(omp_get_max_threads()) 72 | for (int x = start.x; x <= end.x; x += inc) { 73 | for (int y = start.y; y <= end.y; y += inc) { 74 | for (int z = _jpp_config.CONF_NEG_INC; z <= _jpp_config.BOT_HEIGHT; z += _jpp_config.CONF_NEG_INC) { 75 | Point3f q((float)x/1000.,(float)y/1000.,(float)z/1000.); 76 | Point qtl = _stereo->project_point_cam(q, 0); 77 | if (!_stereo->conf_negative(q)) { 78 | circle(vis_img, qtl, 1, Scalar(0,255,0), -1, 8, 0); 79 | } else { 80 | circle(vis_img, qtl, 1, Scalar(0,0,255), -1, 8, 0); 81 | } 82 | } 83 | } 84 | } 85 | return vis_img; 86 | } 87 | 88 | Mat JPP::visualize_empty_cols() 89 | { 90 | _reset(); 91 | Mat vis_img = _stereo->get_img_left(); 92 | cvtColor(vis_img, vis_img, CV_GRAY2BGR); 93 | Point start(_jpp_config.START_X, -_jpp_config.MAX_Y); 94 | Point end(_jpp_config.MAX_X, _jpp_config.MAX_Y); 95 | int inc = _jpp_config.GRID_SIZE; 96 | //#pragma omp parallel for collapse(3)\ 97 | //num_threads(omp_get_max_threads()) 98 | for (int x = start.x; x <= end.x; x += inc) { 99 | for (int y = start.y; y <= end.y; y += inc) { 100 | Point3f q((float)x/1000.,(float)y/1000.,0); 101 | Point qtl = _stereo->project_point_cam(q, 0); 102 | if (_stereo->is_empty_col(q)) { 103 | circle(vis_img, qtl, 1, Scalar(0,255,0), -1, 8, 0); 104 | } else { 105 | circle(vis_img, qtl, 1, Scalar(0,0,255), -1, 8, 0); 106 | } 107 | } 108 | } 109 | return vis_img; 110 | } 111 | 112 | vector< Point > JPP::plan_astar() 113 | { 114 | _reset(); 115 | AStarPlanner planner(_jpp_config); 116 | Point start(_jpp_config.START_X, 0); 117 | Point end(_jpp_config.MAX_X, 0); 118 | bool convex_world = _jpp_config.CONVEX_WORLD; 119 | int safe_radius = max(_jpp_config.BOT_LENGTH/2, _jpp_config.BOT_WIDTH/2); 120 | int inc = _jpp_config.GRID_SIZE; 121 | planner.setParams(start, end, _jpp_config.MAX_Y, inc, safe_radius, _jpp_config.BOT_HEIGHT, convex_world); 122 | planner.findPath(_stereo); 123 | _path = planner.getPath(); 124 | return _path; 125 | } 126 | 127 | vector< Point > JPP::plan_astar(float x, float y) 128 | { 129 | _reset(); 130 | AStarPlanner planner(_jpp_config); 131 | Point start(_jpp_config.START_X, 0); 132 | Point end((int)round(x*1000.0), (int)round(y*1000.0)); 133 | bool convex_world = _jpp_config.CONVEX_WORLD; 134 | int safe_radius = max(_jpp_config.BOT_LENGTH/2, _jpp_config.BOT_WIDTH/2); 135 | int inc = _jpp_config.GRID_SIZE; 136 | planner.setParams(start, end, _jpp_config.MAX_Y, inc, safe_radius, _jpp_config.BOT_HEIGHT, convex_world); 137 | planner.findPath(_stereo); 138 | _path = planner.getPath(); 139 | return _path; 140 | } 141 | 142 | vector< Point > JPP::plan_rrt() 143 | { 144 | _reset(); 145 | Point start(_jpp_config.START_X, 0); 146 | Point end(_jpp_config.MAX_X, 0); 147 | bool convex_world = _jpp_config.CONVEX_WORLD; 148 | int safe_radius = max(_jpp_config.BOT_LENGTH/2, _jpp_config.BOT_WIDTH/2); 149 | int inc = _jpp_config.GRID_SIZE; 150 | RRT planner; 151 | planner.initRRT(start, end, inc, 500, 0.6, safe_radius, _jpp_config.BOT_HEIGHT, convex_world, _jpp_config); 152 | planner.findPath(_stereo); 153 | _path = planner.getPath(); 154 | return _path; 155 | } 156 | 157 | pair< Mat, Mat > JPP::visualize_jpp(const char* outfile) 158 | { 159 | Mat vis_path = _stereo->get_img_left(); 160 | cvtColor(vis_path, vis_path, CV_GRAY2BGR); 161 | for (int i = 0; i < _path.size()-1; i++) { 162 | Point p = _stereo->project_point_cam(Point3f(_path[i].x/1000.,_path[i].y/1000.,0),0); 163 | Point q = _stereo->project_point_cam(Point3f(_path[i+1].x/1000.,_path[i+1].y/1000.,0),0); 164 | Scalar green = Scalar(0, 255, 0); 165 | line(vis_path, p, q, green, 2, 8 ,0); 166 | } 167 | 168 | Mat confPos = _stereo->get_img_left(); 169 | cvtColor(confPos, confPos, CV_GRAY2BGR); 170 | Mat confNeg = Mat(confPos.rows, confPos.cols, CV_8UC3, Scalar(0,0,0)); 171 | _stereo->jpp_visualizations(confPos, confNeg); 172 | _stereo->blend_images(confPos, confNeg, 0.6, confPos); 173 | 174 | if (outfile != NULL) { 175 | char vis_path_file[100], confPos_file[100]; 176 | sprintf(vis_path_file, "%s%s.jpg", outfile, "-path"); 177 | sprintf(confPos_file, "%s%s.jpg", outfile, "-vis"); 178 | imwrite(vis_path_file, vis_path); 179 | imwrite(confPos_file, confPos); 180 | } 181 | 182 | return make_pair(vis_path, confPos); 183 | } 184 | 185 | //returns vector of points representing the path of either astar or rrt 186 | vector< Point > JPP::getPath() 187 | { 188 | return _path; 189 | } 190 | 191 | void JPP::update_jpp_config(JPP_Config& config) 192 | { 193 | _jpp_config = config; 194 | _stereo->update_jpp_config(config); 195 | } 196 | 197 | vector< double > JPP::get_epipolar_costs(Point p, int max_disp) 198 | { 199 | vector< double > costs; 200 | for (int d = 0; d <= max_disp; d++) { 201 | double cost = _stereo->desc_cost_SAD(p, Point(p.x-d,p.y), _jpp_config.SAD_WINDOW_SIZE); 202 | costs.push_back(cost); 203 | } 204 | return costs; 205 | } 206 | 207 | Mat JPP::get_img_left() 208 | { 209 | return _stereo->get_img_left(); 210 | } 211 | 212 | Mat JPP::get_img_right() 213 | { 214 | return _stereo->get_img_right(); 215 | } 216 | 217 | Mat JPP::get_Q_matrix() 218 | { 219 | return _stereo->get_Q_matrix(); 220 | } 221 | 222 | ///////// PRIVATE FUNCTIONS ///////// 223 | 224 | void JPP::_reset() 225 | { 226 | _stereo->init_daisy_descriptors(_jpp_config.DR, _jpp_config.DQ, _jpp_config.DT, _jpp_config.DH, NRM_FULL); 227 | } 228 | 229 | void JPP::_get_jpp_config(config_t* cf) 230 | { 231 | _jpp_config = JPP_Config(cf); 232 | } -------------------------------------------------------------------------------- /src/main.cc: -------------------------------------------------------------------------------- 1 | #include "popt_pp.h" 2 | #include "jpp.h" 3 | 4 | int main(int argc, const char** argv) { 5 | const char* left_img_file; 6 | const char* right_img_file; 7 | const char* calib_file; 8 | const char* jpp_config_file; 9 | const char* img_dir; 10 | const char* output = "astar"; 11 | int w = 0; 12 | int v = 0; 13 | int num_imgs = 0; 14 | 15 | clock_t start, end; 16 | 17 | static struct poptOption options[] = { 18 | { "num_imgs",'n',POPT_ARG_INT,&num_imgs,0,"Number of images to be processed","NUM" }, 19 | { "img_dir",'d',POPT_ARG_STRING,&img_dir,0,"Directory containing image pairs (set if n > 0)","STR" }, 20 | { "left_img",'l',POPT_ARG_STRING,&left_img_file,0,"Left image file name","STR" }, 21 | { "right_img",'r',POPT_ARG_STRING,&right_img_file,0,"Right image file name","STR" }, 22 | { "calib_file",'c',POPT_ARG_STRING,&calib_file,0,"Stereo calibration file name","STR" }, 23 | { "jpp_config_file",'j',POPT_ARG_STRING,&jpp_config_file,0,"JPP config file name","STR" }, 24 | { "output",'o',POPT_ARG_STRING,&output,0,"Output - astar, rrt, debug","STR" }, 25 | { "visualize",'v',POPT_ARG_INT,&v,0,"Set v=1 for displaying visualizations","NUM" }, 26 | { "write_files",'w',POPT_ARG_INT,&w,0,"Set w=1 for writing visualizations to files","NUM" }, 27 | POPT_AUTOHELP 28 | { NULL, 0, 0, NULL, 0, NULL, NULL } 29 | }; 30 | 31 | POpt popt(NULL, argc, argv, options, 0); 32 | int c; 33 | while((c = popt.getNextOpt()) >= 0) {} 34 | 35 | // Read JPP config 36 | config_t cfg, *cf; 37 | cf = &cfg; 38 | config_init(cf); 39 | if (!config_read_file(cf, jpp_config_file)) { 40 | cout << "Could not read config file!" << endl; 41 | config_destroy(cf); 42 | return(EXIT_FAILURE); 43 | } 44 | 45 | // Read stereo calibration info 46 | FileStorage fs = FileStorage(calib_file, FileStorage::READ); 47 | 48 | // JPP instance 49 | JPP jpp(fs, cf); 50 | 51 | if (num_imgs == 0) { 52 | Mat img_left = imread(left_img_file, CV_LOAD_IMAGE_COLOR); 53 | Mat img_right = imread(right_img_file, CV_LOAD_IMAGE_COLOR); 54 | 55 | char dmap_file[100], rrt_file_prefix[100], astar_file_prefix[100]; 56 | sprintf(astar_file_prefix, "%s%d", "astar", 0); 57 | sprintf(rrt_file_prefix, "%s%d", "rrt", 0); 58 | sprintf(dmap_file, "%s%d.jpg", "dmap", 0); 59 | 60 | jpp.load_images(img_left, img_right); 61 | 62 | if (strcmp(output, "astar") == 0) { 63 | vector< Point > path = jpp.plan_astar(); 64 | if (v == 1) { 65 | pair< Mat, Mat > vis; 66 | if (w == 1) 67 | vis = jpp.visualize_jpp(astar_file_prefix); 68 | else 69 | vis = jpp.visualize_jpp(); 70 | while (1) { 71 | imshow("PATH", vis.first); 72 | imshow("CONFIDENCE", vis.second); 73 | if (waitKey(30) > 0) 74 | break; 75 | } 76 | } 77 | } else if (strcmp(output, "rrt") == 0) { 78 | vector< Point > path = jpp.plan_rrt(); 79 | if (v == 1) { 80 | pair< Mat, Mat > vis; 81 | if (w == 1) 82 | vis = jpp.visualize_jpp(rrt_file_prefix); 83 | else 84 | vis = jpp.visualize_jpp(); 85 | while (1) { 86 | imshow("PATH", vis.first); 87 | imshow("CONFIDENCE", vis.second); 88 | if (waitKey(30) > 0) 89 | break; 90 | } 91 | } 92 | } else if (strcmp(output, "debug") == 0) { 93 | Mat conf_pos = jpp.visualize_conf_pos(); 94 | Mat conf_neg = jpp.visualize_conf_neg(); 95 | while (1) { 96 | imshow("CONF POS", conf_pos); 97 | imshow("CONF NEG", conf_neg); 98 | if (waitKey(30) > 0) { 99 | break; 100 | } 101 | } 102 | } 103 | } else if (num_imgs > 0) { 104 | double total_time = 0; 105 | for (int i = 1; i <= num_imgs; i++) { 106 | cout << "Processing pair " << i << endl; 107 | char left_img_file[100], right_img_file[100]; 108 | char dmap_file[100], rrt_file_prefix[100], astar_file_prefix[100]; 109 | sprintf(left_img_file, "%s%s%d.jpg", img_dir, "left", i); 110 | sprintf(right_img_file, "%s%s%d.jpg", img_dir, "right", i); 111 | sprintf(dmap_file, "%s%d.jpg", "dmap", i); 112 | sprintf(astar_file_prefix, "%s%d", "astar", i); 113 | sprintf(rrt_file_prefix, "%s%d", "rrt", i); 114 | 115 | Mat img_left = imread(left_img_file, CV_LOAD_IMAGE_COLOR); 116 | Mat img_right = imread(right_img_file, CV_LOAD_IMAGE_COLOR); 117 | 118 | start = clock(); 119 | jpp.load_images(img_left, img_right); 120 | 121 | if (strcmp(output, "astar") == 0) { 122 | vector< Point > path = jpp.plan_astar(); 123 | if (v == 1) { 124 | pair< Mat, Mat > vis; 125 | if (w == 1) 126 | vis = jpp.visualize_jpp(astar_file_prefix); 127 | else 128 | vis = jpp.visualize_jpp(); 129 | while (1) { 130 | imshow("PATH", vis.first); 131 | imshow("CONFIDENCE", vis.second); 132 | if (waitKey(30) > 0) 133 | break; 134 | } 135 | } 136 | } else if (strcmp(output, "rrt") == 0) { 137 | vector< Point > path = jpp.plan_rrt(); 138 | if (v == 1) { 139 | pair< Mat, Mat > vis; 140 | if (w == 1) 141 | vis = jpp.visualize_jpp(rrt_file_prefix); 142 | else 143 | vis = jpp.visualize_jpp(); 144 | while (1) { 145 | imshow("PATH", vis.first); 146 | imshow("CONFIDENCE", vis.second); 147 | if (waitKey(30) > 0) 148 | break; 149 | } 150 | } 151 | } else if (strcmp(output, "debug") == 0) { 152 | Mat conf_pos = jpp.visualize_conf_pos(); 153 | Mat conf_neg = jpp.visualize_conf_neg(); 154 | while (1) { 155 | imshow("CONF POS", conf_pos); 156 | imshow("CONF NEG", conf_neg); 157 | if (waitKey(30) > 0) { 158 | break; 159 | } 160 | } 161 | } 162 | end = clock(); 163 | double secs = ((double) (end - start)) / CLOCKS_PER_SEC; 164 | total_time += secs; 165 | cout << "Time: " << secs << endl; 166 | } 167 | cout << "Avg time: " << (total_time/(double)num_imgs) << endl; 168 | } 169 | 170 | config_destroy(cf); 171 | return 0; 172 | } 173 | -------------------------------------------------------------------------------- /src/rrt.cc: -------------------------------------------------------------------------------- 1 | #include "rrt.h" 2 | 3 | RRT::RRT() 4 | { 5 | nSAD = nSADCache = nSADConvex = nSADConvexCache = 0; 6 | } 7 | 8 | void RRT::initRRT(Point s, Point e, int step, int iter, float bias, int r, int bh, bool c, JPP_Config conf) 9 | { 10 | srand(time(NULL)); 11 | root = new Node; 12 | root->parent = NULL; 13 | root->position = s; 14 | start = s; 15 | end = e; 16 | step_size = step; 17 | max_iter = iter; 18 | goal_bias = bias; 19 | nodes.push_back(root); 20 | obstacle_step_size = step_size/2; 21 | safe_radius = r; 22 | bot_height = bh; 23 | convex = c; 24 | _config = conf; 25 | } 26 | 27 | Point RRT::getRandomStateSpacePoint(int range_x, int range_y) 28 | { 29 | float r = rand() / (float)RAND_MAX; 30 | if (r < goal_bias) 31 | return end; 32 | int x = rand() % range_x; 33 | int y = rand() % (2*range_y) - range_y; 34 | return Point(x,y); 35 | } 36 | 37 | RRT::Node* RRT::nearest(Point p) 38 | { 39 | float minDist = 1e9; 40 | Node *closest = NULL; 41 | for(int i = 0; i < (int)nodes.size(); i++) { 42 | float dist = norm(p - nodes[i]->position); 43 | if (dist < minDist) { 44 | minDist = dist; 45 | closest = nodes[i]; 46 | } 47 | } 48 | return closest; 49 | } 50 | 51 | Point RRT::extend(Node* n, Point to, Stereo* stereo) 52 | { 53 | Point from = n->position; 54 | Point i = to - from; 55 | float norm_i = norm(i); 56 | Point unit_i((float)(i.x*step_size)/norm_i,(float)(i.y*step_size)/norm_i); 57 | Point q = from + unit_i; 58 | Point3f q3d((float)q.x/1000.,(float)q.y/1000.,0.); 59 | Point imgl = stereo->project_point_cam(q3d, 0); 60 | Point imgr = stereo->project_point_cam(q3d, 1); 61 | if (!stereo->in_img(imgl.x,imgl.y) || !stereo->in_img(imgr.x,imgr.y)) 62 | return Point(0,0); 63 | return q; 64 | } 65 | 66 | void RRT::add(RRT::Node* q, RRT::Node* qNew) 67 | { 68 | qNew->parent = q; 69 | q->children.push_back(qNew); 70 | nodes.push_back(qNew); 71 | } 72 | 73 | vector< Point > RRT::getPointsInLine(Point p, Point q) 74 | { 75 | vector< Point > pts; 76 | int s = obstacle_step_size; 77 | Point a, b; 78 | if (p.x == q.x) { 79 | if (p.y < q.y) { 80 | a = p; 81 | b = q; 82 | } else { 83 | a = q; 84 | b = p; 85 | } 86 | for (int y = a.y; y <= b.y; y += s) { 87 | pts.push_back(Point(p.x, y)); 88 | } 89 | } else { 90 | if (p.x < q.x) { 91 | a = p; 92 | b = q; 93 | } else { 94 | a = q; 95 | b = p; 96 | } 97 | float slope = (float)(b.y - a.y)/(float)(b.x - a.x); 98 | for (int x = a.x; x <= b.x; x += s) { 99 | int y = a.y + slope * (x - a.x); 100 | pts.push_back(Point(x,y)); 101 | } 102 | } 103 | return pts; 104 | } 105 | 106 | bool RRT::isBotClearOfObstacle(Point p, Stereo* stereo) 107 | { 108 | Point3f pt3d = Point3f((float)p.x/1000.,(float)p.y/1000.,0); 109 | return (stereo->is_bot_clear(pt3d, (float)safe_radius/1000., (float)step_size/1000., !convex)); 110 | } 111 | 112 | void RRT::findPath(Stereo* stereo) 113 | { 114 | for (int zz = 0; zz < max_iter; zz++) { 115 | //cout << zz << endl; 116 | Point rand_config = getRandomStateSpacePoint(6000,3000); 117 | Node* qNearest = nearest(rand_config); 118 | if (norm(qNearest->position-rand_config) > step_size) { 119 | Point newConfig = extend(qNearest, rand_config, stereo); 120 | if (newConfig == Point(0,0)) 121 | continue; 122 | // start obstacle check 123 | bool inObstacle = false; 124 | vector< Point > pts = getPointsInLine(qNearest->position, newConfig); 125 | for (int i = 0; i < pts.size(); i++) { 126 | if (!isBotClearOfObstacle(pts[i], stereo)) { 127 | inObstacle = true; 128 | break; 129 | } 130 | } 131 | // end obstacle check 132 | if (inObstacle) 133 | continue; 134 | Node *qNew = new Node; 135 | qNew->position = newConfig; 136 | add(qNearest, qNew); 137 | if (norm(newConfig-end) < 50) { 138 | break; 139 | } 140 | } 141 | } 142 | Node *q = nearest(end); 143 | while (q != NULL) { 144 | path.push_back(q); 145 | q = q->parent; 146 | } 147 | } 148 | 149 | vector< Point > RRT::getPath() 150 | { 151 | vector< Point > ret; 152 | for (int i = 0; i < path.size(); i++) { 153 | ret.push_back(path[i]->position); 154 | } 155 | return ret; 156 | } 157 | 158 | int RRT::getPathLength() 159 | { 160 | int length = 0; 161 | for (int i = 0; i < path.size()-1; i++) { 162 | Point p = path[i]->position; 163 | Point q = path[i+1]->position; 164 | length += norm(p-q); 165 | } 166 | return length; 167 | } 168 | 169 | void RRT::freeMemory() 170 | { 171 | for (int i = 0; i < nodes.size(); i++) { 172 | if (nodes[i] != NULL) 173 | delete nodes[i]; 174 | } 175 | } --------------------------------------------------------------------------------