├── .gitignore ├── LICENSE ├── README.md ├── calib.yml ├── dataset-office ├── 0000-d.pgm ├── 0000-p.yml ├── 0000-r.jpg ├── 0001-d.pgm ├── 0001-p.yml ├── 0001-r.jpg ├── 0002-d.pgm ├── 0002-p.yml ├── 0002-r.jpg ├── 0003-d.pgm ├── 0003-p.yml ├── 0003-r.jpg ├── 0004-d.pgm ├── 0004-p.yml ├── 0004-r.jpg ├── 0005-d.pgm ├── 0005-p.yml ├── 0005-r.jpg ├── 0006-d.pgm ├── 0006-p.yml ├── 0006-r.jpg ├── 0007-d.pgm ├── 0007-p.yml ├── 0007-r.jpg ├── 0008-d.pgm ├── 0008-p.yml ├── 0008-r.jpg ├── 0009-d.pgm ├── 0009-p.yml ├── 0009-r.jpg ├── 0010-d.pgm ├── 0010-p.yml ├── 0010-r.jpg ├── 0011-d.pgm ├── 0011-p.yml ├── 0011-r.jpg ├── 0012-d.pgm ├── 0012-p.yml ├── 0012-r.jpg ├── 0013-d.pgm ├── 0013-p.yml ├── 0013-r.jpg ├── 0014-d.pgm ├── 0014-p.yml ├── 0014-r.jpg ├── 0015-d.pgm ├── 0015-p.yml ├── 0015-r.jpg ├── 0016-d.pgm ├── 0016-p.yml ├── 0016-r.jpg ├── 0017-d.pgm ├── 0017-p.yml ├── 0017-r.jpg ├── 0018-d.pgm ├── 0018-p.yml ├── 0018-r.jpg ├── 0019-d.pgm ├── 0019-p.yml ├── 0019-r.jpg ├── 0020-d.pgm ├── 0020-p.yml ├── 0020-r.jpg ├── 0021-d.pgm ├── 0021-p.yml └── 0021-r.jpg └── src ├── CMakeLists.txt ├── kinect_calibration.cpp ├── kinect_calibration.h ├── kinect_merge.cpp ├── kinect_view.cpp └── kinect_view.h /.gitignore: -------------------------------------------------------------------------------- 1 | *.un~ 2 | README.html 3 | build 4 | *.ply 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2012 Tomi Kyöstilä 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining 4 | a copy of this software and associated documentation files (the 5 | "Software"), to deal in the Software without restriction, including 6 | without limitation the rights to use, copy, modify, merge, publish, 7 | distribute, sublicense, and/or sell copies of the Software, and to 8 | permit persons to whom the Software is furnished to do so, subject to 9 | the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included 12 | in all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 15 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 16 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 17 | IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 18 | CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, 19 | TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 20 | SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Merging Kinect depth maps into a nonredundant point cloud 2 | ========================================================= 3 | 4 | Summary 5 | ------- 6 | 7 | The program takes a sequence of registered views captured with the Kinect depth 8 | camera and outputs a nonredundant point cloud. The algorithm creates the point 9 | cloud incrementally and uses overlapping measurements to reduce the directional 10 | variance of estimated point positions. It does not limit the captured volume, it 11 | allows for varying levels of detail, and it doesn't require any parameters from 12 | the user. 13 | 14 | Process 15 | ------- 16 | 17 | The input to the algorithm is a sequence of views which consist of a depth 18 | map, a color image and extrinsic parameters of the camera. In addition, 19 | connectivity information for the views can be supplied to increase 20 | performance. The output is a point cloud in the ASCII PLY file format. 21 | 22 | The views are processed sequentially in a single pass. When a new view is being 23 | added, existing points from the cloud are projected onto the pixel grid of the 24 | new view. If a new measurement resides near existing points, a new estimate is 25 | created for each existing point with the best linear unbiased estimator (BLUE). 26 | Then, the Mahalanobis distances from the new estimate to both the existing point 27 | and new measurement is calculated. If both distances are smaller than a threshold, 28 | the existing point in the cloud is replaced with the new estimate. This results 29 | in reduced variance for the point with no added redundancy in the cloud. If the 30 | distance condition is not met for any new estimate (i.e. the new measurement 31 | wasn't used to refine any existing point), the measurement represents a novel 32 | point of surface and is added to the cloud as a new point. 33 | 34 | Prerequisites 35 | ------------- 36 | 37 | * CMake 2.8 38 | * OpenCV 2.3.1 39 | * Boost 1.49.0 (the system, filesystem and program_options libraries) 40 | 41 | Building 42 | -------- 43 | 44 | The program has been developed and tested on Linux. 45 | 46 | By default, the root of an OpenCV build is expected to be found at 47 | `../../OpenCV-2.3.1/build` relative to the `src` directory. This can be changed 48 | using the `OpenCV_DIR` CMake variable. 49 | 50 | The steps for building are: 51 | 52 | $ mkdir build 53 | $ cd build 54 | $ cmake ../src 55 | $ make 56 | 57 | For additional debug file output, a debug build can be performed by specifying 58 | the `CMAKE_BUILD_TYPE` CMake variable (e.g. `cmake -D CMAKE_BUILD_TYPE=Debug ../src`). 59 | 60 | Usage 61 | ----- 62 | 63 | The input to the program is a sequence of pgm, yml and jpg files, one of each 64 | per view. The files should be named sequentially in the format shown in the 65 | parenthesis. 66 | 67 | pgm: The grey scale disparity image file (e.g. 0000-d.pgm). 68 | yml: The extrinsic camera parameter data file (e.g. 0000-p.yml). 69 | jpg: The color image file (e.g. 0000-r.jpg or 0000-c1.jpg). 70 | 71 | The extrinsic parameters consist of the 3x3 camera rotation matrix R and 72 | the 3x1 camera translation matrix T in the OpenCV YAML format. 73 | 74 | The program requires calibration data to be found in a `calib.yml` file in the 75 | current directory. 76 | 77 | ### Command-line 78 | 79 | ./kinect_merge [options] capture_dir output_file 80 | 81 | -c [ --connectivity ] FILE The camera connectivity matrix. 82 | -d [ --debug ] Output additional debug files. 83 | --covscalexy FACTOR Scale factor for measurement x- and y-variances (default: 40). 84 | --covscalez FACTOR Scale factor for measurement z-variance (default: 20). 85 | 86 | The connectivity matrix is an NxN symmetric logical matrix in the OpenCV YAML 87 | format. In this matrix, the value in row Y, column X indicates whether the 88 | corresponding views see the same portion of the scene. If no connectivity matrix 89 | is supplied, a matrix full of ones is used (i.e. all views are considered to be 90 | connected to each other). 91 | 92 | The covariance scaling options are supplied for accounting for error resulting 93 | from view alignment. The default values are suitable for the alignment process 94 | used for included data set. The scale factors should not be data set specific. 95 | 96 | ### Example 97 | 98 | $ build/kinect_merge dataset-office office.ply 99 | 100 | Statistics 101 | ---------- 102 | 103 | Some statistics related to the supplied office data set when processed on 104 | a 2 GHz Intel Core 2 processor: 105 | 106 | View count: 22 107 | Point count of a redundant cloud: 5 315 546 108 | Point count of a nonredundant cloud: 823 659 109 | Ratio of reduction: 85 % 110 | Mean merging time per view: 0.21 s 111 | Peak memory usage: 809 MiB 112 | -------------------------------------------------------------------------------- /dataset-office/0000-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0000-d.pgm -------------------------------------------------------------------------------- /dataset-office/0000-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 1.000000,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000,1.000000 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ 0.000000,0.000000,0.000000 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0000-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0000-r.jpg -------------------------------------------------------------------------------- /dataset-office/0001-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0001-d.pgm -------------------------------------------------------------------------------- /dataset-office/0001-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999885,0.004395,-0.014526,-0.004385,0.999990,0.000714,0.014529,-0.000651,0.999894 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.048941,0.003270,-0.004699 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0001-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0001-r.jpg -------------------------------------------------------------------------------- /dataset-office/0002-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0002-d.pgm -------------------------------------------------------------------------------- /dataset-office/0002-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999702,0.008265,-0.022962,-0.008231,0.999965,0.001586,0.022974,-0.001397,0.999735 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.092005,0.005757,-0.008070 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0002-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0002-r.jpg -------------------------------------------------------------------------------- /dataset-office/0003-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0003-d.pgm -------------------------------------------------------------------------------- /dataset-office/0003-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999741,0.008078,-0.021257,-0.008055,0.999967,0.001188,0.021266,-0.001016,0.999773 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.132812,0.009063,-0.013134 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0003-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0003-r.jpg -------------------------------------------------------------------------------- /dataset-office/0004-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0004-d.pgm -------------------------------------------------------------------------------- /dataset-office/0004-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999903,0.005264,-0.012857,-0.005255,0.999986,0.000712,0.012860,-0.000644,0.999917 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.176545,0.011019,-0.014994 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0004-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0004-r.jpg -------------------------------------------------------------------------------- /dataset-office/0005-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0005-d.pgm -------------------------------------------------------------------------------- /dataset-office/0005-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999993,-0.000250,0.003786,0.000252,1.000000,-0.000693,-0.003786,0.000694,0.999993 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.228100,0.014932,-0.017765 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0005-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0005-r.jpg -------------------------------------------------------------------------------- /dataset-office/0006-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0006-d.pgm -------------------------------------------------------------------------------- /dataset-office/0006-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999797,0.006137,-0.019207,-0.006139,0.999981,-0.000060,0.019206,0.000178,0.999816 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.303935,0.019782,-0.026771 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0006-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0006-r.jpg -------------------------------------------------------------------------------- /dataset-office/0007-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0007-d.pgm -------------------------------------------------------------------------------- /dataset-office/0007-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.998991,0.015087,-0.042301,-0.015104,0.999886,-0.000087,0.042295,0.000726,0.999105 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.377638,0.027426,-0.039475 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0007-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0007-r.jpg -------------------------------------------------------------------------------- /dataset-office/0008-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0008-d.pgm -------------------------------------------------------------------------------- /dataset-office/0008-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999698,0.009733,-0.022586,-0.009752,0.999952,-0.000773,0.022578,0.000993,0.999745 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.413097,0.027891,-0.035909 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0008-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0008-r.jpg -------------------------------------------------------------------------------- /dataset-office/0009-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0009-d.pgm -------------------------------------------------------------------------------- /dataset-office/0009-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999778,0.010806,-0.018105,-0.010808,0.999942,-0.000026,0.018104,0.000221,0.999836 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.465116,0.030042,-0.035533 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0009-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0009-r.jpg -------------------------------------------------------------------------------- /dataset-office/0010-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0010-d.pgm -------------------------------------------------------------------------------- /dataset-office/0010-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999697,0.011342,-0.021848,-0.011338,0.999936,0.000326,0.021850,-0.000078,0.999761 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.519851,0.033890,-0.043729 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0010-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0010-r.jpg -------------------------------------------------------------------------------- /dataset-office/0011-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0011-d.pgm -------------------------------------------------------------------------------- /dataset-office/0011-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999864,0.008565,-0.014075,-0.008566,0.999963,0.000021,0.014074,0.000100,0.999901 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.589282,0.038340,-0.048000 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0011-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0011-r.jpg -------------------------------------------------------------------------------- /dataset-office/0012-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0012-d.pgm -------------------------------------------------------------------------------- /dataset-office/0012-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999609,0.011111,-0.025663,-0.011079,0.999938,0.001392,0.025677,-0.001107,0.999670 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.670572,0.041303,-0.047674 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0012-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0012-r.jpg -------------------------------------------------------------------------------- /dataset-office/0013-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0013-d.pgm -------------------------------------------------------------------------------- /dataset-office/0013-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999654,0.012298,-0.023270,-0.012257,0.999923,0.001906,0.023292,-0.001620,0.999727 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.718372,0.046314,-0.053930 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0013-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0013-r.jpg -------------------------------------------------------------------------------- /dataset-office/0014-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0014-d.pgm -------------------------------------------------------------------------------- /dataset-office/0014-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999481,0.014391,-0.028829,-0.014341,0.999895,0.001937,0.028854,-0.001523,0.999582 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.765141,0.049519,-0.060780 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0014-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0014-r.jpg -------------------------------------------------------------------------------- /dataset-office/0015-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0015-d.pgm -------------------------------------------------------------------------------- /dataset-office/0015-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999694,0.012072,-0.021582,-0.012080,0.999927,-0.000284,0.021577,0.000544,0.999767 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.799954,0.051142,-0.056516 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0015-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0015-r.jpg -------------------------------------------------------------------------------- /dataset-office/0016-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0016-d.pgm -------------------------------------------------------------------------------- /dataset-office/0016-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999770,0.011689,-0.017974,-0.011697,0.999932,-0.000362,0.017968,0.000572,0.999838 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.837989,0.051813,-0.057011 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0016-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0016-r.jpg -------------------------------------------------------------------------------- /dataset-office/0017-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0017-d.pgm -------------------------------------------------------------------------------- /dataset-office/0017-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999490,0.015928,-0.027686,-0.015921,0.999873,0.000474,0.027690,-0.000033,0.999617 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.881154,0.055988,-0.066841 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0017-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0017-r.jpg -------------------------------------------------------------------------------- /dataset-office/0018-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0018-d.pgm -------------------------------------------------------------------------------- /dataset-office/0018-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999965,0.008057,-0.002144,-0.008067,0.999956,-0.004751,0.002106,0.004768,0.999986 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.929612,0.055000,-0.040291 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0018-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0018-r.jpg -------------------------------------------------------------------------------- /dataset-office/0019-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0019-d.pgm -------------------------------------------------------------------------------- /dataset-office/0019-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999255,0.018419,-0.033917,-0.018454,0.999829,-0.000703,0.033898,0.001329,0.999424 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -0.984166,0.067058,-0.087741 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0019-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0019-r.jpg -------------------------------------------------------------------------------- /dataset-office/0020-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0020-d.pgm -------------------------------------------------------------------------------- /dataset-office/0020-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999632,0.015323,-0.022377,-0.015412,0.999874,-0.003846,0.022315,0.004189,0.999742 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -1.036482,0.069478,-0.076714 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0020-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0020-r.jpg -------------------------------------------------------------------------------- /dataset-office/0021-d.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0021-d.pgm -------------------------------------------------------------------------------- /dataset-office/0021-p.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | R: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: f 6 | data: [ 0.999433,0.017141,-0.028966,-0.017228,0.999848,-0.002760,0.028914,0.003258,0.999577 ] 7 | T: !!opencv-matrix 8 | rows: 3 9 | cols: 1 10 | dt: f 11 | data: [ -1.075407,0.073055,-0.087289 ] 12 | -------------------------------------------------------------------------------- /dataset-office/0021-r.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomikyos/kinect-merge/28e66a2e3cd720eb48bb2470611ed7e8f1fd89e3/dataset-office/0021-r.jpg -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | project(kinect_merge) 4 | 5 | set(Boost_USE_STATIC_LIBS ON) 6 | set(Boost_USE_MULTITHREADED ON) 7 | set(Boost_USE_STATIC_RUNTIME OFF) 8 | find_package(Boost COMPONENTS system filesystem program_options REQUIRED) 9 | include_directories(${Boost_INCLUDE_DIRS}) 10 | 11 | set(OpenCV_DIR "../../OpenCV-2.3.1/build" CACHE PATH "Path to the OpenCV build directory") 12 | find_package(OpenCV REQUIRED) 13 | include_directories(${OpenCV_INCLUDE_DIRS}) 14 | 15 | file(GLOB_RECURSE INCS "*.h") 16 | set(SRCS 17 | kinect_merge.cpp 18 | kinect_view.cpp 19 | kinect_calibration.cpp) 20 | 21 | IF(NOT CMAKE_BUILD_TYPE) 22 | SET(CMAKE_BUILD_TYPE Release CACHE STRING 23 | "The type of build. Options are: Debug Release RelWithDebInfo" 24 | FORCE) 25 | ENDIF(NOT CMAKE_BUILD_TYPE) 26 | 27 | SET(COMMON_FLAGS "-Wall -Wextra") 28 | 29 | if(CMAKE_COMPILER_IS_GNUCXX) 30 | set(CMAKE_CXX_FLAGS_RELEASE "${COMMON_FLAGS} -O3 -DNDEBUG") 31 | set(CMAKE_CXX_FLAGS_DEBUG "${COMMON_FLAGS} -O0 -ggdb -DDEBUG") 32 | set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${COMMON_FLAGS} -O3 -ggdb -DNDEBUG") 33 | endif() 34 | 35 | add_executable (kinect_merge ${SRCS} ${INCS}) 36 | target_link_libraries (kinect_merge 37 | ${OpenCV_LIBS} 38 | ${Boost_LIBRARIES}) 39 | -------------------------------------------------------------------------------- /src/kinect_calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "kinect_calibration.h" 2 | 3 | namespace kinect_capture { 4 | 5 | void CKinectCalibration::compute_rgb_depthmap(const cv::Mat1s &disp, cv::Mat1f &depth) { 6 | const int SPLAT_RADIUS = 0; 7 | 8 | depth = cv::Mat1f::zeros(rgb_height,rgb_width); 9 | 10 | for(int v=0; v=depth.cols || vci<0 || vci>=depth.rows) 31 | continue; 32 | 33 | float &z_old = depth(vci,uci); 34 | if(z_old == 0 || z_old > z_new) 35 | z_old = z_new; 36 | } 37 | } 38 | } 39 | 40 | void CKinectCalibration::distort(const float xk, const float yk, const float kc[5], float &xd, float &yd) { 41 | float r2 = xk*xk + yk*yk; 42 | float r4 = r2*r2; 43 | float r6 = r4*r2; 44 | float rc = 1 + kc[0]*r2 + kc[1]*r4 + kc[4]*r6; 45 | float xy = xk*yk; 46 | float dx = 2*kc[2]*xy + kc[3]*(r2+2*xk*xk); 47 | float dy = 2*kc[3]*xy + kc[2]*(r2+2*yk*yk); 48 | 49 | xd = rc*xk + dx; 50 | yd = rc*yk + dy; 51 | } 52 | 53 | #if 0 54 | void CMsCalibration::disparity2point(int u,int v,short disp,cv::Matx31f &xc) { 55 | const float DEPTH_X_RES = 640; 56 | const float DEPTH_Y_RES = 480; 57 | uint16_t wz = reg.raw_to_mm_shift[disp]; 58 | 59 | float ref_pix_size = reg.zero_plane_info.reference_pixel_size; 60 | float ref_distance = reg.zero_plane_info.reference_distance; 61 | float xfactor = 2*ref_pix_size * wz / ref_distance; 62 | float yfactor = (1024/480)*ref_pix_size * wz / ref_distance; 63 | //xfactor = ref_pix_size * wz / ref_distance; 64 | //float yfactor = xfactor; 65 | xc(0) = (u - DEPTH_X_RES/2) * xfactor / 1000; 66 | xc(1) = (v - DEPTH_Y_RES/2) * yfactor / 1000; 67 | xc(2) = (float)(wz / 1000); 68 | } 69 | #endif 70 | 71 | void point2rgb(const cv::Matx31f &xc, cv::Matx21f &pc) { 72 | } 73 | 74 | void CHerreraCalibration::load(const std::string &filename) { 75 | const int COLOR_CAMERA_IDX=0; 76 | cv::FileStorage fs; 77 | std::stringstream s; 78 | cv::Mat m; 79 | 80 | fs.open(filename,cv::FileStorage::READ); 81 | 82 | s << "rsize" << (COLOR_CAMERA_IDX+1); 83 | fs[s.str().c_str()] >> m; 84 | rgb_height = m.at(0); 85 | rgb_width = m.at(1); 86 | 87 | s.str(""); 88 | s << "rK" << (COLOR_CAMERA_IDX+1); 89 | fs[s.str().c_str()] >> m; 90 | rK = m; 91 | 92 | s.str(""); 93 | s << "rkc" << (COLOR_CAMERA_IDX+1); 94 | fs[s.str().c_str()] >> m; 95 | memcpy(rkc,m.data,sizeof(rkc)); 96 | 97 | fs["color_error_var"] >> m; 98 | color_error_var = m.at(0,COLOR_CAMERA_IDX); 99 | 100 | fs["dK"] >> m; 101 | dK = m; 102 | 103 | fs["dkc"] >> m; 104 | memcpy(dkc,m.data,sizeof(dkc)); 105 | 106 | fs["dR"] >> m; 107 | dR = m; 108 | 109 | fs["dt"] >> m; 110 | dT = m; 111 | 112 | fs["dc"] >> m; 113 | memcpy(dc,m.data,sizeof(dc)); 114 | 115 | fs["dc_alpha"] >> m; 116 | memcpy(dc_alpha,m.data,sizeof(dc)); 117 | 118 | fs["dc_beta"] >> m; 119 | dc_beta = m; 120 | 121 | fs["depth_error_var"] >> m; 122 | disp_error_var = m.at(0); 123 | } 124 | 125 | float CHerreraCalibration::undistort_disp(int u, int v, float disp) { 126 | return disp + dc_beta(v,u)*std::exp(dc_alpha[0] - dc_alpha[1]*disp); 127 | } 128 | 129 | void CHerreraCalibration::disparity2point(int u,int v,short disp,cv::Matx31f &xc) { 130 | float disp_k = undistort_disp(u,v,disp); 131 | float z = 1.0f / (dc[1]*disp_k + dc[0]); 132 | float xk = (u-dK(0,2))/dK(0,0); 133 | float yk = (v-dK(1,2))/dK(1,1); 134 | float xd,yd; 135 | cv::Matx31f x; 136 | 137 | distort(xk,yk,dkc,xd,yd); 138 | x(0) = xd*z; 139 | x(1) = yd*z; 140 | x(2) = z; 141 | 142 | xc = dR*x + dT; 143 | } 144 | 145 | void CHerreraCalibration::point2rgb(const cv::Matx31f &xc, cv::Matx21f &pc) { 146 | float xk = xc(0)/xc(2); 147 | float yk = xc(1)/xc(2); 148 | float xd,yd; 149 | distort(xk,yk,rkc,xd,yd); 150 | pc(0) = rK(0,0)*xd+rK(0,2); 151 | pc(1) = rK(1,1)*yd+rK(1,2); 152 | } 153 | 154 | } 155 | -------------------------------------------------------------------------------- /src/kinect_calibration.h: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////////////////////// 2 | // Kinect capture for the kinect calibration toolbox 3 | // by Daniel Herrera C. 4 | ////////////////////////////////////////////////////////////////// 5 | #pragma once 6 | 7 | #include 8 | #if 0 9 | #include 10 | #endif 11 | 12 | namespace kinect_capture { 13 | 14 | ////////////////////////////////////////////////////////////////// 15 | // CKinectCalibration: encapsulates the calibration parameters of 16 | // the kinect and performs conversion between image coordinates 17 | // and world coordinates. 18 | ////////////////////////////////////////////////////////////////// 19 | class CKinectCalibration { 20 | public: 21 | int rgb_width; 22 | int rgb_height; 23 | static const int depth_width=640; 24 | static const int depth_height=480; 25 | 26 | //disparity2point: converts a point in the depth image to a 3D point 27 | //in color camera coordinates. 28 | virtual void disparity2point(int u,int v,short disp,cv::Matx31f &xc)=0; 29 | 30 | //point2rgb: projects a 3D point in color camera coordinates onto the 31 | //image plane of the color camera and returns the pixel coordinates. 32 | virtual void point2rgb(const cv::Matx31f &xc, cv::Matx21f &pc)=0; 33 | 34 | virtual void compute_rgb_depthmap(const cv::Mat1s &disp, cv::Mat1f &depth); 35 | 36 | void distort(const float xk, const float yk, const float kc[5], float &xd, float &yd); 37 | 38 | protected: 39 | CKinectCalibration() 40 | {} 41 | 42 | CKinectCalibration(int _rgb_width,int _rgb_height): 43 | rgb_width(_rgb_width), 44 | rgb_height(_rgb_height) 45 | {} 46 | }; 47 | 48 | #if 0 49 | class CMsCalibration: public CKinectCalibration { 50 | public: 51 | freenect_registration reg; 52 | 53 | virtual void disparity2point(int u,int v,short disp,cv::Matx31f &xc); 54 | virtual void point2rgb(const cv::Matx31f &xc, cv::Matx21f &pc); 55 | }; 56 | #endif 57 | 58 | class CBurrusCalibration: public CKinectCalibration { 59 | public: 60 | CBurrusCalibration(): 61 | CKinectCalibration(640,480), 62 | calib_fx_d(586.16f), //These constants come from calibration, 63 | calib_fy_d(582.73f), //replace with your own 64 | calib_px_d(322.30f), 65 | calib_py_d(230.07), 66 | calib_dc1(-0.002851), 67 | calib_dc2(1093.57), 68 | calib_fx_rgb(530.11f), 69 | calib_fy_rgb(526.85f), 70 | calib_px_rgb(311.23f), 71 | calib_py_rgb(256.89f), 72 | calib_R( 0.99999f, -0.0021409f, 0.004993f, 73 | 0.0022251f, 0.99985f, -0.016911f, 74 | -0.0049561f, 0.016922f, 0.99984f), 75 | calib_T( -0.025985f, 0.00073534f, -0.003411f) 76 | {} 77 | 78 | virtual void disparity2point(int u,int v,short disp,cv::Matx31f &xc) { 79 | cv::Matx31f pd; 80 | 81 | pd(2) = 1.0f / (calib_dc1*(disp - calib_dc2)); 82 | pd(0) = ((u-calib_px_d) / calib_fx_d) * pd(2); 83 | pd(1) = ((v-calib_py_d) / calib_fy_d) * pd(2); 84 | 85 | xc = calib_R*pd+calib_T; 86 | } 87 | 88 | virtual void point2rgb(const cv::Matx31f &xc, cv::Matx21f &pc) { 89 | pc(0) = xc(0)*calib_fx_rgb/xc(2) + calib_px_rgb; 90 | pc(1) = xc(1)*calib_fy_rgb/xc(2) + calib_py_rgb; 91 | } 92 | 93 | private: 94 | const float calib_fx_d; 95 | const float calib_fy_d; 96 | const float calib_px_d; 97 | const float calib_py_d; 98 | const float calib_dc1; 99 | const float calib_dc2; 100 | const float calib_fx_rgb; 101 | const float calib_fy_rgb; 102 | const float calib_px_rgb; 103 | const float calib_py_rgb; 104 | cv::Matx33f calib_R; 105 | cv::Matx31f calib_T; 106 | }; 107 | 108 | class CHerreraCalibration:public CKinectCalibration { 109 | public: 110 | cv::Matx33f rK; 111 | float rkc[5]; 112 | float color_error_var; 113 | 114 | cv::Matx33f dK; 115 | float dkc[5]; 116 | float disp_error_var; 117 | 118 | cv::Matx33f dR; 119 | cv::Matx31f dT; 120 | 121 | float dc[2]; 122 | float dc_alpha[2]; 123 | 124 | cv::Mat1f dc_beta; 125 | 126 | CHerreraCalibration() {} 127 | void load(const std::string &filename); 128 | float undistort_disp(int u, int v, float disp); 129 | virtual void disparity2point(int u,int v,short disp,cv::Matx31f &xc); 130 | virtual void point2rgb(const cv::Matx31f &xc, cv::Matx21f &pc); 131 | }; 132 | 133 | } 134 | -------------------------------------------------------------------------------- /src/kinect_merge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "kinect_view.h" 16 | #include "kinect_calibration.h" 17 | 18 | using namespace kinect_merge; 19 | using kinect_capture::CHerreraCalibration; 20 | namespace po = boost::program_options; 21 | 22 | #ifdef DEBUG 23 | void print_matrix(const cv::Mat &m) { 24 | for(int i = 0; i < m.rows; i++) { 25 | for(int j = 0; j < m.cols; j++) { 26 | std::cout << m.at(i, j) << " "; 27 | } 28 | std::cout << std::endl; 29 | } 30 | } 31 | #endif 32 | 33 | static void print_usage(std::string program_name, po::options_description desc) { 34 | std::cerr << "Usage: " << program_name << " [options] capture_dir output_file" << std::endl; 35 | std::cerr << desc << std::endl; 36 | } 37 | 38 | static void parse_options(int argc, const char **argv, po::variables_map &vm) { 39 | po::options_description desc; 40 | desc.add_options() 41 | ("help,h", "display this help and exit") 42 | ("connectivity,c", boost::program_options::value(), 43 | "OpenCV YAML file containing the view connectivity matrix C") 44 | ("debug,d", "output debug files") 45 | ("covscalexy", boost::program_options::value()->default_value(DEFAULT_COVARIANCE_SCALE_XY), 46 | "scale factor for measurement x- and y-variances") 47 | ("covscalez", boost::program_options::value()->default_value(DEFAULT_COVARIANCE_SCALE_Z), 48 | "scale factor for measurement z-variance") 49 | ("capture_dir", "directory to read the capture data from") 50 | ("output_file", "PLY file to write the resulting point cloud to"); 51 | 52 | po::positional_options_description pd; 53 | pd.add("capture_dir", 1); 54 | pd.add("output_file", 1); 55 | 56 | po::parsed_options parsed = po::basic_command_line_parser(argc, argv).options(desc) 57 | .allow_unregistered() 58 | .positional(pd) 59 | .run(); 60 | po::store(parsed, vm); 61 | po::notify(vm); 62 | 63 | if(vm.count("help") || !vm.count("capture_dir") || !vm.count("output_file")) { 64 | print_usage(argv[0], desc); 65 | exit(0); 66 | } 67 | } 68 | 69 | // Load all disparity, extrinsic and color image files for views from the given directory. 70 | // 71 | // The files must be named 0000-d.pgm, 0000-p.yml, 0000-r.jpg, 0001-d.pgm etc. 72 | // The color image file can alternatively be named 0000-c1.ppm. 73 | static void load_views(const std::string &capture_dir, 74 | boost::ptr_vector &views, 75 | float covariance_scale_xy, 76 | float covariance_scale_z, 77 | CKinectCalibration &calibration) { 78 | int view_idx = 0; 79 | while(true) { 80 | std::ostringstream view_file_base_stream; 81 | view_file_base_stream << capture_dir; 82 | view_file_base_stream << "/"; 83 | view_file_base_stream << std::setfill('0') << std::setw(4) << view_idx; 84 | std::string view_file_base = view_file_base_stream.str(); 85 | 86 | // The color file can be in JPG or PPM format. 87 | 88 | std::string disparity_file = view_file_base + "-d.pgm"; 89 | std::string extrinsic_file = view_file_base + "-p.yml"; 90 | std::string jpg_color_file = view_file_base + "-r.jpg"; 91 | std::string ppm_color_file = view_file_base + "-c1.ppm"; 92 | 93 | std::string color_file; 94 | if(boost::filesystem::exists(jpg_color_file)) { 95 | color_file = jpg_color_file; 96 | } else if(boost::filesystem::exists(ppm_color_file)) { 97 | color_file = ppm_color_file; 98 | } 99 | 100 | if(!boost::filesystem::exists(disparity_file) || 101 | !boost::filesystem::exists(extrinsic_file) || 102 | color_file.empty()) { 103 | break; 104 | } 105 | 106 | views.push_back(new CView(extrinsic_file, 107 | disparity_file, 108 | color_file, 109 | covariance_scale_xy, 110 | covariance_scale_z, 111 | calibration)); 112 | std::cout << "." << std::flush; // Progress output 113 | view_idx++; 114 | } 115 | } 116 | 117 | // Output a percentage indicating the progress of a process. 118 | static void print_progress(int current_index, int num_indices) { 119 | std::cout << (int)((float)current_index / num_indices * 100) << " %" << std::flush << "\r"; 120 | } 121 | 122 | // Write a point cloud from a vector into a file in the PLY ASCII format. 123 | template 124 | static void output_point_cloud(const std::vector &point_cloud, const std::string &filename) { 125 | std::cout << "Writing point cloud to " << filename << std::endl; 126 | 127 | std::ofstream ply_file(filename.c_str()); 128 | 129 | if(!ply_file) { 130 | std::cerr << "Error opening output file" << std::endl; 131 | exit(1); 132 | } 133 | 134 | ply_file << "ply" << std::endl; 135 | ply_file << "format ascii 1.0" << std::endl; 136 | ply_file << "element vertex " << point_cloud.size() << std::endl; 137 | ply_file << "property float x" << std::endl; 138 | ply_file << "property float y" << std::endl; 139 | ply_file << "property float z" << std::endl; 140 | ply_file << "property uchar red" << std::endl; 141 | ply_file << "property uchar green" << std::endl; 142 | ply_file << "property uchar blue" << std::endl; 143 | ply_file << "end_header" << std::endl; 144 | for(unsigned int i = 0; i < point_cloud.size(); i++) { 145 | if((i & 0xffff) == 0) { 146 | print_progress(i, point_cloud.size()); 147 | } 148 | 149 | point_ptr p = point_cloud[i]; 150 | cv::Matx color = p->get_color(); 151 | ply_file << p->pos(0) << " " << p->pos(1) << " " << p->pos(2) << " "; 152 | ply_file << static_cast(color(2)) << " " 153 | << static_cast(color(1)) << " " 154 | << static_cast(color(0)) << std::endl; 155 | 156 | if(!ply_file) { 157 | std::cerr << "Error writing point cloud" << std::endl; 158 | exit(2); 159 | } 160 | } 161 | ply_file.close(); 162 | } 163 | 164 | // Statistics functions 165 | 166 | template 167 | static float sum(const std::vector &values) { 168 | return std::accumulate(values.begin(), values.end(), 0.0); 169 | } 170 | 171 | template 172 | static float average(const std::vector &values) { 173 | return sum(values) / values.size(); 174 | } 175 | 176 | template 177 | static float minimum(const std::vector &values) { 178 | return *std::min_element(values.begin(), values.end()); 179 | } 180 | 181 | template 182 | static float maximum(const std::vector &values) { 183 | return *std::max_element(values.begin(), values.end()); 184 | } 185 | 186 | int main(int argc, const char **argv) { 187 | po::variables_map vm; 188 | parse_options(argc, argv, vm); 189 | 190 | bool debug = vm.count("debug"); 191 | std::string capture_dir = vm["capture_dir"].as(); 192 | std::string output_file = vm["output_file"].as(); 193 | float covariance_scale_xy = vm["covscalexy"].as(); 194 | float covariance_scale_z = vm["covscalez"].as(); 195 | 196 | // Statistics variables 197 | float time_loading_stat; 198 | std::vector time_merging_stats; 199 | std::vector num_missing_stats; 200 | std::vector num_added_stats; 201 | 202 | if(!boost::filesystem::exists("calib.yml")) { 203 | std::cerr << "calib.yml does not exist" << std::endl; 204 | exit(9); 205 | } 206 | 207 | CHerreraCalibration calibration; 208 | calibration.load("calib.yml"); 209 | 210 | boost::ptr_vector views; 211 | std::cout << "Loading views" << std::flush; 212 | boost::timer timer; 213 | load_views(capture_dir, views, covariance_scale_xy, covariance_scale_z, calibration); 214 | time_loading_stat = timer.elapsed(); 215 | std::cout << views.size() << " views" << std::endl; 216 | 217 | unsigned int first_view = 0; 218 | unsigned int last_view = views.size(); 219 | 220 | if(debug) { 221 | // Output all the points overlayed in one cloud. 222 | std::vector overlayed_point_cloud; 223 | std::cout << "Overlaying point clouds" << std::endl; 224 | 225 | for(unsigned int i = first_view; i < last_view; i++) { 226 | print_progress(i, views.size()); 227 | views[i].insert_into_point_cloud(overlayed_point_cloud); 228 | } 229 | output_point_cloud(overlayed_point_cloud, "overlayed.ply"); 230 | } 231 | 232 | // Get a view connectivity matrix. 233 | cv::Mat view_connectivity; 234 | if(vm.count("connectivity")) { 235 | std::string connectivity_file = vm["connectivity"].as(); 236 | 237 | if(!boost::filesystem::exists(connectivity_file)) { 238 | std::cerr << "Connectivity file " << connectivity_file << " does not exist" << std::endl; 239 | exit(8); 240 | } 241 | 242 | try { 243 | cv::FileStorage fs(connectivity_file, cv::FileStorage::READ); 244 | cv::Mat temp; 245 | fs["C"] >> temp; 246 | temp.convertTo(view_connectivity, cv::DataType::type); 247 | } catch(cv::Exception &e) { 248 | std::cerr << "Error reading connectivity matrix: " << e.what() << std::endl; 249 | exit(7); 250 | } 251 | } else { 252 | view_connectivity = cv::Mat_::ones(views.size(), views.size()); 253 | } 254 | 255 | if(static_cast(view_connectivity.rows) < views.size() || 256 | static_cast(view_connectivity.cols) < views.size()) { 257 | std::cerr << "Connectivity matrix has dimensions " << view_connectivity.cols << "x" << view_connectivity.rows 258 | << " instead of " << views.size() << "x" << views.size() << std::endl; 259 | exit(8); 260 | } 261 | 262 | std::cout << "Merging views" << std::endl; 263 | std::vector global_point_cloud; 264 | for(unsigned int view_idx = first_view; view_idx < last_view; view_idx++) { 265 | print_progress(view_idx, views.size()); 266 | 267 | timer.restart(); 268 | CView &view = views[view_idx]; 269 | view.merge(views, 270 | view_idx, 271 | view_connectivity, 272 | global_point_cloud); 273 | 274 | // Collect statistics. 275 | float num_pixels = IMAGE_WIDTH * IMAGE_HEIGHT; 276 | time_merging_stats.push_back(timer.elapsed()); 277 | num_missing_stats.push_back(view.get_num_missing()); 278 | num_added_stats.push_back(view.get_num_added() / (num_pixels - view.get_num_missing()) * 100); 279 | 280 | #ifdef DEBUG 281 | if(debug) { 282 | // Output images showing the computed depth values. 283 | 284 | cv::Matx depthmap; 285 | float max_depth = 0; 286 | for(int v = 0; v < IMAGE_HEIGHT; v++) { 287 | for(int u = 0; u < IMAGE_WIDTH; u++) { 288 | float depth = view.has_measurement(u, v) && view.measurement_accepted[v][u] 289 | ? view.get_depth(u, v) 290 | : 0; 291 | if(depth > max_depth) { 292 | max_depth = depth; 293 | } 294 | depthmap(v, u) = depth; 295 | } 296 | } 297 | 298 | depthmap = depthmap * (1 / max_depth) * 255; 299 | 300 | std::ostringstream depthmap_filename; 301 | depthmap_filename << "depthmap-" << view_idx << ".png"; 302 | cv::imwrite(depthmap_filename.str(), depthmap); 303 | 304 | // Output images showing the state of each point. 305 | 306 | cv::Mat state(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC3); 307 | for(int v = 0; v < IMAGE_HEIGHT; v++) { 308 | for(int u = 0; u < IMAGE_WIDTH; u++) { 309 | cv::Vec3b color; // in BGR format 310 | if(view.has_measurement(u, v)) { 311 | if(view.measurement_used[v][u]) { 312 | // The point was used to refine an existing point. 313 | color[0] = 128; 314 | color[1] = 255; 315 | color[2] = 128; 316 | } else { 317 | // The point was added to the cloud. 318 | color[0] = 255; 319 | color[1] = 128; 320 | color[2] = 128; 321 | } 322 | } else { 323 | // There was no measurement for the pixel. 324 | color[0] = 213; 325 | color[1] = 213; 326 | color[2] = 213; 327 | } 328 | state.at(v, u) = color; 329 | } 330 | } 331 | 332 | std::ostringstream state_filename; 333 | state_filename<< "state-" << view_idx << ".png"; 334 | cv::imwrite(state_filename.str(), state); 335 | } 336 | #endif 337 | } 338 | output_point_cloud(global_point_cloud, output_file); 339 | 340 | // Output statistics. 341 | unsigned int num_pixels = IMAGE_WIDTH * IMAGE_HEIGHT; 342 | unsigned int num_original_points = (last_view - first_view) * num_pixels - sum(num_missing_stats); 343 | unsigned int num_resulting_points = global_point_cloud.size(); 344 | std::cout << "Statistics:" << std::endl; 345 | std::cout << "Loading views: avg " << time_loading_stat / views.size() << " s" << std::endl; 346 | std::cout << "Merging views: avg " << average(time_merging_stats) << " s, min " << minimum(time_merging_stats) << " s, max " << maximum(time_merging_stats) << " s" << std::endl; 347 | std::cout << "Missing measurements: avg " << average(num_missing_stats) / num_pixels * 100 << " %, min " << minimum(num_missing_stats) / num_pixels * 100 << " %, max " << maximum(num_missing_stats) / num_pixels * 100 << " %" << std::endl; 348 | std::cout << "Added points: avg " << average(num_added_stats) << " %, min " << minimum(num_added_stats) << " %, max " << maximum(num_added_stats) << " %" << std::endl; 349 | std::cout << "Points in the original cloud: " << num_original_points << std::endl; 350 | std::cout << "Points in the resulting cloud: " << num_resulting_points << std::endl; 351 | std::cout << "Ratio: " << (float)num_resulting_points / num_original_points * 100 << " %" << std::endl; 352 | 353 | return 0; 354 | } 355 | -------------------------------------------------------------------------------- /src/kinect_view.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "kinect_view.h" 10 | 11 | namespace kinect_merge { 12 | 13 | // This is used for deciding whether two points are similar enough to be merged. 14 | static const float SIMILARITY_THRESHOLD = 3.0f; // in units of standard deviations 15 | 16 | // Measurement variance constants 17 | static const float ALPHA_0 = 0.0032225f; 18 | static const float ALPHA_1 = -0.0020925f; 19 | static const float ALPHA_2 = 0.0022078f; 20 | static const float BETA_X = 0.0017228f; 21 | static const float BETA_Y = 0.0017092f; 22 | 23 | CPoint::CPoint() {} 24 | 25 | CPoint::CPoint(const cv::Matx31f& position, const cv::Matx33f& covariance, const cv::Matx& color) : 26 | pos(position), 27 | cov(covariance), 28 | col_sum(color), 29 | num_merged(1) {} 30 | 31 | cv::Matx CPoint::get_color() const { 32 | // NOTE: The following line would segfault on cv::Mat(col_sum). An alternate method follows. 33 | // return cv::Mat(col_sum) / num_merged; 34 | cv::Matx m; 35 | for(int i = 0; i < 3; i++) 36 | m(i) = col_sum(i) / num_merged; 37 | return m; 38 | } 39 | 40 | CView::CView(const std::string &extrinsic_file, 41 | const std::string &disparity_file, 42 | const std::string &color_file, 43 | float covariance_scale_xy, 44 | float covariance_scale_z, 45 | CKinectCalibration &calibration_) : 46 | calibration(calibration_), 47 | original_pointmap(boost::extents[IMAGE_HEIGHT][IMAGE_WIDTH]), 48 | pointmap(boost::extents[IMAGE_HEIGHT][IMAGE_WIDTH]), 49 | num_missing(0), 50 | #ifdef DEBUG // The measurement_used vector is used for outputting debug images. 51 | num_added(0), 52 | measurement_used(IMAGE_HEIGHT) { 53 | #else 54 | num_added(0) { 55 | #endif 56 | 57 | // Initialize the depth map. 58 | for(int v = 0; v < IMAGE_HEIGHT; v++) { 59 | for(int u = 0; u < IMAGE_WIDTH; u++) { 60 | depthmap(v, u) = -1; 61 | } 62 | } 63 | 64 | // Load extrinsic data. 65 | 66 | cv::Mat inv_transformation_tmp = cv::Mat_::eye(4, 4); 67 | cv::Mat inv_rotation(inv_transformation_tmp, cv::Rect(0, 0, 3, 3)); 68 | cv::Mat inv_translation(inv_transformation_tmp, cv::Rect(3, 0, 1, 3)); 69 | 70 | try { 71 | cv::FileStorage fs(extrinsic_file, cv::FileStorage::READ); 72 | fs["R"] >> inv_rotation; 73 | fs["T"] >> inv_translation; 74 | } catch(cv::Exception &e) { 75 | std::cerr << std::endl << "Error reading extrinsic data: " << e.what() << std::endl; 76 | exit(4); 77 | } 78 | 79 | inv_transformation = inv_transformation_tmp; 80 | cv::Mat transformation = inv_transformation_tmp.inv(); 81 | cv::Mat rotation = inv_rotation.t(); 82 | 83 | // Load disparity map. 84 | int flags = -1; // Load as-is (greyscale). Using 0 (force grayscale) results in a segfault. 85 | cv::Mat1s disparitymap = cv::imread(disparity_file, flags); 86 | 87 | if(disparitymap.cols != IMAGE_WIDTH || disparitymap.rows != IMAGE_HEIGHT) { 88 | std::cerr << std::endl << "Disparity map has dimensions " << disparitymap.cols << "x" << disparitymap.rows << 89 | " instead of " << IMAGE_WIDTH << "x" << IMAGE_HEIGHT << std::endl; 90 | exit(5); 91 | } 92 | 93 | // Load color map. 94 | cv::Mat3b colormap = cv::imread(color_file, flags); 95 | 96 | if(colormap.cols != IMAGE_WIDTH || colormap.rows != IMAGE_HEIGHT) { 97 | std::cerr << std::endl << "Color map has dimensions " << colormap.cols << "x" << colormap.rows << 98 | " instead of " << IMAGE_WIDTH << "x" << IMAGE_HEIGHT << std::endl; 99 | exit(6); 100 | } 101 | 102 | // Convert to a point cloud in global coordinates and calculate the covariance matrix. 103 | cv::Matx31f local_point; 104 | cv::Mat local_point_tmp = (cv::Mat_(4, 1) << 0, 0, 0, 1); 105 | cv::Mat global_point; 106 | cv::Mat covariance; 107 | for(int v = 0; v < disparitymap.rows; v++) { 108 | for(int u = 0; u < disparitymap.cols; u++) { 109 | const short disparity = disparitymap(v, u); 110 | if(disparity == 2047) { 111 | continue; 112 | } 113 | 114 | // Convert to local and then global coordinates. 115 | calibration.disparity2point(u, v, disparity, local_point); 116 | local_point_tmp.at(0) = local_point(0); 117 | local_point_tmp.at(1) = local_point(1); 118 | local_point_tmp.at(2) = local_point(2); 119 | global_point = transformation * local_point_tmp; 120 | global_point.pop_back(); // Turn into a 3 by 1 matrix. 121 | 122 | float depth = local_point(2); 123 | if(depth < 0) { 124 | // The point projects behind the camera. Ignore it. 125 | continue; 126 | } 127 | 128 | // Calculate the covariance matrix. 129 | covariance = cv::Mat_::eye(3, 3); 130 | covariance.at(0, 0) = BETA_X * BETA_X * depth * depth / 12; 131 | covariance.at(1, 1) = BETA_Y * BETA_Y * depth * depth / 12; 132 | covariance.at(2, 2) = pow(ALPHA_2 * depth * depth + ALPHA_1 * depth + ALPHA_0, 2); 133 | covariance.at(0, 0) *= covariance_scale_xy; 134 | covariance.at(1, 1) *= covariance_scale_xy; 135 | covariance.at(2, 2) *= covariance_scale_z; 136 | 137 | // Convert the covariance matrix to the global reference frame. 138 | covariance = rotation * covariance * inv_rotation; 139 | assert(covariance.at(0, 0) >= 0); 140 | assert(covariance.at(1, 1) >= 0); 141 | assert(covariance.at(2, 2) >= 0); 142 | 143 | // Get the pixel coordinates for the measurement. 144 | int uc, vc; 145 | to_image_plane(local_point_tmp, uc, vc); 146 | if(uc < 0 || uc >= IMAGE_WIDTH || vc < 0 || vc >= IMAGE_HEIGHT) { 147 | continue; 148 | } 149 | 150 | if(!original_pointmap[vc][uc] || depthmap(vc, uc) > depth) { 151 | depthmap(vc, uc) = depth; 152 | original_pointmap[vc][uc] = boost::make_shared(global_point, 153 | covariance, 154 | colormap(vc, uc)); 155 | } 156 | } 157 | } 158 | } 159 | 160 | void CView::to_image_plane(const cv::Matx41f &local_pos, int &uc, int &vc) const { 161 | cv::Matx31f local_pos_tmp(local_pos(0), local_pos(1), local_pos(2)); 162 | cv::Matx21f cf; 163 | calibration.point2rgb(local_pos_tmp, cf); 164 | uc = cf(0) + 0.5; 165 | vc = cf(1) + 0.5; 166 | } 167 | 168 | static float calculate_mahalanobis_distance(const CPoint &refined_point, const CPoint &p) { 169 | cv::Matx operand = (p.pos - refined_point.pos).t() * p.cov.inv() * (p.pos - refined_point.pos); 170 | return sqrt(operand(0)); 171 | } 172 | 173 | // Calculate a new position estimate, covariance matrix and color for an existing point given a new measurement. 174 | static void refine_point(const CPoint &existing_point, const CPoint &new_measurement, CPoint &refined_point) { 175 | cv::Matx33f new_measurement_cov_inv = new_measurement.cov.inv(); 176 | 177 | // Combine the covariance matrices. 178 | refined_point.cov = (existing_point.cov.inv() + new_measurement_cov_inv).inv(); 179 | 180 | // Calculate the refined position estimate. 181 | refined_point.pos = existing_point.pos + refined_point.cov * new_measurement_cov_inv * (new_measurement.pos - existing_point.pos); 182 | 183 | // Add the color values. 184 | refined_point.col_sum = existing_point.col_sum + new_measurement.col_sum; 185 | refined_point.num_merged = existing_point.num_merged + new_measurement.num_merged; 186 | } 187 | 188 | void CView::refine_points(CView &connected_view, std::vector > &measurement_used) const { 189 | for(int v = 0; v < IMAGE_HEIGHT; v++) { 190 | for(int u = 0; u < IMAGE_WIDTH; u++) { 191 | if(!connected_view.has_point(u, v)) { 192 | continue; 193 | } 194 | 195 | // Project the point onto the current view image plane. 196 | // NOTE: Only points which were added to cloud from the connected view are projected. 197 | // Each point in the cloud is pointed to by exactly one view. This means that a single 198 | // point in the cloud will NOT be refined multiple times with the same new measurement 199 | // through different connected views. 200 | int proj_u, proj_v; 201 | float proj_depth; 202 | CPoint &connected_view_point = connected_view.get_point(u, v); 203 | project(connected_view_point, proj_u, proj_v, proj_depth); 204 | 205 | if(proj_depth < 0) { 206 | // The point projects outside the image plane. 207 | continue; 208 | } 209 | 210 | if(!has_measurement(proj_u, proj_v)) { 211 | // There is nothing to merge with at this pixel. 212 | continue; 213 | } 214 | 215 | const CPoint ¤t_view_point = get_original_point(proj_u, proj_v); 216 | 217 | // Calculate the refined point and use it if it's close enough to the existing point and 218 | // new measurement. 219 | CPoint refined_point; 220 | refine_point(connected_view_point, current_view_point, refined_point); 221 | if(calculate_mahalanobis_distance(refined_point, connected_view_point) < SIMILARITY_THRESHOLD && 222 | calculate_mahalanobis_distance(refined_point, current_view_point) < SIMILARITY_THRESHOLD) { 223 | connected_view_point = refined_point; 224 | measurement_used[proj_v][proj_u] = true; 225 | } 226 | } 227 | } 228 | } 229 | 230 | void CView::insert_into_point_cloud(std::vector &point_cloud) { 231 | for(int v = 0; v < IMAGE_HEIGHT; v++) { 232 | for(int u = 0; u < IMAGE_WIDTH; u++) { 233 | if(has_measurement(u, v)) { 234 | point_cloud.push_back(original_pointmap[v][u]); 235 | } 236 | } 237 | } 238 | } 239 | 240 | void CView::project(const CPoint &global_point, int &proj_u, int &proj_v, float &proj_depth) const { 241 | cv::Matx41f global_pos(global_point.pos(0), 242 | global_point.pos(1), 243 | global_point.pos(2), 244 | 1); 245 | cv::Matx41f local_pos = inv_transformation * global_pos; 246 | to_image_plane(local_pos, proj_u, proj_v); 247 | 248 | if(proj_u < 0 || proj_u >= IMAGE_WIDTH || 249 | proj_v < 0 || proj_v >= IMAGE_HEIGHT) { 250 | // The point projects outside the image plane. 251 | proj_depth = -1; 252 | } else { 253 | proj_depth = local_pos(2); 254 | } 255 | } 256 | 257 | void CView::merge(boost::ptr_vector &views, 258 | unsigned int view_idx, 259 | const cv::Mat view_connectivity, 260 | std::vector &global_point_cloud) { 261 | // Initialize vectors. 262 | // A measurement is marked as used if an existing point is refined with it. 263 | #ifndef DEBUG 264 | // Use a local variable instead of a member variable. 265 | std::vector > measurement_used(IMAGE_HEIGHT); 266 | #endif 267 | for(int v = 0; v < IMAGE_HEIGHT; v++) { 268 | measurement_used[v].resize(IMAGE_WIDTH); 269 | for(int u = 0; u < IMAGE_WIDTH; u++) { 270 | measurement_used[v][u] = false; 271 | } 272 | } 273 | 274 | // Project all connected views into the current view to find points to be refined. 275 | for(unsigned int connected_idx = 0; connected_idx < view_idx; connected_idx++) { 276 | if(!view_connectivity.at(view_idx, connected_idx)) { 277 | // The view is not connected. Skip it. 278 | continue; 279 | } 280 | 281 | CView &connected_view = views[connected_idx]; 282 | 283 | refine_points(connected_view, measurement_used); 284 | } 285 | 286 | // Add all the points that weren't used to refine existing points to the point cloud. 287 | for(int v = 0; v < IMAGE_HEIGHT; v++) { 288 | for(int u = 0; u < IMAGE_WIDTH; u++) { 289 | if(has_measurement(u, v)) { 290 | if(!measurement_used[v][u]) { 291 | pointmap[v][u] = original_pointmap[v][u]; 292 | global_point_cloud.push_back(pointmap[v][u]); 293 | num_added++; 294 | } 295 | } else { 296 | num_missing++; 297 | } 298 | } 299 | } 300 | } 301 | 302 | } 303 | -------------------------------------------------------------------------------- /src/kinect_view.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "kinect_calibration.h" 9 | 10 | namespace kinect_merge { 11 | 12 | using kinect_capture::CKinectCalibration; 13 | 14 | static const int IMAGE_WIDTH = 640; 15 | static const int IMAGE_HEIGHT = 480; 16 | static const int ADJACENCY_SIZE = 2; 17 | static const float DEFAULT_COVARIANCE_SCALE_XY = 40.0f; 18 | static const float DEFAULT_COVARIANCE_SCALE_Z = 20.0f; 19 | 20 | struct CPoint { 21 | typedef boost::shared_ptr ptr; 22 | typedef boost::shared_ptr const_ptr; 23 | 24 | CPoint(); 25 | CPoint(const cv::Matx31f& position, const cv::Matx33f& covariance, const cv::Matx& color); 26 | cv::Matx get_color() const; 27 | 28 | cv::Matx31f pos; // Position 29 | cv::Matx33f cov; // Covariance matrix 30 | cv::Matx col_sum; // Color sum 31 | unsigned int num_merged; // Number of points included in the color sum. 32 | }; 33 | 34 | class CView { 35 | public: 36 | CView(const std::string &exstrinsic_file, 37 | const std::string &disparity_file, 38 | const std::string &color_file, 39 | float covariance_scale_xy, 40 | float covariance_scale_z, 41 | CKinectCalibration& calibration); 42 | 43 | // Does the original depthmap have a measurement at the given pixel? 44 | bool has_measurement(int u, int v) const { return original_pointmap[v][u]; } 45 | 46 | // Has a point been added to the point cloud from the given pixel? 47 | bool has_point(int u, int v) const { return pointmap[v][u]; } 48 | 49 | // Return the depth of the original measurement at the given pixel. 50 | float get_depth(int u, int v) const { assert(has_measurement(u, v)); return depthmap(v, u); } 51 | 52 | // Return a point corresponding to a pixel in the original depth map. 53 | const CPoint& get_original_point(int u, int v) const { assert(has_measurement(u, v)); return *original_pointmap[v][u]; } 54 | 55 | // Return a point corresponding to a pixel. The point might have been refined by later measurements. 56 | CPoint& get_point(int u, int v) { assert(has_point(u, v)); return *pointmap[v][u]; } 57 | 58 | // Insert all original points into the cloud unmodified. 59 | void insert_into_point_cloud(std::vector &point_cloud); 60 | 61 | // Project a global point onto the image plane of this view. 62 | void project(const CPoint &global_point, int &proj_u, int &proj_v, float &proj_depth) const; 63 | 64 | // Add the points of this view to the point cloud. 65 | void merge(boost::ptr_vector &views, 66 | unsigned int view_idx, 67 | const cv::Mat view_connectivity, 68 | std::vector &global_point_cloud); 69 | 70 | // Statistics 71 | int get_num_missing() const { return num_missing; } 72 | int get_num_added() const { return num_added; } 73 | 74 | private: 75 | // Project a local point onto the image plane of this view. 76 | void to_image_plane(const cv::Matx41f &local_pos, int &uc, int &vc) const; 77 | 78 | // Update existing points in the connected views using new measurements that are similar enough. 79 | void refine_points(CView &connected_view, std::vector > &measurement_used) const; 80 | 81 | CKinectCalibration& calibration; 82 | 83 | cv::Matx44f inv_transformation; 84 | cv::Matx depthmap; 85 | 86 | // Contains the global points backprojected from the original depthmap. 87 | // These might have been refined by later measurements. 88 | // Has dimensions IMAGE_HEIGHTxIMAGE_WIDTH. 89 | boost::multi_array original_pointmap; 90 | 91 | // Contains the global points added to the point cloud from this view. 92 | // These might have been refined by later measurements. 93 | // Has dimensions IMAGE_HEIGHTxIMAGE_WIDTH. 94 | boost::multi_array pointmap; 95 | 96 | // Statistics 97 | int num_missing; 98 | int num_added; 99 | 100 | #ifdef DEBUG 101 | public: 102 | // This is used for outputting debug images. 103 | std::vector > measurement_used; 104 | #endif 105 | }; 106 | 107 | } 108 | --------------------------------------------------------------------------------