├── ReadMe.html ├── ReadMe.txt ├── Summary.txt ├── data ├── l90.tar.gz ├── lmov.tar.gz └── lstraight.tar.gz ├── reference-output ├── l90.tuple ├── l90_0.tuple ├── l90_0_results.json ├── l90_0_results.m ├── l90_1.tuple ├── l90_1_results.json ├── l90_1_results.m ├── l90_2.tuple ├── l90_2_results.json ├── l90_2_results.m ├── l90_results.json ├── l90_results.m ├── lmov.tuple ├── lmov_0.tuple ├── lmov_0_results.json ├── lmov_0_results.m ├── lmov_1.tuple ├── lmov_1_results.json ├── lmov_1_results.m ├── lmov_2.tuple ├── lmov_2_results.json ├── lmov_2_results.m ├── lmov_results.json ├── lmov_results.m ├── lstraight.tuple ├── lstraight_0.tuple ├── lstraight_0_results.json ├── lstraight_0_results.m ├── lstraight_1.tuple ├── lstraight_1_results.json ├── lstraight_1_results.m ├── lstraight_2.tuple ├── lstraight_2_results.json ├── lstraight_2_results.m ├── lstraight_results.json ├── lstraight_results.m └── version_string ├── scripts ├── copy_results.sh ├── create_pictures.m ├── create_pictures_iteration.m ├── create_tuples.sh ├── errorbar_tick.m ├── format_ticks.m ├── my_figure.m ├── my_fonts.m ├── results.json ├── run_all.sh ├── script_variables.sh └── table_test.tex └── src ├── CMakeLists.txt ├── analyzer.cpp ├── analyzer.h ├── aux_functions.cpp ├── aux_functions.h ├── calib_tuple.cpp ├── calib_tuple.h ├── gsl_jacobian.cpp ├── gsl_jacobian.h ├── gsl_jacobian_test.cpp ├── solver.cpp ├── solver2.cpp ├── solver2_meat.cpp ├── solver_options.cpp ├── solver_options.h ├── solver_utils.cpp ├── solver_utils.h ├── synchronizer.cpp ├── synchronizer_options.cpp ├── synchronizer_options.h ├── test_json.c ├── verifier.cpp ├── verifier_options.cpp ├── verifier_options.h ├── verifier_utils.cpp └── verifier_utils.h /ReadMe.html: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Supplemental materials for Simultaneous calibration of odometry and sensor parameters for mobile robots 7 | 8 |

Supplemental materials for “Simultaneous calibration of odometry and sensor parameters for mobile robots”

9 | 10 |

This archive contains C++ source code for the calibration method described in the paper, plus the raw data and scripts to reproduce the experiments, in the spirit of reproducible research.

11 | 12 |

Please contact Andrea Censi (andrea at cds.caltech.edu) for any concern regarding installation and usage of this package.

13 | 14 |

An up-to-date version of this package can be found at the website:

15 | 16 |

http://purl.org/censi/2011/calibration

17 | 18 |

Package contents

19 | 20 |

The following is an overview of the contents.

21 | 22 |

Directory `src/`

23 | 24 |

This directory contains the C++ source files for implementing the method.

25 | 26 |

The main executables created are:

27 | 28 |

30 |
synchronizer: Synchronizes laser data and odometry to obtain the tuple files (input to the algorithm).
31 |
34 |
solver: Given the tuple files, solves for the calibration parameters. This is the “meat” of the method.
35 |

37 | 38 |

Directory `scripts/`

39 | 40 |

This directory contains the data files and the scripts for running the experiments and visualizing the data.

41 | 42 |

The main scripts are:

43 | 44 |

scripts/run_all.sh: Runs the complete calibration procedure.
scripts/script_variables.sh: This file contains the environment variables, executable paths, and parameters common to all the scripts.

49 | 50 |

Directory `data/`

51 | 52 |

This directory contains the raw data logs. The raw logs are called lstraight, l90, lmov; these correspond to the configurations A,B,C in the paper.

53 | 54 |

Inside each .tar.gz, there are multiple experiments. See below for a description of the data format.

55 | 56 |

Directory `reference-output/`

57 | 58 |

This directory contains the output of the synchronization procedure and the results of calibration, as well as the intermediate results (in case one is not able to run the scan matching software).

59 | 60 |

The intermediate result of the method is the creation of “tuple files”: the odometry and scan matching data is integrated to obtain a series of tuples, each containing average wheel velocities and scan matching result for the interval.

61 | 62 |

For each configuration X:

63 | 64 |

X.tuple are the complete tuples
X_i.tuple are the tuples for the i-th subset
X_results.json are the calibration results in JSON
X_results.m … and in Matlab format

73 | 74 |

Installation instructions

75 | 76 |

The installation process has been tried on Mac OS X (10.5-10.6) and various versions of Ubuntu. With perhaps a few modifications, the software should run on all flavors of Unix.

77 | 78 |

The first step is installing CSM. Please see the instructions at the website http://purl.org/censi/2007/csm.

79 | 80 |

The minimal steps are as follows:

81 | 82 |

$ git clone git://github.com/AndreaCensi/csm.git
 83 | $ cd csm
 84 | $ cmake .
 85 | $ make 
 86 | $ make install

87 | 88 |

Remember to point PKG_CONFIG_PATH where you installed CSM. In the default case, this is /usr/local:

89 | 90 |

export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig/:$PKG_CONFIG_PATH

91 | 92 |

Compiling the calibration software does not require more dependencies than CSM itself (e.g., GSL).

93 | 94 |

The installation is simple:

95 | 96 |

$ cd src/
 97 | $ cmake .
 98 | $ make install

99 | 100 |

Execution instructions

101 | 102 |

In the directory scripts/ there is a file run_all.sh that does the complete calibration process from the logged data.

103 | 104 |

$ cd logs/
105 | $ ./run_all.sh

106 | 107 |

Note that it might take some time. It is normal to see a few scan matching errors displayed; that is just bad data (synchronization issues between odometry and laser.)

108 | 109 |

The script create_tuples.sh is called by run_all.sh and is the one responsible for processing the data with the scan matcher and running the synchronization between scan matching and odometry. It creates “tuple files” which correspond to a series of what in the paper are called “samples”. Basically, the tuples correspond to the input of the algorithm.

110 | 111 |

Note that the reference-output/ directory contains the generated tuple files, so that the results can be reproduced even if the scan matcher cannot be installed.

112 | 113 |

Data formats

114 | 115 |

These are a few notes about the data formats used.

116 | 117 |

All logs and intermediate results are in the JSON format. See the website http://www.json.org/ for more information (including libraries for reading the format easily for many different languages).

118 | 119 |

Raw log files format

120 | 121 |

The raw log files have this format:

122 | 123 |

{  "timestamp": [949, 943101], 
124 |    "readings": [ ...],  
125 |    "theta": [ ... ], 
126 |    "right": 0, 
127 |    "left": 0, 
128 |    "leftTimestamp": [ 949, 940807], 
129 |    "rightTimestamp": [ 949, 942685] }

130 | 131 |

where:

132 | 133 |

readings is the array of range-finder readings.
theta is the direction of each reading.
left and right are the encoder readings, in ticks.
timestamp is a two-shorts UNIX timestamp of when the laser data was taken;
leftTimestamp, rightTimestamp are the timestamps for when the the encoder data was taken.

144 | 145 |

(there are also other fields, not relevant for calibration)

146 | 147 |

Tuples files format

148 | 149 |

The tuples files are the result of running scan matching, and synchronization of scan matching and odometry data.

150 | 151 |

The field T is the interval; the fields phi_r and phi_r are the average wheel velocities, and sm is the scan matching estimate.

