├── .gitignore ├── srv └── SkimapIntegrationService.srv ├── src ├── nodes │ ├── experiments │ │ └── skimap_grid_test_python │ │ │ ├── matlab │ │ │ ├── bench_test.m │ │ │ ├── skigrid_omp.csv │ │ │ ├── skigrid_single.csv │ │ │ ├── bench_octree_points │ │ │ │ ├── kdskip.csv │ │ │ │ ├── skimap.csv │ │ │ │ ├── octree.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_points_2d │ │ │ │ ├── kdskip_r5.csv │ │ │ │ ├── kdskip_r1.csv │ │ │ │ ├── kdskip_r10.csv │ │ │ │ ├── kdskip_r20.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_points_3d │ │ │ │ ├── kdskip_r1.csv │ │ │ │ ├── kdskip_r5.csv │ │ │ │ ├── kdskip_r10.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_points_5d │ │ │ │ ├── kdskip_r10.csv │ │ │ │ ├── kdskip_r25.csv │ │ │ │ ├── kdskip_r32.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_dimensions │ │ │ │ ├── kdskip_r10.csv │ │ │ │ ├── kdskip_r32.csv │ │ │ │ ├── kdskip_dim2.csv │ │ │ │ └── kdtree.csv │ │ │ └── importfile.m │ │ │ ├── bench_people.py │ │ │ ├── bench.py │ │ │ ├── plots │ │ │ ├── plot_testing.py │ │ │ └── plotutils.py │ │ │ ├── bench_kd.py │ │ │ ├── bench_octree.py │ │ │ ├── skigrid.csv │ │ │ └── grid.csv │ ├── test.cpp │ ├── tutorials │ │ └── integration_of_random_points.cpp │ └── skimap_pose_filtering.cpp ├── skimap_ros │ └── skimap_ros_void_library.cpp ├── slamdunk │ ├── graph_utils.cpp │ ├── internal_timers.hpp │ ├── gpu_features.cpp │ ├── data.cpp │ └── feature_matcher.cpp └── slamdunk_extension │ ├── FIROrder2.cpp │ └── Utility.cpp ├── .vscode ├── tasks.json ├── settings.json └── c_cpp_properties.json ├── launch ├── skimap │ ├── skimap_map_service.launch │ └── skimap_live.launch ├── skimap_live_experiment.launch └── slamdunk_tracker.launch ├── include ├── skimap │ ├── utils │ │ ├── TimingsUtils.hpp │ │ └── CameraUtils.hpp │ ├── voxels │ │ ├── GenericVoxelKD.hpp │ │ ├── GenericTile2D.hpp │ │ ├── GenericVoxel3D.hpp │ │ ├── VoxelDataOccupancy.hpp │ │ ├── Voxel6DPose.hpp │ │ ├── VoxelDataContainer.hpp │ │ ├── VoxelDataRGBW.hpp │ │ ├── VoxelDataMultiLabel.hpp │ │ └── VoxelDataMatrix.hpp │ └── SkiMap.hpp ├── slamdunk │ ├── pretty_printer.h │ ├── slamdunk_defines.h │ ├── gpu_features.h │ ├── data.h │ ├── graph_utils.h │ ├── edge_se3_xyzpair.h │ ├── transformation_estimation.h │ ├── slam_dunk.h │ ├── impl │ │ └── ratio_matcher.hpp │ └── feature_matcher.h ├── slamdunk_extension │ ├── FIROrder2.h │ └── Utility.h └── skimap_ros │ └── SkiMapServiceClient.hpp ├── package.xml ├── cmake └── Modules │ ├── FindFLANN.cmake │ ├── FindEigen3.cmake │ └── FindG2O.cmake ├── CMakeLists.txt └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | nbproject 2 | -------------------------------------------------------------------------------- /srv/SkimapIntegrationService.srv: -------------------------------------------------------------------------------- 1 | string map_name 2 | geometry_msgs/Pose sensor_pose 3 | geometry_msgs/Point[] points 4 | std_msgs/ColorRGBA[] colors 5 | float32[] weights 6 | int8 action 7 | 8 | int8 ACTION_INTEGRATE = 0 9 | 10 | --- 11 | int32 integrated_points 12 | int8 status 13 | 14 | 15 | int8 STATUS_OK=0 16 | int8 STATUS_FAIL=10 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_test.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | kdtree = importfile('bench_points/kdtree.csv'); 4 | kdskip_1 = importfile('bench_points/kdskip_r1.csv'); 5 | kdskip_5 = importfile('bench_points/kdskip_r5.csv'); 6 | kdskip_10 = importfile('bench_points/kdskip_r10.csv'); 7 | kdskip_20 = importfile('bench_points/kdskip_r20.csv'); 8 | 9 | 10 | hold on; 11 | c = 7; 12 | plot(kdtree(:,c)); 13 | plot(kdskip_1(:,c)); 14 | plot(kdskip_5(:,c)); 15 | plot(kdskip_10(:,c)); 16 | plot(kdskip_20(:,c)); 17 | legend('kdtree','kdskip r1','kdskip r5','kdskip r10','kdskip r20'); -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/bench_people.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | from subprocess import check_output 3 | import sys 4 | 5 | proc = './../../../work/ros/skimap_ws/devel/lib/skimap_ros/grid2d' 6 | 7 | step = 2000 8 | coords = 24000 9 | coords_max = coords * coords 10 | 11 | points = [60000000] 12 | 13 | 14 | for i in points: 15 | n_points = i 16 | coords = coords 17 | algo = sys.argv[1] 18 | command = proc + " {} {} {} 0".format(n_points, coords, algo) 19 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 20 | out, err = p.communicate() 21 | print out 22 | -------------------------------------------------------------------------------- /.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | // See https://go.microsoft.com/fwlink/?LinkId=733558 3 | // for the documentation about the tasks.json format 4 | "version": "0.1.0", 5 | "command": "ll", 6 | "isShellCommand": true, 7 | "args": [], 8 | "showOutput": "always", 9 | "tasks": [{ 10 | "taskName": "cwd", 11 | "command": "catkinRootBuild", 12 | "isBuildCommand": true, 13 | "isShellCommand": true, 14 | "args": ["."] 15 | }, { 16 | "taskName": "done", 17 | "command": "echo", 18 | "isShellCommand": true, 19 | "args": ["\"Ready!\""] 20 | }] 21 | 22 | } -------------------------------------------------------------------------------- /src/skimap_ros/skimap_ros_void_library.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | //ROS 15 | #include 16 | 17 | ros::NodeHandle *nh; 18 | 19 | /** MAIN NODE **/ 20 | int main(int argc, char **argv) 21 | { 22 | 23 | // Initialize ROS 24 | ros::init(argc, argv, "skimap_ros_void_file"); 25 | nh = new ros::NodeHandle("~"); 26 | 27 | // Spin 28 | while (nh->ok()) 29 | { 30 | 31 | ros::spinOnce(); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/nodes/test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | //ROS 15 | #include 16 | 17 | #include 18 | 19 | ros::NodeHandle *nh; 20 | 21 | /** MAIN NODE **/ 22 | int main(int argc, char **argv) 23 | { 24 | 25 | // Initialize ROS 26 | ros::init(argc, argv, "slam_test"); 27 | nh = new ros::NodeHandle("~"); 28 | 29 | // Spin 30 | while (nh->ok()) 31 | { 32 | 33 | ros::spinOnce(); 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/bench.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | from subprocess import check_output 3 | import sys 4 | 5 | proc = './../../../work/ros/skimap_ws/devel/lib/skimap_ros/grid2d' 6 | 7 | step = 2000 8 | coords = 20000 9 | coords_max = coords * coords 10 | 11 | points = [1000, 10000, 20000, 50000, 100000, 150000, 12 | 200000, 300000, 500000, 1000000, 1500000, 2000000, 3000000, 3500000, 5000000, 10000000, 20000000] # , 1000000, 2000000, 5000000, 10000000, 20000000, 25000000] 13 | 14 | 15 | for i in points: 16 | n_points = i 17 | coords = coords 18 | algo = sys.argv[1] 19 | command = proc + " {} {} {} 0".format(n_points, coords, algo) 20 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 21 | out, err = p.communicate() 22 | print out 23 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/plots/plot_testing.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import glob 4 | import os 5 | import csv 6 | from plotutils import Bench 7 | 8 | folder = "/home/daniele/work/ros/skimap_ws/src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points" 9 | 10 | files = glob.glob(os.path.join(folder, "*.csv")) 11 | 12 | name_composition = "" # "0_str:1_int:4_int" 13 | columns = "DIMENSIONS:POINTS:MAX_COORDINATES:RESOLUTION:TIME_INTEGRATION:TIME_RADIUS_SEARCH:MEMORY" 14 | 15 | 16 | benchs = [] 17 | for f in files: 18 | bench = Bench(f, name_composition=name_composition, 19 | columns=columns, start_column=1) 20 | benchs.append(bench) 21 | print bench.name 22 | 23 | benchs.sort(key=lambda x: x.name) 24 | 25 | x_data = benchs[0].getDataByName('POINTS') 26 | 27 | for i in range(0, len(benchs)): 28 | y_data = benchs[i].getDataByName('TIME_RADIUS_SEARCH') 29 | plt.plot(x_data, y_data, label=benchs[i].name) 30 | plt.legend() 31 | plt.show() 32 | -------------------------------------------------------------------------------- /launch/skimap/skimap_map_service.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /include/skimap/utils/TimingsUtils.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | */ 11 | struct Timings 12 | { 13 | typedef std::chrono::high_resolution_clock Time; 14 | typedef std::chrono::milliseconds ms; 15 | typedef std::chrono::microseconds us; 16 | typedef std::chrono::duration fsec; 17 | 18 | std::map> times; 19 | 20 | void startTimer(std::string name) 21 | { 22 | times[name] = Time::now(); //IS NOT ROS TIME! 23 | } 24 | 25 | us elapsedMicroseconds(std::string name) 26 | { 27 | fsec elaps = Time::now() - times[name]; 28 | return std::chrono::duration_cast(elaps); 29 | } 30 | 31 | ms elapsedMilliseconds(std::string name) 32 | { 33 | fsec elaps = Time::now() - times[name]; 34 | return std::chrono::duration_cast(elaps); 35 | } 36 | 37 | void printTime(std::string name) 38 | { 39 | printf("Time for %s: %f ms\n", name.c_str(), float(elapsedMicroseconds(name).count()) / 1000.0f); 40 | } 41 | } DEBUG_TIMINGS; -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/skigrid_omp.csv: -------------------------------------------------------------------------------- 1 | skigrid 1000 20000.000000 0.000000 0.011538 0.000041 1189992.000000 2 | skigrid 10000 20000.000000 0.000000 0.021136 0.000364 1197204.000000 3 | skigrid 20000 20000.000000 0.000000 0.030117 0.000896 1201880.000000 4 | skigrid 50000 20000.000000 0.000000 0.063181 0.002352 1214112.000000 5 | skigrid 100000 20000.000000 0.000000 0.106405 0.004357 1235196.000000 6 | skigrid 150000 20000.000000 0.000000 0.157110 0.005972 1246076.000000 7 | skigrid 200000 20000.000000 0.000000 0.208292 0.007826 1261352.000000 8 | skigrid 300000 20000.000000 0.000000 0.319287 0.011021 1291248.000000 9 | skigrid 500000 20000.000000 0.000000 0.532085 0.017489 1351308.000000 10 | skigrid 1000000 20000.000000 0.000000 1.063082 0.032430 1503176.000000 11 | skigrid 1500000 20000.000000 0.000000 1.628623 0.049630 1660568.000000 12 | skigrid 2000000 20000.000000 0.000000 2.191867 0.063446 1808496.000000 13 | skigrid 3000000 20000.000000 0.000000 3.268287 0.103148 2122808.000000 14 | skigrid 3500000 20000.000000 0.000000 3.929934 0.115048 2282760.000000 15 | skigrid 5000000 20000.000000 0.000000 5.619936 0.181303 2735304.000000 16 | skigrid 10000000 20000.000000 0.000000 11.480747 0.385266 4227296.000000 17 | skigrid 20000000 20000.000000 0.000000 24.298524 0.855037 7101572.000000 18 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/skigrid_single.csv: -------------------------------------------------------------------------------- 1 | skigrid 1000 20000.000000 0.000000 0.000485 0.010502 1190332.000000 2 | skigrid 10000 20000.000000 0.000000 0.005496 0.010771 1195868.000000 3 | skigrid 20000 20000.000000 0.000000 0.011367 0.011342 1201376.000000 4 | skigrid 50000 20000.000000 0.000000 0.033307 0.002771 1212544.000000 5 | skigrid 100000 20000.000000 0.000000 0.074049 0.004635 1228368.000000 6 | skigrid 150000 20000.000000 0.000000 0.119242 0.006751 1245736.000000 7 | skigrid 200000 20000.000000 0.000000 0.163327 0.009036 1259316.000000 8 | skigrid 300000 20000.000000 0.000000 0.277160 0.021380 1289952.000000 9 | skigrid 500000 20000.000000 0.000000 0.498934 0.027800 1350480.000000 10 | skigrid 1000000 20000.000000 0.000000 1.228157 0.042421 1506032.000000 11 | skigrid 1500000 20000.000000 0.000000 1.911463 0.049432 1665640.000000 12 | skigrid 2000000 20000.000000 0.000000 2.802714 0.062480 1809340.000000 13 | skigrid 3000000 20000.000000 0.000000 4.392162 0.103948 2136248.000000 14 | skigrid 3500000 20000.000000 0.000000 5.315917 0.117934 2281736.000000 15 | skigrid 5000000 20000.000000 0.000000 8.146614 0.164918 2721200.000000 16 | skigrid 10000000 20000.000000 0.000000 19.486182 0.371671 4225272.000000 17 | skigrid 20000000 20000.000000 0.000000 45.678550 0.828911 7103180.000000 18 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "*.urdf": "html", 4 | "*.xacro": "html", 5 | "*.launch": "html", 6 | "cctype": "cpp", 7 | "cmath": "cpp", 8 | "cstdarg": "cpp", 9 | "cstddef": "cpp", 10 | "cstdio": "cpp", 11 | "cstdlib": "cpp", 12 | "cstring": "cpp", 13 | "ctime": "cpp", 14 | "cwchar": "cpp", 15 | "cwctype": "cpp", 16 | "cstdint": "cpp", 17 | "initializer_list": "cpp", 18 | "slist": "cpp", 19 | "vector": "cpp", 20 | "clocale": "cpp", 21 | "csignal": "cpp", 22 | "array": "cpp", 23 | "atomic": "cpp", 24 | "*.tcc": "cpp", 25 | "chrono": "cpp", 26 | "exception": "cpp", 27 | "functional": "cpp", 28 | "iosfwd": "cpp", 29 | "limits": "cpp", 30 | "new": "cpp", 31 | "ratio": "cpp", 32 | "stdexcept": "cpp", 33 | "thread": "cpp", 34 | "tuple": "cpp", 35 | "type_traits": "cpp", 36 | "typeinfo": "cpp", 37 | "utility": "cpp", 38 | "*.ipp": "cpp", 39 | "algorithm": "cpp", 40 | "codecvt": "cpp", 41 | "condition_variable": "cpp", 42 | "memory": "cpp", 43 | "mutex": "cpp", 44 | "system_error": "cpp" 45 | } 46 | } -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/kdskip.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 100000 365.000000 0.050000 0.093395 0.007467 87244.000000 2 | kdskip 3 150000 365.000000 0.050000 0.154709 0.010184 118844.000000 3 | kdskip 3 200000 365.000000 0.050000 0.191743 0.012871 151332.000000 4 | kdskip 3 250000 365.000000 0.050000 0.246193 0.015747 180536.000000 5 | kdskip 3 300000 365.000000 0.050000 0.298760 0.018668 218672.000000 6 | kdskip 3 350000 365.000000 0.050000 0.335976 0.022016 249112.000000 7 | kdskip 3 400000 365.000000 0.050000 0.335976 0.022016 249112.000000 8 | kdskip 3 450000 365.000000 0.050000 0.440828 0.026456 308936.000000 9 | kdskip 3 500000 365.000000 0.050000 0.486196 0.029746 341636.000000 10 | kdskip 3 550000 365.000000 0.050000 0.523022 0.035309 376448.000000 11 | kdskip 3 600000 365.000000 0.050000 0.586299 0.039612 408640.000000 12 | kdskip 3 650000 365.000000 0.050000 0.622749 0.036346 436128.000000 13 | kdskip 3 700000 365.000000 0.050000 0.700533 0.039648 470996.000000 14 | kdskip 3 750000 365.000000 0.050000 0.721011 0.044963 499268.000000 15 | kdskip 3 800000 365.000000 0.050000 0.800587 0.045966 534252.000000 16 | kdskip 3 850000 365.000000 0.050000 0.830620 0.047785 559880.000000 17 | kdskip 3 900000 365.000000 0.050000 0.875534 0.049676 601436.000000 18 | kdskip 3 950000 365.000000 0.050000 0.924537 0.053269 622484.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/skimap.csv: -------------------------------------------------------------------------------- 1 | skimap 3 100000 365.000000 0.050000 0.095232 0.002332 107844.000000 2 | skimap 3 150000 365.000000 0.050000 0.095232 0.002332 107844.000000 3 | skimap 3 200000 365.000000 0.050000 0.205925 0.003808 192648.000000 4 | skimap 3 250000 365.000000 0.050000 0.250978 0.004408 233900.000000 5 | skimap 3 300000 365.000000 0.050000 0.300703 0.005213 277652.000000 6 | skimap 3 350000 365.000000 0.050000 0.348785 0.010129 319280.000000 7 | skimap 3 400000 365.000000 0.050000 0.388460 0.006731 360164.000000 8 | skimap 3 450000 365.000000 0.050000 0.447114 0.009194 402908.000000 9 | skimap 3 500000 365.000000 0.050000 0.481272 0.008451 444204.000000 10 | skimap 3 550000 365.000000 0.050000 0.544395 0.009294 487808.000000 11 | skimap 3 600000 365.000000 0.050000 0.595937 0.009820 530036.000000 12 | skimap 3 650000 365.000000 0.050000 0.653879 0.010961 573012.000000 13 | skimap 3 700000 365.000000 0.050000 0.690596 0.011423 614056.000000 14 | skimap 3 750000 365.000000 0.050000 0.739642 0.011914 655872.000000 15 | skimap 3 800000 365.000000 0.050000 0.802050 0.012861 697732.000000 16 | skimap 3 850000 365.000000 0.050000 0.839659 0.013433 739988.000000 17 | skimap 3 900000 365.000000 0.050000 0.897173 0.014868 779924.000000 18 | skimap 3 950000 365.000000 0.050000 0.945125 0.015188 823224.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/octree.csv: -------------------------------------------------------------------------------- 1 | octree 3 100000 365.000000 0.001000 0.086353 0.008081 153364.000000 2 | octree 3 150000 365.000000 0.001000 0.128280 0.011317 218352.000000 3 | octree 3 200000 365.000000 0.001000 0.170343 0.013679 281052.000000 4 | octree 3 250000 365.000000 0.001000 0.215232 0.017047 343220.000000 5 | octree 3 300000 365.000000 0.001000 0.261374 0.020370 407024.000000 6 | octree 3 350000 365.000000 0.001000 0.307148 0.024205 469240.000000 7 | octree 3 400000 365.000000 0.001000 0.350118 0.028330 530804.000000 8 | octree 3 450000 365.000000 0.001000 0.397585 0.031959 590236.000000 9 | octree 3 500000 365.000000 0.001000 0.440896 0.034976 651468.000000 10 | octree 3 550000 365.000000 0.001000 0.485383 0.038075 712832.000000 11 | octree 3 600000 365.000000 0.001000 0.533736 0.041620 772652.000000 12 | octree 3 650000 365.000000 0.001000 0.572267 0.044646 835064.000000 13 | octree 3 700000 365.000000 0.001000 0.637531 0.048530 894292.000000 14 | octree 3 750000 365.000000 0.001000 0.666899 0.051920 953460.000000 15 | octree 3 800000 365.000000 0.001000 0.715680 0.055405 1016012.000000 16 | octree 3 850000 365.000000 0.001000 0.765032 0.059372 1075952.000000 17 | octree 3 900000 365.000000 0.001000 0.817587 0.062195 1135144.000000 18 | octree 3 950000 365.000000 0.001000 0.872649 0.065883 1193956.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r5.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 5.000000 0.685599 0.111093 493976.000000 2 | kdskip 2 1500000 5000.000000 5.000000 1.026361 0.141201 623828.000000 3 | kdskip 2 2000000 5000.000000 5.000000 1.365625 0.157038 718500.000000 4 | kdskip 2 2500000 5000.000000 5.000000 1.748177 0.171963 770384.000000 5 | kdskip 2 3000000 5000.000000 5.000000 2.113779 0.181830 833284.000000 6 | kdskip 2 3500000 5000.000000 5.000000 2.635059 0.223364 900488.000000 7 | kdskip 2 4000000 5000.000000 5.000000 3.141288 0.202865 972956.000000 8 | kdskip 2 4500000 5000.000000 5.000000 3.667826 0.218306 1087428.000000 9 | kdskip 2 5000000 5000.000000 5.000000 3.993233 0.192159 1152196.000000 10 | kdskip 2 5500000 5000.000000 5.000000 4.145108 0.235567 1220760.000000 11 | kdskip 2 6000000 5000.000000 5.000000 4.506435 0.245193 1287676.000000 12 | kdskip 2 6500000 5000.000000 5.000000 4.995985 0.260913 1350308.000000 13 | kdskip 2 7000000 5000.000000 5.000000 5.295985 0.260913 1350308.000000 14 | kdskip 2 7500000 5000.000000 5.000000 5.801158 0.290520 1496948.000000 15 | kdskip 2 8000000 5000.000000 5.000000 6.487572 0.302891 1549824.000000 16 | kdskip 2 8500000 5000.000000 5.000000 7.016074 0.299087 1612472.000000 17 | kdskip 2 9000000 5000.000000 5.000000 7.397598 0.312716 1686752.000000 18 | kdskip 2 9500000 5000.000000 5.000000 8.137543 0.395591 1751644.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r1.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 1.000000 0.814965 0.172397 611040.000000 2 | kdskip 2 1500000 5000.000000 1.000000 1.276408 0.260442 795564.000000 3 | kdskip 2 2000000 5000.000000 1.000000 1.683748 0.335462 1001332.000000 4 | kdskip 2 2500000 5000.000000 1.000000 2.127634 0.426662 1159448.000000 5 | kdskip 2 3000000 5000.000000 1.000000 2.542711 0.507590 1345488.000000 6 | kdskip 2 3500000 5000.000000 1.000000 2.991633 0.583608 1449908.000000 7 | kdskip 2 4000000 5000.000000 1.000000 3.478163 0.682334 1704896.000000 8 | kdskip 2 4500000 5000.000000 1.000000 3.937203 0.762582 1848648.000000 9 | kdskip 2 5000000 5000.000000 1.000000 4.396502 0.865608 1969524.000000 10 | kdskip 2 5500000 5000.000000 1.000000 4.871279 0.925383 2092820.000000 11 | kdskip 2 6000000 5000.000000 1.000000 5.298679 1.004504 2238236.000000 12 | kdskip 2 6500000 5000.000000 1.000000 5.769029 1.081194 2398952.000000 13 | kdskip 2 7000000 5000.000000 1.000000 6.207895 1.144101 2550196.000000 14 | kdskip 2 7500000 5000.000000 1.000000 6.812027 1.278237 2681732.000000 15 | kdskip 2 8000000 5000.000000 1.000000 7.123045 1.346384 2829648.000000 16 | kdskip 2 8500000 5000.000000 1.000000 7.729137 1.421366 2964756.000000 17 | kdskip 2 9000000 5000.000000 1.000000 8.280959 1.445313 3125180.000000 18 | kdskip 2 9500000 5000.000000 1.000000 8.870138 1.517883 3297400.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdskip_r1.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 1000000 5000.000000 1.000000 0.934796 0.207735 951944.000000 2 | kdskip 3 1500000 5000.000000 1.000000 1.403396 0.304142 1338764.000000 3 | kdskip 3 2000000 5000.000000 1.000000 1.901982 0.410598 1713148.000000 4 | kdskip 3 2500000 5000.000000 1.000000 2.395175 0.523172 2078928.000000 5 | kdskip 3 3000000 5000.000000 1.000000 2.911742 0.645338 2421912.000000 6 | kdskip 3 3500000 5000.000000 1.000000 3.373410 0.752494 2792212.000000 7 | kdskip 3 4000000 5000.000000 1.000000 3.842186 0.861975 3167372.000000 8 | kdskip 3 4500000 5000.000000 1.000000 4.328054 0.972207 3410668.000000 9 | kdskip 3 5000000 5000.000000 1.000000 4.827996 1.080582 3733096.000000 10 | kdskip 3 5500000 5000.000000 1.000000 5.227996 1.180582 3933096.000000 11 | kdskip 3 6000000 5000.000000 1.000000 5.833935 1.293358 4400688.000000 12 | kdskip 3 6500000 5000.000000 1.000000 6.417843 1.405188 4681160.000000 13 | kdskip 3 7000000 5000.000000 1.000000 6.900629 1.575579 5017512.000000 14 | kdskip 3 7500000 5000.000000 1.000000 7.445980 1.637557 5326772.000000 15 | kdskip 3 8000000 5000.000000 1.000000 8.106903 1.748545 5650964.000000 16 | kdskip 3 8500000 5000.000000 1.000000 8.602430 1.911174 5955332.000000 17 | kdskip 3 9000000 5000.000000 1.000000 9.126322 2.012947 6212988.000000 18 | kdskip 3 9500000 5000.000000 1.000000 9.687495 2.161250 6542368.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdskip_r5.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 1000000 5000.000000 5.000000 0.712404 0.195313 819528.000000 2 | kdskip 3 1500000 5000.000000 5.000000 1.084312 0.298743 1096376.000000 3 | kdskip 3 2000000 5000.000000 5.000000 1.441557 0.400683 1283716.000000 4 | kdskip 3 2500000 5000.000000 5.000000 1.750100 0.460958 1496384.000000 5 | kdskip 3 3000000 5000.000000 5.000000 2.101258 0.552551 1713960.000000 6 | kdskip 3 3500000 5000.000000 5.000000 2.498248 0.626122 1893536.000000 7 | kdskip 3 4000000 5000.000000 5.000000 2.974212 0.749216 2084336.000000 8 | kdskip 3 4500000 5000.000000 5.000000 3.352105 0.811671 2180264.000000 9 | kdskip 3 5000000 5000.000000 5.000000 3.619260 0.912979 2362256.000000 10 | kdskip 3 5500000 5000.000000 5.000000 4.031062 1.001055 2545928.000000 11 | kdskip 3 6000000 5000.000000 5.000000 4.324799 1.098252 2666240.000000 12 | kdskip 3 6500000 5000.000000 5.000000 4.619308 1.204433 2847788.000000 13 | kdskip 3 7000000 5000.000000 5.000000 5.108564 1.310509 3023528.000000 14 | kdskip 3 7500000 5000.000000 5.000000 5.344661 1.349272 3170708.000000 15 | kdskip 3 8000000 5000.000000 5.000000 5.646024 1.468644 3298796.000000 16 | kdskip 3 8500000 5000.000000 5.000000 6.143752 1.519247 3520536.000000 17 | kdskip 3 9000000 5000.000000 5.000000 6.440595 1.650995 3663900.000000 18 | kdskip 3 9500000 5000.000000 5.000000 7.105001 1.757049 3826516.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 10.000000 0.673127 0.048068 387704.000000 2 | kdskip 2 1500000 5000.000000 10.000000 1.088257 0.059753 451092.000000 3 | kdskip 2 2000000 5000.000000 10.000000 1.533063 0.091017 527624.000000 4 | kdskip 2 2500000 5000.000000 10.000000 1.975446 0.091328 597016.000000 5 | kdskip 2 3000000 5000.000000 10.000000 2.478252 0.119050 680096.000000 6 | kdskip 2 3500000 5000.000000 10.000000 3.426770 0.116500 740484.000000 7 | kdskip 2 4000000 5000.000000 10.000000 3.701359 0.128698 841480.000000 8 | kdskip 2 4500000 5000.000000 10.000000 4.290214 0.129612 915384.000000 9 | kdskip 2 5000000 5000.000000 10.000000 4.969801 0.148042 1009252.000000 10 | kdskip 2 5500000 5000.000000 10.000000 5.779322 0.156280 1090892.000000 11 | kdskip 2 6000000 5000.000000 10.000000 6.893417 0.164418 1186212.000000 12 | kdskip 2 6500000 5000.000000 10.000000 7.834441 0.170382 1269364.000000 13 | kdskip 2 7000000 5000.000000 10.000000 9.339149 0.181571 1361980.000000 14 | kdskip 2 7500000 5000.000000 10.000000 10.688451 0.196093 1467256.000000 15 | kdskip 2 8000000 5000.000000 10.000000 11.787132 0.202779 1559204.000000 16 | kdskip 2 8500000 5000.000000 10.000000 11.690665 0.217115 1677724.000000 17 | kdskip 2 9000000 5000.000000 10.000000 13.739477 0.258461 1754236.000000 18 | kdskip 2 9500000 5000.000000 10.000000 14.940938 0.240768 1865908.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 1000000 5000.000000 10.000000 0.644240 0.183188 666248.000000 2 | kdskip 3 1500000 5000.000000 10.000000 0.941575 0.264472 883840.000000 3 | kdskip 3 2000000 5000.000000 10.000000 1.280701 0.354211 1071956.000000 4 | kdskip 3 2500000 5000.000000 10.000000 1.665522 0.426708 1236388.000000 5 | kdskip 3 3000000 5000.000000 10.000000 1.929029 0.530189 1425676.000000 6 | kdskip 3 3500000 5000.000000 10.000000 2.249303 0.602678 1597568.000000 7 | kdskip 3 4000000 5000.000000 10.000000 2.572607 0.677135 1782020.000000 8 | kdskip 3 4500000 5000.000000 10.000000 2.969957 0.766479 1879960.000000 9 | kdskip 3 5000000 5000.000000 10.000000 3.266602 0.865146 2033924.000000 10 | kdskip 3 5500000 5000.000000 10.000000 3.579220 0.926121 2233064.000000 11 | kdskip 3 6000000 5000.000000 10.000000 3.906046 1.027036 2382956.000000 12 | kdskip 3 6500000 5000.000000 10.000000 4.235451 1.101316 2532164.000000 13 | kdskip 3 7000000 5000.000000 10.000000 4.716498 1.167795 2673076.000000 14 | kdskip 3 7500000 5000.000000 10.000000 4.953907 1.275503 2798720.000000 15 | kdskip 3 8000000 5000.000000 10.000000 5.395500 1.334778 3020588.000000 16 | kdskip 3 8500000 5000.000000 10.000000 5.594637 1.381279 3165584.000000 17 | kdskip 3 9000000 5000.000000 10.000000 6.119115 1.524743 3312784.000000 18 | kdskip 3 9500000 5000.000000 10.000000 6.302468 1.677834 3449856.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r20.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 20.000000 0.839913 0.029197 334032.000000 2 | kdskip 2 1500000 5000.000000 20.000000 1.559849 0.035840 424984.000000 3 | kdskip 2 2000000 5000.000000 20.000000 2.487401 0.044774 522940.000000 4 | kdskip 2 2500000 5000.000000 20.000000 3.656181 0.057829 623900.000000 5 | kdskip 2 3000000 5000.000000 20.000000 5.031625 0.081021 731004.000000 6 | kdskip 2 3500000 5000.000000 20.000000 6.629939 0.094072 839408.000000 7 | kdskip 2 4000000 5000.000000 20.000000 8.530534 0.110229 946548.000000 8 | kdskip 2 4500000 5000.000000 20.000000 10.545138 0.109528 1067200.000000 9 | kdskip 2 5000000 5000.000000 20.000000 12.764492 0.104058 1179316.000000 10 | kdskip 2 5500000 5000.000000 20.000000 15.375516 0.081756 1300068.000000 11 | kdskip 2 6000000 5000.000000 20.000000 18.221374 0.086772 1413200.000000 12 | kdskip 2 6500000 5000.000000 20.000000 21.291247 0.035767 1550408.000000 13 | kdskip 2 7000000 5000.000000 20.000000 24.212804 0.035050 1656924.000000 14 | kdskip 2 7500000 5000.000000 20.000000 27.780371 0.044177 1769356.000000 15 | kdskip 2 8000000 5000.000000 20.000000 31.384978 0.039121 1903648.000000 16 | kdskip 2 8500000 5000.000000 20.000000 35.256120 0.038144 2024208.000000 17 | kdskip 2 9000000 5000.000000 20.000000 39.097930 0.048569 2134400.000000 18 | kdskip 2 9500000 5000.000000 20.000000 43.659300 0.030082 2268652.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 5 1000000 5000.000000 10.000000 1.082161 0.242113 1413864.000000 2 | kdskip 5 1500000 5000.000000 10.000000 1.382161 0.242113 1413864.000000 3 | kdskip 5 2000000 5000.000000 10.000000 2.103406 0.480468 2565480.000000 4 | kdskip 5 2500000 5000.000000 10.000000 2.603923 0.592346 3128328.000000 5 | kdskip 5 3000000 5000.000000 10.000000 3.078191 0.699455 3693296.000000 6 | kdskip 5 3500000 5000.000000 10.000000 3.606175 0.812973 4249120.000000 7 | kdskip 5 4000000 5000.000000 10.000000 4.143444 0.931216 4824664.000000 8 | kdskip 5 4500000 5000.000000 10.000000 4.596879 1.039901 5261816.000000 9 | kdskip 5 5000000 5000.000000 10.000000 5.105001 1.130851 5792700.000000 10 | kdskip 5 5500000 5000.000000 10.000000 5.611607 1.240321 6355552.000000 11 | kdskip 5 6000000 5000.000000 10.000000 6.173194 1.353181 6906396.000000 12 | kdskip 5 6500000 5000.000000 10.000000 6.662795 1.466827 7420856.000000 13 | kdskip 5 7000000 5000.000000 10.000000 7.155359 1.574538 7970816.000000 14 | kdskip 5 7500000 5000.000000 10.000000 7.622937 1.707257 8520076.000000 15 | kdskip 5 8000000 5000.000000 10.000000 8.082560 1.819948 9028824.000000 16 | kdskip 5 8500000 5000.000000 10.000000 8.082560 1.819948 9028824.000000 17 | kdskip 5 9000000 5000.000000 10.000000 9.311059 2.065423 10109948.000000 18 | kdskip 5 9500000 5000.000000 10.000000 9.754819 2.163505 10662744.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdskip_r25.csv: -------------------------------------------------------------------------------- 1 | kdskip 5 1000000 5000.000000 25.000000 1.019368 0.226223 1308316.000000 2 | kdskip 5 1500000 5000.000000 25.000000 1.492909 0.328665 1868024.000000 3 | kdskip 5 2000000 5000.000000 25.000000 1.983522 0.451544 2379328.000000 4 | kdskip 5 2500000 5000.000000 25.000000 2.445511 0.562688 2925288.000000 5 | kdskip 5 3000000 5000.000000 25.000000 2.930460 0.682281 3438624.000000 6 | kdskip 5 3500000 5000.000000 25.000000 3.396834 0.767365 3925224.000000 7 | kdskip 5 4000000 5000.000000 25.000000 3.867604 0.888346 4429848.000000 8 | kdskip 5 4500000 5000.000000 25.000000 4.287047 0.982466 4806652.000000 9 | kdskip 5 5500000 5000.000000 25.000000 5.201368 1.207174 5707220.000000 10 | kdskip 5 5500000 5000.000000 25.000000 5.201368 1.207174 5707220.000000 11 | kdskip 5 6000000 5000.000000 25.000000 5.660080 1.296813 6157944.000000 12 | kdskip 5 6500000 5000.000000 25.000000 6.105823 1.406147 6635108.000000 13 | kdskip 5 7000000 5000.000000 25.000000 6.105823 1.406147 6635108.000000 14 | kdskip 5 7500000 5000.000000 25.000000 6.948942 1.593570 7484020.000000 15 | kdskip 5 8000000 5000.000000 25.000000 7.461763 1.733698 7879124.000000 16 | kdskip 5 8500000 5000.000000 25.000000 7.849881 1.848185 8316948.000000 17 | kdskip 5 9000000 5000.000000 25.000000 8.201984 1.944721 8790700.000000 18 | kdskip 5 9500000 5000.000000 25.000000 8.659590 2.094784 9177316.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdskip_r32.csv: -------------------------------------------------------------------------------- 1 | kdskip 5 1000000 5000.000000 32.000000 0.993209 0.223567 1276636.000000 2 | kdskip 5 1500000 5000.000000 32.000000 1.471200 0.325596 1824732.000000 3 | kdskip 5 2000000 5000.000000 32.000000 1.978820 0.508869 2309292.000000 4 | kdskip 5 2500000 5000.000000 32.000000 2.654605 0.642672 2784320.000000 5 | kdskip 5 3000000 5000.000000 32.000000 2.930228 0.637649 3280288.000000 6 | kdskip 5 3500000 5000.000000 32.000000 3.245092 0.780027 3716396.000000 7 | kdskip 5 4000000 5000.000000 32.000000 3.699430 0.862086 4144148.000000 8 | kdskip 5 4500000 5000.000000 32.000000 4.136155 0.958633 4465472.000000 9 | kdskip 5 5000000 5000.000000 32.000000 4.537896 1.082811 4890948.000000 10 | kdskip 5 5500000 5000.000000 32.000000 4.902130 1.171746 5300256.000000 11 | kdskip 5 6000000 5000.000000 32.000000 5.424516 1.270328 5673624.000000 12 | kdskip 5 6500000 5000.000000 32.000000 5.835254 1.349033 6084372.000000 13 | kdskip 5 7000000 5000.000000 32.000000 6.236539 1.464556 6442912.000000 14 | kdskip 5 7500000 5000.000000 32.000000 6.615577 1.555155 6815020.000000 15 | kdskip 5 8000000 5000.000000 32.000000 6.983298 1.661390 7226152.000000 16 | kdskip 5 8500000 5000.000000 32.000000 7.373869 1.741971 7618112.000000 17 | kdskip 5 9000000 5000.000000 32.000000 7.758641 1.883333 7965808.000000 18 | kdskip 5 9500000 5000.000000 32.000000 8.208988 2.008831 8344048.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 3 100000 365.000000 0.001000 0.053696 0.004566 46920.000000 2 | flann_kdtree 3 150000 365.000000 0.001000 0.081967 0.006693 63648.000000 3 | flann_kdtree 3 200000 365.000000 0.001000 0.110485 0.009045 74600.000000 4 | flann_kdtree 3 250000 365.000000 0.001000 0.140574 0.010506 88920.000000 5 | flann_kdtree 3 300000 365.000000 0.001000 0.167284 0.012076 104200.000000 6 | flann_kdtree 3 350000 365.000000 0.001000 0.199567 0.014194 117096.000000 7 | flann_kdtree 3 400000 365.000000 0.001000 0.226392 0.016083 132016.000000 8 | flann_kdtree 3 450000 365.000000 0.001000 0.261273 0.018341 144056.000000 9 | flann_kdtree 3 500000 365.000000 0.001000 0.289858 0.019501 158184.000000 10 | flann_kdtree 3 550000 365.000000 0.001000 0.322065 0.021387 171984.000000 11 | flann_kdtree 3 600000 365.000000 0.001000 0.348963 0.023333 186440.000000 12 | flann_kdtree 3 650000 365.000000 0.001000 0.384814 0.025017 200328.000000 13 | flann_kdtree 3 700000 365.000000 0.001000 0.422366 0.027067 214776.000000 14 | flann_kdtree 3 750000 365.000000 0.001000 0.457865 0.029708 227768.000000 15 | flann_kdtree 3 800000 365.000000 0.001000 0.486370 0.030666 241588.000000 16 | flann_kdtree 3 850000 365.000000 0.001000 0.526671 0.033729 254568.000000 17 | flann_kdtree 3 900000 365.000000 0.001000 0.559139 0.036446 269016.000000 18 | flann_kdtree 3 950000 365.000000 0.001000 0.598645 0.039654 283704.000000 -------------------------------------------------------------------------------- /include/slamdunk/pretty_printer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_PRETTY_PRINTER_H 10 | #define SLAM_DUNK_PRETTY_PRINTER_H 11 | 12 | #ifdef _MSC_VER 13 | #define SLAM_DUNK_FUNCTION_STR __FUNCSIG__ 14 | #else 15 | #ifdef __GNUC__ 16 | #define SLAM_DUNK_FUNCTION_STR __PRETTY_FUNCTION__ 17 | #else 18 | #define SLAM_DUNK_FUNCTION_STR __FUNCTION__ 19 | #endif 20 | #endif 21 | 22 | // std logging string 23 | #define SLAM_DUNK_DEBUG_STR(s) "\033[1;34m[DEBUG]\033[0m" \ 24 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 25 | #define SLAM_DUNK_INFO_STR(s) "\033[1;32m[INFO] \033[0m" \ 26 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 27 | #define SLAM_DUNK_WARNING_STR(s) "\033[1;33m[WARN] \033[0m" \ 28 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 29 | #define SLAM_DUNK_ERROR_STR(s) "\033[1;31m[ERROR]\033[0m" \ 30 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 31 | #define SLAM_DUNK_FATAL_STR(s) "\033[1;31m[FATAL]\033[0m" \ 32 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 33 | 34 | #endif // SLAM_DUNK_PRETTY_PRINTER_H 35 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 2 1000000 5000.000000 10.000000 0.539054 0.231887 449056.000000 2 | flann_kdtree 2 1500000 5000.000000 10.000000 0.851489 0.351304 593224.000000 3 | flann_kdtree 2 2000000 5000.000000 10.000000 1.216012 0.525777 731408.000000 4 | flann_kdtree 2 2500000 5000.000000 10.000000 1.540171 0.651280 870508.000000 5 | flann_kdtree 2 3000000 5000.000000 10.000000 1.897952 0.824892 1010260.000000 6 | flann_kdtree 2 3500000 5000.000000 10.000000 2.278977 1.025024 1151952.000000 7 | flann_kdtree 2 4000000 5000.000000 10.000000 2.637600 1.109519 1290068.000000 8 | flann_kdtree 2 4500000 5000.000000 10.000000 3.011912 1.355155 1434136.000000 9 | flann_kdtree 2 5000000 5000.000000 10.000000 3.419225 1.517113 1571812.000000 10 | flann_kdtree 2 5500000 5000.000000 10.000000 3.859852 1.736507 1713196.000000 11 | flann_kdtree 2 6000000 5000.000000 10.000000 4.169820 1.830540 1851520.000000 12 | flann_kdtree 2 6500000 5000.000000 10.000000 4.661719 2.072784 1991780.000000 13 | flann_kdtree 2 7000000 5000.000000 10.000000 5.025375 2.172728 2132124.000000 14 | flann_kdtree 2 7500000 5000.000000 10.000000 5.504341 2.414933 2271692.000000 15 | flann_kdtree 2 8000000 5000.000000 10.000000 6.069174 2.547094 2412124.000000 16 | flann_kdtree 2 8500000 5000.000000 10.000000 6.216623 2.638016 2550852.000000 17 | flann_kdtree 2 9000000 5000.000000 10.000000 6.559452 2.936697 2694860.000000 18 | flann_kdtree 2 9500000 5000.000000 10.000000 6.993293 3.091916 2833060.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 3 1000000 5000.000000 10.000000 0.642494 0.204250 449920.000000 2 | flann_kdtree 3 1500000 5000.000000 10.000000 1.033402 0.336263 593456.000000 3 | flann_kdtree 3 2000000 5000.000000 10.000000 1.503326 0.484609 735964.000000 4 | flann_kdtree 3 2500000 5000.000000 10.000000 1.782842 0.576000 875812.000000 5 | flann_kdtree 3 3000000 5000.000000 10.000000 2.255901 0.725650 1017044.000000 6 | flann_kdtree 3 3500000 5000.000000 10.000000 2.773111 0.877424 1158928.000000 7 | flann_kdtree 3 4000000 5000.000000 10.000000 3.322972 1.005480 1298624.000000 8 | flann_kdtree 3 4500000 5000.000000 10.000000 3.535072 1.121170 1440668.000000 9 | flann_kdtree 3 5000000 5000.000000 10.000000 4.056507 1.299342 1582872.000000 10 | flann_kdtree 3 5500000 5000.000000 10.000000 4.663234 1.451825 1722588.000000 11 | flann_kdtree 3 6000000 5000.000000 10.000000 5.285430 1.572314 1864396.000000 12 | flann_kdtree 3 6500000 5000.000000 10.000000 5.793652 1.785578 2005236.000000 13 | flann_kdtree 3 7000000 5000.000000 10.000000 6.302779 1.821034 2143720.000000 14 | flann_kdtree 3 7500000 5000.000000 10.000000 6.884936 2.107053 2288116.000000 15 | flann_kdtree 3 8000000 5000.000000 10.000000 7.382275 2.266811 2429360.000000 16 | flann_kdtree 3 8500000 5000.000000 10.000000 7.321527 2.343101 2569704.000000 17 | flann_kdtree 3 9000000 5000.000000 10.000000 7.933009 2.492354 2710308.000000 18 | flann_kdtree 3 9500000 5000.000000 10.000000 8.579343 2.592515 2849964.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 5 1000000 5000.000000 10.000000 0.835496 0.167531 452812.000000 2 | flann_kdtree 5 1500000 5000.000000 10.000000 1.305427 0.253833 601536.000000 3 | flann_kdtree 5 2000000 5000.000000 10.000000 1.776717 0.345364 741984.000000 4 | flann_kdtree 5 2500000 5000.000000 10.000000 2.250851 0.432557 887272.000000 5 | flann_kdtree 5 3000000 5000.000000 10.000000 2.749457 0.530664 1030004.000000 6 | flann_kdtree 5 3500000 5000.000000 10.000000 3.260509 0.633973 1173592.000000 7 | flann_kdtree 5 4000000 5000.000000 10.000000 3.766719 0.756192 1317832.000000 8 | flann_kdtree 5 4500000 5000.000000 10.000000 4.298073 0.828301 1461492.000000 9 | flann_kdtree 5 5000000 5000.000000 10.000000 4.821063 0.948965 1607100.000000 10 | flann_kdtree 5 5500000 5000.000000 10.000000 5.354093 1.012151 1747364.000000 11 | flann_kdtree 5 6000000 5000.000000 10.000000 5.866918 1.133715 1892640.000000 12 | flann_kdtree 5 6500000 5000.000000 10.000000 6.422771 1.215078 2036512.000000 13 | flann_kdtree 5 7000000 5000.000000 10.000000 6.968018 1.414152 2181124.000000 14 | flann_kdtree 5 7500000 5000.000000 10.000000 7.481058 1.458921 2323892.000000 15 | flann_kdtree 5 8000000 5000.000000 10.000000 8.040413 1.535670 2468292.000000 16 | flann_kdtree 5 8500000 5000.000000 10.000000 8.582489 1.641569 2610936.000000 17 | flann_kdtree 5 9000000 5000.000000 10.000000 9.131442 1.769813 2754832.000000 18 | flann_kdtree 5 9500000 5000.000000 10.000000 9.657458 1.871380 2898104.000000 -------------------------------------------------------------------------------- /include/slamdunk_extension/FIROrder2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #ifndef FIRORDER2_H 19 | #define FIRORDER2_H 20 | 21 | class FIROrder2 22 | { 23 | public: 24 | FIROrder2(int data_size, double cutoff, double quality = 0.5); 25 | virtual ~FIROrder2(); 26 | void updateParameters(double cutoff); 27 | void setSampleTime(double sample_time); 28 | 29 | void setInput(int index, double X); 30 | void setInputs(double *Xs); 31 | void setInput(Eigen::Isometry3d &iso); 32 | void setInitialData(double *Xs); 33 | void setInitialData(Eigen::Isometry3d &iso); 34 | 35 | void getOutput(Eigen::Isometry3d &iso); 36 | 37 | double Fc; 38 | double Fs; 39 | double Q; 40 | double W; 41 | double N; 42 | double B0; 43 | double B1; 44 | double B2; 45 | double A1; 46 | double A2; 47 | 48 | std::vector output; 49 | std::vector> data; 50 | bool ready; 51 | 52 | private: 53 | void conversion(Eigen::Isometry3d &iso, double *&data, bool reverse = false); 54 | int data_size; 55 | }; 56 | 57 | #endif /* FIRORDER2_H */ -------------------------------------------------------------------------------- /include/slamdunk/slamdunk_defines.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_SLAMDUNK_DEFINES_H 10 | #define SLAM_DUNK_SLAMDUNK_DEFINES_H 11 | 12 | // The following ifdef block is the standard way of creating macros which make exporting 13 | // from a DLL simpler. All files within this DLL are compiled with the SLAM_DUNK_EXPORTS 14 | // symbol defined on the command line. This symbol should not be defined on any project 15 | // that uses this DLL. This way any other project whose source files include this file see 16 | // SEBA_API functions as being imported from a DLL, whereas this DLL sees symbols 17 | // defined with this macro as being exported. 18 | #ifdef _MSC_VER 19 | #ifdef SLAM_DUNK_EXPORTS 20 | #define SLAM_DUNK_API __declspec(dllexport) 21 | #else 22 | #define SLAM_DUNK_API __declspec(dllimport) 23 | #endif 24 | #else 25 | #define SLAM_DUNK_API 26 | #endif 27 | 28 | #ifdef _MSC_VER 29 | #ifndef _CRT_SECURE_NO_WARNINGS 30 | #define _CRT_SECURE_NO_WARNINGS 31 | #endif 32 | #ifndef NOMINMAX 33 | #define NOMINMAX 34 | #endif 35 | 36 | #include "windows.h" 37 | 38 | #ifndef M_PI 39 | #define M_PI 3.14159265358979323846 40 | #endif 41 | #endif 42 | 43 | #if defined(_MSC_VER) && _MSC_VER <= 1600 // 1400 == VC++ 8.0, 1600 == VC++ 10.0 44 | #pragma warning(disable : 4251 4231 4660) 45 | #endif 46 | 47 | // Automatically generated part 48 | //#define SLAMDUNK_TIMERS_ENABLED false 49 | #define SLAMDUNK_FLANN_ENABLED 50 | #define SLAMDUNK_ICP_ENABLED 51 | 52 | #endif // SLAM_DUNK_SLAMDUNK_DEFINES_H 53 | -------------------------------------------------------------------------------- /launch/skimap_live_experiment.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /launch/skimap/skimap_live.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/plots/plotutils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | 5 | class Bench(object): 6 | NAME_SEPARATOR = '_' 7 | 8 | def __init__(self, filename, name_composition='0_str', columns=[], start_column=0): 9 | self.filename = filename 10 | self.raw_data = np.genfromtxt(filename, dtype=object, delimiter=' ') 11 | self.data = np.array(self.raw_data[:, start_column:], dtype=float) 12 | 13 | col_counter = 0 14 | self.columns = {} 15 | for c in columns.split(":"): 16 | self.columns[c] = col_counter 17 | col_counter += 1 18 | 19 | raw_data_row = self.raw_data[0, :] 20 | self.name = "" 21 | if name_composition == None or len(name_composition) == 0: 22 | self.name = os.path.basename(filename).split(".")[0] 23 | else: 24 | name_chunks = name_composition.split(":") 25 | for i in range(0, len(name_chunks)): 26 | chunk = name_chunks[i] 27 | index = int(chunk.split("_")[0]) 28 | type = str(chunk.split("_")[1]) 29 | self.name += self._buildNameChunk(raw_data_row, index, type) 30 | if i != len(name_chunks) - 1: 31 | self.name += Bench.NAME_SEPARATOR 32 | 33 | def getDataByName(self, column_name): 34 | index = self.columns[column_name] 35 | return self.data[:, index] 36 | 37 | def _buildNameChunk(self, raw_data, index, type): 38 | print "BUILD CHUNK", index, type, raw_data[index] 39 | if type == 'float': 40 | return "{:.2f}".format(float(raw_data[index])) 41 | if type == 'int': 42 | return "{:d}".format(int(float(raw_data[index]))) 43 | if type == 'str': 44 | return "{}".format(str(raw_data[index])) 45 | -------------------------------------------------------------------------------- /include/skimap/voxels/GenericVoxelKD.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef GENERICVOXELKD_HPP 10 | #define GENERICVOXELKD_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace skimap 21 | { 22 | 23 | /** 24 | * Generic KD Voxel representation. 25 | * V template represents datatype for user data. 26 | * D template represents datatype for coordinates. 27 | */ 28 | template 29 | struct GenericVoxelKD 30 | { 31 | std::vector coordinates; 32 | V *data; 33 | 34 | GenericVoxelKD() : data(NULL) 35 | { 36 | } 37 | 38 | GenericVoxelKD(std::vector coordinates, V *data) : coordinates(coordinates), data(data) 39 | { 40 | } 41 | 42 | /** 43 | * Serializes object into stream. 44 | */ 45 | friend std::ostream &operator<<(std::ostream &os, const GenericVoxelKD &voxel) 46 | { 47 | /* os << std::setprecision(std::numeric_limits::digits10 + 2); 48 | os << voxel.x << " "; 49 | os << voxel.y << " "; 50 | os << voxel.z << " "; 51 | if (voxel.data != NULL) 52 | os << *(voxel.data); 53 | return os;*/ 54 | } 55 | 56 | /** 57 | * Hydrates object from stream. 58 | */ 59 | friend std::istream &operator>>(std::istream &is, GenericVoxelKD &voxel) 60 | { 61 | /* double x, y, z; 62 | is >> x; 63 | is >> y; 64 | is >> z; 65 | voxel.x = D(x); 66 | voxel.y = D(y); 67 | voxel.z = D(z); 68 | voxel.data = new V(); 69 | is >> (*voxel.data); 70 | return is;*/ 71 | } 72 | }; 73 | } 74 | 75 | #endif /* GenericVoxelKD_HPP */ 76 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | skimap_ros 4 | 0.0.0 5 | The lar_slam package 6 | 7 | 8 | 9 | 10 | lar 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | cv_bridge 36 | 37 | 38 | 39 | cv_bridge 40 | message_generation 41 | message_runtime 42 | 43 | 44 | catkin 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /include/skimap/voxels/GenericTile2D.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef GENERICTILE2D_HPP 10 | #define GENERICTILE2D_HPP 11 | #include 12 | 13 | namespace skimap 14 | { 15 | 16 | /** 17 | * Generic 3D Voxel representation. 18 | * V template represents datatype for user data. 19 | * D template represents datatype for coordinates. 20 | */ 21 | template 22 | struct GenericTile2D : public GenericVoxel3D 23 | { 24 | 25 | /** 26 | * Void constructor. Sets NULL for user data. 27 | */ 28 | GenericTile2D() : GenericVoxel3D(0, 0, 0, NULL) 29 | { 30 | } 31 | 32 | /** 33 | * Full Constructor 34 | * @param x X coordinate 35 | * @param y Y coordinate 36 | * @param z Z coordinate 37 | * @param data User Data pointer 38 | */ 39 | GenericTile2D(D x, D y, D z, V *data) : GenericVoxel3D(x, y, z, data) 40 | { 41 | } 42 | 43 | /** 44 | * Serializes object into stream. 45 | */ 46 | friend std::ostream &operator<<(std::ostream &os, const GenericTile2D &voxel) 47 | { 48 | os << std::setprecision(std::numeric_limits::digits10 + 2); 49 | os << voxel.x << " "; 50 | os << voxel.y << " "; 51 | os << voxel.z << " "; 52 | os << 1 << " "; 53 | if (voxel.data != NULL) 54 | os << *(voxel.data); 55 | return os; 56 | } 57 | 58 | /** 59 | * Hydrates object from stream. 60 | */ 61 | friend std::istream &operator>>(std::istream &is, GenericTile2D &voxel) 62 | { 63 | double x, y, z; 64 | int t; 65 | is >> x; 66 | is >> y; 67 | is >> z; 68 | is >> t; 69 | voxel.x = D(x); 70 | voxel.y = D(y); 71 | voxel.z = D(z); 72 | voxel.data = new V(); 73 | is >> (*voxel.data); 74 | return is; 75 | } 76 | }; 77 | } 78 | 79 | #endif /* GENERICTILE2D_HPP */ 80 | -------------------------------------------------------------------------------- /src/slamdunk/graph_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include "slamdunk/graph_utils.h" 10 | #include "slamdunk/graph_backend.h" 11 | #include 12 | 13 | std::string slamdunk::graphToDot(const GraphBackend *gb, int root_id, 14 | const g2o::HyperGraph::VertexSet &colored, 15 | const g2o::HyperGraph::VertexSet &highlighted, 16 | const std::string &graph_name) 17 | { 18 | double buf[7]; 19 | std::ostringstream sstream; 20 | sstream << "digraph " << graph_name << " { root=" << root_id << ";"; 21 | 22 | for (g2o::HyperGraph::VertexSet::const_iterator vit = colored.begin(); vit != colored.end(); ++vit) 23 | { 24 | static_cast(*vit)->getEstimateData(buf); 25 | sstream << " " << (*vit)->id() << " [style=filled,fillcolor=" << ((*vit)->id() == root_id ? "yellow" : "orange") 26 | //<< ",pos=\"" << (int)(buf[0]*1000) << "," << (int)(buf[2]*1000) << "!\"];"; 27 | << "];"; 28 | } 29 | 30 | for (g2o::HyperGraph::VertexSet::const_iterator vit = highlighted.begin(); vit != highlighted.end(); ++vit) 31 | if (!colored.count(*vit)) 32 | { 33 | static_cast(*vit)->getEstimateData(buf); 34 | sstream << " " << (*vit)->id() << " [style=filled,fillcolor=" << ((*vit)->id() == root_id ? "yellow" : "lightblue") 35 | //<< ",pos=\"" << (int)(buf[0]*1000) << "," << (int)(buf[2]*1000) << "!\"];"; 36 | << "];"; 37 | } 38 | 39 | std::set> edges; 40 | const g2o::SparseOptimizer::EdgeSet &eset = gb->getGraph().edges(); 41 | for (g2o::SparseOptimizer::EdgeSet::const_iterator eit = eset.begin(); eit != eset.end(); ++eit) 42 | if (edges.insert(std::make_pair((*eit)->vertices()[0]->id(), (*eit)->vertices()[1]->id())).second) 43 | sstream << " " << (*eit)->vertices()[0]->id() << " -> " << (*eit)->vertices()[1]->id() << ";"; 44 | 45 | sstream << " }" << std::endl; 46 | return sstream.str(); 47 | } 48 | -------------------------------------------------------------------------------- /src/slamdunk/internal_timers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_INTERNAL_TIMERS_HPP 10 | #define SLAM_DUNK_INTERNAL_TIMERS_HPP 11 | 12 | #ifdef SLAMDUNK_TIMERS_ENABLED 13 | #include 14 | #include 15 | #include 16 | namespace 17 | { 18 | static std::map> g_timings_; 19 | static boost::posix_time::ptime g_base_time_; 20 | class slam_dunk_cpu_timer : public boost::timer::cpu_timer 21 | { 22 | public: 23 | explicit slam_dunk_cpu_timer(const std::string &title) 24 | : title_(title) 25 | { 26 | start(); 27 | } 28 | ~slam_dunk_cpu_timer() 29 | { 30 | stop(); 31 | std::map>::iterator tIt = 32 | g_timings_.insert(std::make_pair(title_, std::pair(0, 0))).first; 33 | tIt->second.first += 1; 34 | tIt->second.second += elapsed().wall; 35 | 36 | // Print average timings every N seconds 37 | boost::posix_time::ptime current_time = boost::posix_time::second_clock::local_time(); 38 | if (g_base_time_.is_not_a_date_time()) 39 | g_base_time_ = current_time; 40 | if ((current_time - g_base_time_).total_seconds() >= 5) 41 | { 42 | std::ostringstream outstr; 43 | outstr << "[SLAM DUNK TIMERS]\n"; 44 | for (tIt = g_timings_.begin(); tIt != g_timings_.end(); ++tIt) 45 | { 46 | outstr << ("- " + tIt->first + ": average speed (msec) ") << ((double(tIt->second.second) * 1e-6) / double(tIt->second.first)) << std::endl; 47 | tIt->second.first = 0; 48 | tIt->second.second = 0; 49 | } 50 | std::cout << outstr.str(); 51 | g_base_time_ = current_time; 52 | } 53 | } 54 | 55 | private: 56 | const std::string title_; 57 | }; 58 | } 59 | #define SLAM_DUNK_AUTO_CPU_TIMER(title) slam_dunk_cpu_timer scoped_timer(title) 60 | #else // SLAMDUNK_TIMERS_ENABLED 61 | #define SLAM_DUNK_AUTO_CPU_TIMER(title) 62 | #endif // SLAMDUNK_TIMERS_ENABLED 63 | 64 | #endif // SLAM_DUNK_INTERNAL_TIMERS_HPP 65 | -------------------------------------------------------------------------------- /launch/slamdunk_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /include/slamdunk/gpu_features.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_GPU_FEATURES_H 10 | #define SLAM_DUNK_GPU_FEATURES_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | 14 | #include "opencv2/features2d/features2d.hpp" 15 | #include "opencv2/gpu/gpu.hpp" 16 | #include "opencv2/nonfree/gpu.hpp" 17 | 18 | namespace slamdunk 19 | { 20 | 21 | class SLAM_DUNK_API GPUSurf : public cv::Feature2D 22 | { 23 | public: 24 | explicit GPUSurf(double hessianThreshold, int nOctaves = 4, 25 | int nOctaveLayers = 2, bool extended = false, float keypointsRatio = 0.01f, bool upright = false); 26 | 27 | //! returns the descriptor size in floats (64 or 128) 28 | inline int descriptorSize() const { return m_surf_gpu.descriptorSize(); } 29 | 30 | //! returns the descriptor type 31 | inline int descriptorType() const { return CV_32F; } 32 | 33 | //! finds the keypoints using SIFT algorithm 34 | void operator()(cv::InputArray img, cv::InputArray mask, 35 | std::vector &keypoints) const; 36 | //! finds the keypoints and computes descriptors for them using SIFT algorithm. 37 | //! Optionally it can compute descriptors for the user-provided keypoints 38 | void operator()(cv::InputArray img, cv::InputArray mask, 39 | std::vector &keypoints, 40 | cv::OutputArray descriptors, 41 | bool useProvidedKeypoints = false) const; 42 | 43 | // This function has little sense outside of an OpenCV module 44 | cv::AlgorithmInfo *info() const { return NULL; }; 45 | 46 | protected: 47 | void detectImpl(const cv::Mat &image, std::vector &keypoints, const cv::Mat &mask = cv::Mat()) const; 48 | void computeImpl(const cv::Mat &image, std::vector &keypoints, cv::Mat &descriptors) const; 49 | 50 | mutable cv::gpu::SURF_GPU m_surf_gpu; // cv::SURF_GPU needs to change its state during feature extraction 51 | mutable cv::gpu::GpuMat m_image_gpu, m_mask_gpu; // we need to upload the image onto the device before actual computation 52 | }; 53 | 54 | typedef GPUSurf GPUSurfFeatureDetector; 55 | typedef GPUSurf GPUSurfDescriptorExtractor; 56 | } 57 | 58 | #endif // SLAM_DUNK_GPU_FEATURES_H 59 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_dimensions/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 1 1000000 5000.000000 10.000000 0.674344 0.048984 381336.000000 2 | kdskip 2 1000000 5000.000000 10.000000 0.674344 0.048984 381336.000000 3 | kdskip 3 1000000 5000.000000 10.000000 0.648449 0.178386 667528.000000 4 | kdskip 4 1000000 5000.000000 10.000000 0.802086 0.218102 1030156.000000 5 | kdskip 5 1000000 5000.000000 10.000000 1.097474 0.248795 1410780.000000 6 | kdskip 6 1000000 5000.000000 10.000000 1.439023 0.292872 1793048.000000 7 | kdskip 7 1000000 5000.000000 10.000000 1.439023 0.292872 1793048.000000 8 | kdskip 8 1000000 5000.000000 10.000000 2.095330 0.343382 2644444.000000 9 | kdskip 9 1000000 5000.000000 10.000000 2.453285 0.371234 3037140.000000 10 | kdskip 10 1000000 5000.000000 10.000000 2.779600 0.405413 3428652.000000 11 | kdskip 11 1000000 5000.000000 10.000000 3.116769 0.446933 3870876.000000 12 | kdskip 12 1000000 5000.000000 10.000000 3.116769 0.446933 3870876.000000 13 | kdskip 13 1000000 5000.000000 10.000000 3.827201 0.514513 4652716.000000 14 | kdskip 14 1000000 5000.000000 10.000000 4.127972 0.529756 5046532.000000 15 | kdskip 15 1000000 5000.000000 10.000000 4.520226 0.555738 5509820.000000 16 | kdskip 16 1000000 5000.000000 10.000000 4.866411 0.607717 5900076.000000 17 | kdskip 17 1000000 5000.000000 10.000000 5.201311 0.626816 6292120.000000 18 | kdskip 18 1000000 5000.000000 10.000000 5.549316 0.665598 6682628.000000 19 | kdskip 19 1000000 5000.000000 10.000000 5.929376 0.707439 7121148.000000 20 | kdskip 20 1000000 5000.000000 10.000000 6.202403 0.725731 7513032.000000 21 | kdskip 21 1000000 5000.000000 10.000000 6.546089 0.766790 7902424.000000 22 | kdskip 22 1000000 5000.000000 10.000000 6.938184 0.803397 8295404.000000 23 | kdskip 23 1000000 5000.000000 10.000000 6.938184 0.803397 8295404.000000 24 | kdskip 24 1000000 5000.000000 10.000000 7.597998 0.849797 9138388.000000 25 | kdskip 25 1000000 5000.000000 10.000000 7.968272 0.892223 9527388.000000 26 | kdskip 26 1000000 5000.000000 10.000000 8.250544 0.909905 9919896.000000 27 | kdskip 27 1000000 5000.000000 10.000000 8.576606 0.937918 10382252.000000 28 | kdskip 28 1000000 5000.000000 10.000000 8.941732 0.991277 10773444.000000 29 | kdskip 29 1000000 5000.000000 10.000000 9.310804 1.024069 11164484.000000 30 | kdskip 30 1000000 5000.000000 10.000000 9.704137 1.057759 11553432.000000 31 | kdskip 31 1000000 5000.000000 10.000000 9.704137 1.057759 11553432.000000 32 | kdskip 32 1000000 5000.000000 10.000000 10.410001 1.156654 12378372.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_dimensions/kdskip_r32.csv: -------------------------------------------------------------------------------- 1 | kdskip 1 1000000 5000.000000 32.000000 1.447217 0.023413 355004.000000 2 | kdskip 2 1000000 5000.000000 32.000000 1.447217 0.023413 355004.000000 3 | kdskip 3 1000000 5000.000000 32.000000 0.596022 0.147722 550032.000000 4 | kdskip 4 1000000 5000.000000 32.000000 0.702250 0.198724 903644.000000 5 | kdskip 5 1000000 5000.000000 32.000000 1.013412 0.234716 1276008.000000 6 | kdskip 6 1000000 5000.000000 32.000000 1.338032 0.251149 1662172.000000 7 | kdskip 7 1000000 5000.000000 32.000000 1.646094 0.301473 2119988.000000 8 | kdskip 8 1000000 5000.000000 32.000000 1.993549 0.322553 2512464.000000 9 | kdskip 9 1000000 5000.000000 32.000000 2.333649 0.355499 2903852.000000 10 | kdskip 10 1000000 5000.000000 32.000000 2.692977 0.383272 3295484.000000 11 | kdskip 11 1000000 5000.000000 32.000000 3.030865 0.422675 3740868.000000 12 | kdskip 12 1000000 5000.000000 32.000000 3.330562 0.457584 4130468.000000 13 | kdskip 13 1000000 5000.000000 32.000000 3.695394 0.482436 4521232.000000 14 | kdskip 14 1000000 5000.000000 32.000000 4.035857 0.535102 4912068.000000 15 | kdskip 15 1000000 5000.000000 32.000000 4.035857 0.535102 4912068.000000 16 | kdskip 16 1000000 5000.000000 32.000000 4.695685 0.593909 5769512.000000 17 | kdskip 17 1000000 5000.000000 32.000000 5.043141 0.624753 6160932.000000 18 | kdskip 18 1000000 5000.000000 32.000000 5.395551 0.650483 6551100.000000 19 | kdskip 19 1000000 5000.000000 32.000000 5.772160 0.699420 6989360.000000 20 | kdskip 20 1000000 5000.000000 32.000000 6.091953 0.742820 7380752.000000 21 | kdskip 21 1000000 5000.000000 32.000000 6.614057 0.754936 7773284.000000 22 | kdskip 22 1000000 5000.000000 32.000000 6.737995 0.819610 8164240.000000 23 | kdskip 23 1000000 5000.000000 32.000000 7.090939 0.814002 8614660.000000 24 | kdskip 24 1000000 5000.000000 32.000000 7.440571 0.861760 9006744.000000 25 | kdskip 25 1000000 5000.000000 32.000000 7.779695 0.892008 9397812.000000 26 | kdskip 26 1000000 5000.000000 32.000000 8.098206 0.913954 9786936.000000 27 | kdskip 27 1000000 5000.000000 32.000000 8.423368 0.962186 10249636.000000 28 | kdskip 28 1000000 5000.000000 32.000000 8.764888 0.980215 10640868.000000 29 | kdskip 29 1000000 5000.000000 32.000000 9.169572 1.020895 11031444.000000 30 | kdskip 30 1000000 5000.000000 32.000000 9.486545 1.047982 11422720.000000 31 | kdskip 31 1000000 5000.000000 32.000000 9.846071 1.104355 11867968.000000 32 | kdskip 32 1000000 5000.000000 32.000000 10.221925 1.148831 12257568.000000 -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_dimensions/kdskip_dim2.csv: -------------------------------------------------------------------------------- 1 | kdskip 1 1000000 5000.000000 4.000000 0.697775 0.125374 530220.000000 2 | kdskip 2 1000000 5000.000000 4.000000 0.697775 0.125374 530220.000000 3 | kdskip 3 1000000 5000.000000 9.000000 0.680560 0.180693 683368.000000 4 | kdskip 4 1000000 5000.000000 16.000000 0.774712 0.214544 973404.000000 5 | kdskip 5 1000000 5000.000000 25.000000 1.018543 0.236273 1308892.000000 6 | kdskip 6 1000000 5000.000000 36.000000 1.294357 0.247673 1641496.000000 7 | kdskip 7 1000000 5000.000000 49.000000 1.582677 0.295864 2022220.000000 8 | kdskip 8 1000000 5000.000000 64.000000 1.830637 0.320213 2319600.000000 9 | kdskip 9 1000000 5000.000000 81.000000 2.084006 0.351131 2625728.000000 10 | kdskip 10 1000000 5000.000000 100.000000 2.369433 0.393254 2961316.000000 11 | kdskip 11 1000000 5000.000000 121.000000 2.654375 0.420219 3355016.000000 12 | kdskip 12 1000000 5000.000000 144.000000 2.931303 0.453290 3693100.000000 13 | kdskip 13 1000000 5000.000000 169.000000 3.149653 0.458285 3996056.000000 14 | kdskip 14 1000000 5000.000000 196.000000 3.400766 0.502937 4306952.000000 15 | kdskip 15 1000000 5000.000000 225.000000 3.674220 0.530242 4716800.000000 16 | kdskip 16 1000000 5000.000000 256.000000 3.989024 0.554386 5024088.000000 17 | kdskip 17 1000000 5000.000000 289.000000 4.149026 0.614230 5352828.000000 18 | kdskip 18 1000000 5000.000000 324.000000 4.386562 0.552536 5668984.000000 19 | kdskip 19 1000000 5000.000000 361.000000 4.621640 0.589713 6009448.000000 20 | kdskip 20 1000000 5000.000000 400.000000 4.836186 0.727907 6349464.000000 21 | kdskip 21 1000000 5000.000000 441.000000 5.047706 0.748532 6655412.000000 22 | kdskip 22 1000000 5000.000000 484.000000 5.241879 0.869755 6977936.000000 23 | kdskip 23 1000000 5000.000000 529.000000 5.459003 0.842341 7316388.000000 24 | kdskip 24 1000000 5000.000000 576.000000 5.620885 0.943085 7640936.000000 25 | kdskip 25 1000000 5000.000000 625.000000 5.717347 0.778699 7892008.000000 26 | kdskip 26 1000000 5000.000000 676.000000 5.988459 0.755794 8221876.000000 27 | kdskip 27 1000000 5000.000000 729.000000 6.174693 0.872088 8579468.000000 28 | kdskip 28 1000000 5000.000000 784.000000 6.338548 0.940821 8915220.000000 29 | kdskip 29 1000000 5000.000000 841.000000 6.344337 0.804551 9050992.000000 30 | kdskip 30 1000000 5000.000000 900.000000 6.586970 0.947081 9438672.000000 31 | kdskip 31 1000000 5000.000000 961.000000 6.724866 1.153238 9626052.000000 32 | kdskip 32 1000000 5000.000000 1024.000000 6.773105 1.276301 9727748.000000 -------------------------------------------------------------------------------- /include/skimap/utils/CameraUtils.hpp: -------------------------------------------------------------------------------- 1 | //OPENCV 2 | #include 3 | 4 | using namespace cv; 5 | 6 | namespace skimap 7 | { 8 | namespace utils 9 | { 10 | 11 | /** 12 | */ 13 | struct PointRGB 14 | { 15 | cv::Point3f point; 16 | cv::Vec3b color; 17 | float w; 18 | }; 19 | 20 | /** 21 | */ 22 | struct Camera 23 | { 24 | double fx, fy, cx, cy; 25 | int width, height; 26 | double min_distance; 27 | double max_distance; 28 | int point_cloud_downscale; 29 | cv::Mat camera_matrix; 30 | 31 | void build3DTensor(cv::Mat &rgb, cv::Mat &depth, cv::Mat &tensor, int downsample_factor = 1) 32 | { 33 | downsample_factor = downsample_factor > 1 ? downsample_factor : 1; 34 | tensor = cv::Mat(rgb.rows, rgb.cols, CV_32FC3); 35 | 36 | for (int y = 0; y < depth.rows; y += downsample_factor) 37 | { 38 | for (int x = 0; x < depth.cols; x += downsample_factor) 39 | { 40 | float d = depth.at(y, x); 41 | cv::Vec3f p3d; 42 | p3d[0] = (d / this->fx) * (x - this->cx); 43 | p3d[1] = (d / this->fy) * (y - this->cy); 44 | p3d[2] = d; 45 | //printf("(%f,%f,%f,%f) %d,%d,%f = %f,%f,%f\n", this->fx, this->fy, this->cx, this->cy, y, x, d, p3d[0], p3d[1], p3d[2]); 46 | if (p3d[2] != p3d[2]) 47 | p3d[2] = 0.0f; 48 | if (p3d[2] < this->min_distance && p3d[2] > this->max_distance) 49 | p3d[2] = 0.0f; 50 | 51 | tensor.at(y, x) = p3d; 52 | } 53 | } 54 | } 55 | 56 | /** 57 | * Load Camera Object from YAML file 58 | */ 59 | static Camera loadFromFile(std::string yaml_file) 60 | { 61 | Camera camera; 62 | FileStorage fs(yaml_file, FileStorage::READ); 63 | camera.width = fs["image_width"]; 64 | camera.height = fs["image_height"]; 65 | fs["camera_matrix"] >> camera.camera_matrix; 66 | std::cout << camera.camera_matrix.type() << std::endl; 67 | camera.fx = camera.camera_matrix.at(0, 0); 68 | camera.fy = camera.camera_matrix.at(1, 1); 69 | camera.cx = camera.camera_matrix.at(0, 2); 70 | camera.cy = camera.camera_matrix.at(1, 2); 71 | camera.min_distance = fs["min_distance"]; 72 | camera.max_distance = fs["max_distance"]; 73 | return camera; 74 | } 75 | 76 | } camera; 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Mac", 5 | "includePath": [ 6 | "/usr/include", 7 | "/usr/local/include", 8 | "/opt/ros/indigo/include", 9 | "../devel/include", 10 | "${workspaceRoot}" 11 | ], 12 | "browse": { 13 | "limitSymbolsToIncludedHeaders": true, 14 | "databaseFilename": "", 15 | "path": [ 16 | "/usr/include", 17 | "/usr/local/include", 18 | "/opt/ros/indigo/include", 19 | "../devel/include", 20 | "${workspaceRoot}" 21 | ] 22 | }, 23 | "intelliSenseMode": "clang-x64", 24 | "macFrameworkPath": [ 25 | "/System/Library/Frameworks", 26 | "/Library/Frameworks" 27 | ] 28 | }, 29 | { 30 | "name": "Linux", 31 | "includePath": [ 32 | "/usr/include", 33 | "/usr/local/include", 34 | "/opt/ros/indigo/include", 35 | "../devel/include", 36 | "${workspaceRoot}" 37 | ], 38 | "browse": { 39 | "limitSymbolsToIncludedHeaders": true, 40 | "databaseFilename": "", 41 | "path": [ 42 | "/usr/include", 43 | "/usr/local/include", 44 | "/opt/ros/indigo/include", 45 | "../devel/include", 46 | "${workspaceRoot}" 47 | ] 48 | }, 49 | "intelliSenseMode": "clang-x64", 50 | "compilerPath": "/usr/bin/gcc", 51 | "cStandard": "c11", 52 | "cppStandard": "c++17" 53 | }, 54 | { 55 | "name": "Win32", 56 | "includePath": [ 57 | "C:/Program Files (x86)/Microsoft Visual Studio 14.0/VC/include/*", 58 | "${workspaceRoot}" 59 | ], 60 | "browse": { 61 | "limitSymbolsToIncludedHeaders": true, 62 | "databaseFilename": "", 63 | "path": [ 64 | "C:/Program Files (x86)/Microsoft Visual Studio 14.0/VC/include/*", 65 | "${workspaceRoot}" 66 | ] 67 | }, 68 | "intelliSenseMode": "msvc-x64" 69 | } 70 | ], 71 | "version": 3 72 | } -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/bench_kd.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | from subprocess import check_output 3 | import sys 4 | import numpy as np 5 | proc = './../../../../../../devel/lib/skimap_ros/kd_test' 6 | 7 | is_debug = False 8 | current_arg_index = 1 9 | 10 | 11 | def arg(dtype=float): 12 | global current_arg_index 13 | v = dtype(sys.argv[current_arg_index]) 14 | current_arg_index += 1 15 | return v 16 | 17 | 18 | bench_type = arg(str) 19 | 20 | if bench_type == 'points': 21 | 22 | N_DIM = arg(int) 23 | N_POINTS_LIST = np.arange(arg(float), arg(float), arg(float)) 24 | MAX_COORDINATES = arg(float) 25 | RESOLUTION = arg(float) 26 | RADIUS = arg(float) 27 | ALGO = arg(str) 28 | 29 | for points in N_POINTS_LIST: 30 | command = proc + " {} {} {} {} {} {} {}".format( 31 | N_DIM, 32 | points, 33 | MAX_COORDINATES, 34 | RESOLUTION, 35 | RADIUS, 36 | ALGO, 37 | is_debug 38 | ) 39 | # print command 40 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 41 | out, err = p.communicate() 42 | print out 43 | sys.exit(0) 44 | 45 | if bench_type == 'dimensions': 46 | 47 | N_DIMS = range(arg(int), arg(int), arg(int)) 48 | N_POINTS = arg(float) 49 | MAX_COORDINATES = arg(float) 50 | RESOLUTION = arg(float) 51 | RADIUS = arg(float) 52 | ALGO = arg(str) 53 | 54 | for dim in N_DIMS: 55 | command = proc + " {} {} {} {} {} {} {}".format( 56 | dim, 57 | N_POINTS, 58 | MAX_COORDINATES, 59 | RESOLUTION, 60 | RADIUS, 61 | ALGO, 62 | is_debug 63 | ) 64 | # print command 65 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 66 | out, err = p.communicate() 67 | print out 68 | sys.exit(0) 69 | 70 | # step = 2000 71 | # coords = 20000 72 | # coords_max = coords * coords 73 | 74 | # points = [1000, 10000, 20000, 50000, 100000, 150000, 75 | # 200000, 300000, 500000, 1000000, 1500000, 2000000, 3000000, 3500000, 5000000, 10000000, 20000000] # , 1000000, 2000000, 5000000, 10000000, 20000000, 25000000] 76 | 77 | 78 | # for i in points: 79 | # n_points = i 80 | # coords = coords 81 | # algo = sys.argv[1] 82 | # command = proc + " {} {} {} 0".format(n_points, coords, algo) 83 | # p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 84 | # out, err = p.communicate() 85 | # print out 86 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/bench_octree.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | from subprocess import check_output 3 | import sys 4 | import numpy as np 5 | proc = './../../../../../../devel/lib/skimap_ros/octree_test' 6 | 7 | is_debug = False 8 | current_arg_index = 1 9 | 10 | 11 | def arg(dtype=float): 12 | global current_arg_index 13 | v = dtype(sys.argv[current_arg_index]) 14 | current_arg_index += 1 15 | return v 16 | 17 | 18 | bench_type = arg(str) 19 | 20 | if bench_type == 'points': 21 | 22 | N_DIM = arg(int) 23 | N_POINTS_LIST = np.arange(arg(float), arg(float), arg(float)) 24 | MAX_COORDINATES = arg(float) 25 | RESOLUTION = arg(float) 26 | RADIUS = arg(float) 27 | ALGO = arg(str) 28 | 29 | for points in N_POINTS_LIST: 30 | command = proc + " {} {} {} {} {} {} {}".format( 31 | N_DIM, 32 | points, 33 | MAX_COORDINATES, 34 | RESOLUTION, 35 | RADIUS, 36 | ALGO, 37 | is_debug 38 | ) 39 | # print command 40 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 41 | out, err = p.communicate() 42 | print out 43 | sys.exit(0) 44 | 45 | if bench_type == 'dimensions': 46 | 47 | N_DIMS = range(arg(int), arg(int), arg(int)) 48 | N_POINTS = arg(float) 49 | MAX_COORDINATES = arg(float) 50 | RESOLUTION = arg(float) 51 | RADIUS = arg(float) 52 | ALGO = arg(str) 53 | 54 | for dim in N_DIMS: 55 | command = proc + " {} {} {} {} {} {} {}".format( 56 | dim, 57 | N_POINTS, 58 | MAX_COORDINATES, 59 | RESOLUTION, 60 | RADIUS, 61 | ALGO, 62 | is_debug 63 | ) 64 | # print command 65 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 66 | out, err = p.communicate() 67 | print out 68 | sys.exit(0) 69 | 70 | # step = 2000 71 | # coords = 20000 72 | # coords_max = coords * coords 73 | 74 | # points = [1000, 10000, 20000, 50000, 100000, 150000, 75 | # 200000, 300000, 500000, 1000000, 1500000, 2000000, 3000000, 3500000, 5000000, 10000000, 20000000] # , 1000000, 2000000, 5000000, 10000000, 20000000, 25000000] 76 | 77 | 78 | # for i in points: 79 | # n_points = i 80 | # coords = coords 81 | # algo = sys.argv[1] 82 | # command = proc + " {} {} {} 0".format(n_points, coords, algo) 83 | # p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 84 | # out, err = p.communicate() 85 | # print out 86 | -------------------------------------------------------------------------------- /cmake/Modules/FindFLANN.cmake: -------------------------------------------------------------------------------- 1 | ## Adapted from PCL Library pointclouds.org 2 | 3 | ############################################################################### 4 | # Find FLANN 5 | # 6 | # This sets the following variables: 7 | # FLANN_FOUND - True if FLANN was found. 8 | # FLANN_INCLUDE_DIRS - Directories containing the FLANN include files. 9 | # FLANN_LIBRARIES - Library needed to use FLANN. 10 | # If FLANN_USE_STATIC is specified and then look for static libraries ONLY else 11 | # look for shared ones 12 | 13 | if(FLANN_USE_STATIC) 14 | set(FLANN_RELEASE_NAME flann_cpp_s) 15 | set(FLANN_DEBUG_NAME flann_cpp_s-gd) 16 | else(FLANN_USE_STATIC) 17 | set(FLANN_RELEASE_NAME flann_cpp) 18 | set(FLANN_DEBUG_NAME flann_cpp-gd) 19 | endif(FLANN_USE_STATIC) 20 | 21 | find_package(PkgConfig QUIET) 22 | if (FLANN_FIND_VERSION) 23 | pkg_check_modules(PC_FLANN flann>=${FLANN_FIND_VERSION}) 24 | else(FLANN_FIND_VERSION) 25 | pkg_check_modules(PC_FLANN flann) 26 | endif(FLANN_FIND_VERSION) 27 | 28 | find_path(FLANN_INCLUDE_DIR flann/flann.hpp 29 | HINTS ${PC_FLANN_INCLUDEDIR} ${PC_FLANN_INCLUDE_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" 30 | PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" ${FLANN_ROOT}/src/cpp 31 | PATH_SUFFIXES include) 32 | 33 | find_library(FLANN_LIBRARY_RELEASE 34 | NAMES ${FLANN_RELEASE_NAME} 35 | HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" 36 | PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" ${FLANN_ROOT}/build/lib 37 | PATH_SUFFIXES lib) 38 | 39 | find_library(FLANN_LIBRARY_DEBUG 40 | NAMES ${FLANN_DEBUG_NAME} ${FLANN_RELEASE_NAME} 41 | HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" 42 | PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" ${FLANN_ROOT}/build/lib 43 | PATH_SUFFIXES lib) 44 | 45 | if(NOT FLANN_LIBRARY_DEBUG) 46 | set(FLANN_LIBRARY_DEBUG ${FLANN_LIBRARY_RELEASE}) 47 | endif(NOT FLANN_LIBRARY_DEBUG) 48 | 49 | set(FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR}) 50 | 51 | set(FLANN_LIBRARIES 52 | debug ${FLANN_LIBRARY_DEBUG} 53 | optimized ${FLANN_LIBRARY_RELEASE} 54 | ) 55 | 56 | mark_as_advanced(FLANN_LIBRARY_DEBUG FLANN_LIBRARY_RELEASE FLANN_INCLUDE_DIR) 57 | 58 | include(FindPackageHandleStandardArgs) 59 | find_package_handle_standard_args(FLANN DEFAULT_MSG FLANN_LIBRARIES FLANN_INCLUDE_DIRS) 60 | 61 | IF(FLANN_FOUND) 62 | if(FLANN_USE_STATIC) 63 | add_definitions(-DFLANN_STATIC) 64 | endif(FLANN_USE_STATIC) 65 | ENDIF(FLANN_FOUND) 66 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/bench_dimensions/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 1 1000000 5000.000000 32.000000 0.445507 0.212905 448260.000000 2 | flann_kdtree 2 1000000 5000.000000 32.000000 0.525802 0.213801 446860.000000 3 | flann_kdtree 3 1000000 5000.000000 32.000000 0.639976 0.204063 449712.000000 4 | flann_kdtree 4 1000000 5000.000000 32.000000 0.728633 0.183303 452780.000000 5 | flann_kdtree 5 1000000 5000.000000 32.000000 0.836383 0.159822 452816.000000 6 | flann_kdtree 6 1000000 5000.000000 32.000000 0.951540 0.131491 454604.000000 7 | flann_kdtree 7 1000000 5000.000000 32.000000 1.045058 0.122901 471524.000000 8 | flann_kdtree 8 1000000 5000.000000 32.000000 1.129223 0.131423 474904.000000 9 | flann_kdtree 9 1000000 5000.000000 32.000000 1.281974 0.161955 477556.000000 10 | flann_kdtree 10 1000000 5000.000000 32.000000 1.334943 0.189367 481624.000000 11 | flann_kdtree 11 1000000 5000.000000 32.000000 1.463101 0.235368 501964.000000 12 | flann_kdtree 12 1000000 5000.000000 32.000000 1.519052 0.293379 504992.000000 13 | flann_kdtree 13 1000000 5000.000000 32.000000 1.605728 0.356802 509052.000000 14 | flann_kdtree 14 1000000 5000.000000 32.000000 1.703202 0.417953 513184.000000 15 | flann_kdtree 15 1000000 5000.000000 32.000000 1.781646 0.468698 533408.000000 16 | flann_kdtree 16 1000000 5000.000000 32.000000 1.875717 0.520968 536472.000000 17 | flann_kdtree 17 1000000 5000.000000 32.000000 1.956297 0.542415 540608.000000 18 | flann_kdtree 18 1000000 5000.000000 32.000000 2.041681 0.569775 543664.000000 19 | flann_kdtree 19 1000000 5000.000000 32.000000 2.145980 0.586077 563956.000000 20 | flann_kdtree 20 1000000 5000.000000 32.000000 2.212651 0.593429 567988.000000 21 | flann_kdtree 21 1000000 5000.000000 32.000000 2.301197 0.601095 571364.000000 22 | flann_kdtree 22 1000000 5000.000000 32.000000 2.387436 0.603508 575228.000000 23 | flann_kdtree 23 1000000 5000.000000 32.000000 2.460787 0.604609 595112.000000 24 | flann_kdtree 24 1000000 5000.000000 32.000000 2.535145 0.603879 598556.000000 25 | flann_kdtree 25 1000000 5000.000000 32.000000 2.619184 0.601310 602884.000000 26 | flann_kdtree 26 1000000 5000.000000 32.000000 2.713015 0.602951 607560.000000 27 | flann_kdtree 27 1000000 5000.000000 32.000000 2.794393 0.616333 626420.000000 28 | flann_kdtree 28 1000000 5000.000000 32.000000 2.872233 0.604128 631128.000000 29 | flann_kdtree 29 1000000 5000.000000 32.000000 2.966379 0.613092 633832.000000 30 | flann_kdtree 30 1000000 5000.000000 32.000000 3.129583 0.630077 638136.000000 31 | flann_kdtree 31 1000000 5000.000000 32.000000 3.126900 0.605551 657996.000000 32 | flann_kdtree 32 1000000 5000.000000 32.000000 3.231004 0.611787 661304.000000 -------------------------------------------------------------------------------- /include/skimap/voxels/GenericVoxel3D.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef GENERICVOXEL3D_HPP 10 | #define GENERICVOXEL3D_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace skimap 21 | { 22 | 23 | /** 24 | * Generic 3D Voxel representation. 25 | * V template represents datatype for user data. 26 | * D template represents datatype for coordinates. 27 | */ 28 | template 29 | struct GenericVoxel3D 30 | { 31 | D x, y, z; 32 | V *data; 33 | 34 | /** 35 | * Void constructor. Sets NULL for user data. 36 | */ 37 | GenericVoxel3D() : x(D(0)), y(D(0)), z(D(0)), data(NULL) 38 | { 39 | } 40 | 41 | /** 42 | * Full Constructor 43 | * @param x X coordinate 44 | * @param y Y coordinate 45 | * @param z Z coordinate 46 | * @param data User Data pointer 47 | */ 48 | GenericVoxel3D(D x, D y, D z, V *data) : x(x), y(y), z(z), data(data) 49 | { 50 | } 51 | 52 | /** 53 | * Full Constructor 54 | * @param x X coordinate 55 | * @param y Y coordinate 56 | * @param z Z coordinate 57 | * @param data User Data pointer 58 | */ 59 | GenericVoxel3D(D x, D y, V *data) : x(x), y(y), z(0.0), data(data) 60 | { 61 | } 62 | 63 | /** 64 | * Serializes object into stream. 65 | */ 66 | friend std::ostream &operator<<(std::ostream &os, const GenericVoxel3D &voxel) 67 | { 68 | os << std::setprecision(std::numeric_limits::digits10 + 2); 69 | os << voxel.x << " "; 70 | os << voxel.y << " "; 71 | os << voxel.z << " "; 72 | if (voxel.data != NULL) 73 | os << *(voxel.data); 74 | return os; 75 | } 76 | 77 | /** 78 | * Hydrates object from stream. 79 | */ 80 | friend std::istream &operator>>(std::istream &is, GenericVoxel3D &voxel) 81 | { 82 | double x, y, z; 83 | is >> x; 84 | is >> y; 85 | is >> z; 86 | voxel.x = D(x); 87 | voxel.y = D(y); 88 | voxel.z = D(z); 89 | voxel.data = new V(); 90 | is >> (*voxel.data); 91 | return is; 92 | } 93 | }; 94 | } 95 | 96 | #endif /* GENERICVOXEL3D_HPP */ 97 | -------------------------------------------------------------------------------- /include/skimap/voxels/VoxelDataOccupancy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef VOXELDATAOCCUPANCY_HPP 10 | #define VOXELDATAOCCUPANCY_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace skimap 19 | { 20 | 21 | /** 22 | * Voxel Data containing RGB color values and Weight. 23 | * C template represents datatype for color component. 24 | * W template represents datatype for weight. 25 | */ 26 | template 27 | struct VoxelDataOccupancy 28 | { 29 | W w; 30 | 31 | /** 32 | * Pointer Copy Constructor. 33 | * @param data source data 34 | */ 35 | VoxelDataOccupancy(VoxelDataOccupancy *data) 36 | { 37 | w = data->w; 38 | } 39 | 40 | /** 41 | * Void constructor. 42 | */ 43 | VoxelDataOccupancy() 44 | { 45 | w = W(0); 46 | } 47 | 48 | /** 49 | * Full constructor 50 | * @param w Weight value 51 | */ 52 | VoxelDataOccupancy(W w) 53 | { 54 | this->w = w; 55 | } 56 | 57 | /** 58 | * Sum Overload. Defines Sum operations between Voxels 59 | * @param v2 second addend 60 | * @return sum 61 | */ 62 | VoxelDataOccupancy operator+(const VoxelDataOccupancy &v2) const 63 | { 64 | VoxelDataOccupancy c1 = *this; 65 | if (c1.w == 0) 66 | return v2; 67 | VoxelDataOccupancy d; 68 | d.w = (c1.w + v2.w); 69 | return d; 70 | } 71 | 72 | /** 73 | * Subtraction Overload. Defines Subtraction operations between Voxels 74 | * @param v2 second minuend 75 | * @return subtraction 76 | */ 77 | VoxelDataOccupancy operator-(const VoxelDataOccupancy &v2) const 78 | { 79 | VoxelDataOccupancy v1 = *this; 80 | if (v1.w == 0) 81 | return v1; 82 | VoxelDataOccupancy d; 83 | d.w = (v1.w - v2.w); 84 | return d; 85 | } 86 | 87 | /** 88 | * Serializes object into stream. 89 | */ 90 | friend std::ostream &operator<<(std::ostream &os, const VoxelDataOccupancy &voxel) 91 | { 92 | os << std::setprecision(std::numeric_limits::digits10 + 2); 93 | os << double(voxel.w); 94 | return os; 95 | } 96 | 97 | /** 98 | * Hydrates object from stream. 99 | */ 100 | friend std::istream &operator>>(std::istream &is, VoxelDataOccupancy &voxel) 101 | { 102 | double w; 103 | is >> w; 104 | voxel.w = w; 105 | return is; 106 | } 107 | }; 108 | } 109 | 110 | #endif /* VOXELDATAOCCUPANCY_HPP */ 111 | -------------------------------------------------------------------------------- /include/slamdunk_extension/Utility.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef UTILITY_H 10 | #define UTILITY_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "geometry_msgs/Pose.h" 25 | 26 | //OPENCV 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include "opencv2/core/types_c.h" 32 | #include "opencv2/core/version.hpp" 33 | 34 | //PCL 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | //tf 64 | #include 65 | #include 66 | #include 67 | 68 | class Utility 69 | { 70 | public: 71 | Utility(); 72 | Utility(const Utility &orig); 73 | virtual ~Utility(); 74 | 75 | static void kdl_to_eigen_4x4_d(KDL::Frame &frame, Eigen::Matrix4d &mat); 76 | static void create_kdl_frame(float x, float y, float z, float roll, float pitch, float yaw, KDL::Frame &out_frame); 77 | static void create_eigen_4x4_d(float x, float y, float z, float roll, float pitch, float yaw, Eigen::Matrix4d &mat); 78 | static void eigen_4x4_to_geometrypose_d(Eigen::Matrix4d &mat, geometry_msgs::Pose &pose); 79 | static void eigen_4x4_d_to_tf(Eigen::Matrix4d &t, tf::Transform &tf, bool reverse); 80 | static void HSVtoRGB(double &r, double &g, double &b, double h, double s, double v); 81 | 82 | private: 83 | }; 84 | 85 | #endif /* UTILITY_H */ 86 | -------------------------------------------------------------------------------- /src/slamdunk/gpu_features.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include "slamdunk/gpu_features.h" 10 | #include 11 | #include 12 | 13 | ///////////////////////////////////////////////////////////////////////////////////////////////// 14 | slamdunk::GPUSurf::GPUSurf(double hessianThreshold, int nOctaves, int nOctaveLayers, bool extended, float keypointsRatio, bool upright) 15 | : m_surf_gpu(hessianThreshold, nOctaves, nOctaveLayers, extended, keypointsRatio, upright) 16 | { 17 | } 18 | 19 | ///////////////////////////////////////////////////////////////////////////////////////////////// 20 | void slamdunk::GPUSurf::operator()(cv::InputArray _image, cv::InputArray _mask, 21 | cv::vector &keypoints) const 22 | { 23 | (*this)(_image, _mask, keypoints, cv::noArray()); 24 | } 25 | 26 | ///////////////////////////////////////////////////////////////////////////////////////////////// 27 | void slamdunk::GPUSurf::operator()(cv::InputArray _image, cv::InputArray _mask, 28 | std::vector &keypoints, 29 | cv::OutputArray _descriptors, 30 | bool useProvidedKeypoints) const 31 | { 32 | cv::Mat image = _image.getMat(), mask = _mask.getMat(); 33 | 34 | if (image.empty() || image.depth() != CV_8U) 35 | CV_Error(CV_StsBadArg, "image is empty or has incorrect depth (!=CV_8U)"); 36 | 37 | if (!mask.empty() && mask.type() != CV_8UC1) 38 | CV_Error(CV_StsBadArg, "mask has incorrect type (!=CV_8UC1)"); 39 | 40 | if (image.channels() == 3) 41 | cv::cvtColor(image, image, CV_BGR2GRAY); 42 | else if (image.channels() == 4) 43 | cv::cvtColor(image, image, CV_BGRA2GRAY); 44 | else 45 | CV_Error(CV_StsBadArg, "image has incorrect no of channels (!=1|3|4)"); 46 | 47 | m_image_gpu.upload(image); 48 | if (!mask.empty()) 49 | m_mask_gpu.upload(mask); 50 | else if (!m_mask_gpu.empty()) 51 | m_mask_gpu = cv::gpu::GpuMat(); 52 | cv::gpu::GpuMat descriptors_gpu; 53 | m_surf_gpu(m_image_gpu, m_mask_gpu, keypoints, descriptors_gpu, useProvidedKeypoints); 54 | 55 | cv::Mat &descriptors = _descriptors.getMatRef(); 56 | descriptors_gpu.download(descriptors); 57 | } 58 | 59 | ///////////////////////////////////////////////////////////////////////////////////////////////// 60 | void slamdunk::GPUSurf::detectImpl(const cv::Mat &image, std::vector &keypoints, const cv::Mat &mask) const 61 | { 62 | (*this)(image, mask, keypoints, cv::noArray()); 63 | } 64 | 65 | ///////////////////////////////////////////////////////////////////////////////////////////////// 66 | void slamdunk::GPUSurf::computeImpl(const cv::Mat &image, std::vector &keypoints, cv::Mat &descriptors) const 67 | { 68 | (*this)(image, cv::Mat(), keypoints, descriptors, true); 69 | } 70 | -------------------------------------------------------------------------------- /include/slamdunk/data.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_DATA_H 10 | #define SLAM_DUNK_DATA_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | #include 14 | #ifndef NDEBUG 15 | #include 16 | #endif 17 | #include 18 | 19 | namespace slamdunk 20 | { 21 | 22 | struct SLAM_DUNK_API RGBDFrame 23 | { 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | 26 | double m_timestamp; 27 | int secs; 28 | int nsecs; 29 | 30 | cv::Mat m_color_image; 31 | cv::Mat_ m_depth_image; 32 | cv::Mat_ m_depth_image_mm; 33 | 34 | const static Eigen::Matrix3f m_inverse_kcam; 35 | }; 36 | 37 | RGBDFrame cloneRGBDFrame(const RGBDFrame &frame); 38 | 39 | struct SLAM_DUNK_API FrameToFrameMatch 40 | { 41 | int m_matching_frame_id; 42 | int m_matching_frame_feat, m_ref_frame_feat; 43 | float m_score; 44 | }; 45 | 46 | class SLAM_DUNK_API FrameData : public g2o::OptimizableGraph::Data 47 | { 48 | public: 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | 51 | FrameData() {} 52 | FrameData(const FrameData &fd) 53 | : m_timestamp(fd.m_timestamp), m_descriptors(fd.m_descriptors.clone()), 54 | m_keypoints(fd.m_keypoints), m_samplesPts(fd.m_samplesPts), m_samplesNormals(fd.m_samplesNormals) 55 | #ifndef NDEBUG 56 | , 57 | m_kpts2d(fd.m_kpts2d), m_image(fd.m_image) 58 | #endif 59 | { 60 | } 61 | 62 | FrameData &operator=(const FrameData &fd) 63 | { 64 | m_timestamp = fd.m_timestamp; 65 | fd.m_descriptors.copyTo(m_descriptors); 66 | m_keypoints = fd.m_keypoints; 67 | m_samplesPts = fd.m_samplesPts; 68 | m_samplesNormals = fd.m_samplesNormals; 69 | ///////////////////////////////////////////////////////// 70 | #ifndef NDEBUG 71 | m_kpts2d = fd.m_kpts2d; 72 | m_image = fd.m_image; 73 | #endif 74 | ///////////////////////////////////////////////////////// 75 | return *this; 76 | } 77 | 78 | virtual bool read(std::istream &is); 79 | virtual bool write(std::ostream &os) const; 80 | 81 | double m_timestamp; 82 | cv::Mat m_descriptors; 83 | std::vector m_keypoints; 84 | Eigen::VectorXf m_samplesPts; 85 | std::vector m_samplesNormals; 86 | ///////////////////////////////////////////////////////// 87 | #ifndef NDEBUG 88 | // 2d kpts are used for debugging purpose only 89 | std::vector m_kpts2d; 90 | cv::Mat m_image; 91 | #endif 92 | ///////////////////////////////////////////////////////// 93 | }; 94 | 95 | SLAM_DUNK_API cv::Mat_ depthBilateralFilter(const cv::Mat_ &input, double sigmaDepth, double sigmaSpace); 96 | 97 | struct SLAM_DUNK_API ObjectData 98 | { 99 | std::string m_tag; 100 | int m_global_id; 101 | std::vector m_keypoints; 102 | }; 103 | } 104 | 105 | #endif // SLAM_DUNK_DATA_H 106 | -------------------------------------------------------------------------------- /include/slamdunk/graph_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_GRAPH_UTILS_H 10 | #define SLAM_DUNK_GRAPH_UTILS_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | #include 14 | #include 15 | #include 16 | 17 | namespace slamdunk 18 | { 19 | 20 | typedef std::map PoseIsometryMap; 21 | 22 | /** 23 | * \brief stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 24 | * 25 | * If the gain is larger than zero and below the threshold, then the optimizer is stopped. 26 | * Typically usage of this action includes adding it as a postIteration action, by calling 27 | * addPostIterationAction on a sparse optimizer. 28 | */ 29 | class SLAM_DUNK_API ConvergenceCheckAction : public g2o::HyperGraphAction 30 | { 31 | public: 32 | ConvergenceCheckAction(g2o::SparseOptimizer *opt, double error_th = 1e-8) 33 | : m_optimizer(opt), m_gain_th(error_th), m_last_chi2(0.), m_stop_flag(false) 34 | { 35 | } 36 | 37 | void reset(g2o::SparseOptimizer *opt = NULL, double error_th = -1.) 38 | { 39 | if (opt != NULL) 40 | m_optimizer = opt; 41 | if (error_th >= 0) 42 | m_gain_th = error_th; 43 | m_last_chi2 = 0.; 44 | m_stop_flag = false; 45 | } 46 | 47 | #ifdef NDEBUG 48 | virtual g2o::HyperGraphAction *operator()(const g2o::HyperGraph *, g2o::HyperGraphAction::Parameters * = 0) 49 | #else 50 | virtual g2o::HyperGraphAction *operator()(const g2o::HyperGraph *graph, g2o::HyperGraphAction::Parameters * = 0) 51 | #endif 52 | { 53 | assert(graph == m_optimizer); 54 | 55 | m_optimizer->computeActiveErrors(); 56 | const double current_chi2 = m_optimizer->activeChi2(); 57 | const double gain = m_last_chi2 / current_chi2 - 1.0; 58 | if ((gain >= 0 && gain < m_gain_th) || current_chi2 == 0.0) 59 | { 60 | // tell the optimizer to stop 61 | if (m_optimizer->forceStopFlag() != NULL) 62 | *(m_optimizer->forceStopFlag()) = true; 63 | else 64 | { 65 | m_stop_flag = true; 66 | m_optimizer->setForceStopFlag(&m_stop_flag); 67 | } 68 | } 69 | m_last_chi2 = current_chi2; 70 | return this; 71 | } 72 | 73 | private: 74 | g2o::SparseOptimizer *m_optimizer; 75 | double m_gain_th, m_last_chi2; 76 | bool m_stop_flag; 77 | }; 78 | 79 | class GraphBackend; 80 | // The root is yellow, the colored orange if the id != root_id, 81 | // the highlighted are light blue if they are not colored and the id != root_id 82 | SLAM_DUNK_API std::string graphToDot(const GraphBackend *gb, int root_id = 0, 83 | const g2o::HyperGraph::VertexSet &colored = g2o::HyperGraph::VertexSet(), 84 | const g2o::HyperGraph::VertexSet &highlighted = g2o::HyperGraph::VertexSet(), 85 | const std::string &graph_name = "mygraph"); 86 | } 87 | 88 | #endif // SLAM_DUNK_GRAPH_UTILS_H 89 | -------------------------------------------------------------------------------- /include/slamdunk/edge_se3_xyzpair.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_EDGE_SE3_XYZPAIR_H 10 | #define SLAM_DUNK_EDGE_SE3_XYZPAIR_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | #include 14 | #include 15 | #include 16 | 17 | namespace slamdunk 18 | { 19 | 20 | /** col-0 = point-0; col-1 = point-1 */ 21 | typedef Eigen::Matrix XYZPairMeasurement; 22 | 23 | class SLAM_DUNK_API EdgeSE3XYZPair : public g2o::BaseBinaryEdge<3, XYZPairMeasurement, g2o::VertexSE3, g2o::VertexSE3> 24 | { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | EdgeSE3XYZPair(); 29 | 30 | inline void setWeight(double w) { weight_ = w; } 31 | 32 | inline void setPointPointMetric() 33 | { 34 | plane_plane_metric_ = false; 35 | _information.setIdentity(); 36 | } 37 | 38 | inline void setPointPlaneMetric(const Eigen::Vector3d &normal0, double e = 1e-3) 39 | { 40 | plane_plane_metric_ = false; 41 | Eigen::Matrix3d info; 42 | info << e, 0, 0, 43 | 0, e, 0, 44 | 0, 0, 1; 45 | Eigen::Matrix3d rot0 = computeRotation(normal0); 46 | _information = rot0.transpose() * info * rot0; 47 | } 48 | 49 | inline void setPlanePlaneMetric(const Eigen::Vector3d &normal0, const Eigen::Vector3d &normal1, double e = 1e-3) 50 | { 51 | plane_plane_metric_ = true; 52 | Eigen::Matrix3d info; 53 | info << 1, 0, 0, 54 | 0, 1, 0, 55 | 0, 0, e; 56 | 57 | Eigen::Matrix3d rot = computeRotation(normal0); 58 | cov0_ = rot.transpose() * info * rot; 59 | rot = computeRotation(normal1); 60 | cov1_ = rot.transpose() * info * rot; 61 | } 62 | 63 | double weight() const { return weight_; } 64 | const Eigen::Matrix3d &cov0() const { return cov0_; } 65 | const Eigen::Matrix3d &cov1() const { return cov1_; } 66 | 67 | virtual bool read(std::istream &is); 68 | virtual bool write(std::ostream &os) const; 69 | 70 | virtual void computeError(); 71 | 72 | virtual bool setMeasurementData(const double *d) 73 | { 74 | _measurement = Eigen::Map(d); 75 | return true; 76 | } 77 | 78 | virtual bool getMeasurementData(double *d) const 79 | { 80 | Eigen::Map v(d); 81 | v = _measurement; 82 | return true; 83 | } 84 | 85 | virtual void linearizeOplus(); 86 | 87 | virtual int measurementDimension() const { return 6; } 88 | 89 | private: 90 | inline Eigen::Matrix3d computeRotation(const Eigen::Vector3d &normal) const 91 | { 92 | Eigen::Matrix3d rot; 93 | rot.row(2) = normal; 94 | rot.row(1) = (Eigen::Vector3d::UnitY() - normal(1) * normal).normalized(); 95 | rot.row(0) = normal.cross(rot.row(1)); 96 | return rot; 97 | } 98 | 99 | virtual bool resolveCaches(); 100 | g2o::ParameterSE3Offset *offset0_, *offset1_; 101 | g2o::CacheSE3Offset *cache0_, *cache1_; 102 | 103 | double weight_; 104 | bool plane_plane_metric_; 105 | Eigen::Matrix3d cov0_, cov1_; 106 | 107 | Eigen::Matrix3d jac_buffer_; 108 | }; 109 | } 110 | 111 | #endif // SLAM_DUNK_EDGE_SE3_XYZPAIR_H 112 | -------------------------------------------------------------------------------- /include/skimap/voxels/Voxel6DPose.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef VOXEL6DPOSE_HPP 10 | #define VOXEL6DPOSE_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace skimap 22 | { 23 | 24 | /** 25 | * Voxel Data containing matrix-arranged values 26 | * D template represents datatype for cells. 27 | * COLUMNS template represents fixed columns size. 28 | */ 29 | struct Voxel6DPose 30 | { 31 | std::string name; 32 | std::vector trans; 33 | std::vector rots; 34 | 35 | /** 36 | * Pointer Copy Constructor. 37 | * @param data source data 38 | */ 39 | Voxel6DPose(Voxel6DPose *data) 40 | { 41 | name = data->name; 42 | rots = data->rots; 43 | trans = data->trans; 44 | } 45 | 46 | /** 47 | * Void Constructor. 48 | */ 49 | Voxel6DPose() 50 | { 51 | } 52 | 53 | Voxel6DPose(std::string name) 54 | { 55 | this->name = name; 56 | } 57 | 58 | Voxel6DPose(std::string name, cv::Vec3f trans, cv::Vec4f rot) 59 | { 60 | this->rots.push_back(rot); 61 | this->trans.push_back(trans); 62 | this->name = name; 63 | } 64 | 65 | /** 66 | * Sum Overload. Defines Sum operations between Voxels 67 | * @param v2 second addend 68 | * @return sum 69 | */ 70 | Voxel6DPose operator+(const Voxel6DPose &v2) const 71 | { 72 | Voxel6DPose v1 = *this; 73 | v1.trans.insert(v1.trans.end(), v2.trans.begin(), v2.trans.end()); 74 | v1.rots.insert(v1.rots.end(), v2.rots.begin(), v2.rots.end()); 75 | v1.name = v2.name; 76 | return v1; 77 | } 78 | 79 | /** 80 | * Subtraction Overload. Defines Subtraction operations between Voxels 81 | * @param v2 second minuend 82 | * @return subtraction 83 | */ 84 | Voxel6DPose operator-(const Voxel6DPose &v2) const 85 | { 86 | printf("VOXEL6DOF SUBTRACTION: NOT IMPLEMENTED YET\n"); 87 | return *this; 88 | } 89 | 90 | /** 91 | * Serializes object into stream. 92 | */ 93 | friend std::ostream &operator<<(std::ostream &os, const Voxel6DPose &voxel) 94 | { 95 | 96 | printf("VOXEL6DOF OSTREAM << NOT IMPLEMENTED YET \n"); 97 | return os; 98 | } 99 | 100 | /** 101 | * Hydrates object from stream. 102 | */ 103 | friend std::istream &operator>>(std::istream &is, Voxel6DPose &voxel) 104 | { 105 | 106 | printf("VOXEL6DOF OSTREAM >> NOT IMPLEMENTED YET \n"); 107 | return is; 108 | } 109 | 110 | int size() 111 | { 112 | return int(trans.size()); 113 | } 114 | 115 | void average(cv::Vec3f& t,cv::Vec4f& r){ 116 | t = cv::Vec3f(.0,.0,.0); 117 | r = cv::Vec4f(.0,.0,.0,.0); 118 | 119 | for(int i = 0; i < trans.size(); i++){ 120 | t = t + trans[i]; 121 | r = r + rots[i]; 122 | } 123 | t = t / float(trans.size()); 124 | r = r / float(rots.size()); 125 | } 126 | 127 | }; 128 | } 129 | 130 | #endif /* VOXEL6DPOSE_HPP */ 131 | -------------------------------------------------------------------------------- /include/skimap/voxels/VoxelDataContainer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef VOXELDATACONTAINER_HPP 10 | #define VOXELDATACONTAINER_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace skimap 22 | { 23 | 24 | /** 25 | * Voxel Data with a generic container 26 | * D template represents datatype for container. 27 | * COLUMNS template represents fixed columns size. 28 | */ 29 | template 30 | struct VoxelDataContainer 31 | { 32 | std::vector container; 33 | 34 | /** 35 | * Pointer Copy Constructor. 36 | * @param data source data 37 | */ 38 | VoxelDataContainer(VoxelDataContainer *data) 39 | { 40 | container = data->container; 41 | } 42 | 43 | /** 44 | * Void Constructor. 45 | */ 46 | VoxelDataContainer() 47 | { 48 | } 49 | 50 | /** 51 | * Sum Overload. Defines Sum operations between Voxels 52 | * @param v2 second addend 53 | * @return sum 54 | */ 55 | VoxelDataContainer operator+(const VoxelDataContainer &v2) const 56 | { 57 | VoxelDataContainer v1 = *this; 58 | if (v1.container.size() == 0) 59 | return v2; 60 | 61 | VoxelDataContainer d; 62 | d.container = v1.container; 63 | d.container.insert(d.container.end(), v2.container.begin(), v2.container.end()); 64 | return d; 65 | } 66 | 67 | /** 68 | * Subtraction Overload. Defines Subtraction operations between Voxels 69 | * @param v2 second minuend 70 | * @return subtraction 71 | */ 72 | VoxelDataContainer operator-(const VoxelDataContainer &v2) const 73 | { 74 | VoxelDataContainer v1 = *this; 75 | if (v2.container.size() == 0) 76 | return v1; 77 | VoxelDataContainer d; 78 | d.container = this->container; 79 | printf("VOXELDATACONTAINER OPERATOR- NOT IMPLEMENTED YET!\n"); 80 | return d; 81 | } 82 | 83 | /** 84 | * Serializes object into stream. 85 | */ 86 | friend std::ostream &operator<<(std::ostream &os, const VoxelDataContainer &voxel) 87 | { 88 | 89 | int count = 0; 90 | os << std::setprecision(std::numeric_limits::digits10 + 2); 91 | os << voxel.container.size() << " "; 92 | for (int i = 0; i < voxel.container.size(); i++) 93 | { 94 | os << voxel.container[i]; 95 | if (i < voxel.container.size() - 1) 96 | { 97 | os << " "; 98 | } 99 | } 100 | return os; 101 | } 102 | 103 | /** 104 | * Hydrates object from stream. 105 | */ 106 | friend std::istream &operator>>(std::istream &is, VoxelDataContainer &voxel) 107 | { 108 | 109 | int size; 110 | is >> size; 111 | voxel.container.resize(size); 112 | for (int i = 0; i < size; i++) 113 | { 114 | D el; 115 | is >> el; 116 | voxel.container[i] = el; 117 | } 118 | 119 | return is; 120 | } 121 | }; 122 | } 123 | 124 | #endif /* VOXELDATACONTAINER_HPP */ 125 | -------------------------------------------------------------------------------- /cmake/Modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/matlab/importfile.m: -------------------------------------------------------------------------------- 1 | function grid1 = importfile(filename, startRow, endRow) 2 | %IMPORTFILE1 Import numeric data from a text file as a matrix. 3 | % GRID1 = IMPORTFILE1(FILENAME) Reads data from text file FILENAME for 4 | % the default selection. 5 | % 6 | % GRID1 = IMPORTFILE1(FILENAME, STARTROW, ENDROW) Reads data from rows 7 | % STARTROW through ENDROW of text file FILENAME. 8 | % 9 | % Example: 10 | % grid1 = importfile1('grid.csv', 1, 18); 11 | % 12 | % See also TEXTSCAN. 13 | 14 | % Auto-generated by MATLAB on 2017/11/06 12:21:46 15 | 16 | %% Initialize variables. 17 | delimiter = ' '; 18 | if nargin<=2 19 | startRow = 1; 20 | endRow = inf; 21 | end 22 | 23 | %% Read columns of data as text: 24 | % For more information, see the TEXTSCAN documentation. 25 | formatSpec = '%*s%s%s%s%s%s%s%s%[^\n\r]'; 26 | 27 | %% Open the text file. 28 | fileID = fopen(filename,'r'); 29 | 30 | %% Read columns of data according to the format. 31 | % This call is based on the structure of the file used to generate this 32 | % code. If an error occurs for a different file, try regenerating the code 33 | % from the Import Tool. 34 | dataArray = textscan(fileID, formatSpec, endRow(1)-startRow(1)+1, 'Delimiter', delimiter, 'MultipleDelimsAsOne', true, 'TextType', 'string', 'HeaderLines', startRow(1)-1, 'ReturnOnError', false, 'EndOfLine', '\r\n'); 35 | for block=2:length(startRow) 36 | frewind(fileID); 37 | dataArrayBlock = textscan(fileID, formatSpec, endRow(block)-startRow(block)+1, 'Delimiter', delimiter, 'MultipleDelimsAsOne', true, 'TextType', 'string', 'HeaderLines', startRow(block)-1, 'ReturnOnError', false, 'EndOfLine', '\r\n'); 38 | for col=1:length(dataArray) 39 | dataArray{col} = [dataArray{col};dataArrayBlock{col}]; 40 | end 41 | end 42 | 43 | %% Close the text file. 44 | fclose(fileID); 45 | 46 | %% Convert the contents of columns containing numeric text to numbers. 47 | % Replace non-numeric text with NaN. 48 | raw = repmat({''},length(dataArray{1}),length(dataArray)-1); 49 | for col=1:length(dataArray)-1 50 | raw(1:length(dataArray{col}),col) = mat2cell(dataArray{col}, ones(length(dataArray{col}), 1)); 51 | end 52 | numericData = NaN(size(dataArray{1},1),size(dataArray,2)); 53 | 54 | for col=[1,2,3,4,5,6,7] 55 | % Converts text in the input cell array to numbers. Replaced non-numeric 56 | % text with NaN. 57 | rawData = dataArray{col}; 58 | for row=1:size(rawData, 1) 59 | % Create a regular expression to detect and remove non-numeric prefixes and 60 | % suffixes. 61 | regexstr = '(?.*?)(?([-]*(\d+[\,]*)+[\.]{0,1}\d*[eEdD]{0,1}[-+]*\d*[i]{0,1})|([-]*(\d+[\,]*)*[\.]{1,1}\d+[eEdD]{0,1}[-+]*\d*[i]{0,1}))(?.*)'; 62 | try 63 | result = regexp(rawData(row), regexstr, 'names'); 64 | numbers = result.numbers; 65 | 66 | % Detected commas in non-thousand locations. 67 | invalidThousandsSeparator = false; 68 | if numbers.contains(',') 69 | thousandsRegExp = '^\d+?(\,\d{3})*\.{0,1}\d*$'; 70 | if isempty(regexp(numbers, thousandsRegExp, 'once')) 71 | numbers = NaN; 72 | invalidThousandsSeparator = true; 73 | end 74 | end 75 | % Convert numeric text to numbers. 76 | if ~invalidThousandsSeparator 77 | numbers = textscan(char(strrep(numbers, ',', '')), '%f'); 78 | numericData(row, col) = numbers{1}; 79 | raw{row, col} = numbers{1}; 80 | end 81 | catch 82 | raw{row, col} = rawData{row}; 83 | end 84 | end 85 | end 86 | 87 | 88 | %% Create output variable 89 | grid1 = cell2mat(raw); 90 | -------------------------------------------------------------------------------- /include/slamdunk/transformation_estimation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_TRANSFORMATION_ESTIMATION_H 10 | #define SLAM_DUNK_TRANSFORMATION_ESTIMATION_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace slamdunk 21 | { 22 | 23 | void SLAM_DUNK_API estimateTransformationSVD(const std::vector &reference, 24 | const std::vector &query, 25 | const std::vector> &ref_query_ids, 26 | Eigen::Isometry3d &queryToRef); 27 | 28 | class SLAM_DUNK_API SampleConsensus 29 | { 30 | public: 31 | typedef boost::shared_ptr Ptr; 32 | typedef boost::shared_ptr ConstPtr; 33 | 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | 36 | SampleConsensus(double threshold = 0.05) 37 | : m_max_iterations(1000), m_squared_threshold(threshold * threshold), 38 | m_probability(0.99), m_query2ref(Eigen::Isometry3d::Identity()), m_verbose(false) {} 39 | virtual ~SampleConsensus() {} 40 | 41 | inline void setVerbose(bool verbose) { m_verbose = verbose; } 42 | inline bool isVerbose() const { return m_verbose; } 43 | 44 | inline void setMaxIterations(unsigned max_iterations) { m_max_iterations = max_iterations; } 45 | inline unsigned getMaxIterations() const { return m_max_iterations; } 46 | inline void setDistanceThreshold(double threshold) { m_squared_threshold = threshold * threshold; } 47 | inline double getSquaredDistanceThreshold() const { return m_squared_threshold; } 48 | 49 | /** \brief Set the desired probability of choosing at least one sample free from outliers. */ 50 | inline void setProbability(double probability) 51 | { 52 | if (probability < 1 && probability >= 0) 53 | m_probability = probability; 54 | } 55 | inline double getProbability() const { return m_probability; } 56 | 57 | virtual bool findInliers(const std::vector &reference, 58 | const std::vector &query, 59 | const std::vector> &ref_query_ids) = 0; 60 | 61 | inline const std::vector &getRefQueryInliers() { return m_inlier_indices; } 62 | /** \brief Get the best Query->Reference transformation */ 63 | inline const Eigen::Isometry3d &queryToRef() { return m_query2ref; } 64 | 65 | protected: 66 | unsigned m_max_iterations; 67 | double m_squared_threshold; 68 | double m_probability; 69 | std::vector m_inlier_indices; 70 | Eigen::Isometry3d m_query2ref; 71 | bool m_verbose; 72 | }; 73 | 74 | class SLAM_DUNK_API RANSAC : public SampleConsensus 75 | { 76 | public: 77 | RANSAC(bool deterministic, float threshold = 0.05) 78 | : SampleConsensus(threshold), m_deterministic(deterministic) 79 | { 80 | if (!m_deterministic) 81 | m_rnd_eng.seed(static_cast(std::time(NULL))); 82 | } 83 | 84 | virtual bool findInliers(const std::vector &reference, 85 | const std::vector &query, 86 | const std::vector> &ref_query_ids); 87 | 88 | protected: 89 | boost::mt19937 m_rnd_eng; 90 | bool m_deterministic; 91 | }; 92 | } 93 | 94 | #endif // SLAM_DUNK_TRANSFORMATION_ESTIMATION_H 95 | -------------------------------------------------------------------------------- /src/nodes/tutorials/integration_of_random_points.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights 3 | * Reserved 4 | * You may use, distribute and modify this code under the 5 | * terms of the GNU GPLv3 license. 6 | * 7 | * please write to: d.degregorio@unibo.it 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | // OPENCV 15 | #include 16 | 17 | // Skimap 18 | #include 19 | #include 20 | 21 | #define MAX_RANDOM_COORD 10.0 22 | #define MIN_RANDOM_COORD -10.0 23 | #define MAX_RANDOM_COLOR 1.0 24 | #define MIN_RANDOM_COLOR 0.0 25 | 26 | /** 27 | * skimap definition 28 | */ 29 | typedef float CoordinateType; 30 | typedef int16_t IndexType; 31 | typedef uint16_t WeightType; 32 | typedef skimap::VoxelDataRGBW VoxelDataColor; 33 | typedef skimap::SkiMap SKIMAP; 34 | typedef skimap::SkiMap::Voxel3D 35 | Voxel3D; 36 | typedef skimap::SkiMap::Tiles2D 37 | Tiles2D; 38 | SKIMAP *map; 39 | 40 | /** 41 | * Helper function to obtain a random double in range. 42 | */ 43 | double fRand(double fMin, double fMax) { 44 | double f = (double)rand() / RAND_MAX; 45 | return fMin + f * (fMax - fMin); 46 | } 47 | 48 | int main(int argc, char **argv) { 49 | 50 | srand(time(NULL)); 51 | 52 | // Builds the map 53 | float map_resolution = 0.05; 54 | map = new SKIMAP(map_resolution); 55 | 56 | /** 57 | * This command enables the Concurrent Access Self Management. If it is 58 | * enabled 59 | * you can use OPENMP to call the method 'integrateVoxel' in a concurrent 60 | * manner safely. 61 | */ 62 | map->enableConcurrencyAccess(); 63 | 64 | /** 65 | * With this two parameters we can simulate N_MEASUREMENTS sensor measurements 66 | * each of them with N_POINTS points 67 | */ 68 | int N_MEASUREMENTS = 100; 69 | int N_POINTS = 640 * 480 / 2; 70 | 71 | for (int m = 0; m < N_MEASUREMENTS; m++) { 72 | /** 73 | * Integration Timer 74 | */ 75 | 76 | // #pragma omp parallel for 77 | for (int i = 0; i < N_POINTS; i++) { 78 | 79 | /** 80 | * Generates a random 3D Point 81 | */ 82 | double x = fRand(MIN_RANDOM_COORD, MAX_RANDOM_COORD); 83 | double y = fRand(MIN_RANDOM_COORD, MAX_RANDOM_COORD); 84 | double z = fRand(MIN_RANDOM_COORD, MAX_RANDOM_COORD); 85 | 86 | /** 87 | * Creates a Voxel. In this case the voxel is a VoxelDataColor data 88 | * structure with r,g,b color information and a w weight. In general 89 | * the 'weight' is used to fuse voxel togheter: a positive weight 1.0 90 | * means an addiction, a negative weight -1.0 means a subtraction 91 | */ 92 | VoxelDataColor voxel; 93 | voxel.r = fRand(MIN_RANDOM_COLOR, MAX_RANDOM_COLOR); 94 | voxel.g = fRand(MIN_RANDOM_COLOR, MAX_RANDOM_COLOR); 95 | voxel.b = fRand(MIN_RANDOM_COLOR, MAX_RANDOM_COLOR); 96 | voxel.w = 1.0; 97 | 98 | /** 99 | * Integration of custom voxel in the SkiMap data structure. 100 | */ 101 | map->integrateVoxel(CoordinateType(x), CoordinateType(y), 102 | CoordinateType(z), &voxel); 103 | } 104 | 105 | /** 106 | * Map Visiting. With this command you can extract all voxels in SkiMap. 107 | * You can iterate on results to display Voxels in your viewer 108 | */ 109 | std::vector voxels; 110 | map->fetchVoxels(voxels); 111 | 112 | printf("Map voxels: %d\n", int(voxels.size())); 113 | } 114 | } 115 | -------------------------------------------------------------------------------- /src/slamdunk/data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include "slamdunk/data.h" 10 | #include 11 | 12 | // TODO: read/write Mat type 13 | 14 | slamdunk::RGBDFrame slamdunk::cloneRGBDFrame(const slamdunk::RGBDFrame &frame) 15 | { 16 | RGBDFrame f; 17 | f.m_timestamp = frame.m_timestamp; 18 | f.m_color_image = frame.m_color_image.clone(); 19 | f.m_depth_image = frame.m_depth_image.clone(); 20 | return f; 21 | } 22 | 23 | bool slamdunk::FrameData::read(std::istream &is) 24 | { 25 | is >> m_timestamp; 26 | int rows, cols; 27 | is >> rows >> cols; 28 | m_descriptors.create(rows, cols, CV_32F); 29 | m_keypoints.resize(rows); 30 | for (int i = 0; i < rows; ++i) 31 | { 32 | float *dptr = m_descriptors.ptr(i); 33 | for (int j = 0; j < cols; ++j) 34 | is >> dptr[j]; 35 | } 36 | for (int i = 0; i < rows; ++i) 37 | { 38 | is >> m_keypoints[i].x() >> m_keypoints[i].y() >> m_keypoints[i].z(); 39 | m_keypoints[i][3] = 1.0; 40 | } 41 | return true; 42 | } 43 | 44 | bool slamdunk::FrameData::write(std::ostream &os) const 45 | { 46 | os << m_timestamp << " "; 47 | assert((int)m_keypoints.size() == m_descriptors.rows); 48 | // descriptors 49 | os << m_descriptors.rows << " " << m_descriptors.cols; 50 | for (int i = 0; i < m_descriptors.rows; ++i) 51 | { 52 | const float *dptr = m_descriptors.ptr(i); 53 | for (int j = 0; j < m_descriptors.cols; ++j) 54 | os << " " << std::fixed << std::setprecision(15) << dptr[j]; 55 | } 56 | // keypoints 57 | for (unsigned i = 0; i < m_keypoints.size(); ++i) 58 | os << " " << std::fixed << std::setprecision(15) << m_keypoints[i].x() << " " << m_keypoints[i].y() << " " << m_keypoints[i].z(); 59 | return os.good(); 60 | } 61 | 62 | cv::Mat_ slamdunk::depthBilateralFilter(const cv::Mat_ &input, double sigmaDepth, double sigmaSpace) 63 | { 64 | cv::Mat_ output(input.rows, input.cols); 65 | 66 | double one_over_sigmaD22 = 1. / (2. * sigmaDepth * sigmaDepth); 67 | double one_over_sigmaS22 = 1. / (2. * sigmaSpace * sigmaSpace); 68 | double one_over_sigmaD2Pi = 1. / (sigmaDepth * std::sqrt(2. * M_PI)); 69 | double one_over_sigmaS2Pi = 1. / (sigmaSpace * std::sqrt(2. * M_PI)); 70 | 71 | int radius = (int)std::ceil(2. * sigmaSpace); 72 | // spatial weights 73 | Eigen::MatrixXd ws(2 * radius + 1, 2 * radius + 1); 74 | for (int y = -radius; y < radius + 1; ++y) 75 | for (int x = -radius; x < radius + 1; ++x) 76 | ws(y + radius, x + radius) = std::exp(-1. * (y * y + x * x) * one_over_sigmaS22) * one_over_sigmaS2Pi; 77 | 78 | for (int v = 0; v < input.rows; ++v) 79 | for (int u = 0; u < input.cols; ++u) 80 | if (input(v, u) > 0) 81 | { 82 | double value = 0., norm_factor = 0.; 83 | for (int y = v - radius; y < v + radius + 1; ++y) 84 | for (int x = u - radius; x < u + radius + 1; ++x) 85 | { 86 | if (x >= 0 && x < input.cols && y >= 0 && y < input.rows && input(y, x) > 0) 87 | { 88 | double depth_diff = input(y, x) - input(v, u); 89 | double gauss_product = ws(y - v + radius, x - u + radius) * std::exp(-1. * depth_diff * depth_diff * one_over_sigmaD22) * one_over_sigmaD2Pi; 90 | value += gauss_product * input(y, x); 91 | norm_factor += gauss_product; 92 | } 93 | } 94 | output(v, u) = (float)(value / norm_factor); 95 | } 96 | else 97 | output(v, u) = 0.f; 98 | 99 | return output; 100 | } 101 | -------------------------------------------------------------------------------- /src/slamdunk/feature_matcher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include "slamdunk/feature_matcher.h" 10 | #include 11 | 12 | // Ctors 13 | slamdunk::OpenCVFlannRatioMatcher::OpenCVFlannRatioMatcher(bool interModelRatio, float ratio, int cores) 14 | : OpenCVRatioMatcher(interModelRatio, ratio, new cv::FlannBasedMatcher(new cv::flann::KDTreeIndexParams(), new cv::flann::SearchParams()), cores) 15 | //: OpenCVRatioMatcher(interModelRatio, ratio, new cv::FlannBasedMatcher(new cv::flann::KDTreeIndexParams(1), new cv::flann::SearchParams(-1)), cores) // Deterministic 16 | //: OpenCVRatioMatcher(interModelRatio, ratio, new cv::BFMatcher(cv::NORM_L2, false), cores) // Brute Force 17 | { 18 | } 19 | 20 | slamdunk::OpenCVLSHRatioMatcher::OpenCVLSHRatioMatcher(bool interModelRatio, float ratio, int cores) 21 | //: OpenCVRatioMatcher(interModelRatio, ratio, new cv::FlannBasedMatcher(new cv::flann::LshIndexParams(16, 10, 2), new cv::flann::SearchParams()), cores) 22 | : OpenCVRatioMatcher(interModelRatio, ratio, new cv::BFMatcher(cv::NORM_HAMMING, false), cores) // Brute Force 23 | { 24 | } 25 | 26 | void slamdunk::OpenCVRatioMatcher::setModels(const std::vector &models) 27 | { 28 | assert(m_matcher); 29 | 30 | m_matcher->clear(); 31 | m_matcher->add(models); 32 | m_matcher->train(); 33 | 34 | m_model_zeroes.resize(models.size()); 35 | int nDesc = 0; 36 | for (unsigned i = 0; i < models.size(); ++i) 37 | { 38 | m_model_zeroes[i] = nDesc; 39 | nDesc += models[i].rows; 40 | } 41 | 42 | m_model_indices.clear(); 43 | m_model_indices.reserve(nDesc); 44 | for (unsigned i = 0; i < models.size(); ++i) 45 | { 46 | m_model_indices.insert(m_model_indices.end(), (size_t)models[i].rows, i); 47 | } 48 | assert((int)m_model_indices.size() == nDesc); 49 | } 50 | 51 | void slamdunk::OpenCVRatioMatcher::match(cv::Mat query, std::vector &matches) 52 | { 53 | assert(m_matcher); 54 | std::vector> nn_matches; 55 | 56 | m_matcher->knnMatch(query, nn_matches, SLAM_DUNK_KNN_NEIGHBORS); 57 | 58 | std::list match_list; 59 | for (int k = 0; k < query.rows; ++k) 60 | { 61 | const std::vector &q_matches = nn_matches[k]; 62 | if (q_matches.size() < 2) 63 | { 64 | continue; 65 | } 66 | 67 | const cv::DMatch &first_match = q_matches[0]; 68 | const int model1 = first_match.imgIdx; 69 | 70 | // find 2nd nearest neighbor descriptor 71 | for (size_t neigh_idx = 1; neigh_idx < q_matches.size(); ++neigh_idx) 72 | { 73 | const cv::DMatch ¤t_match = q_matches[neigh_idx]; 74 | assert(first_match.distance <= current_match.distance); 75 | 76 | // no need to check the distance because results are sorted 77 | if ((current_match.imgIdx != model1 && m_inter_model_ratio) || 78 | (current_match.imgIdx == model1 && !m_inter_model_ratio)) 79 | { 80 | if (first_match.distance <= m_ratio2 * current_match.distance && current_match.distance > 0) 81 | { 82 | cv::DMatch ratio_match = first_match; 83 | ratio_match.distance = first_match.distance / current_match.distance; 84 | match_list.push_back(ratio_match); 85 | } 86 | break; 87 | } 88 | } 89 | } 90 | 91 | match_list.sort(); 92 | 93 | // clear double matches 94 | std::vector mask(m_model_indices.size(), true); 95 | matches.clear(); 96 | matches.reserve(match_list.size()); 97 | 98 | for (std::list::const_iterator it = match_list.begin(); it != match_list.end(); ++it) 99 | { 100 | if (mask[m_model_zeroes[it->imgIdx] + it->trainIdx]) 101 | { 102 | mask[m_model_zeroes[it->imgIdx] + it->trainIdx] = false; 103 | 104 | FMatch fm; 105 | fm.m_model_idx = it->imgIdx; 106 | fm.m_feat_idx = it->trainIdx; 107 | fm.m_query_idx = it->queryIdx; 108 | fm.m_match_score = 1.f - std::sqrt(it->distance); 109 | matches.push_back(fm); 110 | } 111 | } 112 | } -------------------------------------------------------------------------------- /include/skimap_ros/SkiMapServiceClient.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SKIMAPSERVICECLIENT_HPP 10 | #define SKIMAPSERVICECLIENT_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace skimap_ros 28 | { 29 | 30 | class SkimapServiceClient 31 | { 32 | public: 33 | /** 34 | */ 35 | struct ColorPoint 36 | { 37 | 38 | double x, y, z; 39 | float r, g, b; 40 | float w; 41 | bool valid; 42 | 43 | ColorPoint() 44 | { 45 | this->valid = false; 46 | } 47 | }; 48 | 49 | /** 50 | * 51 | */ 52 | struct IntegrationEntry 53 | { 54 | std::vector points; 55 | geometry_msgs::Pose camera_pose; 56 | }; 57 | 58 | /** 59 | * 60 | */ 61 | SkimapServiceClient(ros::NodeHandle *node_handle, std::string service_name = "/skimap_map_service/integration_service") : service_name(service_name) 62 | { 63 | 64 | this->node_handle = node_handle; 65 | this->service_client = this->node_handle->serviceClient(service_name); 66 | consuming_thread = new std::thread(&SkimapServiceClient::internalDequeuer, this); 67 | } 68 | 69 | virtual ~SkimapServiceClient() 70 | { 71 | delete consuming_thread; 72 | } 73 | 74 | /** 75 | * Sends point to Remote Service for integration 76 | */ 77 | void integratePoints(std::vector &output_points, geometry_msgs::Pose camera_pose) 78 | { 79 | IntegrationEntry entry; 80 | entry.camera_pose = camera_pose; 81 | entry.points.insert(entry.points.end(), output_points.begin(), output_points.end()); 82 | std::lock_guard guard(this->queue_mutex); 83 | this->integration_queue.push(entry); 84 | } 85 | 86 | /** 87 | * Sends point to Remote Service for integration 88 | */ 89 | void integratePoints(std::vector &output_points, tf::Transform &camera_pose) 90 | { 91 | geometry_msgs::Pose pose; 92 | tf::poseTFToMsg(camera_pose, pose); 93 | this->integratePoints(output_points, pose); 94 | } 95 | 96 | protected: 97 | std::string service_name; 98 | ros::NodeHandle *node_handle; 99 | ros::ServiceClient service_client; 100 | 101 | //Queue 102 | std::queue integration_queue; 103 | std::mutex queue_mutex; 104 | std::thread *consuming_thread; 105 | 106 | /** 107 | * 108 | */ 109 | void integrateEntry(IntegrationEntry &entry) 110 | { 111 | skimap_ros::SkimapIntegrationService srv; 112 | srv.request.sensor_pose = entry.camera_pose; 113 | 114 | for (int i = 0; i < entry.points.size(); i++) 115 | { 116 | SkimapServiceClient::ColorPoint p = entry.points[i]; 117 | if (p.valid) 118 | { 119 | geometry_msgs::Point mp; 120 | mp.x = p.x; 121 | mp.y = p.y; 122 | mp.z = p.z; 123 | 124 | std_msgs::ColorRGBA color; 125 | color.r = p.r; 126 | color.g = p.g; 127 | color.b = p.b; 128 | color.a = 1; 129 | srv.request.points.push_back(mp); 130 | srv.request.colors.push_back(color); 131 | srv.request.weights.push_back(p.w); 132 | } 133 | } 134 | 135 | if (service_client.call(srv)) 136 | { 137 | //Nothing 138 | } 139 | else 140 | { 141 | ROS_ERROR("SkimapServiceClient: Failed to call service add_two_ints"); 142 | } 143 | } 144 | 145 | /** 146 | * 147 | */ 148 | void internalDequeuer() 149 | { 150 | while (this->node_handle->ok()) 151 | { 152 | 153 | if (integration_queue.size() > 0) 154 | { 155 | std::unique_lock guard(this->queue_mutex); 156 | IntegrationEntry integration_entry = integration_queue.front(); 157 | integration_queue.pop(); 158 | guard.unlock(); 159 | integrateEntry(integration_entry); 160 | } 161 | } 162 | } 163 | }; 164 | } 165 | 166 | #endif /* SKIMAPSERVICECLIENT_HPP */ 167 | -------------------------------------------------------------------------------- /include/skimap/voxels/VoxelDataRGBW.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef VOXELDATARGBW_HPP 10 | #define VOXELDATARGBW_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace skimap 19 | { 20 | 21 | /** 22 | * Voxel Data containing RGB color values and Weight. 23 | * C template represents datatype for color component. 24 | * W template represents datatype for weight. 25 | */ 26 | template 27 | struct VoxelDataRGBW 28 | { 29 | C r, g, b; 30 | W w; 31 | 32 | /** 33 | * Pointer Copy Constructor. 34 | * @param data source data 35 | */ 36 | VoxelDataRGBW(VoxelDataRGBW *data) 37 | { 38 | r = data->r; 39 | g = data->g; 40 | b = data->b; 41 | w = data->w; 42 | } 43 | 44 | /** 45 | * Void constructor. 46 | */ 47 | VoxelDataRGBW() 48 | { 49 | r = g = b = C(0); 50 | w = W(0); 51 | } 52 | 53 | /** 54 | * Gray color constructor. 55 | * @param v RGB level 56 | */ 57 | VoxelDataRGBW(C v) 58 | { 59 | r = g = b = v; 60 | w = W(0); 61 | } 62 | 63 | /** 64 | * Full constructor 65 | * @param r Red value 66 | * @param g Green value 67 | * @param b Blue value 68 | * @param w Weight value 69 | */ 70 | VoxelDataRGBW(C r, C g, C b, W w) 71 | { 72 | this->r = r; 73 | this->g = g; 74 | this->b = b; 75 | this->w = w; 76 | } 77 | 78 | /** 79 | * Sum Overload. Defines Sum operations between Voxels 80 | * @param v2 second addend 81 | * @return sum 82 | */ 83 | VoxelDataRGBW operator+(const VoxelDataRGBW &v2) const 84 | { 85 | VoxelDataRGBW c1 = *this; 86 | if (c1.w <= W(0)) 87 | return v2; 88 | VoxelDataRGBW d; 89 | d.w = (c1.w + v2.w); 90 | if (d.w <= W(0)) 91 | { 92 | d.r = d.g = d.b = C(0); 93 | d.w = W(0); 94 | } 95 | else 96 | { 97 | d.r = C((double(c1.r) * double(c1.w) + double(v2.r) * double(v2.w)) / double(d.w)); 98 | d.g = C((double(c1.g) * double(c1.w) + double(v2.g) * double(v2.w)) / double(d.w)); 99 | d.b = C((double(c1.b) * double(c1.w) + double(v2.b) * double(v2.w)) / double(d.w)); 100 | } 101 | return d; 102 | } 103 | 104 | /** 105 | * Subtraction Overload. Defines Subtraction operations between Voxels 106 | * @param v2 second minuend 107 | * @return subtraction 108 | */ 109 | VoxelDataRGBW operator-(const VoxelDataRGBW &v2) const 110 | { 111 | VoxelDataRGBW v1 = *this; 112 | if (v1.w <= W(0)) 113 | return v1; 114 | VoxelDataRGBW d; 115 | d.w = (v1.w - v2.w); 116 | if (d.w <= W(0)) 117 | { 118 | d.r = d.g = d.b = C(0); 119 | d.w = W(0); 120 | } 121 | else 122 | { 123 | d.r = C((double(v1.r) * double(v1.w) - double(v2.r) * double(v2.w)) / double(d.w)); 124 | d.g = C((double(v1.g) * double(v1.w) - double(v2.g) * double(v2.w)) / double(d.w)); 125 | d.b = C((double(v1.b) * double(v1.w) - double(v2.b) * double(v2.w)) / double(d.w)); 126 | } 127 | return d; 128 | } 129 | 130 | /** 131 | * Serializes object into stream. 132 | */ 133 | friend std::ostream &operator<<(std::ostream &os, const VoxelDataRGBW &voxel) 134 | { 135 | os << std::setprecision(std::numeric_limits::digits10 + 2); 136 | os << double(voxel.r) << " "; 137 | os << double(voxel.g) << " "; 138 | os << double(voxel.b) << " "; 139 | os << double(voxel.w); 140 | return os; 141 | } 142 | 143 | /** 144 | * Hydrates object from stream. 145 | */ 146 | friend std::istream &operator>>(std::istream &is, VoxelDataRGBW &voxel) 147 | { 148 | double r, g, b, w; 149 | is >> r; 150 | is >> g; 151 | is >> b; 152 | is >> w; 153 | voxel.r = r; 154 | voxel.g = g; 155 | voxel.b = b; 156 | voxel.w = w; 157 | return is; 158 | } 159 | }; 160 | } 161 | 162 | #endif /* VOXELDATARGBW_HPP */ 163 | -------------------------------------------------------------------------------- /cmake/Modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT} 5 | ${G2O_ROOT}/include 6 | $ENV{G2O_ROOT}/include 7 | $ENV{G2O_ROOT} 8 | /usr/local/include 9 | /usr/include 10 | /opt/local/include 11 | /sw/local/include 12 | /sw/include 13 | NO_DEFAULT_PATH 14 | ) 15 | 16 | # Macro to unify finding both the debug and release versions of the 17 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 18 | # macro. 19 | 20 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 21 | 22 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 23 | NAMES "g2o_${MYLIBRARYNAME}_d" 24 | PATHS 25 | ${G2O_ROOT}/lib/Debug 26 | ${G2O_ROOT}/lib 27 | $ENV{G2O_ROOT}/lib/Debug 28 | $ENV{G2O_ROOT}/lib 29 | ${G2O_ROOT}/bin/Debug 30 | ${G2O_ROOT}/bin 31 | $ENV{G2O_ROOT}/bin/Debug 32 | $ENV{G2O_ROOT}/bin 33 | NO_DEFAULT_PATH 34 | ) 35 | 36 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 37 | NAMES "g2o_${MYLIBRARYNAME}_d" 38 | PATHS 39 | ~/Library/Frameworks 40 | /Library/Frameworks 41 | /usr/local/lib 42 | /usr/local/lib64 43 | /usr/lib 44 | /usr/lib64 45 | /opt/local/lib 46 | /sw/local/lib 47 | /sw/lib 48 | ) 49 | 50 | FIND_LIBRARY("${MYLIBRARY}_RELEASE" 51 | NAMES "g2o_${MYLIBRARYNAME}" 52 | PATHS 53 | ${G2O_ROOT}/lib/Release 54 | ${G2O_ROOT}/lib 55 | $ENV{G2O_ROOT}/lib/Release 56 | $ENV{G2O_ROOT}/lib 57 | ${G2O_ROOT}/bin/Release 58 | ${G2O_ROOT}/bin 59 | $ENV{G2O_ROOT}/bin/Release 60 | $ENV{G2O_ROOT}/bin 61 | NO_DEFAULT_PATH 62 | ) 63 | 64 | FIND_LIBRARY("${MYLIBRARY}_RELEASE" 65 | NAMES "g2o_${MYLIBRARYNAME}" 66 | PATHS 67 | ~/Library/Frameworks 68 | /Library/Frameworks 69 | /usr/local/lib 70 | /usr/local/lib64 71 | /usr/lib 72 | /usr/lib64 73 | /opt/local/lib 74 | /sw/local/lib 75 | /sw/lib 76 | ) 77 | 78 | IF(NOT ${MYLIBRARY}_DEBUG) 79 | IF(${MYLIBRARY}_RELEASE) 80 | SET(${MYLIBRARY}_DEBUG ${${MYLIBRARY}_RELEASE}) 81 | ENDIF(${MYLIBRARY}_RELEASE) 82 | ENDIF(NOT ${MYLIBRARY}_DEBUG) 83 | 84 | SET(${MYLIBRARY} 85 | debug ${${MYLIBRARY}_DEBUG} 86 | optimized ${${MYLIBRARY}_RELEASE} 87 | ) 88 | 89 | MARK_AS_ADVANCED(FORCE ${MYLIBRARY}_DEBUG ${MYLIBRARY}_RELEASE ${MYLIBRARY}) 90 | 91 | ENDMACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 92 | 93 | # Find the core elements 94 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 95 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 96 | 97 | # Find the CLI library 98 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 99 | 100 | # Find the pluggable solvers 101 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 102 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 103 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 104 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 105 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 106 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 107 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 108 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 109 | 110 | # Find the predefined types 111 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 112 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 113 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 114 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 115 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 116 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 117 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 118 | 119 | # G2O solvers declared found if we found at least one solver 120 | SET(G2O_SOLVERS_FOUND FALSE) 121 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 122 | SET(G2O_SOLVERS_FOUND TRUE) 123 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 124 | 125 | # G2O itself declared found if we found the core libraries and at least one solver 126 | SET(G2O_FOUND FALSE) 127 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 128 | SET(G2O_FOUND TRUE) 129 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 130 | 131 | 132 | IF(G2O_FOUND) 133 | IF(NOT G2O_FIND_QUIETLY) 134 | MESSAGE(STATUS "Found G2O") 135 | ENDIF(NOT G2O_FIND_QUIETLY) 136 | ELSE(G2O_FOUND) 137 | IF(G2O_FIND_REQUIRED) 138 | MESSAGE(FATAL_ERROR "G2O NOT FOUND") 139 | ELSE(G2O_FIND_REQUIRED) 140 | IF(NOT G2O_FIND_QUIETLY) 141 | MESSAGE(STATUS "G2O NOT FOUND") 142 | ENDIF(NOT G2O_FIND_QUIETLY) 143 | ENDIF(G2O_FIND_REQUIRED) 144 | ENDIF(G2O_FOUND) 145 | -------------------------------------------------------------------------------- /include/slamdunk/slam_dunk.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_SLAM_DUNK_H 10 | #define SLAM_DUNK_SLAM_DUNK_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | #include "slamdunk/camera_tracker.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef SLAMDUNK_ICP_ENABLED 20 | #include 21 | #endif // SLAMDUNK_ICP_ENABLED 22 | 23 | namespace slamdunk 24 | { 25 | 26 | struct SLAM_DUNK_API SlamDunkParams 27 | { 28 | CameraTracker::Ptr tracker; 29 | unsigned min_extracted_features, rba_rings, icp_samples, icp_maxit; 30 | int cores; 31 | float icp_distance_th, icp_normal_th; 32 | float kf_overlapping; // [0,1], the lower it is, the more distant kfs are 33 | bool doicp, try_loop_inference; 34 | bool verbose, debug; 35 | 36 | // default values (you may want to create the tracker then) 37 | SlamDunkParams() 38 | : min_extracted_features(30u), 39 | rba_rings(3), icp_samples(1000), icp_maxit(50u), 40 | cores(4), icp_distance_th(0.2), icp_normal_th(30), 41 | kf_overlapping(0.5f), doicp(false), try_loop_inference(false), verbose(false), debug(false) {} 42 | }; 43 | 44 | class SLAM_DUNK_API SlamDunk 45 | { 46 | public: 47 | enum // return states 48 | { 49 | TRACKING_FAILED = 0, 50 | FRAME_TRACKED, 51 | KEYFRAME_DETECTED 52 | }; 53 | 54 | public: 55 | const Eigen::Matrix3f m_inverse_kcam; 56 | 57 | SlamDunk(const Eigen::Matrix3f &inverse_kcam, const SlamDunkParams ¶ms = SlamDunkParams()) 58 | : m_inverse_kcam(inverse_kcam), m_tracker(params.tracker), 59 | m_min_extracted_features((int)params.min_extracted_features), 60 | m_kf_overlapping(params.kf_overlapping), m_processed_frames(0), 61 | m_rba_rings(params.rba_rings), m_icp_samples(params.icp_samples), 62 | m_icp_maxit(params.icp_maxit), m_cores(params.cores), 63 | m_icp_cosnormal_th(std::cos(M_PI * params.icp_normal_th / 180.f)), 64 | m_last_wmchi2(-1.), m_doicp(params.doicp), m_try_loop_inference(params.try_loop_inference), 65 | m_verbose(params.verbose), m_debug(params.debug) 66 | { 67 | } 68 | 69 | typedef std::vector, 70 | Eigen::aligned_allocator>> 71 | StampedPoseVector; 72 | typedef boost::function debugging_function; 73 | 74 | /** \brief The debugging function is called whenever a new keyframe is detected */ 75 | inline void setDebuggingFunction(const debugging_function &f) { m_deb_func = f; } 76 | inline void unsetDebuggingFunction() { m_deb_func.clear(); } 77 | 78 | int operator()(const RGBDFrame &frame, Eigen::Isometry3d &estimated_pose); 79 | int operator()(const RGBDFrame &frame) 80 | { 81 | Eigen::Isometry3d fake; 82 | return (*this)(frame, fake); 83 | } 84 | 85 | void forceGlobalOptimization(); 86 | 87 | void getMappedPoses(StampedPoseVector &poses) const; 88 | const StampedPoseVector &getMovedFrames() const { return m_moved_frames; } 89 | /// A wrapper to access frame data (thread safe) 90 | const FrameData *getFrameData(int id); 91 | /// A wrapper to get the residual from the last optimization (thread safe) 92 | double lastWeightedMeanChi2(); 93 | unsigned getNumberOfKeyframes(); 94 | 95 | private: 96 | #ifdef SLAMDUNK_ICP_ENABLED 97 | typedef flann::Index> FLANNIndex; 98 | typedef boost::shared_ptr>> FLANNIndexPtr; 99 | 100 | void updateICPConstraints(int ref_frame_id, float icp_sqDistance_th, 101 | const std::vector> &trees, 102 | std::list &icp_edges); 103 | void samplePointsAndNormals(const RGBDFrame &frame, FrameData &frameData) const; 104 | #endif // SLAMDUNK_ICP_ENABLED 105 | 106 | CameraTracker::Ptr m_tracker; 107 | GraphBackend::Ptr m_graph_ptr; 108 | boost::scoped_ptr m_dijkstra; 109 | StampedPoseVector m_moved_frames; 110 | int m_min_extracted_features; // will be compared to cv::Mat::rows which is int 111 | float m_kf_overlapping; 112 | unsigned m_processed_frames, m_rba_rings, m_icp_samples, m_icp_maxit; 113 | int m_cores; 114 | float m_icp_cosnormal_th; 115 | double m_last_wmchi2; 116 | bool m_doicp, m_try_loop_inference; 117 | bool m_verbose, m_debug; 118 | debugging_function m_deb_func; 119 | }; 120 | } 121 | 122 | #endif // SLAM_DUNK_SLAM_DUNK_H 123 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(skimap_ros) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 5 | 6 | ## OPTIONS 7 | option(BUILD_SLAMDUNK "Build Slamdunk Library" OFF) 8 | option(EXPERIMENTAL "Experimental features" OFF) 9 | option(BUILD_TUTORIALS "Build Tutorials Nodes" ON) 10 | 11 | ## Find catkin macros and libraries 12 | find_package(catkin REQUIRED 13 | cv_bridge 14 | image_transport 15 | pcl_ros 16 | nav_msgs 17 | std_msgs 18 | geometry_msgs 19 | message_generation 20 | tf 21 | tf_conversions 22 | ) 23 | 24 | include_directories(${catkin_INCLUDE_DIRS}) 25 | 26 | #Services 27 | add_service_files( 28 | FILES 29 | SkimapIntegrationService.srv 30 | ) 31 | 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | geometry_msgs 36 | ) 37 | 38 | # useful macros 39 | include(CMakeDependentOption) 40 | 41 | # Find modules 42 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/Modules ${CMAKE_MODULE_PATH}) 43 | 44 | 45 | # OpenMP 46 | find_package(OpenMP) 47 | if(OPENMP_FOUND) 48 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 49 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 50 | message (STATUS "Found OpenMP") 51 | else(OPENMP_FOUND) 52 | message (STATUS "Not found OpenMP") 53 | endif(OPENMP_FOUND) 54 | 55 | #Eigen3 56 | find_package(Eigen3 REQUIRED) 57 | 58 | #OpenCV 59 | find_package(OpenCV REQUIRED COMPONENTS core imgproc) 60 | if(OpenCV_INCLUDE_DIRS) 61 | include_directories(${OpenCV_INCLUDE_DIRS}) 62 | endif(OpenCV_INCLUDE_DIRS) 63 | 64 | #BOOST 65 | find_package(Boost 1.50.0 REQUIRED COMPONENTS random timer system) 66 | if(Boost_FOUND) 67 | include_directories(${Boost_INCLUDE_DIR}) # cached 68 | endif(Boost_FOUND) 69 | 70 | 71 | 72 | 73 | ################################################################################ 74 | ################################################################################ 75 | ################################################################################ 76 | 77 | ## Declare a C++ library 78 | add_library(${PROJECT_NAME} 79 | src/${PROJECT_NAME}/skimap_ros_void_library.cpp 80 | ) 81 | 82 | 83 | catkin_package( 84 | INCLUDE_DIRS include/ 85 | LIBRARIES ${PROJECT_NAME} 86 | ) 87 | 88 | 89 | 90 | 91 | ######################### 92 | # SKIMAP LIBRARY 93 | ######################### 94 | include_directories( 95 | include/ 96 | ) 97 | 98 | ######################### 99 | # GRID EXPERIMENTS 100 | ######################### 101 | 102 | # find_package(PCL REQUIRED) 103 | # include_directories(${PCL_INCLUDE_DIRS}) 104 | # link_directories(${PCL_LIBRARY_DIRS}) 105 | # add_definitions(${PCL_DEFINITIONS}) 106 | 107 | # set(ANN_INCLUDE_DIRS /home/daniele/work/workspace_cpp/ann_1.1.2/include) 108 | # set(ANN_LIBRARIES /home/daniele/work/workspace_cpp/ann_1.1.2/lib/libANN.a) 109 | 110 | # include_directories(${ANN_INCLUDE_DIRS}) 111 | # add_executable(grid2d src/nodes/experiments/grid2d.cpp) 112 | # target_link_libraries(grid2d 113 | # ${ANN_LIBRARIES} 114 | # ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 115 | 116 | # add_executable(kd_test src/nodes/experiments/kd_test.cpp) 117 | # target_link_libraries(kd_test 118 | # ${ANN_LIBRARIES} 119 | # ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 120 | 121 | # add_executable(octree_test src/nodes/experiments/octree_test.cpp) 122 | # target_link_libraries(octree_test 123 | # ${ANN_LIBRARIES} 124 | # ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 125 | 126 | # add_executable(debug src/nodes/experiments/debug.cpp) 127 | # target_link_libraries(debug 128 | # ${ANN_LIBRARIES} 129 | # ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 130 | 131 | 132 | 133 | 134 | ######################### 135 | # NODES 136 | ######################### 137 | 138 | 139 | 140 | add_executable(skimap_live src/nodes/skimap_live.cpp) 141 | target_link_libraries(skimap_live ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 142 | 143 | add_executable(skimap_map_service src/nodes/skimap_map_service.cpp) 144 | target_link_libraries(skimap_map_service ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 145 | add_dependencies(skimap_map_service skimap_ros_generate_messages_cpp) 146 | 147 | 148 | 149 | if(BUILD_TUTORIALS) 150 | #INTEGRATION OF RANDOM POINTS 151 | add_executable(integration_of_random_points src/nodes/tutorials/integration_of_random_points.cpp) 152 | target_link_libraries(integration_of_random_points ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 153 | endif(BUILD_TUTORIALS) 154 | 155 | 156 | 157 | ############################################################################### 158 | # INSTALLATION 159 | ############################################################################### 160 | 161 | install(DIRECTORY include/skimap 162 | DESTINATION include 163 | FILES_MATCHING 164 | PATTERN "*.h" 165 | PATTERN "*.hpp" 166 | PATTERN ".svn" EXCLUDE 167 | ) 168 | 169 | -------------------------------------------------------------------------------- /src/slamdunk_extension/FIROrder2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include "FIROrder2.h" 10 | #include 11 | 12 | FIROrder2::FIROrder2(int data_size, double cutoff, double quality) 13 | { 14 | this->data_size = data_size; 15 | this->Q = quality; 16 | this->updateParameters(cutoff); 17 | this->ready = false; 18 | 19 | this->data.resize(data_size); 20 | this->output.resize(data_size); 21 | for (int i = 0; i < data_size; i++) 22 | { 23 | this->data[i].resize(4); 24 | } 25 | } 26 | 27 | FIROrder2::~FIROrder2() 28 | { 29 | } 30 | 31 | void FIROrder2::setSampleTime(double sample_time) 32 | { 33 | this->Fs = 1.0 / sample_time; 34 | this->updateParameters(this->Fc); 35 | } 36 | 37 | void FIROrder2::updateParameters(double cutoff) 38 | { 39 | this->Fc = cutoff; 40 | 41 | this->W = tan(M_PI * this->Fc / this->Fs); 42 | this->N = 1.0 / (pow(this->W, 2) + this->W / Q + 1); 43 | this->B0 = this->N * pow(this->W, 2); 44 | this->B1 = 2 * this->B0; 45 | this->B2 = this->B0; 46 | this->A1 = 2 * this->N * (pow(this->W, 2) - 1); 47 | this->A2 = this->N * (pow(this->W, 2) - this->W / this->Q + 1); 48 | } 49 | 50 | void FIROrder2::setInput(int i, double X) 51 | { 52 | 53 | double Acc = 54 | X * this->B0 + this->data[i][0] * this->B1 + this->data[i][1] * this->B2 - this->data[i][2] * this->A1 - this->data[i][3] * this->A2; 55 | 56 | this->data[i][3] = this->data[i][2]; 57 | this->data[i][2] = Acc; 58 | this->data[i][1] = this->data[i][0]; 59 | this->data[i][0] = X; 60 | 61 | // printf("Set input %i, %f %f %f %f %f \n", i, this->B0,this->A1, this->A2, this->B1, this->B2); 62 | this->output[i] = Acc; 63 | } 64 | 65 | void FIROrder2::setInputs(double *Xs) 66 | { 67 | for (int i = 0; i < this->data_size; i++) 68 | { 69 | this->setInput(i, Xs[i]); 70 | } 71 | } 72 | 73 | void FIROrder2::setInitialData(double *Xs) 74 | { 75 | for (int i = 0; i < this->data_size; i++) 76 | { 77 | this->data[i][0] = Xs[i]; 78 | this->data[i][1] = Xs[i]; 79 | this->data[i][2] = Xs[i]; 80 | this->data[i][3] = Xs[i]; 81 | this->output[i] = Xs[i]; 82 | // printf("In data %i , %f\n", i, output[i]); 83 | } 84 | this->ready = true; 85 | } 86 | 87 | void FIROrder2::setInitialData(Eigen::Isometry3d &iso) 88 | { 89 | 90 | double *data; 91 | conversion(iso, data); 92 | setInitialData(data); 93 | } 94 | 95 | void FIROrder2::setInput(Eigen::Isometry3d &iso) 96 | { 97 | double *data; 98 | conversion(iso, data); 99 | setInputs(data); 100 | } 101 | 102 | void FIROrder2::conversion(Eigen::Isometry3d &iso, double *&data, bool reverse) 103 | { 104 | // if (!reverse) { 105 | // tf::Transform tf; 106 | // tf::transformEigenToTF(iso, tf); 107 | // KDL::Frame frame; 108 | // tf::transformTFToKDL(tf, frame); 109 | // 110 | // data = new double[7]; 111 | // data[0] = frame.p.x(); 112 | // data[1] = frame.p.y(); 113 | // data[2] = frame.p.z(); 114 | // frame.M.GetQuaternion( 115 | // data[3], 116 | // data[4], 117 | // data[5], 118 | // data[6] 119 | // ); 120 | // 121 | // } else { 122 | // KDL::Frame frame; 123 | // frame.p = KDL::Vector(data[0], data[1], data[2]); 124 | // frame.M = KDL::Rotation::Quaternion(data[3], 125 | // data[4], 126 | // data[5], 127 | // data[6] 128 | // ); 129 | // tf::Transform tf; 130 | // tf::transformKDLToTF(frame, tf); 131 | // tf::transformTFToEigen(tf, iso); 132 | // 133 | // } 134 | if (!reverse) 135 | { 136 | tf::Transform tf; 137 | tf::transformEigenToTF(iso, tf); 138 | KDL::Frame frame; 139 | tf::transformTFToKDL(tf, frame); 140 | 141 | data = new double[6]; 142 | data[0] = frame.p.x(); 143 | data[1] = frame.p.y(); 144 | data[2] = frame.p.z(); 145 | frame.M.GetRPY(data[3], data[4], data[5]); 146 | } 147 | else 148 | { 149 | KDL::Frame frame; 150 | frame.p = KDL::Vector(data[0], data[1], data[2]); 151 | frame.M = KDL::Rotation::RPY(data[3], data[4], data[5]); 152 | tf::Transform tf; 153 | tf::transformKDLToTF(frame, tf); 154 | tf::transformTFToEigen(tf, iso); 155 | } 156 | } 157 | 158 | void FIROrder2::getOutput(Eigen::Isometry3d &iso) 159 | { 160 | double *data = &output[0]; 161 | // for (int i = 0; i < 7; i++) { 162 | // printf("Out data %i , %f\n", i, data[i]); 163 | // } 164 | 165 | conversion(iso, data, true); 166 | } 167 | -------------------------------------------------------------------------------- /include/slamdunk/impl/ratio_matcher.hpp: -------------------------------------------------------------------------------- 1 | 2 | #include "slamdunk/feature_matcher.h" 3 | #include 4 | 5 | template 6 | void slamdunk::RatioMatcher::setModels(const std::vector& models) 7 | { 8 | assert(cv::DataType::type == models[0].type()); 9 | 10 | m_model_zeroes.resize(models.size()); 11 | int nDesc = 0; 12 | for(unsigned i = 0; i < models.size(); ++i) 13 | { 14 | m_model_zeroes[i] = nDesc; 15 | nDesc += models[i].rows; 16 | } 17 | m_descriptors.create(nDesc, models[0].cols); 18 | m_model_indices.clear(); 19 | m_model_indices.reserve(nDesc); 20 | for(unsigned i = 0; i < models.size(); ++i) 21 | { 22 | m_model_indices.insert(m_model_indices.end(), (size_t)models[i].rows, i); 23 | models[i].copyTo(m_descriptors.rowRange(m_model_zeroes[i], m_model_zeroes[i]+models[i].rows)); 24 | } 25 | assert((int)m_model_indices.size() == nDesc); 26 | 27 | delete m_index; 28 | m_index = createIndex(flann::Matrix(m_descriptors[0], m_descriptors.rows, m_descriptors.cols)); 29 | m_index->buildIndex(); 30 | } 31 | 32 | namespace 33 | { 34 | struct MatchHypothesis 35 | { 36 | int feat_gidx, query_idx; 37 | float ratio; 38 | }; 39 | 40 | bool matchH_comparison(const MatchHypothesis& mh0, const MatchHypothesis& mh1) 41 | { 42 | return mh0.ratio < mh1.ratio; 43 | } 44 | } 45 | 46 | template 47 | void slamdunk::RatioMatcher::match(cv::Mat query, std::vector& matches) 48 | { 49 | assert(cv::DataType::type == query.type()); 50 | assert(query.cols == m_descriptors.cols); 51 | 52 | typedef typename FLANNDistanceTp::ResultType DistanceType; 53 | 54 | flann::SearchParams search_params(16, 0, true); 55 | //flann::SearchParams search_params(-1, 0, true); 56 | search_params.cores = m_cores; 57 | flann::Matrix query_mtx(query.ptr(0), query.rows, query.cols, query.step.p[0]); 58 | std::vector > indices(SLAM_DUNK_KNN_NEIGHBORS * query.rows); 59 | std::vector > dists(SLAM_DUNK_KNN_NEIGHBORS * query.rows); 60 | 61 | //flann::Matrix indices(new int[SLAM_DUNK_KNN_NEIGHBORS * query.rows], query.rows, SLAM_DUNK_KNN_NEIGHBORS); 62 | //flann::Matrix dists(new DistanceType[SLAM_DUNK_KNN_NEIGHBORS * query.rows], query.rows, SLAM_DUNK_KNN_NEIGHBORS); 63 | 64 | int n_matches = m_index->knnSearch(query_mtx, indices, dists, SLAM_DUNK_KNN_NEIGHBORS, search_params); 65 | 66 | std::list match_list; 67 | for(int k = 0; k < query.rows; ++k) 68 | { 69 | //const int* indexPtr = indices[k]; 70 | //const DistanceType* distsPtr = dists[k]; 71 | const std::vector &indexPtr = indices[k]; 72 | const std::vector &distsPtr = dists[k]; 73 | 74 | if(indexPtr.size() < 2) 75 | { 76 | continue; 77 | } 78 | 79 | const unsigned model1 = m_model_indices[ indexPtr[0] ]; 80 | 81 | // find 2nd nearest neighbor descriptor 82 | for(unsigned ne = 1; ne < indexPtr.size(); ++ne) 83 | { 84 | assert(distsPtr[0] <= distsPtr[ne]); 85 | 86 | // no need to check the distance because results are sorted 87 | if((m_model_indices[ indexPtr[ne] ] != model1 && m_inter_model_ratio) || 88 | (m_model_indices[ indexPtr[ne] ] == model1 && !m_inter_model_ratio)) 89 | { 90 | if(distsPtr[0] <= m_ratio2 * distsPtr[ne] && distsPtr[ne] > 0) 91 | { 92 | MatchHypothesis mh; 93 | mh.feat_gidx = (int)indexPtr[0]; 94 | mh.query_idx = k; 95 | mh.ratio = distsPtr[0] / (float)distsPtr[ne]; 96 | match_list.push_back(mh); 97 | } 98 | break; 99 | } 100 | } 101 | } 102 | match_list.sort(matchH_comparison); 103 | 104 | // clear double matches 105 | std::vector mask(m_model_indices.size(), true); 106 | matches.clear(); 107 | matches.reserve(match_list.size()); 108 | for(std::list::const_iterator it = match_list.begin(); it != match_list.end(); ++it) 109 | if(mask[it->feat_gidx]) 110 | { 111 | mask[it->feat_gidx] = false; 112 | FMatch fm; 113 | fm.m_model_idx = m_model_indices[ it->feat_gidx ]; 114 | fm.m_feat_idx = it->feat_gidx - m_model_zeroes[fm.m_model_idx]; 115 | fm.m_query_idx = it->query_idx; 116 | fm.m_match_score = 1.f - std::sqrt(it->ratio); 117 | matches.push_back(fm); 118 | } 119 | 120 | //delete[] indices.ptr(); 121 | //delete[] dists.ptr(); 122 | } 123 | 124 | 125 | template 126 | typename slamdunk::RatioMatcherKDTreeIndex::FLANNIndex* 127 | slamdunk::RatioMatcherKDTreeIndex::createIndex(const flann::Matrix& features) const 128 | { 129 | return new FLANNIndex(features, flann::KDTreeIndexParams(4)); 130 | //return new FLANNIndex(features, flann::KDTreeIndexParams(1)); 131 | } 132 | 133 | template 134 | typename slamdunk::RatioMatcherLSHIndex::FLANNIndex* 135 | slamdunk::RatioMatcherLSHIndex::createIndex(const flann::Matrix& features) const 136 | { 137 | return new FLANNIndex(features, flann::LshIndexParams(1)); 138 | } 139 | -------------------------------------------------------------------------------- /src/slamdunk_extension/Utility.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | 11 | Utility::Utility() 12 | { 13 | } 14 | 15 | Utility::Utility(const Utility &orig) 16 | { 17 | } 18 | 19 | Utility::~Utility() 20 | { 21 | } 22 | 23 | /** 24 | * KDL Frame to Eigen Matrix 4x4 25 | * @param frame KDL Frame 26 | * @param mat Eigen Matrix 4x4 27 | */ 28 | void Utility::kdl_to_eigen_4x4_d(KDL::Frame &frame, Eigen::Matrix4d &mat) 29 | { 30 | for (int i = 0; i < 3; i++) 31 | { 32 | for (int j = 0; j < 3; j++) 33 | { 34 | mat(i, j) = frame.M(i, j); 35 | } 36 | } 37 | 38 | mat(0, 3) = frame.p[0]; 39 | mat(1, 3) = frame.p[1]; 40 | mat(2, 3) = frame.p[2]; 41 | mat(3, 3) = 1; 42 | } 43 | 44 | /** 45 | * Creates KDL Frame 46 | * @param x 47 | * @param y 48 | * @param z 49 | * @param roll 50 | * @param pitch 51 | * @param yaw 52 | * @param out_frame OUTPUT 53 | */ 54 | void Utility::create_kdl_frame(float x, float y, float z, float roll, float pitch, float yaw, KDL::Frame &out_frame) 55 | { 56 | 57 | out_frame.M = KDL::Rotation::RPY( 58 | roll, 59 | pitch, 60 | yaw); 61 | 62 | out_frame.p[0] = x; 63 | out_frame.p[1] = y; 64 | out_frame.p[2] = z; 65 | } 66 | 67 | /** 68 | * Creates Eigen4x4 Matrix 69 | * @param x 70 | * @param y 71 | * @param z 72 | * @param roll 73 | * @param pitch 74 | * @param yaw 75 | * @param mat 76 | */ 77 | 78 | void Utility::create_eigen_4x4_d(float x, float y, float z, float roll, float pitch, float yaw, Eigen::Matrix4d &mat) 79 | { 80 | mat = Eigen::Matrix4d::Identity(); 81 | KDL::Frame frame; 82 | create_kdl_frame(x, y, z, roll, pitch, yaw, frame); 83 | kdl_to_eigen_4x4_d(frame, mat); 84 | } 85 | 86 | /** 87 | * Builds TF from geometry_msgs::Pose TODO: reverse 88 | */ 89 | 90 | void Utility::eigen_4x4_to_geometrypose_d(Eigen::Matrix4d &mat, geometry_msgs::Pose &pose) 91 | { 92 | 93 | pose.position.x = mat(0, 3); 94 | pose.position.y = mat(1, 3); 95 | pose.position.z = mat(2, 3); 96 | 97 | tf::Matrix3x3 m; 98 | for (int i = 0; i < 3; i++) 99 | for (int j = 0; j < 3; j++) 100 | m[i][j] = mat(i, j); 101 | 102 | tf::Quaternion q; 103 | m.getRotation(q); 104 | q.normalize(); 105 | pose.orientation.x = q.x(); 106 | pose.orientation.y = q.y(); 107 | pose.orientation.z = q.z(); 108 | pose.orientation.w = q.w(); 109 | } 110 | 111 | /** 112 | * Builds TF from geometry_msgs::Pose TODO: reverse 113 | */ 114 | void Utility::eigen_4x4_d_to_tf(Eigen::Matrix4d &t, tf::Transform &tf, bool reverse = false) 115 | { 116 | if (!reverse) 117 | { 118 | tf.setOrigin(tf::Vector3( 119 | t(0, 3), 120 | t(1, 3), 121 | t(2, 3))); 122 | 123 | tf::Matrix3x3 rot; 124 | for (int i = 0; i < 3; i++) 125 | for (int j = 0; j < 3; j++) 126 | rot[i][j] = t(i, j); 127 | 128 | tf::Quaternion q; 129 | rot.getRotation(q); 130 | q.normalize(); 131 | tf.setRotation(q); 132 | } 133 | else 134 | { 135 | t = Eigen::Matrix4d::Identity(); 136 | t(0, 3) = tf.getOrigin()[0]; 137 | t(1, 3) = tf.getOrigin()[1]; 138 | t(2, 3) = tf.getOrigin()[2]; 139 | 140 | tf::Matrix3x3 rot; 141 | rot.setRotation(tf.getRotation()); 142 | for (int i = 0; i < 3; i++) 143 | for (int j = 0; j < 3; j++) 144 | t(i, j) = rot[i][j]; 145 | } 146 | } 147 | 148 | void Utility::HSVtoRGB(double &r, double &g, double &b, double h, double s, double v) 149 | { 150 | int i; 151 | double f, p, q, t; 152 | 153 | if (s == 0) 154 | { 155 | // achromatic (grey) 156 | r = g = b = v; 157 | return; 158 | } 159 | 160 | h /= 60; // sector 0 to 5 161 | i = floor(h); 162 | f = h - i; // factorial part of h 163 | p = v * (1 - s); 164 | q = v * (1 - s * f); 165 | t = v * (1 - s * (1 - f)); 166 | 167 | switch (i) 168 | { 169 | case 0: 170 | r = v; 171 | g = t; 172 | b = p; 173 | break; 174 | case 1: 175 | r = q; 176 | g = v; 177 | b = p; 178 | break; 179 | case 2: 180 | r = p; 181 | g = v; 182 | b = t; 183 | break; 184 | case 3: 185 | r = p; 186 | g = q; 187 | b = v; 188 | break; 189 | case 4: 190 | r = t; 191 | g = p; 192 | b = v; 193 | break; 194 | default: // case 5: 195 | r = v; 196 | g = p; 197 | b = q; 198 | break; 199 | } 200 | } -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/skigrid.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 100000 5000.000000 0.076717 0.014643 202560.000000 2 | kdskip 2 110000 5000.000000 0.071056 0.015742 206084.000000 3 | kdskip 2 120000 5000.000000 0.079478 0.017339 212052.000000 4 | kdskip 2 130000 5000.000000 0.082662 0.018375 212668.000000 5 | kdskip 2 140000 5000.000000 0.085972 0.018895 220456.000000 6 | kdskip 2 150000 5000.000000 0.102983 0.020195 221912.000000 7 | kdskip 2 160000 5000.000000 0.099508 0.020574 224724.000000 8 | kdskip 2 170000 5000.000000 0.110839 0.025082 227112.000000 9 | kdskip 2 180000 5000.000000 0.114855 0.023075 231420.000000 10 | kdskip 2 190000 5000.000000 0.129466 0.023167 230592.000000 11 | kdskip 2 200000 5000.000000 0.122205 0.023060 234292.000000 12 | kdskip 2 210000 5000.000000 0.145698 0.024899 235644.000000 13 | kdskip 2 220000 5000.000000 0.136900 0.025224 237568.000000 14 | kdskip 2 230000 5000.000000 0.153932 0.025772 239704.000000 15 | kdskip 2 240000 5000.000000 0.160058 0.027673 243632.000000 16 | kdskip 2 250000 5000.000000 0.162231 0.028023 245660.000000 17 | kdskip 2 260000 5000.000000 0.166787 0.028840 248268.000000 18 | kdskip 2 270000 5000.000000 0.177735 0.027780 257960.000000 19 | kdskip 2 280000 5000.000000 0.178844 0.029966 257400.000000 20 | kdskip 2 290000 5000.000000 0.192591 0.030030 260256.000000 21 | kdskip 2 300000 5000.000000 0.190648 0.035357 261632.000000 22 | kdskip 2 310000 5000.000000 0.201358 0.030810 263428.000000 23 | kdskip 2 320000 5000.000000 0.199768 0.032693 263872.000000 24 | kdskip 2 330000 5000.000000 0.214105 0.032064 270112.000000 25 | kdskip 2 340000 5000.000000 0.232765 0.033049 270436.000000 26 | kdskip 2 350000 5000.000000 0.236780 0.032447 272224.000000 27 | kdskip 2 360000 5000.000000 0.242713 0.033324 273868.000000 28 | kdskip 2 370000 5000.000000 0.240236 0.035348 276396.000000 29 | kdskip 2 380000 5000.000000 0.243897 0.034202 281448.000000 30 | kdskip 2 390000 5000.000000 0.259697 0.035545 278676.000000 31 | kdskip 2 400000 5000.000000 0.258343 0.034882 284592.000000 32 | kdskip 2 410000 5000.000000 0.267142 0.036072 290680.000000 33 | kdskip 2 420000 5000.000000 0.272802 0.035430 285032.000000 34 | kdskip 2 430000 5000.000000 0.286110 0.036532 287508.000000 35 | kdskip 2 440000 5000.000000 0.293239 0.035554 288180.000000 36 | kdskip 2 450000 5000.000000 0.283041 0.037827 288968.000000 37 | kdskip 2 460000 5000.000000 0.303135 0.036951 295912.000000 38 | kdskip 2 470000 5000.000000 0.305072 0.037187 291264.000000 39 | kdskip 2 480000 5000.000000 0.322343 0.036703 291772.000000 40 | kdskip 2 490000 5000.000000 0.314284 0.039209 293096.000000 41 | kdskip 2 500000 5000.000000 0.319422 0.037577 298460.000000 42 | kdskip 2 510000 5000.000000 0.336004 0.042369 297428.000000 43 | kdskip 2 520000 5000.000000 0.340512 0.040016 300132.000000 44 | kdskip 2 530000 5000.000000 0.344659 0.039314 308464.000000 45 | kdskip 2 540000 5000.000000 0.343114 0.038566 309116.000000 46 | kdskip 2 550000 5000.000000 0.355234 0.040391 320476.000000 47 | kdskip 2 560000 5000.000000 0.360231 0.040761 316532.000000 48 | kdskip 2 570000 5000.000000 0.381767 0.041030 319992.000000 49 | kdskip 2 580000 5000.000000 0.371899 0.039055 316792.000000 50 | kdskip 2 590000 5000.000000 0.395021 0.042096 326076.000000 51 | kdskip 2 600000 5000.000000 0.386899 0.042342 319476.000000 52 | kdskip 2 610000 5000.000000 0.406034 0.041243 321776.000000 53 | kdskip 2 620000 5000.000000 0.400216 0.041989 325176.000000 54 | kdskip 2 630000 5000.000000 0.399271 0.041869 329480.000000 55 | kdskip 2 640000 5000.000000 0.426144 0.042768 330332.000000 56 | kdskip 2 650000 5000.000000 0.410524 0.043080 320136.000000 57 | kdskip 2 660000 5000.000000 0.431861 0.042659 334036.000000 58 | kdskip 2 670000 5000.000000 0.431861 0.042659 334036.000000 59 | kdskip 2 680000 5000.000000 0.435315 0.043951 334172.000000 60 | kdskip 2 690000 5000.000000 0.457893 0.042103 332336.000000 61 | kdskip 2 700000 5000.000000 0.452249 0.043038 342384.000000 62 | kdskip 2 710000 5000.000000 0.455499 0.042976 338556.000000 63 | kdskip 2 720000 5000.000000 0.481082 0.046235 342248.000000 64 | kdskip 2 730000 5000.000000 0.474365 0.043521 346956.000000 65 | kdskip 2 740000 5000.000000 0.478157 0.046456 340896.000000 66 | kdskip 2 750000 5000.000000 0.510607 0.044689 346792.000000 67 | kdskip 2 760000 5000.000000 0.497840 0.044617 345996.000000 68 | kdskip 2 770000 5000.000000 0.515403 0.045601 358100.000000 69 | kdskip 2 780000 5000.000000 0.521823 0.046944 353404.000000 70 | kdskip 2 790000 5000.000000 0.521157 0.044572 346560.000000 71 | kdskip 2 800000 5000.000000 0.520710 0.045507 357636.000000 72 | kdskip 2 810000 5000.000000 0.531248 0.045548 353128.000000 73 | kdskip 2 820000 5000.000000 0.528476 0.046087 359604.000000 74 | kdskip 2 830000 5000.000000 0.536096 0.045239 350700.000000 75 | kdskip 2 840000 5000.000000 0.550808 0.043768 355492.000000 76 | kdskip 2 850000 5000.000000 0.570555 0.047201 363976.000000 77 | kdskip 2 860000 5000.000000 0.586909 0.046017 367288.000000 78 | kdskip 2 870000 5000.000000 0.556248 0.047120 371216.000000 79 | kdskip 2 880000 5000.000000 0.574355 0.047624 361888.000000 80 | kdskip 2 890000 5000.000000 0.592651 0.046672 369864.000000 81 | kdskip 2 900000 5000.000000 0.600340 0.049674 364404.000000 82 | kdskip 2 910000 5000.000000 0.599507 0.050361 369412.000000 83 | kdskip 2 920000 5000.000000 0.609592 0.048879 368488.000000 84 | kdskip 2 930000 5000.000000 0.609127 0.049126 373900.000000 85 | kdskip 2 940000 5000.000000 0.621758 0.047502 373744.000000 86 | kdskip 2 950000 5000.000000 0.645881 0.048250 383604.000000 87 | kdskip 2 960000 5000.000000 0.661433 0.059319 381892.000000 88 | kdskip 2 970000 5000.000000 0.632415 0.050808 381800.000000 89 | kdskip 2 970000 5000.000000 0.635415 0.050808 381800.000000 90 | kdskip 2 990000 5000.000000 0.656017 0.048856 385232.000000 -------------------------------------------------------------------------------- /src/nodes/skimap_pose_filtering.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | //ROS 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | //OPENCV 27 | #include 28 | #include 29 | #include 30 | 31 | //Boost 32 | #include 33 | #include 34 | #include 35 | 36 | //Skimap 37 | #include 38 | #include 39 | #include 40 | 41 | //skimap 42 | typedef skimap::Voxel6DPose VoxelPose; 43 | typedef skimap::SkiMap SKIMAP; 44 | typedef skimap::SkiMap::Voxel3D Voxel3D; 45 | typedef skimap::SkiMap::Tiles2D Tiles2D; 46 | SKIMAP *map; 47 | 48 | //Ros 49 | ros::NodeHandle *nh; 50 | tf::TransformListener *tf_listener; 51 | tf::TransformBroadcaster *tf_broadcaster; 52 | ros::Publisher map_publisher; 53 | 54 | //Live parameters 55 | std::string base_frame_name = "world"; 56 | std::string target_frame_name = "camera"; 57 | 58 | /** 59 | */ 60 | struct MapParameters 61 | { 62 | float map_resolution; 63 | int min_voxel_weight; 64 | } mapParameters; 65 | 66 | /** 67 | * 68 | * @param argc 69 | * @param argv 70 | * @return 71 | */ 72 | int main(int argc, char **argv) 73 | { 74 | 75 | // Initialize ROS 76 | ros::init(argc, argv, "skimap_map_service"); 77 | nh = new ros::NodeHandle("~"); 78 | tf_listener = new tf::TransformListener(); 79 | tf_broadcaster = new tf::TransformBroadcaster(); 80 | //Frames 81 | nh->param("base_frame_name", base_frame_name, "world"); 82 | nh->param("target_frame_name", target_frame_name, "random"); 83 | std::string fitlered_name = target_frame_name + "_fi"; 84 | //Map Publisher 85 | std::string map_topic = nh->param("map_topic", "live_map"); 86 | map_publisher = nh->advertise(map_topic, 1); 87 | 88 | int hz; 89 | nh->param("hz", hz, 30); 90 | 91 | //Visualization 92 | bool auto_publish_markers = false; 93 | nh->param("auto_publish_markers", auto_publish_markers, false); 94 | 95 | //SkiMap 96 | nh->param("map_resolution", mapParameters.map_resolution, 0.05f); 97 | map = new SKIMAP(mapParameters.map_resolution); 98 | 99 | // Spin & Time 100 | ros::Rate r(hz); 101 | 102 | // Spin 103 | while (nh->ok()) 104 | { 105 | 106 | tf::StampedTransform target; 107 | try 108 | { 109 | tf_listener->lookupTransform(base_frame_name, target_frame_name, ros::Time(0), target); 110 | ROS_INFO("%f", target.getOrigin().x()); 111 | 112 | cv::Vec3f trans; 113 | cv::Vec4f rot; 114 | trans[0] = target.getOrigin().x(); 115 | trans[1] = target.getOrigin().y(); 116 | trans[2] = target.getOrigin().z(); 117 | 118 | rot[0] = target.getRotation().x(); 119 | rot[1] = target.getRotation().y(); 120 | rot[2] = target.getRotation().z(); 121 | rot[3] = target.getRotation().w(); 122 | 123 | VoxelPose vp(target_frame_name + "_fi", trans, rot); 124 | 125 | map->integrateVoxel( 126 | trans[0], trans[1], trans[2], 127 | &vp); 128 | } 129 | catch (tf::TransformException ex) 130 | { 131 | ROS_ERROR("%s", ex.what()); 132 | ros::Duration(1.0).sleep(); 133 | } 134 | 135 | std::vector voxels; 136 | map->fetchVoxels(voxels); 137 | int max_voxel = -1; 138 | int max_size = -1; 139 | for (int i = 0; i < voxels.size(); i++) 140 | { 141 | Voxel3D &v = voxels[i]; 142 | if (v.data->size() > max_size) 143 | { 144 | max_size = v.data->size(); 145 | max_voxel = i; 146 | } 147 | } 148 | if (max_voxel >= 0) 149 | { 150 | Voxel3D &v = voxels[max_voxel]; 151 | cv::Vec3f trans; 152 | cv::Vec4f rot; 153 | v.data->average(trans, rot); 154 | ROS_INFO("%f,%f,%f", 155 | trans[0], 156 | trans[1], 157 | trans[2]); 158 | 159 | tf::Vector3 t(trans[0], trans[1], trans[2]); 160 | tf::Quaternion q(rot[0], rot[1], rot[2], rot[3]); 161 | tf::Transform filtered; 162 | filtered.setOrigin(t); 163 | filtered.setRotation(q); 164 | tf_broadcaster->sendTransform( 165 | tf::StampedTransform(filtered, ros::Time::now(), 166 | base_frame_name, 167 | fitlered_name)); 168 | } 169 | ROS_INFO("Size: %d", int(voxels.size())); 170 | 171 | ros::spinOnce(); 172 | r.sleep(); 173 | } 174 | } 175 | -------------------------------------------------------------------------------- /include/skimap/SkiMap.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights 3 | * Reserved 4 | * You may use, distribute and modify this code under the 5 | * terms of the GNU GPLv3 license. 6 | * 7 | * please write to: d.degregorio@unibo.it 8 | */ 9 | 10 | #ifndef SKIMAP_HPP 11 | #define SKIMAP_HPP 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define SKIPLISTMAP_MAX_DEPTH 16 21 | 22 | namespace skimap { 23 | 24 | /** 25 | * 26 | * @param min_index 27 | * @param max_index 28 | */ 29 | template 31 | class SkiMap : public SkipListMapV2 { 32 | public: 33 | typedef GenericTile2D Tiles2D; 34 | typedef SkipListMapV2 ParentMap; 35 | typedef typename ParentMap::X_NODE X_NODE; 36 | typedef typename ParentMap::Y_NODE Y_NODE; 37 | typedef typename ParentMap::Z_NODE Z_NODE; 38 | 39 | /** 40 | * 41 | * @param min_index 42 | * @param max_index 43 | * @param resolution_x 44 | * @param resolution_y 45 | * @param resolution_z 46 | */ 47 | SkiMap(K min_index, K max_index, D resolution_x, D resolution_y, 48 | D resolution_z, D zero_level = D(0.0)) 49 | : ParentMap(min_index, max_index, resolution_x, resolution_y, 50 | resolution_z) { 51 | setZeroLevel(zero_level); 52 | } 53 | 54 | /** 55 | */ 56 | SkiMap(D resolution, D zero_level = D(0.0)) : ParentMap(resolution) { 57 | setZeroLevel(D(zero_level)); 58 | } 59 | 60 | /** 61 | */ 62 | SkiMap() : ParentMap() { setZeroLevel(D(0.0)); } 63 | 64 | /** 65 | * 66 | */ 67 | virtual ~SkiMap() {} 68 | 69 | /** 70 | * 71 | * @param x 72 | * @param y 73 | * @param z 74 | * @param data 75 | * @return 76 | */ 77 | virtual bool integrateTile(D x, D y) { 78 | K ix, iy, iz; 79 | if (this->coordinatesToIndex(x, y, _zero_level, ix, iy, iz)) { 80 | return integrateTile(ix, iy, iz); 81 | } 82 | return false; 83 | } 84 | 85 | /** 86 | * 87 | * @param ix 88 | * @param iy 89 | * @param iz 90 | * @param data 91 | * @return 92 | */ 93 | virtual bool integrateTile(K ix, K iy, K iz) { 94 | if (this->isValidIndex(ix, iy, iz)) { 95 | if (this->_batch_integration) { 96 | this->_current_integration_map.addEntry(ix, iy, iz, NULL, 2); 97 | printf("Batched\n"); 98 | } else { 99 | const typename X_NODE::NodeType *ylist = this->_root_list->find(ix); 100 | if (ylist == NULL) { 101 | ylist = this->_root_list->insert( 102 | ix, new Y_NODE(this->_min_index_value, this->_max_index_value)); 103 | this->_bytes_counter += 104 | sizeof(typename X_NODE::NodeType) + sizeof(Y_NODE); 105 | } 106 | const typename Y_NODE::NodeType *zlist = ylist->value->find(iy); 107 | if (zlist == NULL) { 108 | zlist = ylist->value->insert( 109 | iy, new Z_NODE(this->_min_index_value, this->_max_index_value)); 110 | this->_bytes_counter += 111 | sizeof(typename Y_NODE::NodeType) + sizeof(Z_NODE); 112 | } 113 | } 114 | return true; 115 | } 116 | return false; 117 | } 118 | 119 | /** 120 | * 121 | * @param voxels 122 | */ 123 | virtual void fetchTiles(std::vector &voxels, D min_voxel_height) { 124 | voxels.clear(); 125 | std::vector xnodes; 126 | this->_root_list->retrieveNodes(xnodes); 127 | 128 | #pragma omp parallel 129 | { 130 | std::vector voxels_private; 131 | 132 | #pragma omp for nowait 133 | for (int i = 0; i < xnodes.size(); i++) { 134 | K ix, iy, iz; 135 | D x, y, z; 136 | std::vector ynodes; 137 | xnodes[i]->value->retrieveNodes(ynodes); 138 | 139 | for (int j = 0; j < ynodes.size(); j++) { 140 | std::vector znodes; 141 | 142 | ix = xnodes[i]->key; 143 | iy = ynodes[j]->key; 144 | iz = _zero_level_key; 145 | this->indexToCoordinates(ix, iy, iz, x, y, z); 146 | 147 | if (ynodes[j]->value->empty()) { 148 | voxels_private.push_back(Tiles2D(x, y, z, NULL)); 149 | } else { 150 | typename Z_NODE::NodeType *first_voxel = 151 | ynodes[j]->value->findNearest(_zero_level_key); 152 | if (first_voxel == NULL) { 153 | voxels_private.push_back(Tiles2D(x, y, z, NULL)); 154 | } else { 155 | D vh; 156 | this->singleIndexToCoordinate(first_voxel->key, vh, 157 | this->_resolution_z); 158 | if (vh > min_voxel_height) { 159 | voxels_private.push_back(Tiles2D(x, y, z, NULL)); 160 | } else { 161 | voxels_private.push_back(Tiles2D(x, y, z, first_voxel->value)); 162 | } 163 | } 164 | } 165 | } 166 | } 167 | 168 | #pragma omp critical 169 | voxels.insert(voxels.end(), voxels_private.begin(), voxels_private.end()); 170 | } 171 | } 172 | 173 | /** 174 | * 175 | * @param zero_level 176 | */ 177 | void setZeroLevel(D zero_level) { 178 | _zero_level = zero_level; 179 | _zero_level_key = K(floor(_zero_level / this->_resolution_z)); 180 | } 181 | 182 | protected: 183 | D _zero_level; 184 | K _zero_level_key; 185 | }; 186 | } 187 | 188 | #endif /* SKIMAP_HPP */ 189 | -------------------------------------------------------------------------------- /include/skimap/voxels/VoxelDataMultiLabel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef VOXELDATAMULTILABEL_HPP 10 | #define VOXELDATAMULTILABEL_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace skimap 20 | { 21 | /** 22 | * Voxel Data containing Multi-Labels values and relatives Weight. 23 | * L template represents datatype for a Label. 24 | * W template represents datatype for weights. 25 | */ 26 | template 27 | struct VoxelDataMultiLabel 28 | { 29 | std::map labels_map; 30 | 31 | /** 32 | * Pointer Copy Constructor. 33 | * @param data source data 34 | */ 35 | VoxelDataMultiLabel(VoxelDataMultiLabel *data) 36 | { 37 | labels_map = data->labels_map; 38 | } 39 | 40 | /** 41 | * Void Constructor. 42 | */ 43 | VoxelDataMultiLabel() 44 | { 45 | } 46 | 47 | /** 48 | * Constructor with start Label. 49 | * @param label single Label 50 | * @param weight label weight 51 | */ 52 | VoxelDataMultiLabel(L label, W weight) 53 | { 54 | this->labels_map[label] = weight; 55 | } 56 | 57 | /** 58 | * Sum Overload. Defines Sum operations between Voxels 59 | * @param v2 second addend 60 | * @return sum 61 | */ 62 | VoxelDataMultiLabel operator+(const VoxelDataMultiLabel &v2) const 63 | { 64 | VoxelDataMultiLabel v1 = *this; 65 | if (v1.labels_map.size() <= 0) 66 | return v2; 67 | VoxelDataMultiLabel d; 68 | d.labels_map = v1.labels_map; 69 | for (auto it = v2.labels_map.begin(); it != v2.labels_map.end(); ++it) 70 | { 71 | if (d.labels_map.find(it->first) != d.labels_map.end()) 72 | { 73 | d.labels_map[it->first] += it->second; 74 | } 75 | else 76 | { 77 | d.labels_map[it->first] = it->second; 78 | } 79 | } 80 | return d; 81 | } 82 | 83 | /** 84 | * Subtraction Overload. Defines Subtraction operations between Voxels 85 | * @param v2 second minuend 86 | * @return subtraction 87 | */ 88 | VoxelDataMultiLabel operator-(const VoxelDataMultiLabel &v2) const 89 | { 90 | VoxelDataMultiLabel v1 = *this; 91 | if (v1.labels_map.size() <= 0) 92 | return v2; 93 | VoxelDataMultiLabel d; 94 | d.labels_map = v1.labels_map; 95 | for (auto it = v2.labels_map.begin(); it != v2.labels_map.end(); ++it) 96 | { 97 | if (d.labels_map.find(it->first) != d.labels_map.end()) 98 | { 99 | d.labels_map[it->first] -= it->second; 100 | } 101 | } 102 | return d; 103 | } 104 | 105 | /** 106 | * @return the Label with higher Weight 107 | */ 108 | L heavierLabel(bool only_positive = false) 109 | { 110 | W max = W(0); 111 | L max_label = L(-1); 112 | for (auto it = labels_map.begin(); it != labels_map.end(); ++it) 113 | { 114 | if (only_positive) 115 | { 116 | if (it->second < 0) 117 | continue; 118 | } 119 | if (it->second > max) 120 | { 121 | max = it->second; 122 | max_label = it->first; 123 | } 124 | } 125 | return max_label; 126 | } 127 | 128 | /** 129 | * @return the higher Weight 130 | */ 131 | W heavierWeight(bool only_positive = false) 132 | { 133 | W max = W(0); 134 | for (auto it = labels_map.begin(); it != labels_map.end(); ++it) 135 | { 136 | if (only_positive) 137 | { 138 | if (it->second < 0) 139 | continue; 140 | } 141 | if (it->second > max) 142 | { 143 | max = it->second; 144 | } 145 | } 146 | return max; 147 | } 148 | 149 | /** 150 | * @param label target Label 151 | * @return weight of target label 152 | */ 153 | W weightOf(L label) 154 | { 155 | if (labels_map.find(label) != labels_map.end()) 156 | { 157 | return labels_map[label]; 158 | } 159 | return -1.0; 160 | } 161 | 162 | /** 163 | * Serializes object into stream. 164 | */ 165 | friend std::ostream &operator<<(std::ostream &os, const VoxelDataMultiLabel &voxel) 166 | { 167 | int count = 0; 168 | os << std::setprecision(std::numeric_limits::digits10 + 2); 169 | os << voxel.labels_map.size() << " "; 170 | for (auto it = voxel.labels_map.begin(); it != voxel.labels_map.end(); ++it) 171 | { 172 | os << double(it->first) << " " << double(it->second); 173 | if (count < voxel.labels_map.size() - 1) 174 | { 175 | os << " "; 176 | } 177 | count++; 178 | } 179 | return os; 180 | } 181 | 182 | /** 183 | * Hydrates object from stream. 184 | */ 185 | friend std::istream &operator>>(std::istream &is, VoxelDataMultiLabel &voxel) 186 | { 187 | 188 | int size; 189 | is >> size; 190 | for (int i = 0; i < size; i++) 191 | { 192 | double l; 193 | double w; 194 | is >> l; 195 | is >> w; 196 | voxel.labels_map[L(l)] = W(w); 197 | } 198 | 199 | return is; 200 | } 201 | }; 202 | } 203 | 204 | #endif /* VOXELDATAMULTILABEL_HPP */ 205 | -------------------------------------------------------------------------------- /src/nodes/experiments/skimap_grid_test_python/grid.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 2 100000 5000.000000 0.046346 0.016994 193312.000000 2 | flann_kdtree 2 110000 5000.000000 0.051401 0.018557 196388.000000 3 | flann_kdtree 2 120000 5000.000000 0.056205 0.021295 199216.000000 4 | flann_kdtree 2 130000 5000.000000 0.059961 0.021627 202060.000000 5 | flann_kdtree 2 140000 5000.000000 0.064279 0.025004 205076.000000 6 | flann_kdtree 2 150000 5000.000000 0.070268 0.027968 209040.000000 7 | flann_kdtree 2 160000 5000.000000 0.074698 0.028956 211332.000000 8 | flann_kdtree 2 170000 5000.000000 0.078996 0.030612 213704.000000 9 | flann_kdtree 2 180000 5000.000000 0.085213 0.030549 217224.000000 10 | flann_kdtree 2 190000 5000.000000 0.089222 0.035360 220820.000000 11 | flann_kdtree 2 200000 5000.000000 0.094328 0.034587 221580.000000 12 | flann_kdtree 2 210000 5000.000000 0.099698 0.036791 225608.000000 13 | flann_kdtree 2 220000 5000.000000 0.104937 0.039559 227616.000000 14 | flann_kdtree 2 230000 5000.000000 0.109824 0.043106 230600.000000 15 | flann_kdtree 2 240000 5000.000000 0.114685 0.042842 233476.000000 16 | flann_kdtree 2 250000 5000.000000 0.119633 0.045482 235920.000000 17 | flann_kdtree 2 260000 5000.000000 0.124596 0.049127 239028.000000 18 | flann_kdtree 2 270000 5000.000000 0.129150 0.047367 243608.000000 19 | flann_kdtree 2 280000 5000.000000 0.136196 0.052937 245800.000000 20 | flann_kdtree 2 290000 5000.000000 0.140763 0.054738 251380.000000 21 | flann_kdtree 2 300000 5000.000000 0.146184 0.053247 251780.000000 22 | flann_kdtree 2 310000 5000.000000 0.146231 0.057544 253924.000000 23 | flann_kdtree 2 320000 5000.000000 0.160363 0.059880 257280.000000 24 | flann_kdtree 2 330000 5000.000000 0.160902 0.063274 262500.000000 25 | flann_kdtree 2 340000 5000.000000 0.167727 0.066765 262308.000000 26 | flann_kdtree 2 350000 5000.000000 0.172225 0.066499 265272.000000 27 | flann_kdtree 2 360000 5000.000000 0.173468 0.067301 270148.000000 28 | flann_kdtree 2 370000 5000.000000 0.181073 0.071750 273560.000000 29 | flann_kdtree 2 380000 5000.000000 0.181357 0.075128 273584.000000 30 | flann_kdtree 2 390000 5000.000000 0.190327 0.077810 277336.000000 31 | flann_kdtree 2 400000 5000.000000 0.197791 0.076599 280284.000000 32 | flann_kdtree 2 410000 5000.000000 0.199243 0.081636 283060.000000 33 | flann_kdtree 2 420000 5000.000000 0.202521 0.079387 284252.000000 34 | flann_kdtree 2 430000 5000.000000 0.213564 0.086856 287656.000000 35 | flann_kdtree 2 440000 5000.000000 0.215844 0.082794 289952.000000 36 | flann_kdtree 2 450000 5000.000000 0.224416 0.089409 293300.000000 37 | flann_kdtree 2 460000 5000.000000 0.223469 0.089337 296832.000000 38 | flann_kdtree 2 470000 5000.000000 0.233850 0.087861 298580.000000 39 | flann_kdtree 2 480000 5000.000000 0.235714 0.091333 301640.000000 40 | flann_kdtree 2 490000 5000.000000 0.244503 0.095349 304172.000000 41 | flann_kdtree 2 500000 5000.000000 0.246114 0.096392 306476.000000 42 | flann_kdtree 2 510000 5000.000000 0.252237 0.099831 309644.000000 43 | flann_kdtree 2 520000 5000.000000 0.250320 0.102615 312480.000000 44 | flann_kdtree 2 530000 5000.000000 0.258652 0.109023 316576.000000 45 | flann_kdtree 2 540000 5000.000000 0.267295 0.105436 318724.000000 46 | flann_kdtree 2 550000 5000.000000 0.275703 0.114185 321984.000000 47 | flann_kdtree 2 560000 5000.000000 0.277154 0.113799 326008.000000 48 | flann_kdtree 2 570000 5000.000000 0.285026 0.120942 329376.000000 49 | flann_kdtree 2 580000 5000.000000 0.293696 0.115436 331064.000000 50 | flann_kdtree 2 590000 5000.000000 0.288798 0.117910 335592.000000 51 | flann_kdtree 2 600000 5000.000000 0.293639 0.122107 336408.000000 52 | flann_kdtree 2 610000 5000.000000 0.301777 0.129777 339204.000000 53 | flann_kdtree 2 620000 5000.000000 0.311241 0.123899 344028.000000 54 | flann_kdtree 2 630000 5000.000000 0.310985 0.127803 347940.000000 55 | flann_kdtree 2 640000 5000.000000 0.323982 0.127785 349352.000000 56 | flann_kdtree 2 650000 5000.000000 0.320276 0.134452 350828.000000 57 | flann_kdtree 2 660000 5000.000000 0.335780 0.135812 353628.000000 58 | flann_kdtree 2 670000 5000.000000 0.342950 0.134997 356528.000000 59 | flann_kdtree 2 680000 5000.000000 0.342897 0.138222 358552.000000 60 | flann_kdtree 2 690000 5000.000000 0.351123 0.143474 362300.000000 61 | flann_kdtree 2 700000 5000.000000 0.354193 0.145888 364384.000000 62 | flann_kdtree 2 710000 5000.000000 0.365124 0.144520 369532.000000 63 | flann_kdtree 2 720000 5000.000000 0.363951 0.150696 371196.000000 64 | flann_kdtree 2 730000 5000.000000 0.369679 0.141569 372092.000000 65 | flann_kdtree 2 740000 5000.000000 0.377547 0.155293 374808.000000 66 | flann_kdtree 2 750000 5000.000000 0.377307 0.149474 377164.000000 67 | flann_kdtree 2 760000 5000.000000 0.383280 0.149424 381756.000000 68 | flann_kdtree 2 770000 5000.000000 0.390682 0.163685 384252.000000 69 | flann_kdtree 2 780000 5000.000000 0.407434 0.161681 386948.000000 70 | flann_kdtree 2 790000 5000.000000 0.407307 0.172774 388932.000000 71 | flann_kdtree 2 800000 5000.000000 0.411546 0.164009 391256.000000 72 | flann_kdtree 2 810000 5000.000000 0.414256 0.166512 394768.000000 73 | flann_kdtree 2 820000 5000.000000 0.415517 0.182697 398752.000000 74 | flann_kdtree 2 830000 5000.000000 0.427865 0.181019 400644.000000 75 | flann_kdtree 2 840000 5000.000000 0.434584 0.173961 402848.000000 76 | flann_kdtree 2 850000 5000.000000 0.440886 0.187881 405964.000000 77 | flann_kdtree 2 860000 5000.000000 0.449422 0.173209 409084.000000 78 | flann_kdtree 2 870000 5000.000000 0.450269 0.187391 413048.000000 79 | flann_kdtree 2 880000 5000.000000 0.461508 0.188192 415420.000000 80 | flann_kdtree 2 890000 5000.000000 0.458906 0.198487 417500.000000 81 | flann_kdtree 2 900000 5000.000000 0.472267 0.199043 420628.000000 82 | flann_kdtree 2 910000 5000.000000 0.480268 0.186195 424344.000000 83 | flann_kdtree 2 920000 5000.000000 0.479183 0.191740 426432.000000 84 | flann_kdtree 2 930000 5000.000000 0.483179 0.191624 429200.000000 85 | flann_kdtree 2 940000 5000.000000 0.493064 0.207416 432332.000000 86 | flann_kdtree 2 950000 5000.000000 0.500798 0.216697 434736.000000 87 | flann_kdtree 2 960000 5000.000000 0.511908 0.202779 438276.000000 88 | flann_kdtree 2 970000 5000.000000 0.510715 0.218871 440736.000000 89 | flann_kdtree 2 980000 5000.000000 0.516266 0.209189 442672.000000 90 | flann_kdtree 2 990000 5000.000000 0.534281 0.232136 445252.000000 -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # skimap_ros 2 | **Author:** [Daniele De Gregorio](https://www.unibo.it/sitoweb/d.degregorio/) 3 | 4 | This package contains the **SkiMap Mapping Framework** described here: 5 | 6 | > D. De Gregorio and L. Di Stefano, **SkiMap: An efficient mapping framework for robot navigation**, 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 2017, pp. 2569-2576. 7 | doi: 10.1109/ICRA.2017.7989299 [**LINK**](http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=7989299&isnumber=7988677) || [**PDF ARXIV**](https://arxiv.org/abs/1704.05832) 8 | 9 | Watch the Video: 10 | 11 | paper video 13 | 14 | 15 | 16 | The framework si wrapped in a ROS package to maximize portability but, since it is an *Header-Only* library, 17 | can be easily used elsewhere. 18 | 19 | This package is equipped with a sample BAG file: 20 | [tiago_lar.bag](https://drive.google.com/open?id=0B02158j5inr3Tm9nQjhIQ3Fua3c) 21 | . This bag was collected by means of a mobile robot with an head-mounting 22 | RGB-D camera. So it provides not only RGB-D frames but also a whole TF tree, including Odometry informations. 23 | For this reason this bag can be used as source for **SkiMap** mapping using Odometry to track Camera pose or as source 24 | for **Slam+SkiMap** duet where Camera is tracked by the Slam System. 25 | 26 | ### Dependencies 27 | 28 | * OpenMP 29 | * Eigen3 30 | * OpenCV (2.4) 31 | * Boost 32 | 33 | ### LICENSE 34 | You may use, distribute and modify this code under the terms of the GNU GPLv3 license. 35 | If you use *SkiMap* in an academic work, please cite: 36 | ``` 37 | @inproceedings{degregorio2017skimap, 38 | title={SkiMap: An Efficient Mapping Framework for Robot Navigation}, 39 | author={De Gregorio, Daniele and Di Stefano, Luigi}, 40 | booktitle={Robotics and Automation (ICRA), 2017 IEEE International Conference on}, 41 | year={2017} 42 | } 43 | ``` 44 | ## Simple Example 45 | 46 | The [Tutorial: integration_of_random_points.cpp ](https://github.com/m4nh/skimap_ros/blob/master/src/nodes/tutorials/integration_of_random_points.cpp) is a simple tutorial for the use of SkiMap with your own software. The example just shows how to integrate random 3D points in the map and how to retrieve them (e.g. for visualization purposes). This example is intentionally unlinked from ROS library to show the usage of the data structure only. 47 | 48 | ## SkiMap live mapping: *skimap_live.launch* 49 | 50 | If the Camera 6-DOF pose is available (e.g. the camera is in a eye-to-hand configuration..) we can use **SkiMap** 51 | as a classical Mapping Framework. Furthermore, as described in the paper, if the Global Reference Frames of the Camera Poses 52 | lies on ground then SkiMap is able to perform also a 2D Map of the environment simultaneously with the 3D Map. 53 | The attached BAG is an useful example to understand the operation of this node because it is collected with a mobile robot 54 | with an head-mounted RGB-D camera so the Global Reference frame is certainly on the Ground (Odometry is the Global RF). To 55 | launch the example you have to run two separate commands: 56 | 57 | 58 | ``` 59 | 1) roslaunch skimap_ros skimap_live.launch 60 | 2) rosbag play tiago_lar.bag --clock 61 | ``` 62 | 63 | If you want to run *skimap_live.launch* in your real context you have to change the Camera Topics parameters and the TF parameters 64 | in the launch file: 65 | 66 | 67 | ``` 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | ``` 77 | 78 | Remember to set as *base_frame_name* the name of the TF representing the World Frame, or the Fixed Frame, in which the 79 | *camera_frame_name* is represented. Should be noted that **SkiMap** is not responsible to produce these frames but it uses 80 | them to build the map, so the accuracy of the reconstruction depends on the accuracy of them. 81 | 82 | ## SkiMap Service: *skimap_map_service.launch* 83 | 84 | A less intrusive manner to use SkiMap is the Map Service launching *skimap_map_service.launch*. This service use *SkimapIntegrationService.srv* as interface to integrate new measurements in the global map. To understand how to create a client for this service you can take a look at *SkiMapServiceClient.hpp* (or you can just use it!). To instantiate our off-the-shelf client just do: 85 | 86 | ``` 87 | skimap_ros::SkimapServiceClient* skimap_service_client = new skimap_ros::SkimapServiceClient(&nh, "/skimap_map_service/integration_service"); 88 | ``` 89 | Remember that the string */skimap_map_service/integration_service* is the name of the service, in this case is the default name used also in the *skimap_map_service.launch* node. Once this client is ready you can just call the *integratePoints* method passing a vector filled with the points representing sensor measurements (in the sensor Reference Frame) and the *geometry_msgs/Pose* representing the 6-DOF pose of the sensor: 90 | 91 | ``` 92 | typedef skimap_ros::SkimapServiceClient::ColorPoint CPoint; 93 | std::vector points; 94 | //Fill vector with your points 95 | geometry_msgs::Pose sensor_pose; 96 | //Fill sensor_pose with your camera pose 97 | skimap_service_client->integratePoints(points, sensor_pose); 98 | ``` 99 | 100 | This example Client is built with an asynchronous queue so the *integratePoints* method just appends your data to the queue, the real communication with SkiMap Map Service will be made in another thread. This technique is useful when your node requires real-time performance. 101 | 102 | ### SkiMap with ORBSLAM2: 103 | 104 | This Video: 105 | 106 | paper video 108 | 109 | shows an integration of the SkiMap (using the abovementioned SkiMap Map Service) with the popular [ORBSLAM2](https://github.com/raulmur/ORB_SLAM2) framework. 110 | 111 | -------------------------------------------------------------------------------- /include/skimap/voxels/VoxelDataMatrix.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef VOXELDATAMATRIX_HPP 10 | #define VOXELDATAMATRIX_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace skimap 22 | { 23 | 24 | /** 25 | * Voxel Data containing matrix-arranged values 26 | * D template represents datatype for cells. 27 | * COLUMNS template represents fixed columns size. 28 | */ 29 | template 30 | struct VoxelDataMatrix 31 | { 32 | std::vector> matrix; 33 | 34 | /** 35 | * Pointer Copy Constructor. 36 | * @param data source data 37 | */ 38 | VoxelDataMatrix(VoxelDataMatrix *data) 39 | { 40 | matrix = data->matrix; 41 | } 42 | 43 | /** 44 | * Void Constructor. 45 | */ 46 | VoxelDataMatrix() 47 | { 48 | } 49 | 50 | /** 51 | * Sum Overload. Defines Sum operations between Voxels 52 | * @param v2 second addend 53 | * @return sum 54 | */ 55 | VoxelDataMatrix operator+(const VoxelDataMatrix &v2) const 56 | { 57 | VoxelDataMatrix v1 = *this; 58 | if (v1.matrix.size() <= 0) 59 | return v2; 60 | VoxelDataMatrix d; 61 | d.matrix = v1.matrix; 62 | for (int i = 0; i < v2.matrix.size(); i++) 63 | { 64 | d.matrix.push_back(v2.matrix[i]); 65 | } 66 | return d; 67 | } 68 | 69 | /** 70 | * Subtraction Overload. Defines Subtraction operations between Voxels 71 | * @param v2 second minuend 72 | * @return subtraction 73 | */ 74 | VoxelDataMatrix operator-(const VoxelDataMatrix &v2) const 75 | { 76 | VoxelDataMatrix v1 = *this; 77 | if (v1.matrix.size() <= 0) 78 | return v1; 79 | VoxelDataMatrix d; 80 | d.matrix = v1.matrix; 81 | std::vector> temp = v2.matrix; 82 | std::vector to_delete_mask(d.matrix.size()); 83 | std::fill(to_delete_mask.begin(), to_delete_mask.end(), false); 84 | while (temp.size() > 0) 85 | { 86 | for (int j = 0; j < d.matrix.size(); j++) 87 | { 88 | D distance = std::numeric_limits::max(); 89 | std::vector r1 = d.matrix[j]; 90 | std::vector r2 = temp[0]; 91 | if (r1.size() == r2.size()) 92 | { 93 | distance = D(0.0); 94 | for (int i = 0; i < r1.size(); i++) 95 | { 96 | distance += (r1[i] - r2[i]) * (r1[i] - r2[i]); 97 | } 98 | distance = sqrt(distance); 99 | } 100 | if (distance <= std::numeric_limits::epsilon()) 101 | { 102 | to_delete_mask[j] = true; 103 | break; 104 | } 105 | } 106 | temp.erase(temp.begin()); 107 | } 108 | 109 | VoxelDataMatrix d2; 110 | for (int i = 0; i < to_delete_mask.size(); i++) 111 | { 112 | if (!to_delete_mask[i]) 113 | { 114 | d2.matrix.push_back(d.matrix[i]); 115 | } 116 | } 117 | 118 | return d; 119 | } 120 | 121 | /** 122 | * 123 | * @param row 124 | * @return 125 | */ 126 | int contains(std::vector &row) 127 | { 128 | for (int j = 0; j < matrix.size(); j++) 129 | { 130 | D distance = std::numeric_limits::max(); 131 | std::vector r1 = matrix[j]; 132 | std::vector r2 = row; 133 | if (r1.size() == r2.size()) 134 | { 135 | distance = D(0.0); 136 | for (int i = 0; i < r1.size(); i++) 137 | { 138 | distance += (r1[i] - r2[i]) * (r1[i] - r2[i]); 139 | } 140 | distance = sqrt(distance); 141 | } 142 | if (distance <= std::numeric_limits::epsilon()) 143 | { 144 | return j; 145 | } 146 | } 147 | return -1; 148 | } 149 | 150 | /** 151 | * Serializes object into stream. 152 | */ 153 | friend std::ostream &operator<<(std::ostream &os, const VoxelDataMatrix &voxel) 154 | { 155 | 156 | int count = 0; 157 | os << std::setprecision(std::numeric_limits::digits10 + 2); 158 | os << voxel.matrix.size() << " "; 159 | for (int i = 0; i < voxel.matrix.size(); i++) 160 | { 161 | os << voxel.matrix[i].size() << " "; 162 | int sub_count = 0; 163 | for (int j = 0; j < voxel.matrix[i].size(); j++) 164 | { 165 | os << voxel.matrix[i][j]; 166 | if (sub_count < voxel.matrix[i].size() - 1) 167 | { 168 | os << " "; 169 | } 170 | sub_count++; 171 | } 172 | if (count < voxel.matrix.size() - 1) 173 | { 174 | os << " "; 175 | } 176 | count++; 177 | } 178 | return os; 179 | } 180 | 181 | /** 182 | * Hydrates object from stream. 183 | */ 184 | friend std::istream &operator>>(std::istream &is, VoxelDataMatrix &voxel) 185 | { 186 | 187 | int size; 188 | is >> size; 189 | voxel.matrix.resize(size); 190 | for (int i = 0; i < size; i++) 191 | { 192 | int row_size; 193 | is >> row_size; 194 | std::vector row(row_size); 195 | for (int j = 0; j < row_size; j++) 196 | { 197 | double d; 198 | is >> d; 199 | row[j] = D(d); 200 | } 201 | voxel.matrix[i] = row; 202 | } 203 | 204 | return is; 205 | } 206 | }; 207 | } 208 | 209 | #endif /* VOXELDATAMATRIX_HPP */ 210 | -------------------------------------------------------------------------------- /include/slamdunk/feature_matcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_FEATURE_MATCHER_H 10 | #define SLAM_DUNK_FEATURE_MATCHER_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | #include 14 | #include 15 | 16 | #ifdef SLAMDUNK_FLANN_ENABLED 17 | #include 18 | #endif // SLAMDUNK_FLANN_ENABLED 19 | 20 | #include 21 | 22 | namespace slamdunk 23 | { 24 | 25 | struct SLAM_DUNK_API FMatch 26 | { 27 | int m_model_idx, m_feat_idx, m_query_idx; 28 | float m_match_score; 29 | }; 30 | 31 | class SLAM_DUNK_API FeatureMatcher 32 | { 33 | public: 34 | typedef boost::shared_ptr Ptr; 35 | typedef boost::shared_ptr ConstPtr; 36 | 37 | virtual ~FeatureMatcher(){}; 38 | virtual void setModels(const std::vector &models) = 0; 39 | virtual void match(cv::Mat query, std::vector &matches) = 0; 40 | }; 41 | 42 | #ifdef SLAMDUNK_FLANN_ENABLED 43 | template 44 | class RatioMatcher : public FeatureMatcher 45 | { 46 | public: 47 | typedef typename FLANNDistanceTp::ElementType FLANNElementTp; 48 | typedef flann::Index FLANNIndex; 49 | 50 | RatioMatcher(bool interModelRatio, float ratio, int cores) 51 | : m_cores(cores), m_inter_model_ratio(interModelRatio), m_ratio2(ratio * ratio), m_index(NULL) {} 52 | 53 | virtual ~RatioMatcher() { delete m_index; } 54 | 55 | virtual void setModels(const std::vector &models); 56 | virtual void match(cv::Mat query, std::vector &matches); 57 | 58 | inline void setCores(int cores) { m_cores = cores; } 59 | inline int getCores() const { return m_cores; } 60 | 61 | /// If true, do the ratio among different models (eg obj detection), otherwise within the same model (eg frame tracking) 62 | inline void setInterModelRatio(bool onoff) { m_inter_model_ratio = onoff; } 63 | inline bool getInterModelRatio() const { return m_inter_model_ratio; } 64 | 65 | inline void setRatio(float ratio) { m_ratio2 = ratio * ratio; } 66 | inline float getSquaredRatio() const { return m_ratio2; } 67 | 68 | protected: 69 | virtual FLANNIndex *createIndex(const flann::Matrix &features) const = 0; 70 | 71 | private: 72 | enum 73 | { 74 | SLAM_DUNK_KNN_NEIGHBORS = 32 75 | }; 76 | 77 | int m_cores; 78 | bool m_inter_model_ratio; 79 | float m_ratio2; 80 | FLANNIndex *m_index; 81 | cv::Mat_ m_descriptors; 82 | std::vector m_model_indices; 83 | std::vector m_model_zeroes; 84 | }; 85 | 86 | template 87 | class RatioMatcherKDTreeIndex : public RatioMatcher 88 | { 89 | public: 90 | typedef typename RatioMatcher::FLANNElementTp FLANNElementTp; 91 | typedef typename RatioMatcher::FLANNIndex FLANNIndex; 92 | 93 | RatioMatcherKDTreeIndex(bool interModelRatio, float ratio = 0.8, int cores = 1) 94 | : RatioMatcher(interModelRatio, ratio, cores) {} 95 | protected: 96 | virtual FLANNIndex *createIndex(const flann::Matrix &features) const; 97 | }; 98 | 99 | template 100 | class RatioMatcherLSHIndex : public RatioMatcher 101 | { 102 | public: 103 | typedef typename RatioMatcher::FLANNElementTp FLANNElementTp; 104 | typedef typename RatioMatcher::FLANNIndex FLANNIndex; 105 | 106 | RatioMatcherLSHIndex(bool interModelRatio, float ratio = 0.8, int cores = 1) 107 | : RatioMatcher(interModelRatio, ratio, cores) {} 108 | protected: 109 | virtual FLANNIndex *createIndex(const flann::Matrix &features) const; 110 | }; 111 | 112 | #endif // SLAMDUNK_FLANN_ENABLED 113 | 114 | class SLAM_DUNK_API OpenCVRatioMatcher : public FeatureMatcher 115 | { 116 | public: 117 | virtual void setModels(const std::vector &models); 118 | virtual void match(cv::Mat query, std::vector &matches); 119 | 120 | /// If true, do the ratio among different models (eg obj detection), otherwise within the same model (eg frame tracking) 121 | inline void setInterModelRatio(bool onoff) { m_inter_model_ratio = onoff; } 122 | inline bool getInterModelRatio() const { return m_inter_model_ratio; } 123 | 124 | inline void setRatio(float ratio) { m_ratio2 = ratio * ratio; } 125 | inline float getSquaredRatio() const { return m_ratio2; } 126 | 127 | protected: 128 | OpenCVRatioMatcher(bool interModelRatio, float ratio, cv::Ptr matcher, int cores = 1) 129 | : m_inter_model_ratio(interModelRatio), m_ratio2(ratio * ratio), m_matcher(matcher), m_cores(cores) {} 130 | 131 | private: 132 | enum 133 | { 134 | SLAM_DUNK_KNN_NEIGHBORS = 32 135 | }; 136 | 137 | int m_cores; 138 | bool m_inter_model_ratio; 139 | float m_ratio2; 140 | cv::Ptr m_matcher; 141 | 142 | std::vector m_model_indices; 143 | std::vector m_model_zeroes; 144 | }; 145 | 146 | class SLAM_DUNK_API OpenCVFlannRatioMatcher : public OpenCVRatioMatcher 147 | { 148 | public: 149 | OpenCVFlannRatioMatcher(bool interModelRatio, float ratio, int cores = 1); 150 | }; 151 | 152 | class SLAM_DUNK_API OpenCVLSHRatioMatcher : public OpenCVRatioMatcher 153 | { 154 | public: 155 | OpenCVLSHRatioMatcher(bool interModelRatio, float ratio, int cores = 1); 156 | }; 157 | 158 | #ifdef SLAMDUNK_FLANN_ENABLED 159 | typedef RatioMatcherKDTreeIndex> RatioMatcherL2; 160 | typedef RatioMatcherKDTreeIndex> RatioMatcherL2D; 161 | //typedef RatioMatcherLSHIndex > RatioMatcherHamming; 162 | typedef RatioMatcherLSHIndex RatioMatcherHamming; 163 | #else // SLAMDUNK_FLANN_ENABLED 164 | typedef OpenCVFlannRatioMatcher RatioMatcherL2; 165 | typedef OpenCVFlannRatioMatcher RatioMatcherL2D; 166 | typedef OpenCVLSHRatioMatcher RatioMatcherHamming; 167 | #endif 168 | } 169 | 170 | #ifdef SLAMDUNK_FLANN_ENABLED 171 | #include "impl/ratio_matcher.hpp" 172 | #endif // SLAMDUNK_FLANN_ENABLED 173 | 174 | #endif // SLAM_DUNK_FEATURE_MATCHER_H 175 | --------------------------------------------------------------------------------