152 | 180 | -------------------------------------------------------------------------------- /ReadMe.txt: -------------------------------------------------------------------------------- 1 | Supplemental materials for "Simultaneous calibration of odometry and sensor parameters for mobile robots" 2 | ========================================================================== 3 | 4 | This archive contains C++ source code for the calibration method described 5 | in the paper, plus the raw data and scripts to reproduce the experiments, 6 | in the spirit of [reproducible research](http://reproducibleresearch.net/). 7 | 8 | Please contact Andrea Censi (andrea at cds.caltech.edu) for any concern 9 | regarding installation and usage of this package. 10 | 11 | An up-to-date version of this package can be found at the website: 12 | 13 | 14 | 15 | 16 | 17 | Package contents 18 | --------------------------------------------------------------------------- 19 | 20 | The following is an overview of the contents. 21 | 22 | ### Directory ``src/`` 23 | 24 | This directory contains the C++ source files for implementing the method. 25 | 26 | The main executables created are: 27 | 28 | * ``synchronizer``: Synchronizes laser data and odometry to obtain 29 | the tuple files (input to the algorithm). 30 | 31 | * ``solver``: Given the tuple files, solves for the calibration parameters. 32 | This is the "meat" of the method. 33 | 34 | 35 | 36 | ### Directory ``scripts/`` 37 | 38 | This directory contains the data files and the scripts for running 39 | the experiments and visualizing the data. 40 | 41 | The main scripts are: 42 | 43 | * ``scripts/run_all.sh``: Runs the complete calibration procedure. 44 | * ``scripts/script_variables.sh``: This file contains the 45 | environment variables, executable paths, and parameters common 46 | to all the scripts. 47 | 48 | ### Directory ``data/`` 49 | 50 | This directory contains the raw data logs. The raw logs are called 51 | ``lstraight``, ``l90``, ``lmov``; these correspond to the configurations 52 | A,B,C in the paper. 53 | 54 | Inside each ``.tar.gz``, there are multiple experiments. See below for 55 | a description of the data format. 56 | 57 | 58 | ### Directory ``reference-output/`` 59 | 60 | This directory contains the output of the synchronization 61 | procedure and the results of calibration, as well as the 62 | intermediate results (in case one is not able to run the 63 | scan matching software). 64 | 65 | The intermediate result of the method is the creation of 66 | "tuple files": the odometry and scan matching data is integrated 67 | to obtain a series of tuples, each containing average wheel 68 | velocities and scan matching result for the interval. 69 | 70 | For each configuration X: 71 | 72 | * ``X.tuple`` are the complete tuples 73 | * ``X_i.tuple`` are the tuples for the i-th subset 74 | * ``X_results.json`` are the calibration results in JSON 75 | * ``X_results.m`` ... and in Matlab format 76 | 77 | 78 | Installation instructions 79 | --------------------------------------------------------------------------- 80 | 81 | The installation process has been tried on Mac OS X (10.5-10.6) and various 82 | versions of Ubuntu. With perhaps a few modifications, the software should run 83 | on all flavors of Unix. 84 | 85 | The first step is installing CSM. Please see the instructions at the website 86 | . 87 | 88 | The minimal steps are as follows: 89 | 90 | $ git clone git://github.com/AndreaCensi/csm.git 91 | $ cd csm 92 | $ cmake . 93 | $ make 94 | $ make install 95 | 96 | Remember to point ``PKG_CONFIG_PATH`` where you installed CSM. In the default 97 | case, this is ``/usr/local``: 98 | 99 | export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig/:$PKG_CONFIG_PATH 100 | 101 | Compiling the calibration software does not require more dependencies than 102 | CSM itself (e.g., GSL). 103 | 104 | The installation is simple: 105 | 106 | $ cd src/ 107 | $ cmake . 108 | $ make install 109 | 110 | 111 | Execution instructions 112 | --------------------------------------------------------------------------- 113 | 114 | In the directory ``scripts/`` there is a file ``run_all.sh`` that does 115 | the complete calibration process from the logged data. 116 | 117 | $ cd logs/ 118 | $ ./run_all.sh 119 | 120 | Note that it might take some time. It is normal to see a few scan matching 121 | errors displayed; that is just bad data (synchronization issues between 122 | odometry and laser.) 123 | 124 | The script ``create_tuples.sh`` is called by ``run_all.sh`` and is the 125 | one responsible for processing the data with the scan matcher and running 126 | the synchronization between scan matching and odometry. It creates 127 | "tuple files" which correspond to a series of what in the paper are called 128 | "samples". Basically, the tuples correspond to the input of the algorithm. 129 | 130 | Note that the ``reference-output/`` directory contains the generated tuple 131 | files, so that the results can be reproduced even if the scan matcher 132 | cannot be installed. 133 | 134 | 135 | Data formats 136 | --------------------------------------------------------------------------- 137 | 138 | These are a few notes about the data formats used. 139 | 140 | All logs and intermediate results are in the JSON format. See the website 141 | for more information (including libraries for reading 142 | the format easily for many different languages). 143 | 144 | ### Raw log files format 145 | 146 | The raw log files have this format: 147 | 148 | { "timestamp": [949, 943101], 149 | "readings": [ ...], 150 | "theta": [ ... ], 151 | "right": 0, 152 | "left": 0, 153 | "leftTimestamp": [ 949, 940807], 154 | "rightTimestamp": [ 949, 942685] } 155 | 156 | where: 157 | 158 | * ``readings`` is the array of range-finder readings. 159 | * ``theta`` is the direction of each reading. 160 | * ``left`` and ``right`` are the encoder readings, in ticks. 161 | * ``timestamp`` is a two-shorts UNIX timestamp of when the laser data was taken; 162 | * ``leftTimestamp``, ``rightTimestamp`` are the timestamps for when the 163 | the encoder data was taken. 164 | 165 | (there are also other fields, not relevant for calibration) 166 | 167 | 168 | ### Tuples files format 169 | 170 | The *tuples files* are the result of running scan matching, and 171 | synchronization of scan matching and odometry data. 172 | 173 | The field ``T`` is the interval; the fields ``phi_r`` and 174 | ``phi_r`` are the average wheel velocities, and ``sm`` is 175 | the scan matching estimate. 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 215 | -------------------------------------------------------------------------------- /Summary.txt: -------------------------------------------------------------------------------- 1 | This archive contains C++ source code for the calibration method described 2 | in the paper, plus the raw data and scripts to reproduce the experiments, 3 | in the spirit of "Reproducible Research". 4 | 5 | Please contact Andrea Censi for any concern 6 | regarding installation and usage of this package. 7 | 8 | An up-to-date version of this package can be found at the website: 9 | 10 | http://purl.org/censi/2011/calibration 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /data/l90.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndreaCensi/calibration/026c7c3b21392e6143eea25c424dba69cc29614e/data/l90.tar.gz -------------------------------------------------------------------------------- /data/lmov.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndreaCensi/calibration/026c7c3b21392e6143eea25c424dba69cc29614e/data/lmov.tar.gz -------------------------------------------------------------------------------- /data/lstraight.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndreaCensi/calibration/026c7c3b21392e6143eea25c424dba69cc29614e/data/lstraight.tar.gz -------------------------------------------------------------------------------- /reference-output/l90_0_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0878146, "l_diam": 0.0410529, "r_diam": 0.041384, "l_x": -0.00265427, "l_y": 0.00583491, "l_theta": -1.55197, "E_d": 1.00807, "E_b": 0.975718, "covariance": [ [ 1.53263e-06, 1.49619e-06, 6.45365e-06, -8.45585e-09, -7.06869e-08, 1.34922e-06 ], [ 1.49619e-06, 1.5725e-06, 6.53649e-06, -7.53002e-09, 4.93707e-08, 1.19557e-06 ], [ 6.45365e-06, 6.53649e-06, 2.78659e-05, -3.99052e-08, -3.49801e-08, 5.42463e-06 ], [ -8.45585e-09, -7.53002e-09, -3.99052e-08, 5.23236e-06, 1.55149e-07, -4.1681e-05 ], [ -7.06869e-08, 4.93707e-08, -3.49801e-08, 1.55149e-07, 2.78263e-06, -1.85146e-05 ], [ 1.34922e-06, 1.19557e-06, 5.42463e-06, -4.1681e-05, -1.85146e-05, 0.00689921 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/l90_0_results.m: -------------------------------------------------------------------------------- 1 | function res = l90_0_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0878146, ... 5 | 'l_diam', 0.0410529, ... 6 | 'r_diam', 0.041384, ... 7 | 'l_x', -0.00265427, ... 8 | 'l_y', 0.00583491, ... 9 | 'l_theta', -1.55197, ... 10 | 'E_d', 1.00807, ... 11 | 'E_b', 0.975718, ... 12 | 'covariance', [1.53263e-06, 1.49619e-06, 6.45365e-06, -8.45585e-09, -7.06869e-08, 1.34922e-06; 1.49619e-06, 1.5725e-06, 6.53649e-06, -7.53002e-09, 4.93707e-08, 1.19557e-06; 6.45365e-06, 6.53649e-06, 2.78659e-05, -3.99052e-08, -3.49801e-08, 5.42463e-06; -8.45585e-09, -7.53002e-09, -3.99052e-08, 5.23236e-06, 1.55149e-07, -4.1681e-05; -7.06869e-08, 4.93707e-08, -3.49801e-08, 1.55149e-07, 2.78263e-06, -1.85146e-05; 1.34922e-06, 1.19557e-06, 5.42463e-06, -4.1681e-05, -1.85146e-05, 0.00689921])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/l90_1_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0881706, "l_diam": 0.0412731, "r_diam": 0.0414925, "l_x": -0.00252347, "l_y": 0.00580311, "l_theta": -1.56708, "E_d": 1.00531, "E_b": 0.979673, "covariance": [ [ 8.96027e-07, 8.62063e-07, 3.7459e-06, -3.01418e-09, -4.18678e-08, 6.34109e-07 ], [ 8.62063e-07, 9.17343e-07, 3.7909e-06, -2.33899e-09, 5.33692e-08, 5.15209e-07 ], [ 3.7459e-06, 3.7909e-06, 1.62049e-05, -1.57192e-08, 3.36938e-08, 2.44855e-06 ], [ -3.01418e-09, -2.33899e-09, -1.57192e-08, 7.96771e-06, 1.81379e-07, -6.31006e-05 ], [ -4.18678e-08, 5.33692e-08, 3.36938e-08, 1.81379e-07, 1.63964e-06, -2.76441e-05 ], [ 6.34109e-07, 5.15209e-07, 2.44855e-06, -6.31006e-05, -2.76441e-05, 0.0109162 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/l90_1_results.m: -------------------------------------------------------------------------------- 1 | function res = l90_1_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0881706, ... 5 | 'l_diam', 0.0412731, ... 6 | 'r_diam', 0.0414925, ... 7 | 'l_x', -0.00252347, ... 8 | 'l_y', 0.00580311, ... 9 | 'l_theta', -1.56708, ... 10 | 'E_d', 1.00531, ... 11 | 'E_b', 0.979673, ... 12 | 'covariance', [8.96027e-07, 8.62063e-07, 3.7459e-06, -3.01418e-09, -4.18678e-08, 6.34109e-07; 8.62063e-07, 9.17343e-07, 3.7909e-06, -2.33899e-09, 5.33692e-08, 5.15209e-07; 3.7459e-06, 3.7909e-06, 1.62049e-05, -1.57192e-08, 3.36938e-08, 2.44855e-06; -3.01418e-09, -2.33899e-09, -1.57192e-08, 7.96771e-06, 1.81379e-07, -6.31006e-05; -4.18678e-08, 5.33692e-08, 3.36938e-08, 1.81379e-07, 1.63964e-06, -2.76441e-05; 6.34109e-07, 5.15209e-07, 2.44855e-06, -6.31006e-05, -2.76441e-05, 0.0109162])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/l90_2_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0880557, "l_diam": 0.0412267, "r_diam": 0.0414503, "l_x": -0.0025152, "l_y": 0.00592963, "l_theta": -1.55253, "E_d": 1.00542, "E_b": 0.978396, "covariance": [ [ 1.45666e-06, 1.40625e-06, 6.09942e-06, -2.58693e-08, -9.35208e-08, 3.90479e-06 ], [ 1.40625e-06, 1.49069e-06, 6.16911e-06, -2.48622e-08, 5.00381e-08, 3.73912e-06 ], [ 6.09942e-06, 6.16911e-06, 2.63552e-05, -1.14575e-07, -8.06965e-08, 1.62854e-05 ], [ -2.58693e-08, -2.48622e-08, -1.14575e-07, 1.01842e-05, 3.50495e-07, -8.59871e-05 ], [ -9.35208e-08, 5.00381e-08, -8.06965e-08, 3.50495e-07, 2.63702e-06, -3.5011e-05 ], [ 3.90479e-06, 3.73912e-06, 1.62854e-05, -8.59871e-05, -3.5011e-05, 0.0137884 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/l90_2_results.m: -------------------------------------------------------------------------------- 1 | function res = l90_2_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0880557, ... 5 | 'l_diam', 0.0412267, ... 6 | 'r_diam', 0.0414503, ... 7 | 'l_x', -0.0025152, ... 8 | 'l_y', 0.00592963, ... 9 | 'l_theta', -1.55253, ... 10 | 'E_d', 1.00542, ... 11 | 'E_b', 0.978396, ... 12 | 'covariance', [1.45666e-06, 1.40625e-06, 6.09942e-06, -2.58693e-08, -9.35208e-08, 3.90479e-06; 1.40625e-06, 1.49069e-06, 6.16911e-06, -2.48622e-08, 5.00381e-08, 3.73912e-06; 6.09942e-06, 6.16911e-06, 2.63552e-05, -1.14575e-07, -8.06965e-08, 1.62854e-05; -2.58693e-08, -2.48622e-08, -1.14575e-07, 1.01842e-05, 3.50495e-07, -8.59871e-05; -9.35208e-08, 5.00381e-08, -8.06965e-08, 3.50495e-07, 2.63702e-06, -3.5011e-05; 3.90479e-06, 3.73912e-06, 1.62854e-05, -8.59871e-05, -3.5011e-05, 0.0137884])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/l90_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0880134, "l_diam": 0.0411796, "r_diam": 0.0414408, "l_x": -0.00262327, "l_y": 0.00584952, "l_theta": -1.55772, "E_d": 1.00634, "E_b": 0.977926, "covariance": [ [ 3.91817e-07, 3.79263e-07, 1.643e-06, -3.83492e-09, -2.03582e-08, 6.33479e-07 ], [ 3.79263e-07, 4.01431e-07, 1.66301e-06, -3.56338e-09, 1.66528e-08, 5.87702e-07 ], [ 1.643e-06, 1.66301e-06, 7.10154e-06, -1.75235e-08, -4.5618e-09, 2.60234e-06 ], [ -3.83492e-09, -3.56338e-09, -1.75235e-08, 2.40973e-06, 7.24481e-08, -1.93703e-05 ], [ -2.03582e-08, 1.66528e-08, -4.5618e-09, 7.24481e-08, 7.15567e-07, -8.53942e-06 ], [ 6.33479e-07, 5.87702e-07, 2.60234e-06, -1.93703e-05, -8.53942e-06, 0.00323274 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/l90_results.m: -------------------------------------------------------------------------------- 1 | function res = l90_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0880134, ... 5 | 'l_diam', 0.0411796, ... 6 | 'r_diam', 0.0414408, ... 7 | 'l_x', -0.00262327, ... 8 | 'l_y', 0.00584952, ... 9 | 'l_theta', -1.55772, ... 10 | 'E_d', 1.00634, ... 11 | 'E_b', 0.977926, ... 12 | 'covariance', [3.91817e-07, 3.79263e-07, 1.643e-06, -3.83492e-09, -2.03582e-08, 6.33479e-07; 3.79263e-07, 4.01431e-07, 1.66301e-06, -3.56338e-09, 1.66528e-08, 5.87702e-07; 1.643e-06, 1.66301e-06, 7.10154e-06, -1.75235e-08, -4.5618e-09, 2.60234e-06; -3.83492e-09, -3.56338e-09, -1.75235e-08, 2.40973e-06, 7.24481e-08, -1.93703e-05; -2.03582e-08, 1.66528e-08, -4.5618e-09, 7.24481e-08, 7.15567e-07, -8.53942e-06; 6.33479e-07, 5.87702e-07, 2.60234e-06, -1.93703e-05, -8.53942e-06, 0.00323274])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lmov_0_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.08816, "l_diam": 0.0413154, "r_diam": 0.0414503, "l_x": -0.00601753, "l_y": -0.0387702, "l_theta": -1.86185, "E_d": 1.00327, "E_b": 0.979555, "covariance": [ [ 2.37214e-07, 2.23826e-07, 9.82558e-07, -1.04654e-07, -4.20344e-09, -2.73681e-06 ], [ 2.23826e-07, 2.25471e-07, 9.57127e-07, -1.06902e-07, 1.23114e-08, -2.79502e-06 ], [ 9.82558e-07, 9.57127e-07, 4.20888e-06, -4.5575e-07, -1.64045e-08, -1.17844e-05 ], [ -1.04654e-07, -1.06902e-07, -4.5575e-07, 3.37223e-06, -4.99209e-07, 3.57154e-05 ], [ -4.20344e-09, 1.23114e-08, -1.64045e-08, -4.99209e-07, 1.20135e-06, -5.59695e-06 ], [ -2.73681e-06, -2.79502e-06, -1.17844e-05, 3.57154e-05, -5.59695e-06, 0.000933108 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lmov_0_results.m: -------------------------------------------------------------------------------- 1 | function res = lmov_0_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.08816, ... 5 | 'l_diam', 0.0413154, ... 6 | 'r_diam', 0.0414503, ... 7 | 'l_x', -0.00601753, ... 8 | 'l_y', -0.0387702, ... 9 | 'l_theta', -1.86185, ... 10 | 'E_d', 1.00327, ... 11 | 'E_b', 0.979555, ... 12 | 'covariance', [2.37214e-07, 2.23826e-07, 9.82558e-07, -1.04654e-07, -4.20344e-09, -2.73681e-06; 2.23826e-07, 2.25471e-07, 9.57127e-07, -1.06902e-07, 1.23114e-08, -2.79502e-06; 9.82558e-07, 9.57127e-07, 4.20888e-06, -4.5575e-07, -1.64045e-08, -1.17844e-05; -1.04654e-07, -1.06902e-07, -4.5575e-07, 3.37223e-06, -4.99209e-07, 3.57154e-05; -4.20344e-09, 1.23114e-08, -1.64045e-08, -4.99209e-07, 1.20135e-06, -5.59695e-06; -2.73681e-06, -2.79502e-06, -1.17844e-05, 3.57154e-05, -5.59695e-06, 0.000933108])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lmov_1_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0884639, "l_diam": 0.0414784, "r_diam": 0.0416365, "l_x": -0.00607186, "l_y": -0.0386417, "l_theta": -1.86028, "E_d": 1.00381, "E_b": 0.982932, "covariance": [ [ 5.21005e-07, 4.8981e-07, 2.15235e-06, -4.76076e-07, 3.27168e-08, -1.24119e-05 ], [ 4.8981e-07, 4.93932e-07, 2.09404e-06, -4.82409e-07, 7.15354e-08, -1.25756e-05 ], [ 2.15235e-06, 2.09404e-06, 9.2213e-06, -2.05246e-06, 1.42665e-07, -5.31916e-05 ], [ -4.76076e-07, -4.82409e-07, -2.05246e-06, 1.08737e-05, -1.99243e-06, 0.000114184 ], [ 3.27168e-08, 7.15354e-08, 1.42665e-07, -1.99243e-06, 2.69927e-06, -1.80433e-05 ], [ -1.24119e-05, -1.25756e-05, -5.31916e-05, 0.000114184, -1.80433e-05, 0.00297924 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lmov_1_results.m: -------------------------------------------------------------------------------- 1 | function res = lmov_1_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0884639, ... 5 | 'l_diam', 0.0414784, ... 6 | 'r_diam', 0.0416365, ... 7 | 'l_x', -0.00607186, ... 8 | 'l_y', -0.0386417, ... 9 | 'l_theta', -1.86028, ... 10 | 'E_d', 1.00381, ... 11 | 'E_b', 0.982932, ... 12 | 'covariance', [5.21005e-07, 4.8981e-07, 2.15235e-06, -4.76076e-07, 3.27168e-08, -1.24119e-05; 4.8981e-07, 4.93932e-07, 2.09404e-06, -4.82409e-07, 7.15354e-08, -1.25756e-05; 2.15235e-06, 2.09404e-06, 9.2213e-06, -2.05246e-06, 1.42665e-07, -5.31916e-05; -4.76076e-07, -4.82409e-07, -2.05246e-06, 1.08737e-05, -1.99243e-06, 0.000114184; 3.27168e-08, 7.15354e-08, 1.42665e-07, -1.99243e-06, 2.69927e-06, -1.80433e-05; -1.24119e-05, -1.25756e-05, -5.31916e-05, 0.000114184, -1.80433e-05, 0.00297924])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lmov_2_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.088588, "l_diam": 0.0414757, "r_diam": 0.0416759, "l_x": -0.00571871, "l_y": -0.0387299, "l_theta": -1.85961, "E_d": 1.00483, "E_b": 0.984312, "covariance": [ [ 2.05636e-07, 1.90665e-07, 8.44684e-07, -9.14543e-08, -5.72653e-09, -2.37538e-06 ], [ 1.90665e-07, 1.92663e-07, 8.16749e-07, -9.39495e-08, 1.28215e-08, -2.43963e-06 ], [ 8.44684e-07, 8.16749e-07, 3.62734e-06, -4.00567e-07, -2.29259e-08, -1.02598e-05 ], [ -9.14543e-08, -9.39495e-08, -4.00567e-07, 2.9417e-06, -4.28687e-07, 3.08202e-05 ], [ -5.72653e-09, 1.28215e-08, -2.29259e-08, -4.28687e-07, 1.04124e-06, -4.61074e-06 ], [ -2.37538e-06, -2.43963e-06, -1.02598e-05, 3.08202e-05, -4.61074e-06, 0.000800985 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lmov_2_results.m: -------------------------------------------------------------------------------- 1 | function res = lmov_2_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.088588, ... 5 | 'l_diam', 0.0414757, ... 6 | 'r_diam', 0.0416759, ... 7 | 'l_x', -0.00571871, ... 8 | 'l_y', -0.0387299, ... 9 | 'l_theta', -1.85961, ... 10 | 'E_d', 1.00483, ... 11 | 'E_b', 0.984312, ... 12 | 'covariance', [2.05636e-07, 1.90665e-07, 8.44684e-07, -9.14543e-08, -5.72653e-09, -2.37538e-06; 1.90665e-07, 1.92663e-07, 8.16749e-07, -9.39495e-08, 1.28215e-08, -2.43963e-06; 8.44684e-07, 8.16749e-07, 3.62734e-06, -4.00567e-07, -2.29259e-08, -1.02598e-05; -9.14543e-08, -9.39495e-08, -4.00567e-07, 2.9417e-06, -4.28687e-07, 3.08202e-05; -5.72653e-09, 1.28215e-08, -2.29259e-08, -4.28687e-07, 1.04124e-06, -4.61074e-06; -2.37538e-06, -2.43963e-06, -1.02598e-05, 3.08202e-05, -4.61074e-06, 0.000800985])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lmov_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0883953, "l_diam": 0.0414217, "r_diam": 0.0415888, "l_x": -0.00587095, "l_y": -0.0387078, "l_theta": -1.86026, "E_d": 1.00403, "E_b": 0.98217, "covariance": [ [ 8.8498e-08, 8.25986e-08, 3.64517e-07, -6.25941e-08, 1.51585e-09, -1.63083e-06 ], [ 8.25986e-08, 8.33491e-08, 3.53411e-07, -6.36723e-08, 8.81045e-09, -1.65869e-06 ], [ 3.64517e-07, 3.53411e-07, 1.56324e-06, -2.71119e-07, 7.03626e-09, -7.00584e-06 ], [ -6.25941e-08, -6.36723e-08, -2.71119e-07, 1.59008e-06, -2.69296e-07, 1.67128e-05 ], [ 1.51585e-09, 8.81045e-09, 7.03626e-09, -2.69296e-07, 4.53146e-07, -2.55494e-06 ], [ -1.63083e-06, -1.65869e-06, -7.00584e-06, 1.67128e-05, -2.55494e-06, 0.000435694 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lmov_results.m: -------------------------------------------------------------------------------- 1 | function res = lmov_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0883953, ... 5 | 'l_diam', 0.0414217, ... 6 | 'r_diam', 0.0415888, ... 7 | 'l_x', -0.00587095, ... 8 | 'l_y', -0.0387078, ... 9 | 'l_theta', -1.86026, ... 10 | 'E_d', 1.00403, ... 11 | 'E_b', 0.98217, ... 12 | 'covariance', [8.8498e-08, 8.25986e-08, 3.64517e-07, -6.25941e-08, 1.51585e-09, -1.63083e-06; 8.25986e-08, 8.33491e-08, 3.53411e-07, -6.36723e-08, 8.81045e-09, -1.65869e-06; 3.64517e-07, 3.53411e-07, 1.56324e-06, -2.71119e-07, 7.03626e-09, -7.00584e-06; -6.25941e-08, -6.36723e-08, -2.71119e-07, 1.59008e-06, -2.69296e-07, 1.67128e-05; 1.51585e-09, 8.81045e-09, 7.03626e-09, -2.69296e-07, 4.53146e-07, -2.55494e-06; -1.63083e-06, -1.65869e-06, -7.00584e-06, 1.67128e-05, -2.55494e-06, 0.000435694])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lstraight_0_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.088735, "l_diam": 0.0415778, "r_diam": 0.041715, "l_x": -0.00583202, "l_y": 0.000371476, "l_theta": 0.00999903, "E_d": 1.0033, "E_b": 0.985945, "covariance": [ [ 7.31795e-09, 7.182e-09, 3.08937e-08, 2.6008e-12, -7.8049e-11, -1.57847e-09 ], [ 7.182e-09, 7.36907e-09, 3.10052e-08, 3.05479e-12, 2.7255e-10, -2.6096e-09 ], [ 3.08937e-08, 3.10052e-08, 1.33598e-07, -1.00028e-10, 4.24694e-10, -8.91678e-09 ], [ 2.6008e-12, 3.05479e-12, -1.00028e-10, 1.71904e-08, -2.75095e-10, -9.86937e-10 ], [ -7.8049e-11, 2.7255e-10, 4.24694e-10, -2.75095e-10, 3.89139e-08, -4.44359e-08 ], [ -1.57847e-09, -2.6096e-09, -8.91678e-09, -9.86937e-10, -4.44359e-08, 7.44888e-06 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lstraight_0_results.m: -------------------------------------------------------------------------------- 1 | function res = lstraight_0_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.088735, ... 5 | 'l_diam', 0.0415778, ... 6 | 'r_diam', 0.041715, ... 7 | 'l_x', -0.00583202, ... 8 | 'l_y', 0.000371476, ... 9 | 'l_theta', 0.00999903, ... 10 | 'E_d', 1.0033, ... 11 | 'E_b', 0.985945, ... 12 | 'covariance', [7.31795e-09, 7.182e-09, 3.08937e-08, 2.6008e-12, -7.8049e-11, -1.57847e-09; 7.182e-09, 7.36907e-09, 3.10052e-08, 3.05479e-12, 2.7255e-10, -2.6096e-09; 3.08937e-08, 3.10052e-08, 1.33598e-07, -1.00028e-10, 4.24694e-10, -8.91678e-09; 2.6008e-12, 3.05479e-12, -1.00028e-10, 1.71904e-08, -2.75095e-10, -9.86937e-10; -7.8049e-11, 2.7255e-10, 4.24694e-10, -2.75095e-10, 3.89139e-08, -4.44359e-08; -1.57847e-09, -2.6096e-09, -8.91678e-09, -9.86937e-10, -4.44359e-08, 7.44888e-06])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lstraight_1_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0891065, "l_diam": 0.0418219, "r_diam": 0.0419639, "l_x": -0.00570384, "l_y": -2.59458e-05, "l_theta": 0.0117653, "E_d": 1.0034, "E_b": 0.990072, "covariance": [ [ 7.76667e-09, 7.54915e-09, 3.25734e-08, 7.57012e-13, -1.00874e-10, -1.72091e-09 ], [ 7.54915e-09, 7.81927e-09, 3.2693e-08, 5.92027e-13, 4.27048e-10, -3.2205e-09 ], [ 3.25734e-08, 3.2693e-08, 1.41369e-07, -1.59473e-10, 7.01897e-10, -1.05113e-08 ], [ 7.57012e-13, 5.92027e-13, -1.59473e-10, 1.86649e-08, -3.55955e-10, 3.51451e-09 ], [ -1.00874e-10, 4.27048e-10, 7.01897e-10, -3.55955e-10, 4.06934e-08, -4.78241e-08 ], [ -1.72091e-09, -3.2205e-09, -1.05113e-08, 3.51451e-09, -4.78241e-08, 8.11254e-06 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lstraight_1_results.m: -------------------------------------------------------------------------------- 1 | function res = lstraight_1_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0891065, ... 5 | 'l_diam', 0.0418219, ... 6 | 'r_diam', 0.0419639, ... 7 | 'l_x', -0.00570384, ... 8 | 'l_y', -2.59458e-05, ... 9 | 'l_theta', 0.0117653, ... 10 | 'E_d', 1.0034, ... 11 | 'E_b', 0.990072, ... 12 | 'covariance', [7.76667e-09, 7.54915e-09, 3.25734e-08, 7.57012e-13, -1.00874e-10, -1.72091e-09; 7.54915e-09, 7.81927e-09, 3.2693e-08, 5.92027e-13, 4.27048e-10, -3.2205e-09; 3.25734e-08, 3.2693e-08, 1.41369e-07, -1.59473e-10, 7.01897e-10, -1.05113e-08; 7.57012e-13, 5.92027e-13, -1.59473e-10, 1.86649e-08, -3.55955e-10, 3.51451e-09; -1.00874e-10, 4.27048e-10, 7.01897e-10, -3.55955e-10, 4.06934e-08, -4.78241e-08; -1.72091e-09, -3.2205e-09, -1.05113e-08, 3.51451e-09, -4.78241e-08, 8.11254e-06])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lstraight_2_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0892719, "l_diam": 0.0418956, "r_diam": 0.0419852, "l_x": -0.00595008, "l_y": 9.61081e-05, "l_theta": 0.00639555, "E_d": 1.00214, "E_b": 0.99191, "covariance": [ [ 6.81667e-09, 6.65954e-09, 2.86838e-08, 2.20819e-12, -1.17014e-10, -7.37342e-10 ], [ 6.65954e-09, 6.84662e-09, 2.87499e-08, 2.34903e-12, 2.56226e-10, -1.83452e-09 ], [ 2.86838e-08, 2.87499e-08, 1.24015e-07, -1.07416e-10, 3.00778e-10, -5.47042e-09 ], [ 2.20819e-12, 2.34903e-12, -1.07416e-10, 1.48734e-08, -1.39472e-10, 6.24179e-10 ], [ -1.17014e-10, 2.56226e-10, 3.00778e-10, -1.39472e-10, 3.49991e-08, -4.01593e-08 ], [ -7.37342e-10, -1.83452e-09, -5.47042e-09, 6.24179e-10, -4.01593e-08, 6.57068e-06 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lstraight_2_results.m: -------------------------------------------------------------------------------- 1 | function res = lstraight_2_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0892719, ... 5 | 'l_diam', 0.0418956, ... 6 | 'r_diam', 0.0419852, ... 7 | 'l_x', -0.00595008, ... 8 | 'l_y', 9.61081e-05, ... 9 | 'l_theta', 0.00639555, ... 10 | 'E_d', 1.00214, ... 11 | 'E_b', 0.99191, ... 12 | 'covariance', [6.81667e-09, 6.65954e-09, 2.86838e-08, 2.20819e-12, -1.17014e-10, -7.37342e-10; 6.65954e-09, 6.84662e-09, 2.87499e-08, 2.34903e-12, 2.56226e-10, -1.83452e-09; 2.86838e-08, 2.87499e-08, 1.24015e-07, -1.07416e-10, 3.00778e-10, -5.47042e-09; 2.20819e-12, 2.34903e-12, -1.07416e-10, 1.48734e-08, -1.39472e-10, 6.24179e-10; -1.17014e-10, 2.56226e-10, 3.00778e-10, -1.39472e-10, 3.49991e-08, -4.01593e-08; -7.37342e-10, -1.83452e-09, -5.47042e-09, 6.24179e-10, -4.01593e-08, 6.57068e-06])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/lstraight_results.json: -------------------------------------------------------------------------------- 1 | { "axle": 0.0890519, "l_diam": 0.0417709, "r_diam": 0.0418957, "l_x": -0.00581279, "l_y": 0.000185606, "l_theta": 0.00949588, "E_d": 1.00299, "E_b": 0.989465, "covariance": [ [ 2.40635e-09, 2.35815e-09, 1.0142e-08, 6.94695e-13, -2.83484e-11, -4.68241e-10 ], [ 2.35815e-09, 2.42122e-09, 1.01746e-08, 7.69756e-13, 9.22658e-11, -8.18211e-10 ], [ 1.0142e-08, 1.01746e-08, 4.38323e-08, -3.48432e-11, 1.38396e-10, -2.73703e-09 ], [ 6.94695e-13, 7.69756e-13, -3.48432e-11, 5.62752e-09, -8.53495e-11, 1.44416e-10 ], [ -2.83484e-11, 9.22658e-11, 1.38396e-10, -8.53495e-11, 1.26646e-08, -1.45311e-08 ], [ -4.68241e-10, -8.18211e-10, -2.73703e-09, 1.44416e-10, -1.45311e-08, 2.44109e-06 ] ] } 2 | -------------------------------------------------------------------------------- /reference-output/lstraight_results.m: -------------------------------------------------------------------------------- 1 | function res = lstraight_results 2 | res = ... 3 | { ... 4 | struct('axle', 0.0890519, ... 5 | 'l_diam', 0.0417709, ... 6 | 'r_diam', 0.0418957, ... 7 | 'l_x', -0.00581279, ... 8 | 'l_y', 0.000185606, ... 9 | 'l_theta', 0.00949588, ... 10 | 'E_d', 1.00299, ... 11 | 'E_b', 0.989465, ... 12 | 'covariance', [2.40635e-09, 2.35815e-09, 1.0142e-08, 6.94695e-13, -2.83484e-11, -4.68241e-10; 2.35815e-09, 2.42122e-09, 1.01746e-08, 7.69756e-13, 9.22658e-11, -8.18211e-10; 1.0142e-08, 1.01746e-08, 4.38323e-08, -3.48432e-11, 1.38396e-10, -2.73703e-09; 6.94695e-13, 7.69756e-13, -3.48432e-11, 5.62752e-09, -8.53495e-11, 1.44416e-10; -2.83484e-11, 9.22658e-11, 1.38396e-10, -8.53495e-11, 1.26646e-08, -1.45311e-08; -4.68241e-10, -8.18211e-10, -2.73703e-09, 1.44416e-10, -1.45311e-08, 2.44109e-06])... 13 | }; 14 | -------------------------------------------------------------------------------- /reference-output/version_string: -------------------------------------------------------------------------------- 1 | sets=l90 lmov lstraight\ndecimate_period=4\nsm_options=-restart 0 -recover_from_error 1 -max_iterations 1000 -min_reading 0.3 -max_angular_correction_deg 10 -max_linear_correction 0.05 -epsilon_xy 0.000001 -epsilon_theta 0.000001 \nsynchronizer_options=-tk_threshold 5 -perc_threshold 0.95 -time_tolerance 0.5\nsolver_options=-mode 0 -outliers_iterations 4 2 | -------------------------------------------------------------------------------- /scripts/copy_results.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | src=last_tuples 3 | dest=../submitted/experiments 4 | mkdir -p $dest 5 | 6 | cp -a $src/var*.eps $dest 7 | cp -a $src/*correlation*.eps $dest 8 | cp -a $src/lstraight_res*.eps $dest 9 | cp -a $src/*.tex $dest 10 | 11 | for a in $dest/*.eps; do 12 | echo "epstopdf $a" 13 | epstopdf $a; 14 | done 15 | -------------------------------------------------------------------------------- /scripts/create_pictures.m: -------------------------------------------------------------------------------- 1 | function res = create_pictures(tuple_dir) 2 | 3 | clear functions 4 | addpath(tuple_dir) 5 | tuple_files = dir(sprintf('%s/*.tuple', tuple_dir)); 6 | 7 | res = {}; 8 | 9 | for i=1:numel(tuple_files) 10 | [pathstr, basename, ext, versn] = fileparts(tuple_files(i).name); 11 | 12 | results_file = sprintf('%s_results', basename); 13 | l = eval(results_file); l = l{1}; 14 | l.basename = basename; 15 | res{end+1} = l; 16 | end 17 | 18 | 19 | 20 | % compute Jij as params 7-10 21 | 22 | for i=1:numel(res) 23 | % Original: res{i}.x = [res{i}.l_diam res{i}.r_diam res{i}.axle res{i}.l_x res{i}.l_y res{i}.l_theta]; 24 | res{i}.x = [res{i}.l_diam/2 res{i}.r_diam/2 res{i}.axle res{i}.l_x res{i}.l_y res{i}.l_theta]; 25 | 26 | % J11 = r_l /2 27 | % Original: res{i}.x(7) = res{i}.x(1) / 4; 28 | res{i}.x(7) = res{i}.x(1) / 2; 29 | 30 | df_dx = [1/4 0 0 0 0 0 ]; 31 | res{i}.covariance(1:6,7) = res{i}.covariance(1:6,1:6) * df_dx'; 32 | res{i}.covariance(7,1:6) = res{i}.covariance(1:6,7)'; 33 | res{i}.covariance(7,7) = df_dx * res{i}.covariance(1:6,1:6) * df_dx'; 34 | 35 | % J12 36 | % Original: res{i}.x(8) = res{i}.x(2) / 4; 37 | res{i}.x(8) = res{i}.x(2) / 2; 38 | 39 | df_dx = [0 1/4 0 0 0 0 0]; 40 | res{i}.covariance(1:7,8) = res{i}.covariance(1:7,1:7) * df_dx'; 41 | res{i}.covariance(8,1:7) = res{i}.covariance(1:7,8)'; 42 | res{i}.covariance(8,8) = df_dx * res{i}.covariance(1:7,1:7) * df_dx'; 43 | 44 | 45 | % J21 46 | a = res{i}.x(1); 47 | b = res{i}.x(3); 48 | f = - 0.5*a / b; 49 | df_da = - 0.5/b; 50 | df_db = + 0.5*a/b^2; 51 | df_dx = [df_da 0 df_db 0 0 0 0 0]; 52 | 53 | res{i}.x(9) = f; 54 | res{i}.covariance(1:8,9) = res{i}.covariance(1:8,1:8) * df_dx'; 55 | res{i}.covariance(9,1:8) = res{i}.covariance(1:8,9)'; 56 | res{i}.covariance(9,9) = df_dx * res{i}.covariance(1:8,1:8) * df_dx'; 57 | 58 | % J22 59 | a = res{i}.x(2); 60 | b = res{i}.x(3); 61 | f = 0.5 * a / b; 62 | df_da = 0.5 /b; 63 | df_db = -0.5*a/b^2; 64 | df_dx = [df_da df_db]; 65 | covx = res{i}.covariance([2 3],[2 3]); 66 | covf = df_dx * covx * df_dx'; 67 | 68 | res{i}.x(10) = f; 69 | 70 | df_dx = [0 df_da df_db 0 0 0 0 0 0]; 71 | 72 | res{i}.x(10) = f; 73 | res{i}.covariance(1:9,10) = res{i}.covariance(1:9,1:9) * df_dx'; 74 | res{i}.covariance(10,1:9) = res{i}.covariance(1:9,10)'; 75 | res{i}.covariance(10,10) = df_dx * res{i}.covariance(1:9,1:9) * df_dx'; 76 | 77 | 78 | [s,R] = cov2corr(res{i}.covariance); 79 | res{i}.corr = R; 80 | end 81 | 82 | 83 | 84 | 85 | 86 | figure; 87 | 88 | nr=3;nc=4; 89 | 90 | titles = { 'r_L','r_R','b','l_x','l_y','l_\theta','J11','J12','J21','J22'}; 91 | simpletitlestex = { 'r_L','r_R','b','l_x','l_y','l_\theta','J_{11}','J_{12}','J_{21}','J_{22}'}; 92 | titlestex = { 'r_{\mathrm{L}}','r_{\mathrm{R}}','b','\ell_x','\ell_y','\ell_\theta','J_{11}','J_{12}','J_{21}','J_{22}'}; 93 | 94 | titlestexstr = {}; for i=1:numel(titlestex); titlestexstr{i} = sprintf('$\\rule{0pt}{4mm}%s\\rule{2mm}{0pt}$', titlestex{i}); end 95 | 96 | scales = { 1000, 1000, 1000, 1000, 1000, 180/pi, 1000, 1000, 180/pi, 180/pi}; 97 | units = { 'mm','mm','mm','mm','mm', 'deg','mm/s','mm/s','deg/s','deg/s'}; 98 | 99 | names={}; 100 | for i=1:numel(res) 101 | names{i} = res{i}.basename; 102 | end 103 | 104 | for v=1:10 105 | subplot(nr,nc,v) 106 | hold on 107 | 108 | for i=1:numel(res) 109 | val = res{i}.x(v) ; 110 | 111 | % if (i == 1) | (i==2) 112 | % % quick fix for radius/diameter problem 113 | % val = val / 2; 114 | % end 115 | 116 | sigma = sqrt( res{i}.covariance(v,v) ); 117 | errorbar(i, scales{v}*val, scales{v}* 3*sigma); 118 | end 119 | ylabel(units{v}) 120 | set(gca,'XTick',1:numel(res)) 121 | set(gca,'XTickLabel',names); 122 | 123 | title(titles{v}) 124 | end 125 | 126 | 127 | 128 | names={}; 129 | for i=1:numel(res) 130 | names{i} = res{i}.basename; 131 | end 132 | names 133 | 134 | bigx=6.5; bigy=2.5; 135 | % smallx=4.5; smally=2.5; 136 | % all same size 137 | smallx=6.5; smally=2.5; 138 | sizex = {bigx,bigx,bigx,bigx,bigx,bigx,smallx,smallx,smallx,smallx}; 139 | sizey = {bigy,bigy,bigy,bigy,bigy,bigy,smally,smally,smally,smally}; 140 | 141 | names = {'A','A1','A2','A3','B','B1','B2','B3','C','C1','C2','C3'} 142 | namestex = {'A','A_1','A_2','A_3','B','B_1','B_2','B_3','C','C_1','C_2','C_3'}; 143 | for i=1:numel(namestex); namestex{i} = sprintf('$\\rule{0pt}{3mm}%s$', namestex{i}); end 144 | %for i=1:numel(namestex); namestex{i} = sprintf('$%s$', namestex{i}); end 145 | 146 | tables_vars = { 1:6, 7:10, 1:10 }; 147 | 148 | for t=1:numel(tables_vars) 149 | f = fopen(sprintf('%s/bigtable%d.tex',tuple_dir, t), 'w'); 150 | fprintf(f, '\\begin{tabular}{|r|c|c|c|c|c|c|c|c|c|c|}\n') 151 | fprintf(f, '\\hline\n') 152 | fprintf(f, ' ') 153 | 154 | tvars = tables_vars{t}; 155 | 156 | for tv=1:numel(tvars) 157 | v = tables_vars{t}(tv); 158 | fprintf(f, '& %s (%s)', titlestexstr{v}, units{v}); 159 | end 160 | fprintf(f, '\\\\ \n') 161 | fprintf(f, '\\hline\n') 162 | 163 | for i=1:numel(res) 164 | 165 | if mod(i,4) == 1 166 | fprintf(f, '\\hline\n') 167 | end 168 | 169 | fprintf(f, '%s & ', namestex{i}); 170 | 171 | for tv=1:numel(tvars) 172 | v = tvars(tv); 173 | val = scales{v}* res{i}.x(v) ; 174 | sigma = scales{v}* sqrt( res{i}.covariance(v,v) ); 175 | fprintf(f, '$%.2f \\pm %.2f$', val, sigma ); 176 | if tv b 240 | pc = floor(res{i}.corr(a,b) * 100); 241 | if abs(pc) == 100 242 | pc = 99 *sign(pc); 243 | end 244 | if pc > 0 245 | s = sprintf('+.%02d', abs(pc)); 246 | else 247 | s = sprintf('-.%02d', abs(pc)); 248 | end 249 | my_fonts(text(a+0.1,b+0.5,s)); 250 | end 251 | end 252 | end 253 | 254 | set(gca,'XTick',(1:10) + 0.6) 255 | set(gca,'XTickLabel',titles); 256 | set(gca,'YTick',(1:10) + 0.6) 257 | set(gca,'YTickLabel',titles); 258 | 259 | format_ticks(gca, titlestexstr, titlestexstr, [],[],[],[], 0, 0); 260 | 261 | my_fonts(gca) 262 | 263 | print('-depsc', sprintf('%s/%s_correlation.eps',tuple_dir,res{i}.basename)) 264 | 265 | close(f) 266 | end 267 | 268 | 269 | %%%%%%%% Only the numbers 270 | for i=1:numel(res) 271 | f = my_figure(8,8); 272 | 273 | % M = [res{i}.corr res{i}.corr(:,end); res{i}.corr(end,:) 0]; 274 | M = zeros(11,11); 275 | % all white 276 | colormap([1 1 1]) 277 | pcolor(M); 278 | 279 | for a=1:10 280 | for b=1:10 281 | if a > b 282 | pc = floor(res{i}.corr(a,b) * 100); 283 | if abs(pc) == 100 284 | pc = 99 *sign(pc); 285 | end 286 | if pc > 0 287 | s = sprintf('+.%02d', abs(pc)); 288 | else 289 | s = sprintf('-.%02d', abs(pc)); 290 | end 291 | my_fonts(text(a+0.1,b+0.5,s)); 292 | end 293 | end 294 | end 295 | 296 | set(gca,'XTick',(1:10) + 0.6) 297 | set(gca,'XTickLabel',titles); 298 | set(gca,'YTick',(1:10) + 0.6) 299 | set(gca,'YTickLabel',titles); 300 | 301 | format_ticks(gca, titlestexstr, titlestexstr, [],[],[],[], 0, 0); 302 | 303 | my_fonts(gca) 304 | 305 | print('-depsc', sprintf('%s/%s_correlation_bw.eps',tuple_dir,res{i}.basename)) 306 | 307 | close(f) 308 | end 309 | 310 | 311 | 312 | 313 | 314 | fprintf('And now the long part (press enter)...\n') 315 | pause 316 | 317 | 318 | for i=1:numel(res) 319 | iterations = dir(sprintf('%s/%s_results_iter*.asc', tuple_dir, res{i}.basename)); 320 | for a=1:numel(iterations) 321 | file = sprintf('%s/%s',tuple_dir,iterations(a).name); 322 | [pathstr, basename, ext] = fileparts(file); 323 | prefix = sprintf('%s/%s',tuple_dir, basename); 324 | 325 | create_pictures_iteration(file,prefix); 326 | end 327 | 328 | end 329 | 330 | 331 | 332 | 333 | 334 | 335 | -------------------------------------------------------------------------------- /scripts/create_pictures_iteration.m: -------------------------------------------------------------------------------- 1 | function create_pictures_iteration(iteration_file, prefix) 2 | 3 | 4 | if exist(sprintf('%s_sm.eps', prefix)) 5 | fprintf('Already did %s -- remove files if you want to do it again\n', prefix); 6 | return 7 | end 8 | 9 | fprintf('Drawing %s\n', prefix); 10 | 11 | fclose = @(f) close(f); 12 | fsizex = 5; 13 | fsizey = 3; 14 | 15 | 16 | d = load(iteration_file)'; 17 | 18 | T = d(1,:); 19 | phi_r = d(2,:); 20 | phi_l = d(3,:); 21 | 22 | sm = d(4:6,:); 23 | o = d(7:9,:); 24 | 25 | err = d(10:12,:); 26 | 27 | est_sm = d(13:15,:); 28 | err_sm = d(16:18,:); 29 | 30 | 31 | time = [T(1)]; 32 | for i=2:numel(T) 33 | time(i) = time(i-1) + T(i); 34 | end 35 | 36 | 37 | f=my_figure(fsizex*2,fsizey); nr=1;nc=2; 38 | subplot(nr,nc,1); 39 | h=plot(sm(1,:)*1000,sm(2,:)*1000,'.'); 40 | set(h,'MarkerSize',1) 41 | my_fonts(xlabel('mm')) 42 | my_fonts(ylabel('mm')) 43 | axis('equal') 44 | my_fonts(title('scan matching x,y')); 45 | my_fonts(gca); 46 | 47 | subplot(nr,nc,2); 48 | hist(rad2deg(sm(3,:)),100); 49 | my_fonts(xlabel('deg')) 50 | my_fonts(title('scan matching theta')); 51 | my_fonts(gca); 52 | 53 | print('-depsc2', sprintf('%s_sm.eps', prefix)) 54 | fclose(f) 55 | 56 | 57 | f=my_figure(fsizex,fsizey); 58 | plot(time, phi_l,'b-'); hold on; 59 | plot(time, phi_r,'r-') 60 | my_fonts(xlabel('time (s)')) 61 | my_fonts(ylabel('speed (rad/s)')) 62 | my_fonts(legend('\omega_l','\omega_r')) 63 | my_fonts(title('velocity profile')) 64 | my_fonts(gca); 65 | print('-depsc2', sprintf('%s_vel.eps', prefix)) 66 | fclose(f) 67 | 68 | 69 | f=my_figure(fsizex*2,fsizey); nr=1;nc=2; 70 | subplot(nr,nc,1); 71 | h=plot(o(1,:)*1000,o(2,:)*1000,'.'); 72 | set(h,'MarkerSize',1) 73 | my_fonts(xlabel('mm')) 74 | my_fonts(ylabel('mm')) 75 | axis('equal') 76 | my_fonts(title('motion x,y')); 77 | my_fonts(gca); 78 | 79 | subplot(nr,nc,2); 80 | hist(rad2deg(o(3,:)),100); 81 | my_fonts(xlabel('deg')) 82 | my_fonts(ylabel('samples')); 83 | my_fonts(title('motion theta')); 84 | my_fonts(gca); 85 | 86 | print('-depsc2', sprintf('%s_motion.eps', prefix)) 87 | fclose(f) 88 | 89 | 90 | 91 | f=my_figure(fsizex*4,fsizey); nr=1;nc=4; 92 | 93 | subplot(nr,nc,1); 94 | h=plot(err(1,:)*1000,err(2,:)*1000,'.'); 95 | set(h,'MarkerSize',1) 96 | my_fonts(xlabel('mm')) 97 | my_fonts(ylabel('mm')) 98 | axis('equal') 99 | my_fonts(title('error x,y')); 100 | my_fonts(gca); 101 | 102 | subplot(nr,nc,2); 103 | hist(err(1,:)*1000,100); 104 | my_fonts(xlabel('mm')) 105 | my_fonts(title('error x')); 106 | my_fonts(gca); 107 | 108 | subplot(nr,nc,3); 109 | hist(err(2,:)*1000,100); 110 | my_fonts(xlabel('mm')) 111 | my_fonts(title('error y')); 112 | my_fonts(gca); 113 | 114 | subplot(nr,nc,4); 115 | hist(rad2deg(err(3,:)),100); 116 | my_fonts(xlabel('deg')) 117 | my_fonts(title('error theta')); 118 | my_fonts(gca); 119 | 120 | print('-depsc2', sprintf('%s_errors.eps', prefix)) 121 | fclose(f) 122 | 123 | 124 | 125 | f=my_figure(fsizex*2,fsizey); nr=1;nc=2; 126 | 127 | subplot(nr,nc,1); 128 | h=plot(est_sm(1,:)*1000,est_sm(2,:)*1000,'.'); 129 | set(h,'MarkerSize',1) 130 | my_fonts(xlabel('mm')) 131 | my_fonts(ylabel('mm')) 132 | axis('equal') 133 | my_fonts(title('estimated sm x,y')); 134 | my_fonts(gca); 135 | 136 | subplot(nr,nc,2); 137 | hist(rad2deg(est_sm(3,:)),100); 138 | my_fonts(xlabel('deg')) 139 | my_fonts(title('estimated sm theta')); 140 | my_fonts(gca); 141 | 142 | print('-depsc2', sprintf('%s_estsm.eps', prefix)) 143 | fclose(f) 144 | 145 | 146 | 147 | 148 | f=my_figure(fsizex,fsizey); 149 | my_fonts(title('scan matching \theta vs estimated')) 150 | plot(rad2deg(est_sm(3,:)), rad2deg(sm(3,:)), 'r.') 151 | 152 | hold on; 153 | m = max(abs(rad2deg(est_sm(3,:)))); 154 | plot([-m m],[-m m],'k--'); 155 | axis('equal') 156 | my_fonts(xlabel('scan matching (deg)')) 157 | my_fonts(ylabel('estimated from odo + calib (deg)')) 158 | my_fonts(gca); 159 | 160 | print('-depsc2', sprintf('%s_es.eps', prefix)) 161 | fclose(f) 162 | 163 | 164 | 165 | -------------------------------------------------------------------------------- /scripts/create_tuples.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e # exit on error 4 | set -o pipefail 5 | source script_variables.sh 6 | 7 | 8 | #Verify input 9 | if [ "$1" = "" ] || [ "$2" = "" ]; then 10 | echo "Usage: 11 | ./create_tuples.sh [data_path] [Calibration_path] [Scan_Matcher_path]" 12 | exit 1 13 | fi 14 | 15 | if [ "$3" = "" ]; then 16 | DATA="" 17 | else 18 | DATA="${3}/" 19 | fi 20 | 21 | if [ "$4" = "" ]; then 22 | PATH_CALIB="`pwd`/" 23 | else 24 | PATH_CALIB="${4}/" 25 | fi 26 | 27 | if [ "$5" = "" ]; then 28 | PATH_SM="" 29 | else 30 | PATH_SM="${5}/" 31 | fi 32 | 33 | #FILES 34 | LAS_FILE_BZ2="${DATA}${1}" 35 | ODO_FILE_BZ2="${DATA}${2}" 36 | LAS_FILE=`dirname ${LAS_FILE_BZ2}`/`basename $LAS_FILE_BZ2 .bz2` 37 | ODO_FILE=`dirname ${ODO_FILE_BZ2}`/`basename $ODO_FILE_BZ2 .bz2` 38 | SM_STATS_FILE="${LAS_FILE}_stats" 39 | SM_OUTPUT_FILE="${LAS_FILE}_out" 40 | TUPLE_FILE="${LAS_FILE}_tuple" 41 | OUT_CALIB_PARAM_FILE="${LAS_FILE}_calibParams" 42 | 43 | 44 | 45 | #funzione per rimuovere file già esistenti 46 | checkrm() { 47 | local path="$1" 48 | if [ -e $path ]; then 49 | echo "Removing old file: $path" 50 | rm $path 51 | fi 52 | } 53 | 54 | check_exec "bunzip2" 55 | check_existence ${LAS_FILE_BZ2} 56 | check_existence ${ODO_FILE_BZ2} 57 | checkrm ${SM_STATS_FILE} 58 | checkrm ${TUPLE_FILE} 59 | checkrm ${OUT_CALIB_PARAM_FILE} 60 | checkrm ${LAS_FILE} 61 | checkrm ${ODO_FILE} 62 | 63 | 64 | # Decompress files 65 | echo -n "$0: Uncompressing files..." 66 | `bunzip2 -k "$LAS_FILE_BZ2"` 67 | if [ ! ${LAS_FILE_BZ2} == ${ODO_FILE_BZ2} ]; then 68 | `bunzip2 -k "$ODO_FILE_BZ2"` 69 | fi 70 | echo "done." 71 | 72 | #Scan Matcher 73 | 74 | echo "$0: Decimating file..." 75 | 76 | # cat $LAS_FILE | ld_remove_doubles -epsilon 0.4 > $LAS_FILE.sel 77 | 78 | cat ${LAS_FILE} | ${json_decimate} -period ${decimate_period} > ${LAS_FILE}.sel 79 | 80 | 81 | echo "$0: Scan Matcher... " 82 | ${sm2} -in ${LAS_FILE}.sel -out ${SM_OUTPUT_FILE} -out_stats ${SM_STATS_FILE} ${sm_options} 83 | 84 | echo "$0: Syncronization..." 85 | ${synchronizer} -las_file ${SM_STATS_FILE} -odo_file ${ODO_FILE} -output ${TUPLE_FILE} ${synchronizer_options} 86 | echo "done" 87 | 88 | ## THAT'S ALL FOLKS ## 89 | -------------------------------------------------------------------------------- /scripts/errorbar_tick.m: -------------------------------------------------------------------------------- 1 | function errorbar_tick(h,w,xtype) 2 | %ERRORBAR_TICK Adjust the width of errorbars 3 | % ERRORBAR_TICK(H) adjust the width of error bars with handle H. 4 | % Error bars width is given as a ratio of X axis length (1/80). 5 | % ERRORBAR_TICK(H,W) adjust the width of error bars with handle H. 6 | % The input W is given as a ratio of X axis length (1/W). The result 7 | % is independent of the x-axis units. A ratio between 20 and 80 is usually fine. 8 | % ERRORBAR_TICK(H,W,'UNITS') adjust the width of error bars with handle H. 9 | % The input W is given in the units of the current x-axis. 10 | % 11 | % See also ERRORBAR 12 | % 13 | 14 | % Author: Arnaud Laurent 15 | % Creation : Jan 29th 2009 16 | % MATLAB version: R2007a 17 | % 18 | % Notes: This function was created from a post on the french forum : 19 | % http://www.developpez.net/forums/f148/environnements-developpement/matlab/ 20 | % Author : Jerome Briot (Dut) 21 | % http://www.mathworks.com/matlabcentral/newsreader/author/94805 22 | % http://www.developpez.net/forums/u125006/dut/ 23 | % It was further modified by Arnaud Laurent and Jerome Briot. 24 | 25 | % Check numbers of arguments 26 | error(nargchk(1,3,nargin)) 27 | 28 | % Check for the use of V6 flag ( even if it is depreciated ;) ) 29 | flagtype = get(h,'type'); 30 | 31 | % Check number of arguments and provide missing values 32 | if nargin==1 33 | w = 80; 34 | end 35 | 36 | if nargin<3 37 | xtype = 'ratio'; 38 | end 39 | 40 | % Calculate width of error bars 41 | if ~strcmpi(xtype,'units') 42 | dx = diff(get(gca,'XLim')); % Retrieve x limits from current axis 43 | w = dx/w; % Errorbar width 44 | end 45 | 46 | % Plot error bars 47 | if strcmpi(flagtype,'hggroup') % ERRORBAR(...) 48 | 49 | hh=get(h,'children'); % Retrieve info from errorbar plot 50 | x = get(hh(2),'xdata'); % Get xdata from errorbar plot 51 | 52 | x(4:9:end) = x(1:9:end)-w/2; % Change xdata with respect to ratio 53 | x(7:9:end) = x(1:9:end)-w/2; 54 | x(5:9:end) = x(1:9:end)+w/2; 55 | x(8:9:end) = x(1:9:end)+w/2; 56 | 57 | set(hh(2),'xdata',x(:)) % Change error bars on the figure 58 | 59 | else % ERRORBAR('V6',...) 60 | 61 | x = get(h(1),'xdata'); % Get xdata from errorbar plot 62 | 63 | x(4:9:end) = x(1:9:end)-w/2; % Change xdata with respect to the chosen ratio 64 | x(7:9:end) = x(1:9:end)-w/2; 65 | x(5:9:end) = x(1:9:end)+w/2; 66 | x(8:9:end) = x(1:9:end)+w/2; 67 | 68 | set(h(1),'xdata',x(:)) % Change error bars on the figure 69 | 70 | end 71 | -------------------------------------------------------------------------------- /scripts/format_ticks.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | %% BEGIN HEADER 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 6 | %Name: format_tick.m 7 | % 8 | %Usage: [hx,hy] = ... 9 | % format_tick(h,tickx,ticky,tickposx,tickposy,rotx,roty,offset,... 10 | % varargin); 11 | % 12 | %Description: Replace or appends XTickLabels and YTickLabels of axis handle 13 | % h with input tickx and ticky array 14 | % 15 | %***NOTE!***: BE SURE TO DELETE ANY PREVIOUS TEXT OBJECTS CREATED BY THIS 16 | % FUNCTION BEFORE RUNNING THIS ON THE SAME FIGURE TWICE 17 | % 18 | %Required Inputs: 19 | % h : handle of axis to change tick labels (can use gca) 20 | % tickx : cell array of tick labels or string to append to current 21 | % labels 22 | % (Defaults to appending degree symbols if not input) 23 | % 24 | %Optional Inputs 25 | % ticky : cell array of tick labels or string to append to current 26 | % labels (Can use [] or not specify to ignore) 27 | % tickposx : Vector of x positions where you want the tick labels 28 | % (Can use [] or not specify to ignore) 29 | % tickposy : Vector of y positions where you want the tick labels 30 | % (Can use [] or not specify to ignore) 31 | % rotx : Number of degrees to rotate x tick labels 32 | % (Can use [] or not specify to ignore) Default = 0.0 33 | % roty : Number of degrees to rotate y tick labels 34 | % (Can use [] or not specify to ignore) Default = 0.0 35 | % offset : Label offsets from axis in fraction of total range 36 | % (Can use [] or not specify to ignore) Default = 0.0 37 | % 38 | %Optional Inputs:% 39 | % Any standard text formatting parameters such as 40 | % 'FontSize','FontWeight',etc. 41 | % Use the same way you would in a set command after putting 42 | % in the required input values. 43 | % 44 | %Outputs: 45 | % hx: handle of text objects created for XTickLabels 46 | % hy: handle of text objects created for YTickLabels 47 | % 48 | %Function Calls: 49 | % None 50 | % 51 | %Required Data Files: 52 | % None 53 | % 54 | % 55 | %Example: 56 | % ;Example 1: Append Degree Symbols to X-Axis of a Plot 57 | % figure; 58 | % plot(1:10,1:10); 59 | % [hx,hy] = format_ticks(gca); 60 | % 61 | % ;Example 2: Append Degree Symbolts to X and Y Axes of a Plot 62 | % figure; 63 | % plot(1:10,1:10); 64 | % [hx,hy] = format_ticks(gca,'^{\circ}','^{\circ}'); 65 | % 66 | % ;Example 2: Append Degree Symbolts to X and Y Axes of a Plot and 67 | % ; put a 45 degree tilt on them 68 | % figure; 69 | % plot(1:10,1:10); 70 | % [hx,hy] = format_ticks(gca,'^{\circ}','^{\circ}',[],[],45,45); 71 | % 72 | % ;Example 3: Make a plot with fractions on the x tick labels 73 | % figure 74 | % plot(1:10,1:10); 75 | % [hx,hy] = format_ticks(gca,{'$1$','$2\frac{1}{2}$','$9\frac{1}{2}$'},... 76 | % [],[1,2.5,9.5]); 77 | % 78 | % ;Example 4: Make a plot with degrees on y tick label and fractions 79 | % ; on x 80 | % figure 81 | % plot(0:10,0:10); 82 | % [hx,hy] = format_ticks(gca,... 83 | % {'$0$','$2\frac{1}{2}$','$5$','$7\frac{1}{2}$','$10$'},... 84 | % '$^{\circ}$',[0,2.5,5,7.5,10],[],0,45,[],... 85 | % 'FontSize',16,'FontWeight','Bold'); 86 | % 87 | %Change Log: 88 | % 08/19/2007: Origin Version Created by Alex Hayes 89 | % (hayes@gps.caltech.edu) 90 | % 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 93 | %% BEGIN FUNCTION 94 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 95 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 96 | function [hx,hy] = ... 97 | format_ticks(h,tickx,ticky,tickposx,tickposy,rotx,roty,offset,varargin) 98 | 99 | %define axis text offset (percentage of total range) 100 | if ~exist('offset','var'); 101 | offset = 0.02; 102 | elseif length(offset) == 0; 103 | offset = 0.02; 104 | end; 105 | 106 | %make sure the axis handle input really exists 107 | if ~exist('h','var'); 108 | h = gca; 109 | warning(['Axis handle NOT Input, Defaulting to Current Axes, '... 110 | num2str(h)]); 111 | elseif length(h) == 0; 112 | h = gca; 113 | warning(['Axis Handle NOT Input, Defaulting to Current Axes, '... 114 | num2str(h)]); 115 | elseif ~ishandle(h(1)) 116 | warning(['Input (' num2str(h(1)) ') is NOT an axis handle, ' ... 117 | 'defaulting to current axis, ' num2str(h)]); 118 | h = gca; 119 | end; 120 | 121 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 122 | %%BEGIN: FIRST THE X-AXIS TICK LABELS 123 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 124 | %fix the XTickLabels if they have been erased in the past 125 | if length(get(h,'XTickLabel'))==0; 126 | set(h,'XTickLabel',get(h,'XTick')); 127 | end; 128 | %set the xtick positions if entered 129 | if exist('tickposx','var'); 130 | if length(tickposx) > 0; 131 | set(h,'XTick',tickposx); 132 | end; 133 | tickposx = get(h,'XTick'); 134 | set(h,'XTickLabel',tickposx); 135 | end; 136 | %make sure the xtick positions are in the xlimit range 137 | if exist('tickposx','var'); 138 | if length(tickposx) > 0; 139 | lim = get(h,'XLim'); 140 | if lim(1) > min(tickposx); 141 | lim(1) = min(tickposx); 142 | end; 143 | if lim(2) < max(tickposx); 144 | lim(2) = max(tickposx); 145 | end; 146 | set(h,'XLim',lim); 147 | end; 148 | end; 149 | %get the tick labels and positions if the user did not input them 150 | if ~exist('tickx','var'); 151 | tickx = get(h,'XTickLabel'); 152 | if ischar(tickx); 153 | temp = tickx; 154 | tickx = cell(1,size(temp,1)); 155 | for j=1:size(temp,1); 156 | tickx{j} = strtrim( temp(j,:) ); 157 | end; 158 | end; 159 | append = '^{\circ}'; 160 | for j=1:length(tickx); 161 | tickx{j} = [tickx{j} append]; 162 | end; 163 | elseif length(tickx) == 0; 164 | tickx = get(h,'XTickLabel'); 165 | if ischar(tickx); 166 | temp = tickx; 167 | tickx = cell(1,size(temp,1)); 168 | for j=1:size(temp,1); 169 | tickx{j} = strtrim( temp(j,:) ); 170 | end; 171 | end; 172 | append = '^{\circ}'; 173 | for j=1:length(tickx); 174 | tickx{j} = [tickx{j} append]; 175 | end; 176 | elseif isstr(tickx); 177 | append = tickx; 178 | tickx = get(h,'XTickLabel'); 179 | if ischar(tickx); 180 | temp = tickx; 181 | tickx = cell(1,size(temp,1)); 182 | for j=1:size(temp,1); 183 | tickx{j} = strtrim( temp(j,:) ); 184 | end; 185 | end; 186 | if strcmp(append(1),'$'); 187 | for j=1:length(tickx); 188 | tickx{j} = ['$' tickx{j} append(2:end)]; 189 | end; 190 | else; 191 | for j=1:length(tickx); 192 | tickx{j} = [tickx{j} append]; 193 | end; 194 | end; 195 | elseif ~iscell(tickx ); 196 | warning(['Input TICKX variable is not a compatible string ' ... 197 | 'or cell array! Returning...']); 198 | return; 199 | end; 200 | %find out if we have to use the LaTex interpreter 201 | temp = tickx{1}; 202 | if strcmp(temp(1),'$'); 203 | latex_on = 1; 204 | else; 205 | latex_on = 0; 206 | end; 207 | %erase the current tick label 208 | set(h,'XTickLabel',{}); 209 | %get the x tick positions if the user did not input them 210 | if ~exist('tickposx','var'); 211 | tickposx = get(h,'XTick'); 212 | elseif length(tickx) == 0; 213 | tickposx = get(h,'XTick'); 214 | end; 215 | %get the y tick positions if the user did not input them 216 | if ~exist('tickposy','var'); 217 | tickposy = get(h,'YTick'); 218 | elseif length(tickposy) == 0; 219 | tickposy = get(h,'YTick'); 220 | end; 221 | %set the new tick positions 222 | set(h,'YTick',tickposy); 223 | set(h,'XTick',tickposx); 224 | %check the lengths of the xtick positions and xtick labels 225 | l1 = length(tickx); 226 | l2 = length(tickposx); 227 | if l1==0; 228 | set(h,'XTickLabel',tickx); 229 | end; 230 | if l1~=l2; 231 | disp(['Length of XTick = ' num2str(length(tickposx))]); 232 | disp(['Length of XTickLabel = ' num2str(length(tickx))]); 233 | if l2 < l1; 234 | warning(['Reducing Length of XTickLabel!']); 235 | else; 236 | warning(['Reducing Length of XTick!']); 237 | end; 238 | l3 = min([l1,l2]); 239 | 240 | 241 | tickx = tickx{1:l3}; 242 | tickposx = tickposx(1:l3); 243 | end; 244 | %set rotation to 0 if not input 245 | if ~exist('rotx','var'); 246 | rotx = 0; 247 | elseif length(rotx) == 0; 248 | rotx = 0; 249 | end; 250 | %Convert the cell labels to a character string 251 | %tickx = char(tickx); 252 | tickx = cellstr(tickx); 253 | %Make the XTICKS! 254 | lim = get(h,'YLim'); 255 | if min(tickposy) < lim(1); 256 | lim(1) = min(tickposy); 257 | end; 258 | if max(tickposy) > lim(2); 259 | lim(2) = max(tickposy); 260 | end; 261 | if rotx == 0; 262 | if latex_on; 263 | hx = text(tickposx,... 264 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposx),1),... 265 | tickx,'HorizontalAlignment','center',... 266 | 'VerticalAlignment','top','rotation',rotx,'interpreter','LaTex'); 267 | else; 268 | hx = text(tickposx,... 269 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposx),1),... 270 | tickx,'HorizontalAlignment','center',... 271 | 'VerticalAlignment','top','rotation',rotx); 272 | end; 273 | elseif rotx < 0; 274 | if latex_on; 275 | hx = text(tickposx,... 276 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposx),1),... 277 | tickx,'HorizontalAlignment','left','interpreter','LaTex',... 278 | 'VerticalAlignment','middlefi','rotation',rotx); 279 | else; 280 | hx = text(tickposx,... 281 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposx),1),... 282 | tickx,'HorizontalAlignment','left',... 283 | 'VerticalAlignment','middle','rotation',rotx); 284 | end; 285 | else; 286 | if latex_on; 287 | hx = text(tickposx,... 288 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposx),1),... 289 | tickx,'HorizontalAlignment','right','interpreter','LaTex',... 290 | 'VerticalAlignment','middle','rotation',rotx); 291 | else; 292 | hx = text(tickposx,... 293 | repmat(lim(1)-offset*(lim(2)-lim(2)),length(tickposx),1),... 294 | tickx,'HorizontalAlignment','right',... 295 | 'VerticalAlignment','middle','rotation',rotx); 296 | end; 297 | end; 298 | %Get and set the text size and weight 299 | set(hx,'FontSize',get(h,'FontSize')); 300 | set(hx,'FontWeight',get(h,'FontWeight')); 301 | 302 | %Set the additional parameters if they were input 303 | if length(varargin) > 2; 304 | command_string = ['set(hx']; 305 | for j=1:2:length(varargin); 306 | command_string = [command_string ',' ... 307 | '''' varargin{j} ''',varargin{' num2str(j+1) '}']; 308 | end; 309 | command_string = [command_string ');']; 310 | eval(command_string); 311 | end; 312 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 313 | %%END: FIRST THE X-AXIS TICK LABELS 314 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 315 | 316 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 317 | %%BEGIN: NOW THE Y-AXIS TICK LABELS 318 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 319 | %only move forward if we are doing anything to the yticks 320 | if ~exist('ticky'); 321 | hy = -1; 322 | elseif length(ticky)==0; 323 | hy = -1; 324 | else; 325 | %fix the YTickLabels if they have been erased in the past 326 | if length(get(h,'YTickLabel'))==0; 327 | set(h,'YTickLabel',get(h,'YTick')); 328 | end; 329 | %set the ytick positions if entered 330 | if exist('tickposy','var'); 331 | if length(tickposy) > 0; 332 | set(h,'YTick',tickposy); 333 | set(h,'YTickLabel',tickposy); 334 | end; 335 | end; 336 | %make sure the xtick positions are in the xlimit range 337 | if exist('tickposy','var'); 338 | if length(tickposy) > 0; 339 | lim = get(h,'YLim'); 340 | if lim(1) > min(tickposy); 341 | lim(1) = min(tickposy); 342 | end; 343 | if lim(2) < max(tickposy); 344 | lim(2) = max(tickposy); 345 | end; 346 | set(h,'YLim',lim); 347 | end; 348 | end; 349 | %get the tick labels and positions if the user did not input them 350 | if ~exist('ticky','var'); 351 | ticky = get(h,'YTickLabel'); 352 | if ischar(ticky); 353 | temp = ticky; 354 | ticky = cell(1,size(temp,1)); 355 | for j=1:size(temp,1); 356 | ticky{j} = strtrim( temp(j,:) ); 357 | end; 358 | end; 359 | append = '^{\circ}'; 360 | for j=1:length(ticky); 361 | ticky{j} = [ticky{j} append]; 362 | end; 363 | elseif length(ticky) == 0; 364 | ticky = get(h,'YTickLabel'); 365 | if ischar(ticky); 366 | temp = ticky; 367 | ticky = cell(1,size(temp,1)); 368 | for j=1:size(temp,1); 369 | ticky{j} = strtrim( temp(j,:) ); 370 | end; 371 | end; 372 | append = '^{\circ}'; 373 | for j=1:length(ticky); 374 | ticky{j} = [ticky{j} append]; 375 | end; 376 | elseif isstr(ticky); 377 | append = ticky; 378 | ticky = get(h,'YTickLabel'); 379 | if ischar(ticky); 380 | temp = ticky; 381 | ticky = cell(1,size(temp,1)); 382 | for j=1:size(temp,1); 383 | ticky{j} = strtrim( temp(j,:) ); 384 | end; 385 | end; 386 | if strcmp(append(1),'$'); 387 | for j=1:length(ticky); 388 | ticky{j} = ['$' ticky{j} append(2:end)]; 389 | end; 390 | else; 391 | for j=1:length(ticky); 392 | ticky{j} = [ticky{j} append]; 393 | end; 394 | end; 395 | elseif ~iscell(ticky ); 396 | warning(['Input TICKY variable is not a compatible string ' ... 397 | 'or cell array! Returning...']); 398 | return; 399 | end; 400 | %find out if we have to use the LaTex interpreter 401 | temp = ticky{1}; 402 | if strcmp(temp(1),'$'); 403 | latex_on = 1; 404 | else; 405 | latex_on = 0; 406 | end; 407 | %erase the current tick label 408 | set(h,'YTickLabel',{}); 409 | %get the x tick positions if the user did not input them 410 | if ~exist('tickposy','var'); 411 | tickposy = get(h,'YTick'); 412 | elseif length(ticky) == 0; 413 | tickposy = get(h,'YTick'); 414 | end; 415 | %get the x tick positions if the user did not input them 416 | if ~exist('tickposx','var'); 417 | tickposx = get(h,'YTick'); 418 | elseif length(tickposx) == 0; 419 | tickposx = get(h,'XTick'); 420 | end; 421 | %set the new tick positions 422 | set(h,'YTick',tickposy); 423 | % set(h,'XTick',tickposx); 424 | %check the lengths of the xtick positions and xtick labels 425 | l1 = length(ticky); 426 | l2 = length(tickposy); 427 | if l1==0; 428 | set(h,'YTickLabel',ticky); 429 | end; 430 | if l1~=l2; 431 | disp(['Length of YTick = ' num2str(length(tickposy))]); 432 | disp(['Length of YTickLabel = ' num2str(length(ticky))]); 433 | if l2 < l1; 434 | warning(['Reducing Length of YTickLabel!']); 435 | else; 436 | warning(['Reducing Length of YTick!']); 437 | end; 438 | l3 = min([l1,l2]); 439 | ticky = ticky{1:l3}; 440 | tickposy = tickposy(1:l3); 441 | end; 442 | %set rotation to 0 if not input 443 | if ~exist('roty','var'); 444 | roty = 0; 445 | elseif length(roty) == 0; 446 | roty = 0; 447 | end; 448 | %Convert the cell labels to a character string 449 | %ticky = char(ticky); 450 | ticky = cellstr(ticky); 451 | %Make the YTICKS! 452 | lim = get(h,'XLim'); 453 | if min(tickposx) < lim(1); 454 | lim(1) = min(tickposx); 455 | end; 456 | if max(tickposx) > lim(2); 457 | lim(2) = max(tickposx); 458 | end; 459 | if roty == 0; 460 | if latex_on; 461 | hy = text(... 462 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposy),1),... 463 | tickposy,... 464 | ticky,'VerticalAlignment','middle',... 465 | 'HorizontalAlignment','right','rotation',roty,'interpreter','LaTex'); 466 | else; 467 | hy = text(... 468 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposy),1),... 469 | tickposy,... 470 | ticky,'VerticalAlignment','middle',... 471 | 'HorizontalAlignment','right','rotation',roty); 472 | end; 473 | elseif roty < 180; 474 | if latex_on; 475 | hy = text(... 476 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposy),1),... 477 | tickposy,... 478 | ticky,'VerticalAlignment','middle',... 479 | 'HorizontalAlignment','right','rotation',roty,'interpreter','LaTex'); 480 | else; 481 | hy = text(... 482 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposy),1),... 483 | tickposy,... 484 | ticky,'VerticalAlignment','middle',... 485 | 'HorizontalAlignment','right','rotation',roty); 486 | end; 487 | else; 488 | if latex_on; 489 | hy = text(... 490 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposy),1),... 491 | tickposy,... 492 | ticky,'VerticalAlignment','middle',... 493 | 'HorizontalAlignment','right','rotation',roty,'interpreter','LaTex'); 494 | else; 495 | hy = text(... 496 | repmat(lim(1)-offset*(lim(2)-lim(1)),length(tickposy),1),... 497 | tickposy,... 498 | ticky,'VerticalAlignment','middle',... 499 | 'HorizontalAlignment','right','rotation',roty); 500 | end; 501 | end; 502 | %Get and set the text size and weight 503 | set(hy,'FontSize',get(h,'FontSize')); 504 | set(hy,'FontWeight',get(h,'FontWeight')); 505 | 506 | %Set the additional parameters if they were input 507 | if length(varargin) > 2; 508 | command_string = ['set(hy']; 509 | for j=1:2:length(varargin); 510 | command_string = [command_string ',' ... 511 | '''' varargin{j} ''',varargin{' num2str(j+1) '}']; 512 | end; 513 | command_string = [command_string ');']; 514 | eval(command_string); 515 | end; 516 | end; 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | -------------------------------------------------------------------------------- /scripts/my_figure.m: -------------------------------------------------------------------------------- 1 | function f= my_figure(fsizex, fsizey) 2 | f= figure; 3 | set(f,'Units','centimeters'); 4 | set(f,'Position',[0 0 fsizex fsizey]); 5 | set(f,'PaperUnits','centimeters'); 6 | set(f,'PaperPosition',[0 0 fsizex fsizey]); 7 | set(f,'PaperSize',[fsizex fsizey]) -------------------------------------------------------------------------------- /scripts/my_fonts.m: -------------------------------------------------------------------------------- 1 | function my_fonts(h) 2 | set(h, 'fontunits', 'points'); 3 | set(h, 'fontsize', 7); 4 | set(h, 'fontname', 'Times'); -------------------------------------------------------------------------------- /scripts/results.json: -------------------------------------------------------------------------------- 1 | { "axle": 9.115259e-02, "l_diam": 4.197573e-02, "r_diam": 4.223467e-02, "l_x": -3.693202e-03, "l_y": -3.782233e-03, "l_theta": -1.530913e+00, "E_d": 1.006169e+00, "E_b": 1.012807e+00 } 2 | -------------------------------------------------------------------------------- /scripts/run_all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x # show commands 3 | set -e # exit on error 4 | set -o pipefail 5 | source script_variables.sh 6 | 7 | mkdir -p ${working_dir} 8 | echo ${version_string} > ${working_dir}/version_string 9 | mkdir -p ${tuple_dir} 10 | echo ${version_string} > ${tuple_dir}/version_string 11 | 12 | for set in ${sets}; do 13 | 14 | archive=${archive_dir}/${set}.tar.gz 15 | wd=${working_dir}/${set} 16 | 17 | check_existence ${archive} 18 | 19 | echo "Extracting contents of ${archive} to ${wd}." 20 | mkdir -p ${wd} 21 | 22 | tar xvzf ${archive} -C ${wd} 23 | 24 | done 25 | 26 | echo "$0: Done preparing." 27 | 28 | echo "$0: Now creating tuples." 29 | 30 | for set in ${sets}; do 31 | 32 | wd=${working_dir}/${set} 33 | 34 | for a in ${wd}/**/log.log.bz2; do 35 | ./create_tuples.sh $a $a 36 | echo 37 | done 38 | done 39 | 40 | 41 | echo "$0: Now collecting tuples in tuple directory." 42 | 43 | rm -rf ${tuple_dir}/*.tuple 44 | mkdir -p ${tuple_dir}/ 45 | 46 | for set in ${sets}; do 47 | 48 | wd=${working_dir}/${set} 49 | cat ${wd}/exp*/*.log_tuple > ${tuple_dir}/${set}.tuple 50 | ${json_decimate} -period 3 -phase 0 -in ${tuple_dir}/${set}.tuple -out ${tuple_dir}/${set}_0.tuple 51 | ${json_decimate} -period 3 -phase 1 -in ${tuple_dir}/${set}.tuple -out ${tuple_dir}/${set}_1.tuple 52 | ${json_decimate} -period 3 -phase 2 -in ${tuple_dir}/${set}.tuple -out ${tuple_dir}/${set}_2.tuple 53 | done 54 | 55 | echo "$0: Now solving for all tuples" 56 | 57 | for file in ${tuple_dir}/*.tuple; do 58 | a=`basename ${file} .tuple` 59 | ${solver} -input_file ${tuple_dir}/${a}.tuple -output_file ${tuple_dir}/${a}_results.json ${solver_options} 60 | ${json2matlab} ${tuple_dir}/${a}_results.json 61 | done 62 | 63 | 64 | ln -f -s ${tuple_dir} 'last_tuples' 65 | 66 | 67 | -------------------------------------------------------------------------------- /scripts/script_variables.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | archive_dir="../data/" 4 | 5 | json_decimate=json_decimate 6 | json2matlab=json2matlab 7 | 8 | #Executables 9 | sm2="sm2" 10 | 11 | calib_bin=../bin/ 12 | synchronizer=${calib_bin}/synchronizer 13 | solver=${calib_bin}/solver2 14 | 15 | 16 | # parameters 17 | sets="l90 lmov lstraight" 18 | decimate_period=4 19 | sm_options="-restart 0 -recover_from_error 1 -max_iterations 1000 -min_reading 0.3 -max_angular_correction_deg 10 -max_linear_correction 0.05 -epsilon_xy 0.000001 -epsilon_theta 0.000001 " 20 | synchronizer_options="-tk_threshold 5 -perc_threshold 0.95 -time_tolerance 0.5" 21 | solver_options="-mode 0 -outliers_iterations 4" 22 | 23 | 24 | check_exec() { 25 | local path="$1" 26 | if ! type $path >/dev/null 2>&1; then 27 | echo "ERROR: \"$path\" not found." 28 | echo " This is needed to work. " 29 | exit 2 30 | fi 31 | } 32 | 33 | check_existence() { 34 | local type="$1" 35 | local path="$2" 36 | if [ ! -e $path ]; then 37 | echo "Missing $type: $path" 38 | exit 1 39 | fi 40 | } 41 | 42 | 43 | # Check we can find everything 44 | check_exec md5sum 45 | check_exec awk 46 | check_exec ${json_decimate} 47 | check_exec ${sm2} 48 | check_exec ${json2matlab} 49 | check_exec ${solver} 50 | check_exec ${synchronizer} 51 | check_existence ${archive_dir} 52 | 53 | 54 | version_string="sets=${sets}\ndecimate_period=${decimate_period}\nsm_options=${sm_options}\nsynchronizer_options=${synchronizer_options}\nsolver_options=${solver_options}" 55 | version_id=`echo ${version_string}|md5sum|awk '{print $1}'` 56 | 57 | working_dir="working_dir_${version_id}/" 58 | tuple_dir="tuples_${version_id}/" 59 | 60 | echo version_id: ${version_id} 61 | echo working_dir: ${working_dir} 62 | echo tuple_dir: ${tuple_dir} 63 | 64 | 65 | -------------------------------------------------------------------------------- /scripts/table_test.tex: -------------------------------------------------------------------------------- 1 | \documentclass{book} 2 | \usepackage{fullpage} 3 | \begin{document} 4 | \footnotesize 5 | \input{last_tuples/bigtable1.tex} 6 | 7 | \input{last_tuples/bigtable2.tex} 8 | 9 | \input{last_tuples/bigtable3.tex} 10 | \end{document} -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | 3 | # Detection of CSM library using CMake routines 4 | find_package(PkgConfig REQUIRED) 5 | pkg_check_modules(GSL REQUIRED gsl) 6 | pkg_check_modules(CSM REQUIRED csm) 7 | 8 | IF(${CSM_FOUND}) 9 | MESSAGE("CSM_LIBRARY_DIRS: ${CSM_LIBRARY_DIRS}") 10 | MESSAGE("CSM_LIBRARIES: ${CSM_LIBRARIES}") 11 | MESSAGE("CSM_INCLUDE_DIRS: ${CSM_INCLUDE_DIRS}") 12 | 13 | INCLUDE_DIRECTORIES(${CSM_INCLUDE_DIRS}) 14 | LINK_DIRECTORIES(${CSM_LIBRARY_DIRS}) 15 | 16 | ELSE(${CSM_FOUND}) 17 | MESSAGE(SEND_ERROR "CSM not found. Make sure PKG_CONFIG_PATH contains the directory where the file 'csm.pc' can be found.") 18 | ENDIF(${CSM_FOUND}) 19 | 20 | 21 | # Various flags 22 | 23 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99") 24 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb") 25 | # SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O2 ") 26 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ggdb") 27 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2") 28 | 29 | #build dirs 30 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ../bin) 31 | 32 | # A test for JSON 33 | ADD_EXECUTABLE(test_json test_json.c) 34 | TARGET_LINK_LIBRARIES(test_json ${CSM_LIBRARIES}) 35 | 36 | 37 | ADD_EXECUTABLE(synchronizer synchronizer.cpp synchronizer_options.cpp aux_functions.cpp) 38 | TARGET_LINK_LIBRARIES(synchronizer ${CSM_LIBRARIES}) 39 | 40 | ADD_EXECUTABLE(solver solver.cpp solver_options.cpp solver_utils.cpp aux_functions.cpp) 41 | TARGET_LINK_LIBRARIES(solver ${CSM_LIBRARIES}) 42 | 43 | ADD_EXECUTABLE(solver2 solver2.cpp solver2_meat.cpp solver_options.cpp solver_utils.cpp aux_functions.cpp calib_tuple.cpp gsl_jacobian.cpp) 44 | TARGET_LINK_LIBRARIES(solver2 ${CSM_LIBRARIES}) 45 | 46 | 47 | ADD_EXECUTABLE(verifier verifier.cpp verifier_options.cpp verifier_utils.cpp aux_functions.cpp analyzer.cpp) 48 | TARGET_LINK_LIBRARIES(verifier ${CSM_LIBRARIES}) 49 | 50 | ADD_EXECUTABLE(gsl_jacobian_test gsl_jacobian_test.cpp gsl_jacobian.cpp ) 51 | TARGET_LINK_LIBRARIES(gsl_jacobian_test ${CSM_LIBRARIES}) 52 | -------------------------------------------------------------------------------- /src/analyzer.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file analyzer.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Implements methods of class Analyzer 5 | */ 6 | 7 | #include "analyzer.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // #include 16 | // #include 17 | // #include 18 | using namespace std; 19 | 20 | void Analyzer::set_par(Graph * g, const char * name, const char * title, 21 | const char * x_axis, const char * y_axis, const char * type, 22 | const char * legend, double *mx, double *Mx, double *my, double *My) { 23 | g->name = name; 24 | g->title = title; 25 | g->x_axis = x_axis; 26 | g->y_axis = y_axis; 27 | g->type = type; 28 | g->legend = legend; 29 | g->mx = mx; 30 | g->Mx = Mx; 31 | g->my = my; 32 | g->My = My; 33 | } 34 | 35 | 36 | void Analyzer::add_data_graph(double * x, double * y, int N, Graph * g) { 37 | 38 | // scrive su un file nome_graph.gp i vettori x,y 39 | ofstream gr_os(g->name); 40 | 41 | for (int k = 0; k < N; k++) { 42 | gr_os << y[k]; 43 | gr_os << "\t"; 44 | gr_os << x[k]; 45 | gr_os << "\n"; 46 | } 47 | gr_os.close(); 48 | } 49 | 50 | void Analyzer::create_plot(Graph * plots, int num_g, const char * file_plot, bool grid, bool legend, 51 | double *mx, double *Mx, double *my, double *My) { 52 | 53 | // creazione del file 54 | ofstream pl_os(file_plot); 55 | int num_row, num_col; 56 | double x_delay, y_delay, x_size, y_size; 57 | // impostaione multiplot 58 | bool multi = (num_g > 1); 59 | if (multi) { 60 | pl_os << "set multiplot\n"; 61 | 62 | num_row = (int) floor((double)num_g / 3.0) + 1; 63 | num_col = (int) ceil((double)num_g / (double)num_row); 64 | 65 | x_delay = 1.0 / num_col; 66 | y_delay = 1.0 / num_row; 67 | x_size = x_delay - x_delay * 0.1; 68 | y_size = y_delay - y_delay * 0.1; 69 | } 70 | 71 | if (grid) pl_os << "set grid\n"; 72 | if (legend) pl_os << "set key outside\n\n"; 73 | 74 | int i = 0; 75 | 76 | double x_origin, y_origin; 77 | for (int r = 0; r < num_row; r++) { 78 | for (int c = 0; c < num_col; c++) { 79 | 80 | i = r * num_col + c; 81 | if ( i < num_g ) { 82 | // setting title for graph[i] 83 | pl_os << "set title \""; pl_os << plots[i].title; pl_os << "\" \n"; 84 | // setting origin of graph[i] 85 | x_origin = x_delay * c; 86 | y_origin = y_delay * (num_row - r - 1); 87 | if (multi) { 88 | pl_os << "set origin "; pl_os << x_origin; pl_os << ","; pl_os << y_origin; pl_os << "\n"; 89 | // setting size of graph[i] 90 | pl_os << "set size "; pl_os << x_size; pl_os << ","; pl_os << y_size; pl_os << "\n"; 91 | } 92 | // setting labels 93 | pl_os << "set xlabel \""; pl_os << plots[i].x_axis; pl_os << "\" 0,0\n"; 94 | pl_os << "set ylabel \""; pl_os << plots[i].y_axis; pl_os << "\" 0,0\n"; 95 | // plotting graph[i] 96 | if (plots[i].mx != NULL) { 97 | pl_os << "plot [" << mx[0] << " to " << Mx[0] << "] [" << my[0] << " to " << My[0] << "] '"; 98 | } 99 | else pl_os << "plot '"; 100 | pl_os << plots[i].name; 101 | pl_os << "' using \"%lf%lf\" "; 102 | if (plots[i].type != NULL) {pl_os << "w "; pl_os << plots[i].type; pl_os << " ";} 103 | if (legend) pl_os << "title '" << plots[i].legend << "'\n\n"; 104 | else pl_os << "notitle \n\n"; 105 | if(!multi) { 106 | pl_os.close(); 107 | return; 108 | } 109 | } 110 | } 111 | } 112 | 113 | pl_os.close(); 114 | } 115 | 116 | void Analyzer::create_same_plot(const char * title, const char * x_axis, const char * y_axis, Graph * plots, int num_g, const char * file_plot, bool grid, bool legend, double *mx, double *Mx, double *my, double *My) { 117 | 118 | // creazione del file 119 | ofstream pl_os(file_plot); 120 | 121 | // impostaione multiplot 122 | if (grid) pl_os << "set grid\n"; 123 | if (legend) pl_os << "set key outside right top nobox\n\n"; 124 | 125 | int i = 0; 126 | 127 | // setting title for graph[i] 128 | pl_os << "set title \""; pl_os << title; pl_os << "\" \n"; 129 | // setting labels 130 | pl_os << "set xlabel \""; pl_os << x_axis; pl_os << "\" 0,0\n"; 131 | pl_os << "set ylabel \""; pl_os << y_axis; pl_os << "\" 0,0\n"; 132 | // plotting graph[i] 133 | pl_os << "plot "; 134 | if (mx != NULL) { 135 | pl_os << "[" << mx[0] << " to " << Mx[0] << "] [" << my[0] << " to " << My[0] << "] "; 136 | } 137 | for (int i = 0; i < num_g; i++ ) { 138 | pl_os << "'" << plots[i].name << "'" 139 | << " using \"%lf%lf\" "; 140 | if (plots[i].type != NULL) {pl_os << "w "; pl_os << plots[i].type; pl_os << " ";} 141 | if (legend) pl_os << "title '" << plots[i].legend << "'"; 142 | else pl_os << "notitle \n\n"; 143 | if (i != num_g-1) pl_os << ", \\\n"; 144 | } 145 | pl_os << "\n" << endl; 146 | 147 | pl_os.close(); 148 | } 149 | 150 | // execute Gnuplot with file "file_plot" 151 | void Analyzer::plot(const char * file_plot) { 152 | // usato nella chiamta a popen 153 | stringstream cmd; 154 | cmd << "gnuplot -persist " << file_plot << "\0"; 155 | system(cmd.str().c_str()); 156 | } 157 | 158 | -------------------------------------------------------------------------------- /src/analyzer.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file analyzer.h 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Header of class Analyzer 5 | */ 6 | 7 | 8 | #ifndef __ANALYZER_H__ 9 | #define __ANALYZER_H__ 10 | 11 | #include 12 | #include 13 | //#include 14 | //#include 15 | 16 | /*! 17 | used to handle information about plot (not data) 18 | @param name name of data file for graph 19 | @param title title of graph 20 | @param x_axis x-axis label 21 | @param y_axis y-axis label 22 | @param type "impulses" "points" "lines" 23 | @param legend string for legend 24 | @param mx,Mx,my,My pointers to values for bounds 25 | */ 26 | typedef struct{ 27 | const char * name; 28 | const char * title; 29 | const char * x_axis; 30 | const char * y_axis; 31 | const char * type; 32 | const char * legend; 33 | double *mx, *Mx, *my, *My; 34 | } Graph; 35 | 36 | 37 | /*! 38 | \class Analyzer 39 | \brief class for printing graphs, using Gnuplot 40 | 41 | Create Gnuplot data-file from vectors, 42 | create Gnuplot script to plot data. 43 | Possibility of multiplot. 44 | */ 45 | class Analyzer { 46 | 47 | public: 48 | /*! 49 | Assign parameters to Graph object 50 | @param g Graph object 51 | @note For bounds (mx,Mx,my,My) default value is null. You can call this function omitting these parameters. 52 | */ 53 | void set_par(Graph * g, const char * name, const char * title, 54 | const char * x_axis, const char * y_axis, 55 | const char * type, const char * legend, 56 | double *mx=NULL, double *Mx=NULL, double *my=NULL, double *My=NULL); 57 | 58 | /*! 59 | Create Gnuplot data-file from vectors x[] and y[] 60 | @param x pointer to a vector of double (x data) 61 | @param y pointer to a vector of double (y data) 62 | @param N number of elements in x[] and y[] (plot y to x) 63 | @param g pointer to Graph object 64 | */ 65 | void add_data_graph(double * x, double * y, int N, Graph * g); 66 | 67 | 68 | /*! 69 | Create Gnuplot script to plot (multiplot) data from files 70 | @param file_plot name of Gnuplot script 71 | @param plots vector of Graph objects 72 | @param num_g number of subplots 73 | @param grid grid on/off 74 | @param legend legend on/off 75 | @param mx,Mx,my,My pointers to values for bounds (default is null) 76 | */ 77 | void create_plot(Graph * plots, int num_g, const char * file_plot, bool grid, bool legend, 78 | double *mx=NULL, double *Mx=NULL, double *my=NULL, double *My=NULL); 79 | 80 | /*! 81 | Create Gnuplot script to plot multiple data on the same plot 82 | @param file_plot name of Gnuplot script 83 | @param plots vector of Graph objects 84 | @param num_g number of subplots 85 | @param grid grid on/off 86 | @param legend legend on/off 87 | @param mx,Mx,my,My pointers to values for bounds (default is null) 88 | */ 89 | void create_same_plot(const char * title, const char * x_axis, const char * y_axis, 90 | Graph * plots, int num_g, const char * file_plot, bool grid, bool legend, 91 | double *mx=NULL, double *Mx=NULL, double *my=NULL, double *My=NULL); 92 | 93 | /*! 94 | Execute Gnuplot with file "file_plot" 95 | @param file_plot name of Gnuplot script 96 | */ 97 | void plot(const char * file_plot); 98 | 99 | }; 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /src/aux_functions.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file aux_functions.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Collection of auxiliary functions (implementation file) 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "aux_functions.h" 16 | 17 | using namespace std; 18 | using namespace CSM; 19 | 20 | int read_jsonlist(const char * filename, vector *list) { 21 | FILE * input = open_file_for_reading(filename); 22 | if(!input) return 0; 23 | 24 | JO tmp; 25 | while (tmp = json_read_stream(input)) 26 | list->push_back(tmp); 27 | 28 | if(!feof(input)) { 29 | sm_error("Error reading JSON file, after %d objects.\n", list->size()); 30 | return 0; 31 | } 32 | 33 | fclose(input); 34 | return (1); 35 | } 36 | 37 | int write_jsonlist(const char * filename, vector *list) { 38 | FILE * output = open_file_for_writing(filename); 39 | if(!output) return 0; 40 | 41 | for (int i = 0; i < (int)list->size(); i++ ) { 42 | if (i > 0) fprintf(output,"\n"); 43 | 44 | fprintf(output, "%s", json_object_to_json_string(list[0][i]) ); 45 | } 46 | 47 | fclose(output); 48 | return (1); 49 | } 50 | 51 | int find_closest(double ts, vector odo, int start_at) { 52 | for ( int i = start_at; i < (int)odo.size() - 1; i++ ) { 53 | double ts_i = gettime(odo[i], "timestamp"); 54 | double ts_ip = gettime(odo[i+1], "timestamp"); 55 | if ( (ts_i < ts) && ( ts_ip > ts) ) { 56 | return(i); 57 | } 58 | if (ts_i > ts) { 59 | sm_error("find_closest: Sorry, could not find match for ts = %f\n", ts); 60 | return(-1); 61 | } 62 | } 63 | return(-1); 64 | } 65 | int find_closest_ref(double ts, vector odo, int bound) { 66 | double ts_i, ts_ip; 67 | if (bound==0) bound = -1; 68 | for ( int i = bound + 1; i < (int)odo.size() - 1; i++ ) { 69 | ts_i = gettime(odo[i], "timestamp"); 70 | ts_ip = gettime(odo[i+1], "timestamp"); 71 | if ( (ts_i <= ts) && ( ts_ip >= ts) ) return (i); 72 | } 73 | sm_error("find_closest_ref: Sorry, could not find match for ts = %f\n", ts); 74 | return(-1); 75 | } 76 | int find_closest_sens(double ts, vector odo, int bound) { 77 | double ts_i, ts_ip; 78 | for ( int i = bound+1; i < (int)odo.size(); i++ ) { 79 | ts_i = gettime(odo[i], "timestamp"); 80 | ts_ip = gettime(odo[i-1], "timestamp"); 81 | if ( (ts_i >= ts) && ( ts_ip <= ts) ) return (i); 82 | } 83 | sm_error("find_closest_sens: Sorry, could not find match for ts = %f\n", ts); 84 | return(-1); 85 | } 86 | 87 | void mysort(double *sort, int length) { 88 | int i, j; 89 | double tmp; 90 | 91 | for( i = 0; i < length - 1; i++) { 92 | for(j = i; j < length; j++) { 93 | if(sort[i] > sort[j]) { 94 | tmp = sort[i]; 95 | sort[i] = sort[j]; 96 | sort[j] = tmp; 97 | } 98 | } 99 | } 100 | } 101 | 102 | void mysort(int *sort, int length) { 103 | int i, j; 104 | int tmp; 105 | 106 | for( i = 0; i < length - 1; i++) { 107 | for(j = i; j < length; j++) { 108 | if(sort[i] > sort[j]) { 109 | tmp = sort[i]; 110 | sort[i] = sort[j]; 111 | sort[j] = tmp; 112 | } 113 | } 114 | } 115 | } 116 | 117 | int getticks(JO obj, const char* name) { 118 | int tick; 119 | jo_read_int(obj, name, &tick); 120 | return tick; 121 | } 122 | 123 | double getticksD(JO obj, const char* name) { 124 | double tick; 125 | jo_read_double(obj, name, &tick); 126 | return tick; 127 | } 128 | 129 | double gettime(JO obj, const char* name) { 130 | double read_time[2]; 131 | jo_read_double_array (obj, name, read_time, 2, 0.0); 132 | return read_time[0] + 0.000001 * read_time[1]; 133 | } 134 | 135 | int logSync(vector odo, vector las_stats, int* linit, int* lend, int* oinit, int* oend) { 136 | int nLas = (int)las_stats.size(); 137 | 138 | bool found = false; 139 | int ref; 140 | for(int l = 0; (l < nLas) && !found; l++ ) { 141 | double lT = gettime(las_stats[l], "laser_ref_timestamp"); 142 | ref = find_closest_ref(lT, odo, 0); 143 | if (ref != -1) { 144 | found = true; 145 | linit[0] = l; 146 | oinit[0] = ref; 147 | } 148 | } 149 | if (!found) { 150 | sm_error("Could not find intersection of logs.\n"); 151 | return 0; 152 | } 153 | 154 | found = false; 155 | int sens; 156 | for(int l = (nLas - 1); (l >= 0) && !found; l-- ) { 157 | double lT = gettime(las_stats[l], "laser_sens_timestamp"); 158 | sens = find_closest_sens(lT, odo, 0); 159 | if (sens != -1) { 160 | found = true; 161 | lend[0] = l; 162 | oend[0] = sens; 163 | } 164 | } 165 | if (!found) { 166 | sm_error("Could not find intersection of logs.\n"); 167 | return 0; 168 | } 169 | return 1; 170 | } 171 | 172 | int matchOdoLas(vector odo, vector smStats, vector * matchedOdo, vector * matchedLas){ 173 | cout << "Checking correspondances between laser and odometry files..."<push_back(new_obj); 206 | 207 | for( int i = oinit + 1; i < oend; i++ ) { 208 | new_obj = json_object_new_object(); 209 | ts = gettime(odo[i], "timestamp"); 210 | timestamp[0] = int(ts); 211 | timestamp[1] = int(ts*1000000 - floor(ts)*1000000); 212 | jo_add_int_array (new_obj, "timestamp", timestamp, 2); 213 | lticks = getticks(odo[i],"left"); 214 | rticks = getticks(odo[i],"right"); 215 | json_object_object_add(new_obj, "left", json_object_new_double(lticks) ); 216 | json_object_object_add(new_obj, "right", json_object_new_double(rticks) ); 217 | jo_read_double_array (odo[i], "odometry", os, 3, 0.0); 218 | jo_add_double_array (new_obj, "odometry", os, 3); 219 | matchedOdo->push_back(new_obj); 220 | } 221 | 222 | ts = gettime(smStats[lend],"laser_sens_timestamp"); 223 | t1 = gettime(odo[oend-1], "timestamp"); 224 | t2 = gettime(odo[oend], "timestamp"); 225 | double sens_alpha = (t2-ts)/(t2-t1); 226 | lticks = (1-sens_alpha)*getticks(odo[oend-1],"left") + sens_alpha*getticks(odo[oend], "left"); 227 | rticks = (1-sens_alpha)*getticks(odo[oend-1],"right") + sens_alpha*getticks(odo[oend], "right"); 228 | jo_read_double_array (odo[oend-1], "odometry", o1, 3, 0.0); 229 | jo_read_double_array (odo[oend], "odometry", o2, 3, 0.0); 230 | os[0] = (1-sens_alpha)*o1[0] + sens_alpha*o2[0]; 231 | os[1] = (1-sens_alpha)*o1[1] + sens_alpha*o2[1]; 232 | os[2] = (1-sens_alpha)*o1[2] + sens_alpha*o2[2]; 233 | 234 | new_obj = json_object_new_object(); 235 | timestamp[0] = int(ts); 236 | timestamp[1] = int(ts*1000000 - floor(ts)*1000000); 237 | jo_add_int_array (new_obj, "timestamp", timestamp, 2); 238 | json_object_object_add(new_obj, "left", json_object_new_double(lticks) ); 239 | json_object_object_add(new_obj, "right", json_object_new_double(rticks) ); 240 | jo_add_double_array (new_obj, "odometry", os, 3); 241 | matchedOdo->push_back(new_obj); 242 | 243 | double error; 244 | int valid; 245 | int nvalid; 246 | int nLas = lend - linit; 247 | double mean_error[nLas]; 248 | double mean_error_sorted[nLas]; 249 | int nvalid_sorted[nLas]; 250 | double perc = 0.99; 251 | for(int i = linit; i <= lend; i++) { 252 | int nvalid; 253 | 254 | jo_read_double(smStats[i], "error", &error); 255 | jo_read_int(smStats[i], "nvalid", &nvalid); 256 | mean_error[i] = error/nvalid; 257 | mean_error_sorted[i] = mean_error[i]; 258 | nvalid_sorted[i] = nvalid; 259 | } 260 | mysort(mean_error_sorted, nLas); 261 | mysort(nvalid_sorted, nLas); 262 | double mean_error_threshold = mean_error_sorted[ (int)round(perc*nLas)]; 263 | cout << "mean_error_threshold " << mean_error_threshold << endl; 264 | int nvalid_threshold = nvalid_sorted[ (int)round((1 - perc)*nLas)]; 265 | cout << "nvalid_threshold " << nvalid_threshold << endl; 266 | 267 | for (int i = linit; i <= lend; i++) { 268 | jo_read_int (smStats[i], "valid", &valid); 269 | if (valid == 0) { 270 | cout << "VALID N" << i << endl; 271 | continue; 272 | } 273 | // if (mean_error[i] > mean_error_threshold) { 274 | // cout << "MEAN ERROR N" << i << "\t" << mean_error[i] << endl; 275 | // continue; 276 | // } 277 | // jo_read_int (smStats[i], "nvalid", &nvalid); 278 | // if (nvalid < nvalid_threshold) { 279 | // cout << "NVALID N" << i << endl; 280 | // continue; 281 | // } 282 | jo_read_double (smStats[i], "error", &error); 283 | if (error > 5.0) { 284 | cout << "ERROR N" << i << "\t" << error << endl; 285 | continue; 286 | } 287 | matchedLas->push_back(smStats[i]); 288 | } 289 | return 0; 290 | } 291 | 292 | -------------------------------------------------------------------------------- /src/aux_functions.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file aux_functions.h 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Collection of auxiliary functions (header file) 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #ifndef M_PI 13 | #define M_PI 3.14159265 14 | #endif 15 | 16 | using namespace std; 17 | using namespace CSM; 18 | 19 | /*! 20 | \brief Reads a list of JSON objects from filename 21 | @param filename name of the file to be read 22 | @param list pointer to a vector of JsonObject (JO) used to store data 23 | @return 1 if successfull, 0 otherwhise 24 | */ 25 | int read_jsonlist(const char * filename, vector *list); 26 | 27 | /*! 28 | \brief Writes a list of JSON objects to filename 29 | @param filename name of the file to be written 30 | @param list pointer to a vector of JsonObject (JO) 31 | @returns 1 if successfull, 0 otherwhise 32 | */ 33 | int write_jsonlist(const char * filename, vector *list); 34 | 35 | /*! 36 | \brief Searches the object (odometry object) closest to a given timestamp 37 | @param ts timestamp to be matched 38 | @param odo vector of odometry objects (JO) 39 | @param start_at starting index for search 40 | @return 0 if no interval matching has been found, otherwhise the index of the closest before 41 | */ 42 | int find_closest(double ts, vector odo, int start_at); 43 | 44 | /*! 45 | \brief Searches the object closest (before) to a given timestamp 46 | @param ts timestamp to be matched 47 | @param odo vector of odometry objects (JO) 48 | @param bound starting index for search 49 | @return 0 if no interval matching has been found, otherwhise the index of the closest before 50 | */ 51 | int find_closest_ref(double ts, vector odo, int bound); 52 | 53 | /*! 54 | \brief Searches the object closest (after) to a given timestamp 55 | @param ts timestamp to be matched 56 | @param odo vector of odometry objects (JO) 57 | @param bound starting index (from the end of the list) for search 58 | @return 0 if no interval matching has been found, otherwhise the index of the closest before 59 | */ 60 | int find_closest_sens(double ts, vector odo, int bound); 61 | 62 | /*! 63 | \brief Sorts in ascending order the elements of an array of doubles 64 | @param sort array to be sorted 65 | @param length length of array 66 | */ 67 | void mysort(double *sort, int length); 68 | 69 | /*! 70 | \brief Sorts in ascending order the elements of an array of ints 71 | @param sort array to be sorted 72 | @param length length of array 73 | */ 74 | void mysort(int *sort, int length); 75 | 76 | /*! 77 | \brief Gets the ticks from an odometry object (JO) 78 | @param obj odometry object 79 | @param name left/right, selects left or right ticks 80 | @return number of ticks (int) 81 | */ 82 | int getticks(JO obj, const char* name); 83 | 84 | /*! 85 | \brief Gets the ticks from an odometry object (JO) 86 | @param obj odometry object 87 | @param name left/right, selects left or right ticks 88 | @return number of ticks (double) 89 | */ 90 | double getticksD(JO obj, const char* name); 91 | 92 | /*! 93 | \brief Gets the timestamp from an odometry/laser object (JO). 94 | Time is supposed to be represented by a two values array, an int for seconds and another for microseconds 95 | @param obj odometry object 96 | @param name field name (es: "Timestamp") 97 | @return timestamp 98 | */ 99 | double gettime(JO obj, const char* name); 100 | 101 | /*! 102 | \brief Given two vectors (laser and odometry) computates two subvectors with matching timestamps 103 | @param odo vector of JO for odometry 104 | @param las_stats vector of JO for laser (laser statistics) 105 | @param linit starting index of laser subvector 106 | @param lend ending index of laser subvector 107 | @param oinit starting index of odometry subvector 108 | @param oend ending index of odometry subvector 109 | @return 1 if successfull, 0 otherwhise 110 | 111 | */ 112 | int logSync(vector odo, vector las_stats, int* linit, int* lend, int* oinit, int* oend); 113 | 114 | /*! 115 | \brief Calculates matching between laser_stats and odometry objects 116 | @param odo vector of JO for odometry 117 | @param smStats vector of JO for laser (laser statistics) 118 | @param matchedOdo vector of JO to store matched odometry 119 | @param matchedLas vector of JO to store matched laser (statistics) 120 | @return 1 if successfull, 0 otherwhise 121 | */ 122 | int matchOdoLas(vector odo, vector smStats, vector * matchedOdo, vector * matchedLas); 123 | -------------------------------------------------------------------------------- /src/calib_tuple.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "calib_tuple.h" 4 | #include "gsl_jacobian.h" 5 | 6 | calib_tuple::calib_tuple() { 7 | 8 | } 9 | /*calib_tuple::calib_tuple(const calib_tuple& x) { 10 | // fim = gsl_matrix_alloc (6,6); 11 | // gsl_matrix_memcpy(fim, x.fim); 12 | }*/ 13 | calib_tuple::~calib_tuple() { 14 | // gsl_matrix_free(fim); 15 | } 16 | 17 | gsl_vector * jacobian_helper(const gsl_vector* calib, void*params) { 18 | calib_tuple * t = (calib_tuple*) params; 19 | calib_result r; 20 | r.radius_l = gsl_vector_get(calib, 0); 21 | r.radius_r = gsl_vector_get(calib, 1); 22 | r.axle = gsl_vector_get(calib, 2); 23 | r.l[0] = gsl_vector_get(calib, 3); 24 | r.l[1] = gsl_vector_get(calib, 4); 25 | r.l[2] = gsl_vector_get(calib, 5); 26 | t->compute_disagreement(r); 27 | 28 | gsl_vector*res = gsl_vector_alloc(3); 29 | gsl_vector_set(res, 0, t->est_sm[0]); 30 | gsl_vector_set(res, 1, t->est_sm[1]); 31 | gsl_vector_set(res, 2, t->est_sm[2]); 32 | return res; 33 | } 34 | 35 | 36 | gsl_matrix* calib_tuple::compute_fim(struct calib_result&r, gsl_matrix * inf_sm) { 37 | double state[6] = { r.radius_l, r.radius_r, r.axle, r.l[0], r.l[1], r.l[2]}; 38 | gsl_vector_view v = gsl_vector_view_array(state, 6); 39 | gsl_matrix * J = gsl_jacobian(jacobian_helper, &(v.vector), this); 40 | 41 | // XXXXXXXXXX 42 | // FIM = J' * inf_sm * J 43 | // A = J' * inf_sm 44 | gsl_matrix * A = gsl_matrix_alloc(6,3); 45 | gsl_blas_dgemm (CblasTrans, CblasNoTrans, 1, J, inf_sm, 0, A); 46 | // FIM = A * J 47 | gsl_matrix * fim = gsl_matrix_alloc(6,6); 48 | gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1, A, J, 0, fim); 49 | 50 | gsl_matrix_free(J); 51 | gsl_matrix_free(A); 52 | return fim; 53 | } 54 | 55 | void calib_tuple::write_as_long_line(std::ostream&os) { 56 | os 57 | << T << " " 58 | << phi_l << " " 59 | << phi_r << " " 60 | << sm[0] << " " 61 | << sm[1] << " " 62 | << sm[2] << " " 63 | << o[0] << " " 64 | << o[1] << " " 65 | << o[2] << " " 66 | << err[0] << " " 67 | << err[1] << " " 68 | << err[2] << " " 69 | << est_sm[0] << " " 70 | << est_sm[1] << " " 71 | << est_sm[2] << " " 72 | << err_sm[0] << " " 73 | << err_sm[1] << " " 74 | << err_sm[2] << " " 75 | << mark_as_outlier << 76 | std::endl; 77 | } 78 | 79 | void calib_tuple::compute_disagreement(const struct calib_result&res) { 80 | double J11 = +res.radius_l/2; 81 | double J12 = +res.radius_r/2; 82 | double J21 = -res.radius_l/res.axle; 83 | double J22 = +res.radius_r/res.axle; 84 | 85 | static int once = 1; 86 | // if(once) 87 | // cout << "My J21,J22 "<< J21 << ", " << J22 << endl; 88 | once = 0; 89 | 90 | double speed = J11 * this->phi_l + J12 * this->phi_r; 91 | double omega = J21 * this->phi_l + J22 * this->phi_r; 92 | 93 | double o_theta = T * omega; 94 | 95 | double t1,t2; 96 | if ( fabs(o_theta) > 1e-12 ) { 97 | t1 = ( sin(o_theta) / (o_theta) ); 98 | t2 = ( (1-cos(o_theta)) / (o_theta) ); 99 | } else { 100 | t1 = 1; 101 | t2 = 0; 102 | } 103 | 104 | this->o[0] = t1 * (speed*T); 105 | this->o[1] = t2 * (speed*T); 106 | this->o[2] = o_theta; 107 | 108 | double l_plus_s[3]; 109 | double o_plus_l[3]; 110 | oplus_d(res.l, this->sm, l_plus_s); 111 | oplus_d(o, res.l, o_plus_l); 112 | 113 | for(int i=0;i<3;i++) 114 | this->err[i] = l_plus_s[i] - o_plus_l[i]; 115 | 116 | pose_diff_d(o_plus_l, res.l, this->est_sm); 117 | 118 | for(int i=0;i<3;i++) 119 | this->err_sm[i] = this->est_sm[i] - this->sm[i]; 120 | 121 | } 122 | 123 | bool json2tuple(const JO jo, calib_tuple&tuple) { 124 | int res = 125 | jo_read_double(jo, "T", &tuple.T) && 126 | jo_read_double(jo, "phi_l", &tuple.phi_l) && 127 | jo_read_double(jo, "phi_r", &tuple.phi_r) && 128 | jo_read_double_array (jo, "sm", tuple.sm, 3, GSL_NAN); 129 | 130 | if(!res) 131 | sm_error("Cannot read tuple form JSON.\n"); 132 | return res; 133 | } 134 | 135 | JO calib_result2json(const calib_result&res) { 136 | // Build json object 137 | struct json_object *new_obj; 138 | new_obj = json_object_new_object(); 139 | 140 | double est_E_d = res.radius_r / res.radius_l; 141 | /** XXX ??? */ 142 | double est_E_b = res.axle / 0.09; 143 | json_object_object_add(new_obj, "axle", json_object_new_double(res.axle) ); 144 | json_object_object_add(new_obj, "l_diam", json_object_new_double(res.radius_l*2) ); 145 | json_object_object_add(new_obj, "r_diam", json_object_new_double(res.radius_r*2) ); 146 | json_object_object_add(new_obj, "l_x", json_object_new_double(res.l[0]) ); 147 | json_object_object_add(new_obj, "l_y", json_object_new_double(res.l[1]) ); 148 | json_object_object_add(new_obj, "l_theta", json_object_new_double(res.l[2]) ); 149 | json_object_object_add(new_obj, "E_d", json_object_new_double(est_E_d) ); 150 | json_object_object_add(new_obj, "E_b", json_object_new_double(est_E_b) ); 151 | 152 | return new_obj; 153 | } 154 | 155 | 156 | 157 | -------------------------------------------------------------------------------- /src/calib_tuple.h: -------------------------------------------------------------------------------- 1 | #ifndef H_CALIB_TUPLE 2 | #define H_CALIB_TUPLE 3 | 4 | #include 5 | #include "calib_tuple.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | struct calib_result { 15 | double radius_l, radius_r; 16 | double axle; 17 | 18 | /** Laser pose */ 19 | double l[3]; 20 | }; 21 | 22 | struct calib_tuple { 23 | 24 | /** Input data */ 25 | 26 | /** Period */ 27 | double T; 28 | /** Left and right wheel velocities */ 29 | double phi_l; 30 | double phi_r; 31 | /** Scan matching estimate */ 32 | double sm[3]; 33 | 34 | /** Temp data, used for outliers computations */ 35 | 36 | /** Estimated rototranslation based on odometry params. */ 37 | double o[3]; 38 | 39 | /** Estimated disagreement sm - est_sm */ 40 | // double e_sm[3]; 41 | 42 | double est_sm[3]; 43 | double err_sm[3]; 44 | 45 | /** Other way to estimate disagreement: l (+) s - o (+) l */ 46 | double err[3]; 47 | 48 | int mark_as_outlier; 49 | 50 | 51 | /** Computes plenty of statistics, including estimated sm. */ 52 | void compute_disagreement(const calib_result&); 53 | 54 | /** Computes fisher information matrix. inf_sm is the inverse of the covariance of sm */ 55 | gsl_matrix* compute_fim(struct calib_result&, gsl_matrix * inf_sm); 56 | 57 | void write_as_long_line(std::ostream&os); 58 | 59 | calib_tuple(); 60 | ~calib_tuple(); 61 | 62 | }; 63 | 64 | 65 | bool json2tuple(const JO jo, calib_tuple&tuple); 66 | JO tuple2json(const calib_tuple&tuple); 67 | 68 | JO calib_result2json(const calib_result&res) ; 69 | 70 | #endif 71 | 72 | 73 | -------------------------------------------------------------------------------- /src/gsl_jacobian.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "gsl_jacobian.h" 3 | 4 | struct my_params_struct { 5 | int i, j; 6 | gsl_vector * (*function)(const gsl_vector* x, void*params); 7 | const gsl_vector * x0; 8 | void * params; 9 | }; 10 | 11 | double my_gsl_function(double xj, void * my_params) { 12 | struct my_params_struct * s = (struct my_params_struct*) my_params; 13 | 14 | // create x by copying x0 and setting xj 15 | gsl_vector * x = gsl_vector_alloc(s->x0->size); 16 | gsl_vector_memcpy(x, s->x0); 17 | gsl_vector_set(x, s->j, xj); 18 | 19 | gsl_vector * fx = s->function(x, s->params); 20 | 21 | double fi = gsl_vector_get(fx, s->i); 22 | 23 | gsl_vector_free(fx); 24 | gsl_vector_free(x); 25 | 26 | return fi; 27 | } 28 | 29 | 30 | gsl_matrix * gsl_jacobian( gsl_vector * (*function)(const gsl_vector* x, void*params), const gsl_vector *x, void*params) { 31 | 32 | gsl_vector * fx = function(x,params); 33 | 34 | int rows = fx->size; 35 | int columns = x->size; 36 | 37 | gsl_matrix * m = gsl_matrix_alloc(rows,columns); 38 | 39 | 40 | for(int i=0;i 4 | #include 5 | 6 | gsl_matrix * gsl_jacobian( gsl_vector * (*function)(const gsl_vector* x, void*params), const gsl_vector *x, void*params); 7 | -------------------------------------------------------------------------------- /src/gsl_jacobian_test.cpp: -------------------------------------------------------------------------------- 1 | #include "gsl_jacobian.h" 2 | 3 | gsl_vector* test1(const gsl_vector*x, void*dummy) { 4 | gsl_vector*res = gsl_vector_alloc(x->size); 5 | for(int i=0;isize;i++) { 6 | gsl_vector_set(res, i, gsl_vector_get(x, i) * gsl_vector_get(x, i)); 7 | } 8 | 9 | return res; 10 | } 11 | 12 | int main() { 13 | 14 | gsl_vector * v = gsl_vector_alloc(4); 15 | gsl_vector_set(v,0,1); 16 | gsl_vector_set(v,1,2); 17 | gsl_vector_set(v,2,3); 18 | gsl_vector_set(v,3,4); 19 | 20 | gsl_matrix * J = gsl_jacobian(test1, v, 0); 21 | 22 | gsl_matrix_fprintf(stderr, J, "%g"); 23 | 24 | fprintf(stderr, "\n"); 25 | 26 | } 27 | -------------------------------------------------------------------------------- /src/solver.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file solver.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Estimates odometry and sensor parameters 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "aux_functions.h" 24 | #include "solver_options.h" 25 | #include "solver_utils.h" 26 | 27 | #define vg(X,Y) ( gsl_vector_get(X,Y) ) 28 | #define vs(X,Y,Z) ( gsl_vector_set(X,Y,Z) ) 29 | #define mg(X,Y,Z) ( gsl_matrix_get(X,Y,Z) ) 30 | #define ms(X,Y,Z,Q) ( gsl_matrix_set(X,Y,Z,Q) ) 31 | 32 | using namespace std; 33 | 34 | extern void solver_options(solver_params*p, struct option*ops); 35 | double f (const gsl_vector *z, void *params); 36 | 37 | /*! 38 | Solver routine 39 | \brief This program has been developed to estimate parameters (odometry and sensor) given a list of tuples generated by synchronizer. 40 | 41 | It uses gsl libraries to handle matrix and vectors, as well as minimization and eigenvalues solution. 42 | 43 | */ 44 | int main(int argc, const char * argv[]) { 45 | /*!*/ 46 | solver_params params; 47 | struct option* ops = options_allocate(100); 48 | solver_options(¶ms, ops); 49 | if(!options_parse_args(ops, argc, argv)) { 50 | options_print_help(ops, stderr); 51 | return 1; 52 | } 53 | 54 | /*!*/ 55 | vector tuple; 56 | if (!read_jsonlist(params.input_file, &tuple) ) { 57 | sm_error("Error opening input files.\n"); 58 | return 2; 59 | } 60 | 61 | /*!*/ 62 | double J21, J22; 63 | gsl_matrix * A = gsl_matrix_calloc(2,2); 64 | gsl_vector * g = gsl_vector_calloc(2); 65 | gsl_vector * L_i = gsl_vector_alloc(2); 66 | double th_i; 67 | int n = (int) tuple.size(); 68 | 69 | for (int i = 0; i < n; i++) { 70 | double T, phi_l, phi_r; 71 | double sm[3]; 72 | jo_read_double(tuple[i], "T", &T); 73 | jo_read_double(tuple[i], "phi_l", &phi_l); 74 | jo_read_double(tuple[i], "phi_r", &phi_r); 75 | jo_read_double_array (tuple[i], "sm", sm, 3, 0.0); 76 | vs( L_i, 0, T*phi_l); 77 | vs( L_i, 1, T*phi_r); 78 | gsl_blas_dger (1, L_i, L_i, A); // A = A + L_i'*L_i; A symmetric 79 | gsl_blas_daxpy (sm[2], L_i, g); // g = g + L_i'*y_i; sm :{x , y, theta} 80 | } 81 | gsl_vector_free(L_i); 82 | 83 | // Verify that A isn't singular 84 | double cond = cond_number(A); 85 | if ( cond > 50 ) { 86 | sm_error("Matrix A is almost singular. Not very exciting data!\n"); 87 | return 3; 88 | } 89 | 90 | // Ay = g --> y = inv(A)g; A square matrix; 91 | gsl_vector * y = gsl_vector_calloc(2); 92 | if (!solve_square_linear(A, g, y)) { 93 | sm_error("Cannot solve Ay=g. Invalid arguments\n"); 94 | return 4; 95 | } 96 | J21 = vg(y, 0); J22 = vg(y, 1); 97 | if ( gsl_isnan(J21) || gsl_isnan(J22) ) { 98 | sm_error("Could not find first two parameters J21, J22"); 99 | return 5; 100 | } 101 | //cout << "J21,J22: " << J21 << ", " << J22 << endl; 102 | gsl_vector_free(y); 103 | 104 | /*!*/ 105 | // Build M, M2 106 | gsl_matrix * M = gsl_matrix_calloc(5,5); 107 | gsl_matrix * M2 = gsl_matrix_calloc(6,6); 108 | gsl_matrix * L_k = gsl_matrix_calloc(2,5); 109 | gsl_matrix * L_2k = gsl_matrix_calloc(2,6); 110 | double c, cx, cy, cx1, cx2, cy1, cy2, t1, t2; 111 | double o_theta, T, phi_l, phi_r, w0; 112 | double sm[3]; 113 | 114 | for (int k = 0; k < n; k++) { 115 | jo_read_double(tuple[k], "T", &T); 116 | jo_read_double(tuple[k], "phi_l", &phi_l); 117 | jo_read_double(tuple[k], "phi_r", &phi_r); 118 | jo_read_double_array (tuple[k], "sm", sm, 3, 0.0); 119 | 120 | o_theta = T * (J21*phi_l + J22*phi_r); 121 | w0 = o_theta / T; 122 | 123 | if ( fabs(o_theta) > 1e-12 ) { 124 | t1 = ( sin(o_theta) / (o_theta) ); 125 | t2 = ( (1-cos(o_theta)) / (o_theta) ); 126 | } 127 | else { 128 | t1 = 1; 129 | t2 = 0; 130 | } 131 | cx1 = 0.5 * T * (-J21 * phi_l) * t1; 132 | cx2 = 0.5 * T * (+J22 * phi_r) * t1; 133 | cy1 = 0.5 * T * (-J21 * phi_l) * t2; 134 | cy2 = 0.5 * T * (+J22 * phi_r) * t2; 135 | if ( (params.mode == 0)||(params.mode == 1) ) { 136 | cx = cx1 + cx2; 137 | cy = cy1 + cy2; 138 | double array[] = { 139 | -cx, 1-cos(o_theta), sin(o_theta), sm[0], -sm[1], 140 | -cy, -sin(o_theta), 1-cos(o_theta), sm[1], sm[0] 141 | }; 142 | gsl_matrix_view tmp = gsl_matrix_view_array(array, 2, 5); 143 | L_k = &tmp.matrix; 144 | // M = M + L_k' * L_k; M is symmetric 145 | gsl_blas_dgemm (CblasTrans,CblasNoTrans, 1, L_k, L_k, 1, M); 146 | } 147 | else { 148 | double array2[] = { 149 | -cx1, -cx2, 1-cos(o_theta), sin(o_theta), sm[0], -sm[1], 150 | -cy1, -cy2, -sin(o_theta), 1-cos(o_theta), sm[1], sm[0] 151 | }; 152 | gsl_matrix_view tmp = gsl_matrix_view_array(array2, 2, 6); 153 | L_2k = &tmp.matrix; 154 | // M2 = M2 + L_2k' * L_2k; M2 is symmetric 155 | gsl_blas_dgemm (CblasTrans,CblasNoTrans, 1, L_2k, L_2k, 1, M2); 156 | } 157 | } 158 | 159 | double est_b, est_d_l, est_d_r, laser_x, laser_y, laser_th; 160 | gsl_vector * x; 161 | switch (params.mode) 162 | { 163 | case 0: /*!*/ 164 | { 165 | x = full_calibration_min(M); 166 | 167 | est_b = vg(x, 0); 168 | est_d_l = 2*(-est_b * J21); 169 | est_d_r = 2*(est_b * J22); 170 | laser_x = vg(x, 1); 171 | laser_y = vg(x, 2); 172 | laser_th = atan2(vg(x, 4), vg(x, 3)); 173 | break; 174 | } 175 | case 1: /*!*/ 176 | { 177 | x = numeric_calibration(M); 178 | 179 | est_b = vg(x, 0); 180 | est_d_l = 2*(-est_b * J21); 181 | est_d_r = 2*(est_b * J22); 182 | laser_x = vg(x, 1); 183 | laser_y = vg(x, 2); 184 | laser_th = atan2(vg(x, 4), vg(x, 3)); 185 | break; 186 | } 187 | case 2: /*!*/ 188 | { 189 | x = numeric_calibration(M2); 190 | 191 | est_b = (vg(x, 0) + vg(x, 1))/2; 192 | est_d_l = 2*(-est_b * J21); 193 | est_d_r = 2*(est_b * J22); 194 | laser_x = vg(x, 2); 195 | laser_y = vg(x, 3); 196 | laser_th = atan2(vg(x, 5), vg(x, 4)); 197 | break; 198 | } 199 | default: 200 | break; 201 | } 202 | 203 | /*!*/ 204 | // gsl_matrix * M3 = gsl_matrix_calloc(6,6); 205 | // gsl_matrix * L_3k = gsl_matrix_calloc(2,6); 206 | // double dx1, dx2, dy1, dy2; 207 | // double J11, J12; 208 | // 209 | // for (int k = 0; k < n; k++) { 210 | // jo_read_double(tuple[k], "T", &T); 211 | // jo_read_double(tuple[k], "phi_l", &phi_l); 212 | // jo_read_double(tuple[k], "phi_r", &phi_r); 213 | // jo_read_double_array (tuple[k], "sm", sm, 3, 0.0); 214 | // 215 | // o_theta = sm[2]; 216 | // w0 = o_theta / T; 217 | // 218 | // if ( fabs(o_theta) > 1e-12 ) { 219 | // t1 = ( sin(o_theta) / (o_theta) ); 220 | // t2 = ( (1-cos(o_theta)) / (o_theta) ); 221 | // } 222 | // else { 223 | // t1 = 1; 224 | // t2 = 0; 225 | // } 226 | // dx1 = T * ( phi_l) * t1; 227 | // dx2 = T * ( phi_r) * t1; 228 | // dy1 = T * ( phi_l) * t2; 229 | // dy2 = T * ( phi_r) * t2; 230 | // double array3[] = { 231 | // -dx1, -dx2, 1-cos(o_theta), sin(o_theta), sm[0], -sm[1], 232 | // -dy1, -dy2, -sin(o_theta), 1-cos(o_theta), sm[1], sm[0] 233 | // }; 234 | // gsl_matrix_view tmp = gsl_matrix_view_array(array3, 2, 6); 235 | // L_3k = &tmp.matrix; 236 | // gsl_blas_dgemm (CblasTrans,CblasNoTrans, 1, L_3k, L_3k, 1, M3); 237 | // 238 | // } 239 | // gsl_vector * x3; 240 | // x3 = numeric_calibration(M3); 241 | // J11 = vg(x3,0); 242 | // J12 = vg(x3,1); 243 | // cout << "J11,J12: " << J11 << ", " << J12 << endl; 244 | // double axle2 = 2*((J11+J12)/(J22-J21)); 245 | // cout << " new d_l = " << J11*4 << endl; 246 | // cout << " new d_r = " << J12*4 << endl; 247 | // cout << "TEST\n"; 248 | // cout << " axle " << axle2 << endl; 249 | // cout << " l_diam " << 2*J11 - J21*axle2 << endl; 250 | // cout << " r_diam " << 2*J12 + J22*axle2 << endl; 251 | // cout << " l_x " << vg(x3,2) << endl; 252 | // cout << " l_y " << vg(x3,3) << endl; 253 | // cout << " l_th " << atan2(vg(x3, 5), vg(x3, 4)) << endl; 254 | 255 | /*!*/ 256 | // Build json object 257 | struct json_object *new_obj; 258 | new_obj = json_object_new_object(); 259 | 260 | double est_E_d = est_d_r / est_d_l; 261 | double est_E_b = est_b / 0.09; 262 | json_object_object_add(new_obj, "axle", json_object_new_double(est_b) ); 263 | json_object_object_add(new_obj, "l_diam", json_object_new_double(est_d_l) ); 264 | json_object_object_add(new_obj, "r_diam", json_object_new_double(est_d_r) ); 265 | json_object_object_add(new_obj, "l_x", json_object_new_double(laser_x) ); 266 | json_object_object_add(new_obj, "l_y", json_object_new_double(laser_y) ); 267 | json_object_object_add(new_obj, "l_theta", json_object_new_double(laser_th) ); 268 | json_object_object_add(new_obj, "E_d", json_object_new_double(est_E_d) ); 269 | json_object_object_add(new_obj, "E_b", json_object_new_double(est_E_b) ); 270 | 271 | // Write json object to file 272 | FILE * output; 273 | if(!strcmp(params.output_file, "stdout")) { output = stdout;} 274 | else {output = fopen(params.output_file, "w");} 275 | fprintf(output, "%s\n", json_object_to_json_string(new_obj) ); 276 | fclose(output); 277 | return 0; 278 | } 279 | 280 | 281 | -------------------------------------------------------------------------------- /src/solver2.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file solver.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Estimates odometry and sensor parameters 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "aux_functions.h" 20 | #include "solver_options.h" 21 | #include "solver_utils.h" 22 | 23 | #include 24 | #include 25 | 26 | 27 | #include "calib_tuple.h" 28 | 29 | 30 | 31 | extern void solver_options(solver_params*p, struct option*ops); 32 | int solve_jackknife(const std::vector & tuples, int mode, double max_cond_number, struct calib_result& res); 33 | int solve(const std::vector & tuple, int mode, double max_cond_number, struct calib_result& res); 34 | void estimate_noise(std::vector&tuples, const calib_result&res, double&std_x,double&std_y,double&std_th); 35 | 36 | 37 | const char* banner = 38 | "This program estimates parameters (odometry and sensor) given a list of tuples generated by synchronizer.\n"; 39 | 40 | /*! 41 | Solver routine 42 | \brief This program has been developed to estimate parameters (odometry and sensor) given a list of tuples generated by synchronizer. 43 | 44 | It uses gsl libraries to handle matrix and vectors, as well as minimization and eigenvalues solution. 45 | 46 | */ 47 | int main(int argc, const char * argv[]) { 48 | sm_set_program_name(argv[0]); 49 | 50 | /*!*/ 51 | solver_params params; 52 | struct option* ops = options_allocate(100); 53 | solver_options(¶ms, ops); 54 | options_banner(banner); 55 | if(!options_parse_args(ops, argc, argv)) { 56 | options_print_help(ops, stderr); 57 | return 1; 58 | } 59 | 60 | if(strlen(params.output_file)==0) { 61 | char base[PATH_MAX]; 62 | my_no_suffix(params.input_file, base); 63 | char buf[PATH_MAX]; 64 | sprintf(buf, "%s_results.json", base); 65 | params.output_file = my_strdup(buf); 66 | sm_info("No output filename given; writing on file '%s'.\n", params.output_file); 67 | } 68 | 69 | if(params.outliers_percentage <= 0 || params.outliers_percentage > 0.5) { 70 | sm_error("Outlier percentage %f does not seem a sane value.\n", params.outliers_percentage); 71 | return 2; 72 | } 73 | 74 | sm_debug_write(params.debug); 75 | 76 | 77 | /*!*/ 78 | vector jos; 79 | if (!read_jsonlist(params.input_file, &jos) ) { 80 | sm_error("Error opening input files.\n"); 81 | return 2; 82 | } 83 | 84 | vector tuples; 85 | for(int i=0;i original_tuples = tuples; 95 | 96 | vector tuple_history[params.outliers_iterations+1]; 97 | 98 | calib_result res; 99 | 100 | for(int iteration=0;iteration<=params.outliers_iterations;iteration++) { 101 | tuple_history[iteration] = tuples; 102 | 103 | // Calibration 104 | if(!solve(tuples, params.mode, params.max_cond_number, res)) { 105 | sm_error("Failed calibration.\n"); 106 | return 3; 107 | } 108 | 109 | // Compute residuals 110 | for(int i=0;i err_theta; 115 | for(int i=0;i err_xy; 119 | for(int i=0;i err_theta_sorted(err_theta); 126 | sort( err_theta_sorted.begin(), err_theta_sorted.end() ); 127 | 128 | vector err_xy_sorted(err_xy); 129 | sort( err_xy_sorted.begin(), err_xy_sorted.end() ); 130 | 131 | int threshold_index = (int) round ( (1-params.outliers_percentage) * tuples.size() ); 132 | if(threshold_index<0) threshold_index = 0; 133 | if(threshold_index>=tuples.size()) threshold_index = tuples.size()-1; 134 | 135 | double threshold_theta = err_theta_sorted[threshold_index]; 136 | double threshold_xy = err_xy_sorted[threshold_index]; 137 | 138 | int noutliers = 0; 139 | int noutliers_theta = 0; 140 | int noutliers_xy = 0; 141 | int noutliers_both = 0; 142 | 143 | for(int i=0;i threshold_xy; 145 | int theta = err_theta[i] > threshold_theta; 146 | 147 | tuples[i].mark_as_outlier = xy | theta; 148 | 149 | if(xy) noutliers_xy ++; 150 | if(theta) noutliers_theta ++; 151 | if(xy&&theta) noutliers_both ++; 152 | if(xy||theta) noutliers ++; 153 | } 154 | 155 | // Output statistics 156 | sm_info("Outlier detection: #tuples = %d, removing percentage = %.0f%% (index=%d)\n", tuples.size(), 157 | 100*params.outliers_percentage, threshold_index); 158 | sm_info(" err_theta min = %f max = %f threshold = %f \n", 159 | err_theta[0], err_theta[tuples.size()-1], threshold_theta); 160 | sm_info(" err_xy min = %f max = %f threshold = %f \n", 161 | err_xy[0], err_xy[tuples.size()-1], threshold_xy); 162 | 163 | sm_info(" num_outliers = %d; for theta = %d; for xy = %d; for both = %d\n", noutliers, noutliers_theta, noutliers_xy, noutliers_both); 164 | 165 | 166 | // Write debug info 167 | char filename[256]; 168 | char base[255]; 169 | my_no_suffix(params.output_file, base); 170 | 171 | sprintf(filename, "%s_iter%d.asc", base, iteration); 172 | ofstream ofs(filename); 173 | sm_info("%s\n",filename); 174 | if(!ofs) sm_error("could not open '%s'\n",filename); 175 | for(int i=0;i n; 181 | for(int i=0;i 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "aux_functions.h" 14 | #include "solver_options.h" 15 | #include "solver_utils.h" 16 | 17 | #include 18 | #include 19 | 20 | 21 | #include "calib_tuple.h" 22 | 23 | #define vg(X,Y) ( gsl_vector_get(X,Y) ) 24 | #define vs(X,Y,Z) ( gsl_vector_set(X,Y,Z) ) 25 | #define mg(X,Y,Z) ( gsl_matrix_get(X,Y,Z) ) 26 | #define ms(X,Y,Z,Q) ( gsl_matrix_set(X,Y,Z,Q) ) 27 | 28 | using namespace std; 29 | 30 | 31 | 32 | double f (const gsl_vector *z, void *params); 33 | 34 | 35 | int solve(const vector & tuple, int mode, double max_cond_number, struct calib_result& res) { 36 | 37 | /*!*/ 38 | double J21, J22; 39 | gsl_matrix * A = gsl_matrix_calloc(2,2); 40 | gsl_vector * g = gsl_vector_calloc(2); 41 | gsl_vector * L_i = gsl_vector_alloc(2); 42 | double th_i; 43 | int n = (int) tuple.size(); 44 | 45 | for (int i = 0; i < n; i++) { 46 | const calib_tuple &t = tuple[i]; 47 | vs( L_i, 0, t.T*t.phi_l); 48 | vs( L_i, 1, t.T*t.phi_r); 49 | gsl_blas_dger (1, L_i, L_i, A); // A = A + L_i'*L_i; A symmetric 50 | gsl_blas_daxpy (t.sm[2], L_i, g); // g = g + L_i'*y_i; sm :{x , y, theta} 51 | } 52 | gsl_vector_free(L_i); 53 | 54 | // Verify that A isn't singular 55 | double cond = cond_number(A); 56 | if ( cond > max_cond_number ) { 57 | sm_error("Matrix A is almost singular (condition number: %f). Not very exciting data!\n", cond); 58 | return 0; 59 | } 60 | 61 | // Ay = g --> y = inv(A)g; A square matrix; 62 | gsl_vector * y = gsl_vector_calloc(2); 63 | if (!solve_square_linear(A, g, y)) { 64 | sm_error("Cannot solve Ay=g. Invalid arguments\n"); 65 | return 0; 66 | } 67 | J21 = vg(y, 0); J22 = vg(y, 1); 68 | if ( gsl_isnan(J21) || gsl_isnan(J22) ) { 69 | sm_error("Could not find first two parameters J21, J22 by solving linear equation.\n"); 70 | sm_error("This is A: \n\n"); gsl_matrix_fwrite(stderr, A); fprintf(stderr, "\n"); 71 | sm_error("This is g: \n\t"); gsl_vector_fwrite(stderr, g); fprintf(stderr, "\n"); 72 | sm_error("This is y: \n\t"); gsl_vector_fwrite(stderr, y); fprintf(stderr, "\n"); 73 | return 0; 74 | } 75 | // cerr << "J21,J22: " << J21 << ", " << J22 << endl; 76 | gsl_vector_free(y); 77 | 78 | /*!*/ 79 | // Build M, M2 80 | gsl_matrix * M = gsl_matrix_calloc(5,5); 81 | gsl_matrix * M2 = gsl_matrix_calloc(6,6); 82 | gsl_matrix * L_k = gsl_matrix_calloc(2,5); 83 | gsl_matrix * L_2k = gsl_matrix_calloc(2,6); 84 | double c, cx, cy, cx1, cx2, cy1, cy2, t1, t2; 85 | double o_theta, T, phi_l, phi_r, w0; 86 | double sm[3]; 87 | 88 | int nused = 0; 89 | for (int k = 0; k < n; k++) { 90 | const calib_tuple &t = tuple[k]; 91 | 92 | o_theta = t.T * (J21*t.phi_l + J22*t.phi_r); 93 | w0 = o_theta / t.T; 94 | 95 | /*if( fabs(o_theta) > deg2rad(1) ) { 96 | continue; 97 | } else { 98 | nused ++; 99 | }*/ 100 | 101 | if ( fabs(o_theta) > 1e-12 ) { 102 | t1 = ( sin(o_theta) / (o_theta) ); 103 | t2 = ( (1-cos(o_theta)) / (o_theta) ); 104 | } 105 | else { 106 | t1 = 1; 107 | t2 = 0; 108 | } 109 | cx1 = 0.5 * t.T * (-J21 * t.phi_l) * t1; 110 | cx2 = 0.5 * t.T * (+J22 * t.phi_r) * t1; 111 | cy1 = 0.5 * t.T * (-J21 * t.phi_l) * t2; 112 | cy2 = 0.5 * t.T * (+J22 * t.phi_r) * t2; 113 | if ( (mode == 0)||(mode == 1) ) { 114 | cx = cx1 + cx2; 115 | cy = cy1 + cy2; 116 | double array[] = { 117 | -cx, 1-cos(o_theta), sin(o_theta), t.sm[0], -t.sm[1], 118 | -cy, -sin(o_theta), 1-cos(o_theta), t.sm[1], t.sm[0] 119 | }; 120 | gsl_matrix_view tmp = gsl_matrix_view_array(array, 2, 5); 121 | L_k = &tmp.matrix; 122 | // M = M + L_k' * L_k; M is symmetric 123 | gsl_blas_dgemm (CblasTrans,CblasNoTrans, 1, L_k, L_k, 1, M); 124 | } 125 | else { 126 | double array2[] = { 127 | -cx1, -cx2, 1-cos(o_theta), sin(o_theta), t.sm[0], -t.sm[1], 128 | -cy1, -cy2, -sin(o_theta), 1-cos(o_theta), t.sm[1], t.sm[0] 129 | }; 130 | gsl_matrix_view tmp = gsl_matrix_view_array(array2, 2, 6); 131 | L_2k = &tmp.matrix; 132 | // M2 = M2 + L_2k' * L_2k; M2 is symmetric 133 | gsl_blas_dgemm (CblasTrans,CblasNoTrans, 1, L_2k, L_2k, 1, M2); 134 | } 135 | } 136 | 137 | // sm_info("Andrea's hack: using %d / %d\n", nused, n); 138 | 139 | double est_b, est_d_l, est_d_r, laser_x, laser_y, laser_th; 140 | gsl_vector * x; 141 | switch (mode) 142 | { 143 | case 0: /*!*/ 144 | { 145 | x = full_calibration_min(M); 146 | 147 | est_b = vg(x, 0); 148 | est_d_l = 2*(-est_b * J21); 149 | est_d_r = 2*(est_b * J22); 150 | laser_x = vg(x, 1); 151 | laser_y = vg(x, 2); 152 | laser_th = atan2(vg(x, 4), vg(x, 3)); 153 | break; 154 | } 155 | case 1: /*!*/ 156 | { 157 | x = numeric_calibration(M); 158 | 159 | est_b = vg(x, 0); 160 | est_d_l = 2*(-est_b * J21); 161 | est_d_r = 2*(est_b * J22); 162 | laser_x = vg(x, 1); 163 | laser_y = vg(x, 2); 164 | laser_th = atan2(vg(x, 4), vg(x, 3)); 165 | break; 166 | } 167 | case 2: /*!*/ 168 | { 169 | x = numeric_calibration(M2); 170 | 171 | est_b = (vg(x, 0) + vg(x, 1))/2; 172 | est_d_l = 2*(-est_b * J21); 173 | est_d_r = 2*(est_b * J22); 174 | laser_x = vg(x, 2); 175 | laser_y = vg(x, 3); 176 | laser_th = atan2(vg(x, 5), vg(x, 4)); 177 | break; 178 | } 179 | default: 180 | break; 181 | } 182 | 183 | /*!*/ 184 | // gsl_matrix * M3 = gsl_matrix_calloc(6,6); 185 | // gsl_matrix * L_3k = gsl_matrix_calloc(2,6); 186 | // double dx1, dx2, dy1, dy2; 187 | // double J11, J12; 188 | // 189 | // for (int k = 0; k < n; k++) { 190 | // jo_read_double(tuple[k], "T", &T); 191 | // jo_read_double(tuple[k], "phi_l", &phi_l); 192 | // jo_read_double(tuple[k], "phi_r", &phi_r); 193 | // jo_read_double_array (tuple[k], "sm", sm, 3, 0.0); 194 | // 195 | // o_theta = sm[2]; 196 | // w0 = o_theta / T; 197 | // 198 | // if ( fabs(o_theta) > 1e-12 ) { 199 | // t1 = ( sin(o_theta) / (o_theta) ); 200 | // t2 = ( (1-cos(o_theta)) / (o_theta) ); 201 | // } 202 | // else { 203 | // t1 = 1; 204 | // t2 = 0; 205 | // } 206 | // dx1 = T * ( phi_l) * t1; 207 | // dx2 = T * ( phi_r) * t1; 208 | // dy1 = T * ( phi_l) * t2; 209 | // dy2 = T * ( phi_r) * t2; 210 | // double array3[] = { 211 | // -dx1, -dx2, 1-cos(o_theta), sin(o_theta), sm[0], -sm[1], 212 | // -dy1, -dy2, -sin(o_theta), 1-cos(o_theta), sm[1], sm[0] 213 | // }; 214 | // gsl_matrix_view tmp = gsl_matrix_view_array(array3, 2, 6); 215 | // L_3k = &tmp.matrix; 216 | // gsl_blas_dgemm (CblasTrans,CblasNoTrans, 1, L_3k, L_3k, 1, M3); 217 | // 218 | // } 219 | // gsl_vector * x3; 220 | // x3 = numeric_calibration(M3); 221 | // J11 = vg(x3,0); 222 | // J12 = vg(x3,1); 223 | // cout << "J11,J12: " << J11 << ", " << J12 << endl; 224 | // double axle2 = 2*((J11+J12)/(J22-J21)); 225 | // cout << " new d_l = " << J11*4 << endl; 226 | // cout << " new d_r = " << J12*4 << endl; 227 | // cout << "TEST\n"; 228 | // cout << " axle " << axle2 << endl; 229 | // cout << " l_diam " << 2*J11 - J21*axle2 << endl; 230 | // cout << " r_diam " << 2*J12 + J22*axle2 << endl; 231 | // cout << " l_x " << vg(x3,2) << endl; 232 | // cout << " l_y " << vg(x3,3) << endl; 233 | // cout << " l_th " << atan2(vg(x3, 5), vg(x3, 4)) << endl; 234 | 235 | /*!*/ 236 | 237 | res.axle = est_b; 238 | res.radius_l = est_d_l/2; 239 | res.radius_r = est_d_r/2; 240 | res.l[0] = laser_x; 241 | res.l[1] = laser_y; 242 | res.l[2] = laser_th; 243 | 244 | return 1; 245 | 246 | } 247 | 248 | 249 | int solve_jackknife(const vector & tuples, int mode, double max_cond_number, struct calib_result& res) { 250 | 251 | union { 252 | double tmp_params[6]; 253 | struct calib_result tmp_res; 254 | }; 255 | 256 | // check that the union trick is working 257 | tmp_res.radius_l = 0; 258 | tmp_res.radius_r = 1; 259 | tmp_res.axle = 2; 260 | tmp_res.l[0] = 3; 261 | tmp_res.l[1] = 4; 262 | tmp_res.l[2] = 5; 263 | 264 | for(int i=0;i<6;i++) 265 | if(tmp_params[i] != i) { 266 | sm_error("Ooops! the union trick is not working"); 267 | exit(-1); 268 | } 269 | 270 | int n = tuples.size(); 271 | vector minusone; 272 | for(int i=1;i&tuples, const calib_result&res, double&std_x,double&std_y,double&std_th) { 322 | int n = tuples.size(); 323 | double err_sm[3][n]; 324 | for(int i=0;i 9 | #include "solver_options.h" 10 | 11 | void solver_options(solver_params *p, struct option*ops) { 12 | 13 | options_string(ops, "input_file", 14 | &(p->input_file), "", 15 | "Input file containing a list of tuples"); 16 | 17 | options_string(ops, "output_file", 18 | (&p->output_file), "", 19 | "Output file "); 20 | 21 | options_int(ops, "mode", 22 | (&p->mode), 0, 23 | "Minimization method "); 24 | 25 | options_int(ops, "outliers_iterations", (&p->outliers_iterations), 4, 26 | "Number of outliers removal iterations."); 27 | options_double(ops, "outliers_percentage", &(p->outliers_percentage), 0.01, 28 | "Percentage of outliers to reject at each iteration. Double value: 0.01 == 1%%" ); 29 | 30 | options_int(ops, "debug", &(p->debug), 0, "Shows debug information"); 31 | 32 | options_double(ops, "max_cond_number", &(p->max_cond_number), 75, 33 | "Max condition number allowed for the first linear estimation step." ); 34 | 35 | } 36 | -------------------------------------------------------------------------------- /src/solver_options.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file solver_options.h 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief options for the solver executable 5 | */ 6 | 7 | #ifndef _SOLVER_OPTIONS_H 8 | #define _SOLVER_OPTIONS_H 9 | 10 | /*! 11 | \struct solver_params 12 | \brief Solver parameter instance structure 13 | @param input_file filename containing a list of tuples obtained from synchronizer 14 | @param output_file filename for output 15 | @param mode computation mode 16 | */ 17 | typedef struct { 18 | const char * input_file; 19 | const char * output_file; 20 | int mode; 21 | 22 | double max_cond_number; 23 | 24 | int outliers_iterations; 25 | double outliers_percentage; 26 | 27 | int debug; 28 | } solver_params; 29 | 30 | /*! 31 | Initializes parameters for solver procedure 32 | \param p pointer to parameters' structure 33 | \param ops options' structure 34 | */ 35 | void solver_options(solver_params*p, struct option*ops); 36 | 37 | #endif 38 | 39 | -------------------------------------------------------------------------------- /src/solver_utils.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file solver_utils.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Implements methods for Solver_utils 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include "aux_functions.h" 27 | #include "solver_utils.h" 28 | 29 | #define sp(X) ( pow(X,2) ) 30 | #define cp(X) ( pow(X,3) ) 31 | #define fp(X) ( pow(X,4) ) 32 | #define vg(X,Y) ( gsl_vector_get(X,Y) ) 33 | #define vs(X,Y,Z) ( gsl_vector_set(X,Y,Z) ) 34 | #define mg(X,Y,Z) ( gsl_matrix_get(X,Y,Z) ) 35 | #define ms(X,Y,Z,Q) ( gsl_matrix_set(X,Y,Z,Q) ) 36 | 37 | using namespace std; 38 | 39 | double cond_number(gsl_matrix * M) 40 | { 41 | size_t row = M->size1; 42 | size_t col = M->size2; 43 | gsl_matrix * V = gsl_matrix_alloc(col,col); 44 | gsl_matrix * U = gsl_matrix_alloc(row,col); 45 | gsl_matrix_memcpy(U, M); 46 | 47 | //vettore per i valori singolari di A 48 | gsl_vector * S = gsl_vector_alloc( min(row,col) ); 49 | gsl_vector * work = gsl_vector_alloc( min(row,col) ); 50 | gsl_linalg_SV_decomp(U, V, S, work ); 51 | 52 | double cond = gsl_vector_max(S) / gsl_vector_min(S); 53 | 54 | gsl_matrix_free(U); 55 | gsl_matrix_free(V); 56 | gsl_vector_free(work); 57 | gsl_vector_free(S); 58 | return cond; 59 | } 60 | 61 | int solve_square_linear(gsl_matrix * A, gsl_vector * g, gsl_vector * x) 62 | { 63 | size_t sz = A->size1; 64 | if (A->size2 != sz) return 0; 65 | if (x->size != sz) return 0; 66 | if (g->size != sz) return 0; 67 | gsl_matrix * LU = gsl_matrix_alloc(sz,sz); 68 | gsl_permutation * p = gsl_permutation_alloc(sz); 69 | gsl_matrix_memcpy(LU, A); 70 | int signum; 71 | gsl_linalg_LU_decomp (LU, p, &signum ); 72 | gsl_linalg_LU_solve (LU, p, g, x ); 73 | gsl_matrix_free(LU); 74 | gsl_permutation_free(p); 75 | 76 | return 1; 77 | } 78 | 79 | int eigenv(gsl_matrix * M, gsl_vector * eigenvalues, gsl_matrix * eigenvectors) 80 | { 81 | size_t sz2 = M->size2; 82 | size_t sz = M->size1; 83 | if (sz != sz2) return 0; 84 | gsl_eigen_symmv_workspace * wk = gsl_eigen_symmv_alloc(sz); 85 | gsl_matrix *M_tmp = gsl_matrix_alloc(sz, sz); 86 | 87 | gsl_matrix_memcpy(M_tmp, M); 88 | gsl_eigen_symmv(M_tmp, eigenvalues, eigenvectors, wk); 89 | gsl_eigen_gensymmv_sort (eigenvalues, eigenvectors, GSL_EIGEN_SORT_ABS_ASC); 90 | 91 | gsl_matrix_free(M_tmp); 92 | gsl_eigen_symmv_free(wk); 93 | return 1; 94 | } 95 | 96 | double calculate_error(gsl_vector * x, gsl_matrix * M) 97 | { 98 | double error; 99 | gsl_vector * tmp = gsl_vector_calloc( x->size ); 100 | 101 | gsl_blas_dgemv(CblasNoTrans, 1, M, x, 0, tmp); 102 | gsl_blas_ddot(x ,tmp, &error); 103 | 104 | gsl_vector_free(tmp); 105 | return error; 106 | } 107 | 108 | gsl_vector * full_calibration_min(gsl_matrix * M) 109 | { 110 | double m11 = mg(M,0,0); 111 | double m13 = mg(M,0,2); 112 | double m14 = mg(M,0,3); 113 | double m15 = mg(M,0,4); 114 | double m22 = mg(M,1,1); 115 | double m25 = mg(M,1,4); 116 | double m34 = mg(M,2,3); 117 | double m35 = mg(M,2,4); 118 | double m44 = mg(M,3,3); 119 | double m55 = mg(M,4,4); 120 | double a,b,c; 121 | 122 | /* Coefficienti del determinante M + lambda*W */ 123 | a = m11 * sp(m22) - m22 * sp(m13); 124 | 125 | b = 2 * m11 * sp(m22) * m44 - sp(m22) * sp(m14) 126 | - 2 * m22 * sp(m13) * m44 - 2 * m11 * m22 * sp(m34) 127 | - 2 * m11 * m22 * sp(m35) - sp(m22) * sp(m15) 128 | + 2 * m13 * m22 * m34 * m14 + sp(m13) * sp(m34) 129 | + 2 * m13 * m22 * m35 * m15 + sp(m13) * sp(m35); 130 | 131 | c = - 2 * m13 * cp(m35) * m15 - m22 * sp(m13) * sp(m44) + m11 * sp(m22) * sp(m44) 132 | + sp(m13) * sp(m35) * m44 + 2 * m13 * m22 * m34 * m14 * m44 + sp(m13) * sp(m34) * m44 133 | - 2 * m11 * m22 * sp(m34) * m44 - 2 * m13 * cp(m34) * m14 - 2 * m11 * m22 * sp(m35) * m44 134 | + 2 * m11 * sp(m35) * sp(m34) + m22 * sp(m14) * sp(m35) - 2 * m13 * sp(m35) * m34 * m14 135 | - 2 * m13 * sp(m34) * m35 * m15 + m11 * fp(m34) + m22 * sp(m15) * sp(m34) 136 | + m22 * sp(m35) * sp(m15) + m11 * fp(m35) - sp(m22) * sp(m14) * m44 137 | + 2 * m13 * m22 * m35 * m15 * m44 + m22 * sp(m34) * sp(m14) - sp(m22) * sp(m15) * m44; 138 | 139 | /* Calcolo radice del polinomio */ 140 | gsl_complex * r0 = new gsl_complex; 141 | gsl_complex * r1 = new gsl_complex; 142 | 143 | gsl_poly_complex_solve_quadratic(a, b, c, r0, r1); 144 | 145 | if ( (r0->dat[1] == 0) ) { //|| (GSL_IMAG(r1) == 0)) { 146 | /* Costruzione matrice W */ 147 | gsl_matrix * W = gsl_matrix_calloc(5,5); 148 | gsl_matrix_set(W,3,3,1); 149 | gsl_matrix_set(W,4,4,1); 150 | gsl_vector * x0 = x_given_lambda(M, r0->dat[0], W); 151 | gsl_vector * x1 = x_given_lambda(M, r1->dat[0], W); 152 | gsl_matrix_free(W); 153 | 154 | double e0 = calculate_error(x0, M); 155 | double e1 = calculate_error(x1, M); 156 | 157 | return e0 < e1 ? x0 : x1; 158 | } 159 | else { 160 | sm_error("bad thing: imaginary solution\n"); 161 | return NULL; 162 | } 163 | } 164 | 165 | gsl_vector * x_given_lambda(gsl_matrix * M, double lambda, gsl_matrix * W) 166 | { 167 | gsl_matrix * Z = gsl_matrix_calloc(5,5); 168 | gsl_matrix * ZZ = gsl_matrix_calloc(5,5); 169 | 170 | // Z = M + lamda*W 171 | gsl_matrix_memcpy(Z, W); 172 | gsl_matrix_scale (Z, lambda); 173 | gsl_matrix_add (Z, M); 174 | 175 | gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, Z, Z, 0, ZZ); /* ZZ = Z * Z' */ 176 | /** Calculate eigenvalues and eigenvectors */ 177 | gsl_vector * eigenvalues = gsl_vector_alloc(5); 178 | gsl_matrix * eigenvectors = gsl_matrix_alloc(5,5); 179 | eigenv(ZZ, eigenvalues, eigenvectors); 180 | gsl_vector * v0 = gsl_vector_alloc(5); 181 | gsl_matrix_get_col(v0, eigenvectors, gsl_vector_min_index(eigenvalues) ); 182 | 183 | /** Conditions: x1 > 0; x4^2 + x5^2 = 1 */ 184 | gsl_vector * tmp_v = gsl_vector_alloc(2); 185 | vs(tmp_v ,0, vg(v0, 3)); 186 | vs(tmp_v ,1, vg(v0, 4)); 187 | double norm = gsl_blas_dnrm2(tmp_v); 188 | double coeff = ( GSL_SIGN( vg(v0,0) ) / norm ); 189 | gsl_vector_scale(v0 ,coeff); 190 | 191 | gsl_matrix_free(Z); 192 | gsl_matrix_free(ZZ); 193 | gsl_matrix_free(eigenvectors); 194 | gsl_vector_free(eigenvalues); 195 | gsl_vector_free(tmp_v); 196 | return v0; 197 | } 198 | 199 | int fminimize_simplex(gsl_vector * init, gsl_vector * min_found, gsl_matrix * Q, double (*f) (const gsl_vector *z, void *params) ) 200 | { 201 | int sz = (int) init->size; 202 | if ( (Q->size1 != sz) || (Q->size2 != sz) ) return 0; 203 | const gsl_multimin_fminimizer_type * T = gsl_multimin_fminimizer_nmsimplex; 204 | gsl_multimin_fminimizer * s = gsl_multimin_fminimizer_alloc (T, sz); 205 | 206 | void *par = Q->data; 207 | gsl_multimin_function func; 208 | func.n = sz; 209 | func.f = f; 210 | func.params = par; 211 | gsl_vector *step = gsl_vector_alloc(sz); 212 | gsl_vector_set_all(step, 1e-12); 213 | 214 | int iter = 0; 215 | double size; 216 | int status; 217 | gsl_multimin_fminimizer_set (s, &func, init, step); 218 | do { 219 | iter++; 220 | status = gsl_multimin_fminimizer_iterate (s); 221 | if (status) break; 222 | 223 | size = gsl_multimin_fminimizer_size (s); 224 | status = gsl_multimin_test_size(size, 1e-8); 225 | 226 | if (status == GSL_SUCCESS) { 227 | sm_debug("Minimum found at:\n"); 228 | for (int i = 0; i < sz; i++) 229 | sm_debug("%f \n", vg(s->x, i)); 230 | } 231 | } while (status == GSL_CONTINUE && iter < 2000); 232 | 233 | if (status != GSL_SUCCESS) cout << "Symplex method: Maximum iteration reached before the minimum is found\n"; 234 | gsl_vector_memcpy( min_found, gsl_multimin_fminimizer_x(s) ); 235 | gsl_multimin_fminimizer_free (s); 236 | return 1; 237 | } 238 | 239 | gsl_vector * numeric_calibration(gsl_matrix * H) { 240 | int sz = (int)(H->size1); 241 | gsl_vector * eigenvalues = gsl_vector_alloc(sz); 242 | gsl_matrix * eigenvectors = gsl_matrix_alloc(sz,sz); 243 | eigenv(H, eigenvalues, eigenvectors); 244 | 245 | gsl_vector * v0 = gsl_vector_alloc(sz); 246 | gsl_matrix_get_col(v0, eigenvectors, sz-5); /*Starting point for numerical search*/ 247 | 248 | gsl_vector * tmp_v = gsl_vector_alloc(2); 249 | vs(tmp_v , 0, vg(v0, sz-2)); 250 | vs(tmp_v , 1, vg(v0, sz-1)); 251 | double norm = gsl_blas_dnrm2(tmp_v); 252 | double coeff = ( GSL_SIGN( vg(v0,0) ) / norm ); 253 | gsl_vector_scale(v0 ,coeff); 254 | gsl_vector_free(tmp_v); 255 | 256 | gsl_vector * min = gsl_vector_alloc(sz); /*vector where minimum will be stored*/ 257 | fminimize_simplex(v0, min, H, &f); 258 | 259 | return min; 260 | } 261 | 262 | /** 263 | Function f(z) = [z(1) z(2) ... z(n)]' * M * [z(1) z(2) ... z(n)]. 264 | Used for minimization in solver routine 265 | */ 266 | double f (const gsl_vector *z, void *params) { 267 | int sz = (int) z->size; 268 | gsl_vector * v = gsl_vector_alloc(sz); 269 | gsl_matrix * M = gsl_matrix_alloc(sz, sz); 270 | gsl_vector_memcpy(v, z); 271 | 272 | double *p = (double *)params; 273 | M->data = p; 274 | double * res = new double; 275 | gsl_vector * tmp = gsl_vector_calloc(sz); 276 | gsl_blas_dgemv(CblasNoTrans, 1, M, v, 0, tmp); 277 | gsl_blas_ddot(v ,tmp, res); 278 | gsl_vector_free(tmp); 279 | 280 | return( res[0] ); 281 | } 282 | 283 | 284 | -------------------------------------------------------------------------------- /src/solver_utils.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file solver_utils.h 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Header of Solver_utils 5 | */ 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "aux_functions.h" 14 | 15 | using namespace std; 16 | 17 | /*! 18 | \brief calculates condition number for a matrix 19 | @param M gsl_matrix pointer 20 | @return condition number of matrix M 21 | */ 22 | double cond_number(gsl_matrix * M); 23 | 24 | /*! 25 | \brief Solves linear system \f$ Ax=g \f$, where A is a square matrix \f$ ( x = inv(A)g ) \f$ 26 | @param A gsl_matrix pointer 27 | @param g gsl_vector pointer 28 | @param x gsl_vector pointer to store result 29 | @return 1 if succesfull, 0 otherwhise 30 | */ 31 | int solve_square_linear(gsl_matrix * A, gsl_vector * g, gsl_vector * x); 32 | /*! 33 | Compute eigenvalues and eigenvectors of a symmetric matrix M 34 | */ 35 | /*! 36 | \brief Calculates eigenvalues and eigenvectors (sorted in ascending order) of a symmetric matrix 37 | @param M gsl_matrix pointer 38 | @param eigenvalues gsl_vector pointer 39 | @param eigenvectors gsl_vector pointer to store eigenvectors 40 | @return 1 if succesfull, 0 otherwise 41 | */ 42 | int eigenv(gsl_matrix * M, gsl_vector * eigenvalues, gsl_matrix * eigenvectors); 43 | 44 | /*! 45 | \brief Calculates values of a quadratic function \f$ x'Mx \f$ for a given x 46 | @param x gsl_vector pointer 47 | @param M gsl_matrix pointer 48 | @return value of function 49 | */ 50 | double calculate_error(gsl_vector * x, gsl_matrix * M); 51 | 52 | /*! 53 | \brief Calculate x solution of \f$ (M + \lambda W)x = 0 \f$; \lambda such that \f$ (M + \lambda W) \f$ is singular 54 | @param M gsl_matrix pointer 55 | @param lambda gsl_vector pointer 56 | @param W gsl_matrix pointer 57 | @return pointer to gsl_vector storing solution 58 | \note Caller must deallocate returned pointer 59 | */ 60 | gsl_vector * x_given_lambda(gsl_matrix * M, double lambda, gsl_matrix * W); 61 | 62 | /*! 63 | \brief Solves \f$ (M + \lambda W)x = 0 \f$, M is a square matrix, \f$ W = \left[ \begin{array}{cc} 0_{3x3} & 0_{3x2} \\ 0_{2x3} & I_{2x2} \end{array} \right] \f$ 64 | @param M gsl_matrix pointer 65 | @return pointer to gsl_vector storing solution 66 | \note Caller must deallocate returned pointer 67 | */ 68 | gsl_vector * full_calibration_min(gsl_matrix * M); 69 | 70 | /*! 71 | \brief Numerically search minimum for a function \f$ x'Qx \f$, starting from a specified point, using symplex method 72 | @param init initial point for minimization 73 | @param min_found pointer to gsl_vector to store result 74 | @param Q pointer to gsl_matrix 75 | @param double(*f) is a pointer to function 76 | @return 1 if successfull, 0 otherwhise 77 | */ 78 | int fminimize_simplex(gsl_vector * init, gsl_vector * min_found, gsl_matrix * Q, double (*f) (const gsl_vector *z, void *params) ); 79 | 80 | /*! 81 | \brief Uses symplex method to estimate parameters 82 | @param H pointer to matrix of quadratic function (\f$ x'Hx \f$) 83 | @return pointer to gsl_vector storing minimum 84 | */ 85 | gsl_vector * numeric_calibration(gsl_matrix * H); 86 | 87 | /*! 88 | \brief Implements a quadratic function (used in minimization functions) 89 | @param z pointer to point where the function has to be evaluated 90 | @param params pointer to parameters vector 91 | */ 92 | double f (const gsl_vector *z, void *params); 93 | 94 | 95 | -------------------------------------------------------------------------------- /src/synchronizer.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file synchronizer.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief synchronizes an odometry log and a laser log 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "synchronizer_options.h" 18 | #include "aux_functions.h" 19 | 20 | using namespace std; 21 | using namespace CSM; 22 | 23 | extern void synchro_options(synchro_params*p, struct option*ops); 24 | 25 | int main(int argc, const char * argv[]) { 26 | sm_set_program_name("Synchronizer"); 27 | 28 | //! 29 | synchro_params params; 30 | struct option* ops = options_allocate(100); 31 | synchro_options(¶ms, ops); 32 | if(!options_parse_args(ops, argc, argv)) { 33 | options_print_help(ops, stderr); 34 | return -1; 35 | } 36 | 37 | //! 38 | vector laser; 39 | vector odo; 40 | vector result; 41 | if (!read_jsonlist(params.las_file, &laser) ) { 42 | sm_error("Error reading laser_file '%s'.\n", params.las_file); 43 | return -1; 44 | } 45 | if (!read_jsonlist(params.odo_file, &odo) ) { 46 | sm_error("Error reading odometry file '%s'.\n", params.odo_file); 47 | return -1; 48 | } 49 | 50 | 51 | //! 52 | sm_info("Starting synchronization\n"); 53 | int n = (int) laser.size(); 54 | 55 | int discarded_valid = 0; 56 | int discarded_error = 0; 57 | int discarded_nvalid = 0; 58 | int discarded_t_k = 0; 59 | int discarded_time_tol = 0; 60 | int discarded_no_odo = 0; 61 | int still_num = 0; 62 | 63 | //! 64 | double mean_error[n]; 65 | double mean_error_sorted[n]; 66 | int nvalid_sorted[n]; 67 | double perc = params.perc_threshold; 68 | 69 | for(int i = 0; i < n; i++) { 70 | double error; 71 | int nvalid; 72 | 73 | jo_read_double(laser[i], "error", &error); 74 | jo_read_int(laser[i], "nvalid", &nvalid); 75 | mean_error[i] = error/nvalid; 76 | mean_error_sorted[i] = mean_error[i]; 77 | nvalid_sorted[i] = nvalid; 78 | } 79 | 80 | mysort(mean_error_sorted, n); 81 | mysort(nvalid_sorted, n); 82 | double mean_error_threshold = mean_error_sorted[ (int)round(perc*n)]; 83 | // cout << "mean_error_threshold " << mean_error_threshold << endl; 84 | int nvalid_threshold = nvalid_sorted[ (int)round((1 - perc)*n)]; 85 | 86 | //! 87 | int m = 0; 88 | int last = 0; 89 | int jb_ref, jb_sens; 90 | double error, t_k, left_speed, right_speed; 91 | int nvalid, valid; 92 | double tr1, tr2, ts1, ts2; 93 | double Lr1, Lr2, Ls1, Ls2; 94 | double Rr1, Rr2, Rs1, Rs2; 95 | double laser_ref_ts, laser_sens_ts, t_delta; 96 | double times[4]; 97 | double time_tol; 98 | double ref_alpha, sens_alpha, avg_left_start, avg_right_start, 99 | avg_left_end, avg_right_end, left_inc, right_inc; 100 | 101 | double ticks_to_rad = 2 * M_PI / ( 691.2 * 4 ); 102 | for (int k = 0; k < n; k++) { 103 | valid = jo_read_int(laser[k], "valid", &valid); 104 | if (valid == 0) { 105 | discarded_valid = discarded_valid + 1; 106 | continue; 107 | } 108 | 109 | /* 110 | for each entry in raw_sm we seek the two indices 111 | which are immediately before and after in raw_odo 112 | */ 113 | t_delta = params.laser_timestamp_correction; 114 | laser_ref_ts = t_delta + gettime(laser[k], "laser_ref_timestamp"); 115 | laser_sens_ts = t_delta + gettime(laser[k], "laser_sens_timestamp"); 116 | 117 | jb_ref = find_closest_ref(laser_ref_ts, odo, last); 118 | jb_sens = find_closest_sens(laser_sens_ts, odo, last); 119 | // jb_ref = find_closest(laser_ref_ts, odo, last); 120 | // jb_sens = find_closest(laser_sens_ts, odo, last); 121 | 122 | if ( (jb_ref == -1) || (jb_sens == -1) ) { 123 | discarded_no_odo = discarded_no_odo + 1; 124 | sm_info("merge_logs: Could not match timestamp %f %f\n", laser_ref_ts, laser_sens_ts); 125 | continue; 126 | } 127 | last = jb_ref; 128 | 129 | tr1 = gettime(odo[jb_ref] , "timestamp"); 130 | tr2 = gettime(odo[jb_ref +1] , "timestamp"); 131 | ts1 = gettime(odo[jb_sens -1] , "timestamp"); 132 | ts2 = gettime(odo[jb_sens] , "timestamp"); 133 | 134 | times[0] = fabs( tr1 - laser_ref_ts ); 135 | times[1] = fabs( tr2 - laser_ref_ts ); 136 | times[2] = fabs( ts1 - laser_sens_ts ); 137 | times[3] = fabs( ts2 - laser_sens_ts ); 138 | mysort(times, 4); 139 | time_tol = times[3]; 140 | 141 | ref_alpha = (laser_ref_ts - tr1) / (tr2 - tr1); 142 | sens_alpha = (laser_sens_ts - ts1) /(ts2 - ts1); 143 | 144 | Lr1 = getticks(odo[jb_ref] , "left"); 145 | Lr2 = getticks(odo[jb_ref+1] , "left"); 146 | Ls1 = getticks(odo[jb_sens-1] , "left"); 147 | Ls2 = getticks(odo[jb_sens] , "left"); 148 | Rr1 = getticks(odo[jb_ref] , "right"); 149 | Rr2 = getticks(odo[jb_ref+1] , "right"); 150 | Rs1 = getticks(odo[jb_sens-1] , "right"); 151 | Rs2 = getticks(odo[jb_sens] , "right"); 152 | 153 | avg_left_start = (1-ref_alpha) * Lr1 + ref_alpha * Lr2; 154 | avg_right_start = (1-ref_alpha) * Rr1 + ref_alpha * Rr2; 155 | avg_left_end = (1-sens_alpha)* Ls1 + sens_alpha* Ls2; 156 | avg_right_end = (1-sens_alpha)* Rs1 + sens_alpha* Rs2; 157 | 158 | left_inc = (avg_left_end - avg_left_start); 159 | right_inc = (avg_right_end - avg_right_start); 160 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 161 | // // Calculate time tolerance 162 | // double times[4]; 163 | // times[0] = fabs( gettime(odo[jb_ref], "timestamp") - laser_ref_ts ); 164 | // times[1] = fabs( gettime(odo[jb_ref+1], "timestamp") - laser_ref_ts ); 165 | // times[2] = fabs( gettime(odo[jb_sens], "timestamp") - laser_sens_ts ); 166 | // times[3] = fabs( gettime(odo[jb_sens+1], "timestamp") - laser_sens_ts ); 167 | // mysort(times, 4); 168 | // double time_tol = times[3]; 169 | // 170 | // // Check if the robot is still 171 | // double ref_alpha = (laser_ref_ts - gettime(odo[jb_ref],"timestamp")) / (gettime(odo[jb_ref+1],"timestamp") - gettime(odo[jb_ref],"timestamp") ); 172 | // double sens_alpha = ( laser_sens_ts - gettime(odo[jb_sens],"timestamp")) / (gettime(odo[jb_sens+1],"timestamp") - gettime(odo[jb_sens],"timestamp") ); 173 | // 174 | // // double avg_left_start = ( getticks(odo[jb_ref], "left") + getticks(odo[jb_ref+1], "left") )/2; 175 | // // double avg_right_start = ( getticks(odo[jb_ref], "right") + getticks(odo[jb_ref+1], "right") )/2; 176 | // // double avg_left_end = ( getticks(odo[jb_sens], "left") + getticks(odo[jb_sens+1], "left") )/2; 177 | // // double avg_right_end = ( getticks(odo[jb_sens], "right") + getticks(odo[jb_sens+1], "right") )/2; 178 | // double avg_left_start = ref_alpha*getticks(odo[jb_ref], "left") + (1-ref_alpha)*getticks(odo[jb_ref+1], "left"); 179 | // double avg_right_start = ref_alpha*getticks(odo[jb_ref],"right")+(1-ref_alpha)*getticks(odo[jb_ref+1],"right"); 180 | // 181 | // double avg_left_end = sens_alpha*getticks(odo[jb_sens],"left") + (1-sens_alpha)*getticks(odo[jb_sens+1],"left"); 182 | // double avg_right_end = sens_alpha*getticks(odo[jb_sens],"right")+(1-sens_alpha)*getticks(odo[jb_sens+1],"right"); 183 | // 184 | // double left_inc = (avg_left_end - avg_left_start); 185 | // double right_inc = (avg_right_end - avg_right_start); 186 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 187 | 188 | if ( (left_inc == 0) && (right_inc == 0) ) { 189 | still_num = still_num + 1; 190 | continue; 191 | } 192 | 193 | /* Check if data is awful and eventually discard it */ 194 | jo_read_double(laser[k], "error", &error); 195 | if( error == 0 ) { 196 | continue; 197 | } 198 | 199 | if(mean_error[k] > mean_error_threshold) { 200 | discarded_error = discarded_error + 1; 201 | continue; 202 | } 203 | 204 | jo_read_int(laser[k], "nvalid", &nvalid); 205 | if( nvalid <= nvalid_threshold ) { 206 | discarded_nvalid = discarded_nvalid + 1; 207 | continue; 208 | } 209 | 210 | if ( time_tol > params.time_tolerance ) { 211 | discarded_time_tol = discarded_time_tol + 1; 212 | continue; 213 | } 214 | 215 | t_k = laser_sens_ts - laser_ref_ts; 216 | 217 | if( t_k > params.tk_threshold) { 218 | discarded_t_k = discarded_t_k + 1; 219 | cout << "Interval between scans exceedes limit: " << t_k << " > " << params.tk_threshold << endl; 220 | continue; 221 | } 222 | 223 | left_speed = (left_inc * ticks_to_rad) / t_k; 224 | right_speed = (right_inc * ticks_to_rad) / t_k; 225 | 226 | /* Build k-esim json object*/ 227 | struct json_object *new_obj; 228 | new_obj = json_object_new_object(); 229 | json_object_object_add(new_obj, "T", json_object_new_double(t_k) ); 230 | json_object_object_add(new_obj, "phi_l", json_object_new_double(left_speed) ); 231 | json_object_object_add(new_obj, "phi_r", json_object_new_double(right_speed) ); 232 | double sm[3]; 233 | jo_read_double_array (laser[k], "x", sm, 3, 0.0); 234 | jo_add_double_array (new_obj, "sm", sm, 3); 235 | // double odometry[3]; 236 | // jo_read_double_array (odo[jb_ref], "odometry", odometry, 3, 0.0); 237 | // jo_add_double_array (new_obj, "odometry", odometry, 3); 238 | // json_object_object_add(new_obj, "time_tol", json_object_new_double(time_tol) ); 239 | // json_object_object_add(new_obj, "error", json_object_new_double(error) ); 240 | // json_object_object_add(new_obj, "mean_error", json_object_new_double(mean_error[k]) ); 241 | // json_object_object_add(new_obj, "nvalid", json_object_new_int(nvalid) ); 242 | // json_object_object_add(new_obj, "sens_alpha", json_object_new_double(sens_alpha) ); 243 | // json_object_object_add(new_obj, "ref_alpha", json_object_new_double(ref_alpha) ); 244 | result.push_back(new_obj) ; 245 | 246 | m = m + 1; 247 | } 248 | 249 | //! 250 | if ( !write_jsonlist(params.out_file, &result) ) { 251 | sm_error("Error writing on output file '%s'.\n", params.out_file); 252 | return (-1); 253 | } 254 | 255 | sm_info(" still number: %d\n", still_num); 256 | sm_info(" Number of valid data: %d\n", m); 257 | sm_info(" discarded valid: %d\n", discarded_valid); 258 | sm_info(" discarded error: %d\n", discarded_error); 259 | sm_info(" discarded nvalid: %d\n", discarded_nvalid); 260 | sm_info(" discarded_t_k: %d\n", discarded_t_k); 261 | sm_info(" discarded_time_tol: %d\n", discarded_time_tol); 262 | sm_info(" discarded_no_odo: %d\n", discarded_no_odo); 263 | 264 | return 0; 265 | } 266 | -------------------------------------------------------------------------------- /src/synchronizer_options.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file synchronizer_options.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Options for the syncronizer executable. (implementation file) 5 | */ 6 | 7 | #include 8 | #include "synchronizer_options.h" 9 | 10 | void synchro_options(synchro_params *p, struct option*ops) { 11 | 12 | options_string(ops, "las_file", 13 | &(p->las_file), "", 14 | "Input sm_stats file"); 15 | 16 | options_string(ops, "odo_file", 17 | (&p->odo_file), "", 18 | "Input odometry file "); 19 | 20 | options_string(ops, "output", 21 | &(p->out_file), "stdout", 22 | "Output file "); 23 | 24 | options_double(ops, "laser_timestamp_correction", 25 | &(p->laser_timestamp_correction), 0.0, 26 | "Correction for laser timestamp"); 27 | 28 | options_double(ops, "perc_threshold", 29 | &(p->perc_threshold), 0.95, 30 | "Percentual threshold"); 31 | 32 | options_double(ops, "time_tolerance", 33 | &(p->time_tolerance), 0.05, 34 | "A threshold on time tolerance (sec)"); 35 | 36 | options_double(ops, "tk_threshold", 37 | &(p->tk_threshold), 3.0, 38 | "A threshold on the differerence between laser_sens_timestamp and laser_ref_timestamp (sec)"); 39 | 40 | } 41 | -------------------------------------------------------------------------------- /src/synchronizer_options.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file synchronizer_options.h 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Options for the synchronizer executable (header file) 5 | */ 6 | 7 | #ifndef _SYNCHRONIZER_OPTIONS_H 8 | #define _SYNCHRONIZER_OPTIONS_H 9 | 10 | /*! 11 | \struct synchro_params 12 | \brief Synchronizer parameter instance structure 13 | @param las_file Input sm_stats file for synchronizer procedure.\n Default Null 14 | @param odo_file Input odometry file for synchronizer procedure.\n Default Null 15 | @param out_file Output file of synchronizer procedure.\n Default stdout 16 | @param laser_timestamp_correction Correction for laser timestamp.\n Default 0.0 17 | @param perc_threshold Value used to define a threshold on mean error and nvalid.\n Default 0.95, range is [0,1] 18 | @param time_tolerance A threshold on time tolerance.\n Default 0.05 19 | @param tk_threshold A threshold on the differerence between laser_sens_timestamp and laser_ref_timestamp.\n Default 5.0 20 | */ 21 | typedef struct { 22 | const char * las_file; 23 | const char * odo_file; 24 | const char * out_file; 25 | double laser_timestamp_correction; 26 | double perc_threshold; 27 | double time_tolerance; 28 | double tk_threshold; 29 | } synchro_params; 30 | 31 | /*! 32 | \brief Initializes parameters for synchronization procedure 33 | \param p pointer to parameters' structure 34 | \param ops options' structure 35 | */ 36 | void synchro_options(synchro_params*p, struct option*ops); 37 | 38 | #endif 39 | 40 | -------------------------------------------------------------------------------- /src/test_json.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, const char * argv[]) { 5 | sm_set_program_name(argv[0]); 6 | 7 | int n; 8 | 9 | /* Clean treatment of parameters using the "options" library */ 10 | struct option* ops = options_allocate(3); 11 | options_int(ops, "n", &n, 1, "Number of copies"); 12 | 13 | if(!options_parse_args(ops, argc, argv)) { 14 | sm_info("%s : reads a JSON stream and copies it multiplied by n." 15 | "\n\nOptions:\n", (char*)argv[0]); 16 | options_print_help(ops, stderr); 17 | return -1; 18 | } 19 | 20 | JO jo; 21 | 22 | while((jo = json_read_stream(stdin))) { 23 | const char * s = json_object_to_json_string(jo); 24 | int i; for(i=0;i 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "verifier_utils.h" 26 | #include "verifier_options.h" 27 | #include "aux_functions.h" 28 | 29 | #include "analyzer.h" 30 | 31 | using namespace std; 32 | using namespace CSM; 33 | 34 | int main(int argc, const char * argv[]) { 35 | 36 | /*!*/ 37 | verifier_params params; 38 | struct option* ops = options_allocate(100); 39 | verifier_options(¶ms, ops); 40 | if(!options_parse_args(ops, argc, argv)) { 41 | options_print_help(ops, stderr); 42 | return -2; 43 | } 44 | 45 | JO est_params; 46 | FILE * input = open_file_for_reading(params.calib_params); 47 | if (!input) return 1; 48 | est_params = json_read_stream(input); 49 | fclose(input); 50 | double l_diam, r_diam, axle; 51 | double laser[3]; 52 | jo_read_double(est_params, "l_diam", &l_diam); 53 | jo_read_double(est_params, "r_diam", &r_diam); 54 | jo_read_double(est_params, "axle", &axle); 55 | jo_read_double(est_params, "l_x", &laser[0]); 56 | jo_read_double(est_params, "l_y", &laser[1]); 57 | jo_read_double(est_params, "l_theta", &laser[2]); 58 | 59 | printf("\nparametri stimati: b = %f, l_diam = %f, r_diam = %f, l_x = %f, l_y = %f, l_theta = %f\n", axle, l_diam, r_diam, laser[0], laser[1], laser[2]); 60 | 61 | double J11 = l_diam/4; 62 | double J12 = r_diam/4; 63 | double J21 = -l_diam/(2*axle); 64 | double J22 = r_diam/(2*axle); 65 | 66 | vector odo; 67 | if (!read_jsonlist(params.odometry, &odo) ) { 68 | sm_error("Error opening input files.\n"); 69 | return -1; 70 | } 71 | vector smStats; 72 | if (!read_jsonlist(params.smStats, &smStats) ) { 73 | sm_error("Error opening input files.\n"); 74 | return -1; 75 | } 76 | 77 | vector matchedOdo; 78 | vector matchedLas; 79 | matchOdoLas(odo, smStats, &matchedOdo, &matchedLas); 80 | int nOdo = (int)matchedOdo.size(); 81 | int nLas = (int)matchedLas.size(); 82 | vector integratedOdo; 83 | integratedOdo.reserve(nOdo); 84 | vector integratedLas; 85 | integratedLas.reserve(nLas); 86 | /*!*/ 87 | double tmp_q[3]; 88 | double q_int[3]; 89 | double o[3]; 90 | double T_i; 91 | double left_inc, right_inc; 92 | double ticks_to_rad; 93 | double w_l, w_r, v_i, w_i; 94 | double o_increment[3], t1, t2; 95 | struct json_object *new_obj; 96 | 97 | //set initial position. 98 | jo_read_double_array (matchedOdo[0], "odometry", q_int, 3, 0.0); 99 | new_obj = json_object_new_object(); 100 | jo_add_double_array (new_obj, "odometry", q_int, 3); 101 | integratedOdo.push_back(new_obj); 102 | 103 | for (int i = 0; i < nOdo-1; i++) { 104 | //calcolo v_k, w_k; 105 | T_i = gettime( matchedOdo[i+1], "timestamp") - gettime( matchedOdo[i], "timestamp"); 106 | left_inc = getticksD(matchedOdo[i+1], "left") - getticksD(matchedOdo[i], "left"); 107 | right_inc = getticksD(matchedOdo[i+1], "right") - getticksD(matchedOdo[i], "right"); 108 | ticks_to_rad = 2 * M_PI / ( 691.2 * 4 ); 109 | w_l = (left_inc * ticks_to_rad) / T_i; 110 | w_r = (right_inc * ticks_to_rad) / T_i; 111 | v_i = J11*w_l + J12*w_r; 112 | w_i = J21*w_l + J22*w_r; 113 | o_increment[2] = w_i*T_i; 114 | 115 | if ( fabs(o_increment[2]) > 1e-7 ) { 116 | t1 = ( sin(o_increment[2]) / (o_increment[2]) ); 117 | t2 = ( (1-cos(o_increment[2])) / (o_increment[2]) ); 118 | } 119 | else { 120 | t1 = 1; 121 | t2 = 0; 122 | } 123 | //i-esimo passo di integrazione 124 | o_increment[0] = ( (v_i*T_i)*t1 ); 125 | o_increment[1] = ( (v_i*T_i)*t2 ); 126 | tmp_q[0] = q_int[0]; 127 | tmp_q[1] = q_int[1]; 128 | tmp_q[2] = q_int[2]; 129 | planarComp(tmp_q, o_increment, q_int); 130 | if (q_int[2] > M_PI) q_int[2] = q_int[2] - 2*M_PI; 131 | if (q_int[2] < -M_PI) q_int[2] = q_int[2] + 2*M_PI; 132 | //store integration step 133 | new_obj = json_object_new_object(); 134 | jo_add_double_array (new_obj, "odometry", q_int, 3); 135 | integratedOdo.push_back(new_obj); 136 | } // end for 137 | 138 | // calcolo della rototraslazione tra q0 e la qk ricavata dall'integrazione dell'odometria con i nuovi parametri 139 | jo_read_double_array (matchedOdo[0], "odometry", tmp_q, 3, 0.0); 140 | inv_planarComp(q_int, tmp_q, o); 141 | 142 | jo_read_double_array (matchedOdo[nOdo-1], "odometry", tmp_q, 3, 0.0); 143 | printf("\nfinal odometry:\t%3.6f\t%3.6f\t%3.6f\n", tmp_q[0], tmp_q[1], tmp_q[2]); 144 | 145 | /*!*/ 146 | double tmp_s[3]; 147 | double s[3]; 148 | double s_int[3]; 149 | 150 | switch(params.use_gsl) { 151 | case 1: { 152 | cout << "Using GSL...\n"; 153 | gsl_matrix * T = gsl_matrix_calloc(4,4); 154 | gsl_matrix * A = gsl_matrix_calloc(4,4); 155 | gsl_matrix * R = gsl_matrix_calloc(4,4); 156 | 157 | jo_read_double_array (matchedOdo[0], "odometry", tmp_s, 3, 0.0); 158 | new_obj = json_object_new_object(); 159 | jo_add_double_array (new_obj, "odometrySm", tmp_s, 3); 160 | integratedLas.push_back(new_obj); 161 | rototras(A, tmp_s[0], tmp_s[1], tmp_s[2]); 162 | rototras(T, laser[0], laser[1], laser[2]); 163 | gsl_blas_dgemm (CblasNoTrans,CblasNoTrans, 1, A, T, 1, R); 164 | gsl_matrix_memcpy(A,R); 165 | gsl_matrix_free(R); 166 | 167 | for(int u = 0; u < nLas; u++) { 168 | R = gsl_matrix_calloc(4,4); 169 | jo_read_double_array (matchedLas[u], "x", tmp_s, 3, 0.0); 170 | rototras(T, tmp_s[0], tmp_s[1], tmp_s[2]); 171 | gsl_blas_dgemm (CblasNoTrans,CblasNoTrans, 1, A, T, 1, R); 172 | gsl_matrix_memcpy(A,R); 173 | gsl_matrix_free(R); 174 | 175 | R = gsl_matrix_calloc(4,4); 176 | rototras(T, laser[0], laser[1], laser[2]); 177 | gsl_matrix * LU = gsl_matrix_alloc(4,4); 178 | gsl_permutation * p = gsl_permutation_alloc(4); 179 | gsl_matrix_memcpy(LU, T); 180 | int signum; 181 | gsl_linalg_LU_decomp (LU, p, &signum ); 182 | gsl_linalg_LU_invert (LU, p, T ); 183 | gsl_blas_dgemm (CblasNoTrans,CblasNoTrans, 1, A, T, 1, R); 184 | s[0] = gsl_matrix_get(R, 0, 3); 185 | s[1] = gsl_matrix_get(R, 1, 3); 186 | s[2] = atan2( gsl_matrix_get(R, 1, 0) , gsl_matrix_get(R, 0, 0) ); 187 | new_obj = json_object_new_object(); 188 | jo_add_double_array (new_obj, "odometrySm", s, 3); 189 | integratedLas.push_back(new_obj); 190 | gsl_matrix_free(R); 191 | } 192 | break; 193 | } 194 | case 0: { 195 | cout << "no GSL\n"; 196 | jo_read_double_array (matchedOdo[0], "odometry", s_int, 3, 0.0); 197 | tmp_s[0] = s_int[0]; 198 | tmp_s[1] = s_int[1]; 199 | tmp_s[2] = s_int[2]; 200 | new_obj = json_object_new_object(); 201 | jo_add_double_array (new_obj, "odometrySm", s_int, 3); 202 | integratedLas.push_back(new_obj); 203 | 204 | planarComp(tmp_s, laser, s_int); 205 | if (s_int[2] > M_PI) s_int[2] = s_int[2] - 2*M_PI; 206 | if (s_int[2] < -M_PI) s_int[2] = s_int[2] + 2*M_PI; 207 | 208 | for (int u = 0; u < nLas; u++) { 209 | tmp_s[0] = s_int[0]; 210 | tmp_s[1] = s_int[1]; 211 | tmp_s[2] = s_int[2]; 212 | jo_read_double_array (matchedLas[u], "x", s, 3, 0.0); 213 | planarComp(tmp_s, s, s_int); 214 | if (s_int[2] > M_PI) s_int[2] = s_int[2] - 2*M_PI; 215 | if (s_int[2] < -M_PI) s_int[2] = s_int[2] + 2*M_PI; 216 | 217 | tmp_s[2] = s_int[2]- laser[2]; 218 | tmp_s[0] = s_int[0] - laser[0]*cos(tmp_s[2]) + laser[1]*sin(tmp_s[2]); 219 | tmp_s[1] = s_int[1] - laser[0]*sin(tmp_s[2]) - laser[1]*cos(tmp_s[2]); 220 | if (tmp_s[2] > M_PI) tmp_s[2] = tmp_s[2] - 2*M_PI; 221 | if (tmp_s[2] < -M_PI) tmp_s[2] = tmp_s[2] + 2*M_PI; 222 | new_obj = json_object_new_object(); 223 | jo_add_double_array (new_obj, "odometrySm", s_int, 3); 224 | integratedLas.push_back(new_obj); 225 | 226 | } 227 | tmp_s[0] = s_int[0]; 228 | tmp_s[1] = s_int[1]; 229 | tmp_s[2] = s_int[2]; 230 | 231 | s_int[2] = tmp_s[2]- laser[2]; 232 | s_int[0] = tmp_s[0] - laser[0]*cos(s_int[2]) + laser[1]*sin(s_int[2]); 233 | s_int[1] = tmp_s[1] - laser[0]*sin(s_int[2]) - laser[1]*cos(s_int[2]); 234 | if (s_int[2] > M_PI) s_int[2] = s_int[2] - 2*M_PI; 235 | if (s_int[2] < -M_PI) s_int[2] = s_int[2] + 2*M_PI; 236 | 237 | new_obj = json_object_new_object(); 238 | jo_add_double_array (new_obj, "odometrySm", s_int, 3); 239 | integratedLas.push_back(new_obj); 240 | 241 | jo_read_double_array (matchedOdo[0], "odometry", tmp_s, 3, 0.0); 242 | inv_planarComp(s_int, tmp_s, s); 243 | } 244 | default: break; 245 | } 246 | 247 | /*!*/ 248 | double e[3]; 249 | inv_planarComp(s, o, e); 250 | if (e[2] > M_PI) e[2] = e[2] - 2*M_PI; 251 | if (e[2] < -M_PI) e[2] = e[2] + 2*M_PI; 252 | 253 | /*!*/ 254 | new_obj = json_object_new_object(); 255 | jo_add_double_array (new_obj, "o", o, 3); 256 | jo_add_double_array (new_obj, "s", s, 3); 257 | jo_add_double_array (new_obj, "e", e, 3); 258 | // Write json object to file 259 | FILE * output; 260 | if(!strcmp(params.out, "stdout")) { output = stdout;} 261 | else {output = fopen(params.out, "w");} 262 | fprintf(output, "%s\n", json_object_to_json_string(new_obj) ); 263 | fclose(output); 264 | 265 | /*!*/ 266 | 267 | Analyzer * an; 268 | 269 | //Creating Graph using integrated odometry 270 | Graph * ix_graph = new Graph(); Graph * iy_graph = new Graph(); Graph * ith_graph = new Graph(); 271 | an->set_par(ix_graph, "x_int.vgr" , "X int coordinate" , "step", "x", "lines", "Integrated Odo"); 272 | an->set_par(iy_graph, "y_int.vgr" , "Y int coordinate" , "step", "y", "lines", "Integrated Odo"); 273 | an->set_par(ith_graph,"th_int.vgr","THETA int coordinate", "step", "th","lines", "Integrated Odo"); 274 | double * int_x = new double[nOdo]; 275 | double * int_y = new double[nOdo]; 276 | double * int_th = new double[nOdo]; 277 | double * int_axis = new double[nOdo]; 278 | for (int i = 0; i < nOdo; i++) { 279 | int_axis[i] = i; 280 | jo_read_double_array (integratedOdo[i], "odometry", tmp_s, 3, 0.0); 281 | int_x[i] = tmp_s[0]; int_y[i] = tmp_s[1]; int_th[i] = tmp_s[2]; 282 | } 283 | an->add_data_graph(int_x, int_axis, nOdo, ix_graph); 284 | an->add_data_graph(int_y, int_axis, nOdo, iy_graph); 285 | an->add_data_graph(int_th,int_axis, nOdo, ith_graph); 286 | 287 | //Creating Graph using Scan Matcher rototranslation 288 | Graph * lx_graph = new Graph(); Graph * ly_graph = new Graph(); Graph * lth_graph = new Graph(); 289 | an->set_par(lx_graph, "x_las.vgr" , "X las coordinate" , "step", "x", "lines", "Scan Matcher"); 290 | an->set_par(ly_graph, "y_las.vgr" , "Y las coordinate" , "step", "y", "lines", "Scan Matcher"); 291 | an->set_par(lth_graph,"th_las.vgr","THETA las coordinate", "step", "th","lines", "Scan Matcher"); 292 | 293 | nLas = nLas+1; 294 | double * las_x = new double[nLas]; 295 | double * las_y = new double[nLas]; 296 | double * las_th = new double[nLas]; 297 | double * las_axis = new double[nLas]; 298 | for (int i = 0; i < nLas; i++) { 299 | las_axis[i] = i; 300 | jo_read_double_array (integratedLas[i], "odometrySm", tmp_s, 3, 0.0); 301 | las_x[i] = tmp_s[0]; las_y[i] = tmp_s[1]; las_th[i] = tmp_s[2]; 302 | } 303 | an->add_data_graph(las_x, las_axis, nLas, lx_graph); 304 | an->add_data_graph(las_y, las_axis, nLas, ly_graph); 305 | an->add_data_graph(las_th,las_axis, nLas, lth_graph); 306 | 307 | //Creating Graph using odometry 308 | Graph * ox_graph = new Graph(); Graph * oy_graph = new Graph(); Graph * oth_graph = new Graph(); 309 | an->set_par(ox_graph, "x_odo.vgr" , "X odo coordinate" , "step", "x", "lines", "Odometry"); 310 | an->set_par(oy_graph, "y_odo.vgr" , "Y odo coordinate" , "step", "y", "lines", "Odometry"); 311 | an->set_par(oth_graph,"th_odo.vgr", "THETA odo coordinate", "step", "th","lines", "Odometry"); 312 | double * odo_x = new double[nOdo]; 313 | double * odo_y = new double[nOdo]; 314 | double * odo_th = new double[nOdo]; 315 | double * odo_axis = new double[nOdo]; 316 | for (int i = 0; i < nOdo; i++) { 317 | odo_axis[i] = i; 318 | jo_read_double_array (matchedOdo[i], "odometry", tmp_s, 3, 0.0); 319 | odo_x[i] = tmp_s[0]; odo_y[i] = tmp_s[1]; odo_th[i] = tmp_s[2]; 320 | } 321 | an->add_data_graph(odo_x, odo_axis, nOdo, ox_graph); 322 | an->add_data_graph(odo_y, odo_axis, nOdo, oy_graph); 323 | an->add_data_graph(odo_th,odo_axis, nOdo, oth_graph); 324 | 325 | // Creating Graph s_theta (as y) vs i_theta (as x) 326 | Graph * thesi_graph = new Graph(); 327 | an->set_par(thesi_graph, "th_s-i.vgr", "THETA sm versus integrated", "i_theta", "s_theta", "points", ""); 328 | an->add_data_graph(las_th, int_th, nOdo, thesi_graph); 329 | 330 | // Creating Graph trajectory 331 | double * spec_int_y = new double[nOdo]; 332 | double * spec_odo_y = new double[nOdo]; 333 | double * spec_las_y = new double[nLas]; 334 | for (int i = 0; i < nOdo; i++) { 335 | spec_int_y[i] = -int_y[i]; 336 | spec_odo_y[i] = -odo_y[i]; 337 | } 338 | for (int i = 0; i < nLas; i++) { 339 | spec_las_y[i] = -las_y[i]; 340 | } 341 | Graph traj_graph[3]; 342 | an->set_par(&(traj_graph[0]), "i_traj.vgr", "Trajectory integrated" , "", "", "lines", "integrated Odo"); 343 | an->set_par(&(traj_graph[1]), "l_traj.vgr", "Trajectory scan matcher", "", "", "lines", "Scan matcher"); 344 | an->set_par(&(traj_graph[2]), "o_traj.vgr", "Trajectory odometry" , "", "", "lines", "odometry"); 345 | 346 | an->add_data_graph(int_x, spec_int_y, nOdo, &traj_graph[0]); 347 | an->add_data_graph(las_x, spec_las_y, nOdo, &traj_graph[1]); 348 | an->add_data_graph(odo_x, spec_odo_y, nOdo, &traj_graph[2]); 349 | 350 | // Creating error plots 351 | Graph * eth_graph, * ethAbs_graph, * releth_graph; 352 | 353 | if (nLas == nOdo) { 354 | // Creating graph absolute theta error (int_th - las_th) 355 | ethAbs_graph = new Graph(); 356 | an->set_par(ethAbs_graph, "err_th_abs.vgr", "Absolute THETA error", "step", "|E_th|", "lines", ""); 357 | double * ethAbs = new double[nOdo]; 358 | for (int i = 0; i < nOdo; i++) ethAbs[i] = fabs(int_th[i] - las_th[i]); 359 | an->add_data_graph(ethAbs, int_axis, nOdo, ethAbs_graph); 360 | 361 | // Creating graph theta error (int_th - las_th) 362 | eth_graph = new Graph(); 363 | an->set_par(eth_graph, "err_th.vgr", "THETA error", "step", "E_th", "lines", ""); 364 | double * eth = new double[nOdo]; 365 | for (int i = 0; i < nOdo; i++) eth[i] = (int_th[i] - las_th[i]); 366 | an->add_data_graph(eth, int_axis, nOdo, eth_graph); 367 | 368 | // Creating graph error theta vs integrated theta 369 | releth_graph = new Graph(); 370 | an->set_par(releth_graph, "rel_err_th.vgr", "relative THETA error", "i_th", "i_th - s_th", "points", ""); 371 | an->add_data_graph(eth, int_th, nOdo, releth_graph); 372 | } 373 | else { 374 | cout << "Note: number of laser entries is: " << nLas 375 | << ", while number of odometry entries is: " << nOdo 376 | << "\n Omitting error graphs!\n"; 377 | } 378 | 379 | // Creating plots 380 | int n_plots = 3; 381 | Graph * x_plots = new Graph[n_plots]; 382 | Graph * y_plots = new Graph[n_plots]; 383 | Graph * th_plots = new Graph[n_plots]; 384 | 385 | x_plots[0] = ix_graph[0]; x_plots[1] = lx_graph[0]; x_plots[2] = ox_graph[0]; 386 | y_plots[0] = iy_graph[0]; y_plots[1] = ly_graph[0]; y_plots[2] = oy_graph[0]; 387 | th_plots[0] = ith_graph[0]; th_plots[1] = lth_graph[0]; th_plots[2] = oth_graph[0]; 388 | 389 | // Plotting 390 | cout << "Plotting odometry's and laser's graphs" << endl; 391 | an->create_same_plot("X coordinate", "step", "x", x_plots, n_plots, "x_plots.vpl", 1, 1); 392 | an->plot("x_plots.vpl"); 393 | an->create_same_plot("Y coordinate", "step", "y", y_plots, n_plots, "y_plots.vpl", 1, 1); 394 | an->plot("y_plots.vpl"); 395 | an->create_same_plot("Theta coordinate", "step", "th", th_plots, n_plots, "th_plots.vpl", 1, 1); 396 | an->plot("th_plots.vpl"); 397 | an->create_same_plot("Trajectory", "world x", "world y", traj_graph, 3, "traj_plot.vpl", 1, 1); 398 | an->plot("traj_plot.vpl"); 399 | 400 | if (nLas == nOdo) { 401 | cout << "Plotting errors' graphs" << endl; 402 | an->create_plot(thesi_graph, 1, "thesi_plot.vpl", 1, 0); 403 | an->plot("thesi_plot.vpl"); 404 | an->create_plot(eth_graph , 1, "eth_plot.vpl", 1, 0); 405 | an->plot("eth_plot.vpl"); 406 | an->create_plot(ethAbs_graph,1, "ethAbs_plot.vpl", 1, 0); 407 | an->plot("ethAbs_plot.vpl"); 408 | an->create_plot(releth_graph,1, "releth_plot.vpl", 1, 0); 409 | an->plot("releth_plot.vpl"); 410 | } 411 | return 0; 412 | } 413 | -------------------------------------------------------------------------------- /src/verifier_options.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file verifier_options.ccp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Manages options for verifier (implementation file) 5 | */ 6 | #include 7 | #include "verifier_options.h" 8 | 9 | void verifier_options(verifier_params *p, struct option*ops) { 10 | 11 | options_string(ops, "smStats", 12 | &(p->smStats), "", 13 | "Input laser file"); 14 | 15 | options_string(ops, "odometry", 16 | (&p->odometry), "", 17 | "Input odometry file "); 18 | 19 | options_string(ops, "calib_params", 20 | &(p->calib_params), "", 21 | "Input file with estimated parameters"); 22 | 23 | options_string(ops, "out", 24 | &(p->out), "stdout", 25 | "Output file"); 26 | 27 | options_int(ops, "use_gsl", 28 | &(p->use_gsl), 0, 29 | "0: no GSL; 1 use GSL"); 30 | } 31 | -------------------------------------------------------------------------------- /src/verifier_options.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file verifier_options.h 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Manages options for verifier (header file) 5 | */ 6 | #ifndef _VERIFIER_OPTIONS_H 7 | #define _VERIFIER_OPTIONS_H 8 | 9 | /*! 10 | \struct verifier_params 11 | @param sm_stats laser_stats input filename 12 | @param odometry odometry input filename 13 | @param parameters estimated parameters filename 14 | @param out output filename 15 | */ 16 | typedef struct { 17 | const char * smStats; 18 | const char * odometry; 19 | const char * calib_params; 20 | const char * out; 21 | int use_gsl; 22 | } verifier_params; 23 | 24 | /*! 25 | Initializes parameters for verifier procedure 26 | \param p pointer to parameters' structure 27 | \param ops options' structure 28 | */ 29 | void verifier_options(verifier_params*p, struct option*ops); 30 | 31 | #endif 32 | 33 | -------------------------------------------------------------------------------- /src/verifier_utils.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | \file verifier_utils.cpp 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Auxiliary methods for verifier.cpp 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | 13 | // a, b, result are pointers to array with three elements! 14 | // a(+)b 15 | void planarComp(double * a, double * b, double * result) { 16 | result[0] = a[0] + b[0]*cos(a[2]) - b[1]*sin(a[2]); 17 | result[1] = a[1] + b[0]*sin(a[2]) + b[1]*cos(a[2]); 18 | result[2] = a[2] + b[2]; 19 | } 20 | 21 | // a, b, result are pointers to array with three elements! 22 | // a(-)b 23 | void inv_planarComp(double * a, double * b, double * result) { 24 | result[2] = a[2] - b[2]; 25 | result[0] = + a[0]*cos(b[2]) + a[1]*sin(b[2]) - b[0]*cos(b[2]) - b[1]*sin(b[2]); 26 | result[1] = - a[0]*sin(b[2]) + a[1]*cos(b[2]) + b[0]*sin(b[2]) - b[1]*cos(b[2]); 27 | } 28 | 29 | void rototras(gsl_matrix * M, double x, double y, double theta){ 30 | /* M = [ cos(theta) -sin(theta) 0 x 31 | sin(theta) cos(theta) 0 y 32 | 0 0 1 0 33 | 0 0 0 1 ] */ 34 | gsl_matrix_set_zero(M); 35 | gsl_matrix_set(M , 0, 0, cos(theta) ); 36 | gsl_matrix_set(M , 0, 1, -sin(theta) ); 37 | gsl_matrix_set(M , 0, 2, 0 ); 38 | gsl_matrix_set(M , 0, 3, x ); 39 | gsl_matrix_set(M , 1, 0, sin(theta) ); 40 | gsl_matrix_set(M , 1, 1, cos(theta) ); 41 | gsl_matrix_set(M , 1, 2, 0 ); 42 | gsl_matrix_set(M , 1, 3, y ); 43 | gsl_matrix_set(M , 2, 0, 0 ); 44 | gsl_matrix_set(M , 2, 1, 0 ); 45 | gsl_matrix_set(M , 2, 2, 1.0 ); 46 | gsl_matrix_set(M , 2, 3, 0 ); 47 | gsl_matrix_set(M , 3, 0, 0 ); 48 | gsl_matrix_set(M , 3, 1, 0 ); 49 | gsl_matrix_set(M , 3, 2, 0 ); 50 | gsl_matrix_set(M , 3, 3, 1.0 ); 51 | } 52 | 53 | -------------------------------------------------------------------------------- /src/verifier_utils.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file verifier_utils.h 3 | \author Carlo Masone, Margherita Petrocchi, Lorenzo Rosa 4 | \brief Header of verifier_utils 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "aux_functions.h" 11 | 12 | using namespace std; 13 | 14 | /*! 15 | \brief Computate composition of planar transformations 16 | @param a \f$ \left[ \begin{array}{ccc} x & y & \theta \end{array} \right] \f$ 17 | @param b \f$ \left[ \begin{array}{ccc} x & y & \theta \end{array} \right] \f$ 18 | @param result \f$ a \oplus b \f$ = \f$ \left[ \begin{array}{ccc} x & y & \theta \end{array} \right] \f$ 19 | */ 20 | void planarComp(double * a, double * b, double * result); 21 | 22 | /*! 23 | \brief Computate inverse composition of planar transformations 24 | @param a \f$ \left[ \begin{array}{ccc} x & y & \theta \end{array} \right] \f$ 25 | @param b \f$ \left[ \begin{array}{ccc} x & y & \theta \end{array} \right] \f$ 26 | @param result \f$ a \ominus b \f$ = \f$ \left[ \begin{array}{ccc} x & y & \theta \end{array} \right] \f$ 27 | */ 28 | void inv_planarComp(double * a, double * b, double * result); 29 | 30 | /*! 31 | \brief Computate homogeneus transformation 32 | @param M pointer to gsl_matrix to store result 33 | @param x translation along x axis 34 | @param y translation along y axis 35 | @param theta rotation (z axis) 36 | */ 37 | void rototras(gsl_matrix * M, double x, double y, double theta); 38 | 39 | --------------------------------------------------------------------------------