├── OptimiseSensorPoses.m ├── README.md ├── applications ├── calibrate.m └── calibration-prepare ├── costfunctions ├── GeometricCostOfConstrainedLine.m ├── GeometricCostOfConstrainedPlane.m ├── GeometricCostOfUnconstrainedLine.m └── GeometricCostOfUnconstrainedPlane.m ├── impl ├── CoordinateTransform.m ├── MultiSensorMultiRegionCost.m ├── RemoveMean.m ├── RemovePointOutliers2.m ├── SensorXYZtoWorldNED.m └── ValidateInputData.m └── visualise ├── VisualiseDataSets.m ├── VisualiseFeature.m └── VisualiseResults.m /OptimiseSensorPoses.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | % Optimise/Calibrate multiple sensors using data collected from them 31 | % viewing several geometric features and an initial guess of their poses 32 | % with respect to the nav data. 33 | % 34 | % [ SensorTransformsXYZYPR ] = OptimiseSensorPose( SensorTransformsXYZYPR, DATA, COSTFUNS, 35 | % SensorTransformsLower, SensorTransformsUpper, OPTIONS ) 36 | % 37 | % Inputs: 38 | % All inputs and outputs are in metres or radians as appropriate. 39 | % 40 | % For the purpose of explaining the format/size of each input argument, the 41 | % number of Range Sensors being calibrated will be denoted NS, and the 42 | % number of Geometric Features over which the optimisation(s) will take 43 | % place will be denoted NF. 44 | % 45 | % SensorTransformsXYZYPR is an NS-by-6 matrix of the initial guess/estimate 46 | % of the pose of each range sensor with respect to the 'NavSolutions' 47 | % provided in the DATA. Each row represents one sensor's pose, and the 48 | % 6 columns represent the offsets of the sensor with respect to the nav 49 | % box as follows: 50 | % [ X, Y, Z, Yaw, Pitch, Roll ] 51 | % NOTE: The order of angles is Yaw,Pitch,Roll as these are the euler 52 | % angles, and their order (as listed) is important. 53 | % 54 | % DATA is an NF-by-NS struct array, in which the struct located at 55 | % DATA(F,S) is the data representing sensor S's view of feature F. 56 | % For example, given two geometric features, a Pole and a Ground 57 | % Plane, which are viewed by three laser range scanning sensors with 58 | % different poses with respect to the Nav Solution of a vehicle. 59 | % The first row of the DATA array will be a set of data from each 60 | % sensor that represents its view of the Pole, and the second row 61 | % will be a set of data from each sensor that represents its view of 62 | % the Ground Plane. 63 | % Each of the elements of DATA is a struct with the following fields 64 | % RangeData - matrix with M rows. Columns depend on which 65 | % GeoregisterFunction you're using 66 | % NOTE: M must be the same as M in NavData 67 | % NavData - matrix with M rows. Columns depend on which 68 | % georegisterFunction you're using. 69 | % NOTE: M must be the same as M in RangeData. 70 | % GeoregisterFunction - A function pointer that knows how to compute 71 | % the global position of each point in RangeData. eg 72 | % GetNEDPoints or GetXYZPoint_XYZq_RAE 73 | % NOTE: All structs in a struct array must have the same fields, and for 74 | % the case where a particular sensor (S) does not have any data for a 75 | % particular geometric feature (F), the fields within DATA(F,S) should 76 | % simply be empty matrices. 77 | % 78 | % COSTFUNS is an NF-by-1 cell vector of function handles, where COSTFUNS{F} 79 | % is the function to be used to determine the cost of the data provided 80 | % by each sensor in DATA(F,:). 81 | % The cost functions that the function handles in COSTFUNS point to 82 | % must take a single argument which is an M-by-3 matricx of cartesian 83 | % points, one per row in the format: 84 | % [ Northing, Easting, Down ] 85 | % The output of the cost function is a scalar cost with respect to 86 | % the internally defined geometric feature. Two simple such cost 87 | % functions have been provided: 88 | % - GeometricCostOfVerticalLine(pointsNED) 89 | % The cost is computed as the standard deviation of all the 90 | % points from the vertical line at the mean N,E 91 | % coordinates of all the data points. 92 | % - GeometricCostOfHorizontalPlane(pointsNED) 93 | % The cost is computed as the standard deviation of all the 94 | % points from the horizontal plane at the mean D 95 | % (height) of all the data points. 96 | % NOTE: The framework of this code has been specifically designed 97 | % to allow users to implement their own cost functions meeting 98 | % the requirements above, as the number of possible geometric 99 | % features, and cost metrics of these are almost unlimited. For 100 | % example, if the object's location in the N,E,D coordinate frame 101 | % is accurately known, it may be much more useful to use this 102 | % location rather than the mean as in the functions provided. 103 | % 104 | % SensorTransformsLower and SensorTransformsUpper are the (optional) lower 105 | % and upper bounds for the optimisation of SensorTransformsXYZYPR. They 106 | % take the same format as the SensorTransformsXYZYPR but are the numerical 107 | % minimum and maximum constraints used by fmincon. This is typically not 108 | % required if there is sufficient data and a sufficiently accurate initial 109 | % guess. However, depending on the range of vehicle movements and range of 110 | % poses of the vehicle for which there is data, some values may not be able 111 | % to be optimised especially well (e.g. the Z offset when only a vertical 112 | % pole is used to optimise the sensor pose). Setting these to empty 113 | % matrices allows OPTIONS to be specified without applying these 114 | % constraints. 115 | % 116 | % OPTIONS can be created manually or with the OPTIMSET function. See 117 | % OPTIMSET and FMINCON for details on how to set these options and which 118 | % particular options are used. If OPTIONS is provided, all options should 119 | % be set as desired, otherwise a default OPTIONS is used, specified within 120 | % this matlab function. 121 | % 122 | % Outputs: 123 | % 124 | % SensorTransformsXYZYPR is the optimised set of range sensor poses in the 125 | % same format as the input estimate of these poses. 126 | % All of the outputs of fmincon are also available in the same order and 127 | % format. 128 | % 129 | % See also STRUCT, CELL, FUNCTION_HANDLE, FMINCON, 130 | % GeometricCostOfVerticalLine, GEOMETRICCOSTOFHORIZONTALPLANE, 131 | % GetNEDPoints, RBSensorMeanNED, MultiSensorMultiRegionCost. 132 | 133 | function [ SensorTransformsXYZYPR, FVAL,EXITFLAG,OUTPUT,LAMBDA,GRAD,HESSIAN ] = OptimiseSensorPoses( sensorTransformsXYZRPY0, dataSets, costFunctions, bounds, options ) 134 | 135 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 136 | % Defaults where not specified 137 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 138 | options = optimset( 'Algorithm','active-set','MaxFunEvals', 2000, 'MaxIter', 1000, 'Display','iter', 'TolFun', 1e-6, 'TolX', 1e-9); 139 | 140 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 141 | % Input validation 142 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 143 | ValidateInputData( sensorTransformsXYZRPY0, dataSets, costFunctions, bounds ); 144 | 145 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 146 | % Optimisation 147 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 148 | [SensorTransformsXYZYPR,FVAL,EXITFLAG,OUTPUT,LAMBDA,GRAD,HESSIAN] = fmincon( ... 149 | @(SensorTransformsXYZYPR)MultiSensorMultiRegionCost(SensorTransformsXYZYPR, dataSets, costFunctions), ... 150 | sensorTransformsXYZRPY0, [], [], [], [], sensorTransformsXYZRPY0-bounds, sensorTransformsXYZRPY0+bounds, [], options); 151 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | range-sensor-calibration 2 | ======================== 3 | 4 | A matlab toolbox for calibrating range sensors (like lasers and radars) on a moving platform with a navigation source (like GPS/INS). 5 | -------------------------------------------------------------------------------- /applications/calibrate.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | %check dependencies 32 | if exist('bin_load')~=2 33 | error('Could not find required dependency: comma/csv/examples/bin_load.m - check your path settings') 34 | end 35 | reload=1; 36 | dataFolder='/home/data/my-calibration-data' 37 | 38 | featureFiles=dir([dataFolder,'/*features.bin']); 39 | nSensors=length(featureFiles); 40 | if nSensors==0 41 | error(['No feature files found: ', dataFolder,'/*features.bin']) 42 | end 43 | if( reload ) 44 | rawSensorData=cell(1,nSensors); 45 | for i=1:nSensors 46 | %rawSensorData{i}=load(featureFiles(2).name); % csv 47 | rawSensorData{i}=bin_load([dataFolder,'/',featureFiles(i).name],'t,d,d,d,d,d,d,d,d,d,d,d,d,ui'); 48 | end 49 | end 50 | 51 | %todo: parse from json file using something like: 52 | %fieldname='cc'; 53 | %if(length(strfind(fieldstring,fieldname))==1) 54 | % fieldnum=length(strfind(fieldstring(1:strfind(fieldstring,fieldname)),','))+1 55 | %end 56 | xs=2; 57 | ys=3; 58 | zs=4; 59 | xn=5; 60 | yn=6; 61 | zn=7; 62 | north=8; 63 | east=9; 64 | down=10; 65 | roll=11; 66 | pitch=12; 67 | yaw=13; 68 | feature=14; 69 | 70 | sensorTransformsXYZRPY0=zeros(nSensors,6); 71 | for i=1:nSensors 72 | system( sprintf('cat %s | name-value-get offset/initial > tmp.offset.csv',[dataFolder,'/',featureFiles(i).name(1:end-3),'json']) ); 73 | sensorTransformsXYZRPY0(i,:)=load('tmp.offset.csv'); 74 | delete tmp.offset.csv 75 | end 76 | 77 | %todo: get from json file instead 78 | bounds = [0.2, 0.2, 0.2, deg2rad(5), deg2rad(5), deg2rad(5) ]; 79 | bounds = repmat(bounds,nSensors,1); 80 | 81 | nFeatures=0; 82 | for s=1:nSensors 83 | nFeatures = max( nFeatures, max(rawSensorData{s}(:,feature)) );%largest feature id of all sensors. Id 0 is rubish non-feature. 84 | end 85 | for( f=1:nFeatures ) 86 | for( s=1:nSensors ) 87 | fId = find(rawSensorData{s}(:,feature)==f); %f are indices to all points in feature i 88 | data(f,s).sensor = [rawSensorData{s}(fId,xs),rawSensorData{s}(fId,ys),rawSensorData{s}(fId,zs)]; %fill out the X,Y,Z sensor data for feature i 89 | data(f,s).nav = [rawSensorData{s}(fId,north),rawSensorData{s}(fId,east),rawSensorData{s}(fId,down),rawSensorData{s}(fId,roll),rawSensorData{s}(fId,pitch),rawSensorData{s}(fId,yaw)]; 90 | data(f,s).georegister = @SensorXYZtoWorldNED; 91 | end 92 | end 93 | 94 | 95 | %todo: specify feature shapes from json file? 96 | %todo: auto calc based on min cost? 97 | 98 | %the following bindings add constrained functions or allow ignoring a feature 99 | %GeometricCostOfVerticalLine = @(x)(GeometricCostOfConstrainedLine([0,0,1],x)); 100 | Ignore=@(x)(0); 101 | 102 | costfunctions = cell(nFeatures,1); 103 | costfunctions{1} = 'GeometricCostOfUnconstrainedLine'; 104 | costfunctions{2} = 'GeometricCostOfUnconstrainedLine'; 105 | costfunctions{3} = 'GeometricCostOfUnconstrainedPlane'; 106 | costfunctions{4} = 'GeometricCostOfUnconstrainedPlane'; 107 | costfunctions{5} = 'GeometricCostOfUnconstrainedPlane'; 108 | costfunctions{6} = 'GeometricCostOfUnconstrainedPlane'; 109 | costfunctions{7} = 'GeometricCostOfUnconstrainedPlane'; 110 | costfunctions{7} = Ignore; 111 | 112 | options = optimset( 'Algorithm','active-set','MaxFunEvals', 2000, 'MaxIter', 1000, 'Display','iter', 'TolFun', 1e-10, 'TolX', 1e-10); 113 | 114 | disp('optimising...') 115 | tic 116 | [ sensorTransformsXYZRPY ] = OptimiseSensorPoses( sensorTransformsXYZRPY0, data, costfunctions, bounds, options ); 117 | toc 118 | 119 | fprintf('optimised result:\noffset=%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\n',sensorTransformsXYZRPY) 120 | 121 | [p0,p1] = VisualiseResults( sensorTransformsXYZRPY0, sensorTransformsXYZRPY, data, costfunctions ); 122 | VisualiseDataSets( sensorTransformsXYZRPY0, data ); 123 | VisualiseDataSets( sensorTransformsXYZRPY, data ); 124 | -------------------------------------------------------------------------------- /applications/calibration-prepare: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 4 | # Copyright (c) 2011 The University of Sydney 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # 3. Neither the name of the University of Sydney nor the 15 | # names of its contributors may be used to endorse or promote products 16 | # derived from this software without specific prior written permission. 17 | # 18 | # NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 19 | # GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 20 | # HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 21 | # WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 22 | # MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 24 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 27 | # BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 28 | # WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 29 | # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 30 | # IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | 33 | #todo: migrate to latest standards for argument parsing 34 | #todo: add a before and after view operation 35 | name=$( basename $0 ) 36 | 37 | function errcho { (>&2 echo "$name: $1") } 38 | function error 39 | { 40 | errcho "error: $1" 41 | exit 1 42 | } 43 | 44 | function usage() 45 | { 46 | echo 47 | echo "prepare sensor data for feature labeling and calibration" 48 | echo 49 | echo "usage: $name []" 50 | echo " : base directory of calibration log (contains nav.bin (--binary=t,6d --fields=t,x,y,z,roll,pitch,yaw) and sensor.bin (--binary=t,3d --fields=t,x,y,z))" 51 | echo " : sensor-specific options, e.g. offsets, etc" 52 | echo 53 | echo "" 54 | echo " --name= : e.g. sick-front (default is sensor) (note: maps to sick-front.bin or sensor.bin" 55 | echo " --offset= : e.g. 0,0,-0.8845,0,0,$(math-deg2rad 180)" 56 | echo " --thin= : e.g. 0.001 will keep only this fraction of points at random, default is no thinning" 57 | echo " --rough= : e.g. if specified, voxelise at this resolution and label roughly first, followed by fine labelling" 58 | echo " --format : will return the format of the binary output of this script " 59 | echo "examples" 60 | echo " $name ./ --name=puck --offset=0,0,0,0,0,0 --rough=0.2 --thin=0.001" 61 | echo " $name ./ --name=puck --offset=0,0,0,0,0,0 --thin=0.0001" 62 | echo " $name --format" 63 | echo 64 | exit 1 65 | } 66 | LOG_DIR=$1 67 | RUN_DIR=$(pwd) 68 | OUTPUT="calibration" 69 | OFFSET= 70 | THIN=1.0 71 | ROUGH= 72 | MAXSTDDEV=0.05 #filter for 'bad' nav data 73 | MAXGAP=0.04 #nav must be spaced closer in seconds than this to be used 74 | FEATURE_FILTER_SIZE=0.5 #spatial filter size when using filter 75 | NAME="sensor" 76 | 77 | #get inputs 78 | for cmd_args in "$@" ; do 79 | if [[ "$cmd_args" == '--help' || "${cmd_args}" == '-h' ]] ; then usage ; exit -1; fi 80 | if [[ "$cmd_args" == '--format' ]] ; then echo 't,15d,ui' ; exit 0; fi 81 | if [[ "$( echo $cmd_args | grep '\-\-offset=' )" != "" ]]; then OFFSET="${cmd_args#--offset=}"; fi 82 | if [[ "$( echo $cmd_args | grep '\-\-name=' )" != "" ]]; then NAME="${cmd_args#--name=}"; fi 83 | if [[ "$( echo $cmd_args | grep '\-\-thin=' )" != "" ]]; then THIN="${cmd_args#--thin=}"; fi 84 | if [[ "$( echo $cmd_args | grep '\-\-rough=' )" != "" ]]; then ROUGH="${cmd_args#--rough=}"; fi 85 | if [[ "$( echo $cmd_args | grep '\-\-features=' )" != "" ]]; then FEATURES="${cmd_args#--features=}"; fi 86 | done 87 | 88 | #use supplied or default offset plus nav 89 | if [[ "$OFFSET" != "" ]] ; then 90 | OFFSET_STRING="$OFFSET + $LOG_DIR/nav.bin" 91 | errcho 92 | errcho "using given offset: $OFFSET" 93 | errcho 94 | else 95 | errcho "error: no initial offset supplied" 96 | usage 97 | fi 98 | 99 | [[ -s $LOG_DIR/nav.bin ]] || error "could not find trajectory file: $LOG_DIR/nav.bin" 100 | [[ -s $LOG_DIR/$NAME.bin ]] || error "could not find sensor file: $LOG_DIR/$NAME.bin" 101 | 102 | errcho "view nav data - check the trajectory is smooth, indicating a good nav solution" 103 | cat $LOG_DIR/nav.bin | view-points --binary=t,6d --fields=t,x,y,z,,, 104 | 105 | function filter 106 | { 107 | if [ -n "$ROUGH" ] ; then 108 | points-join --binary=t,12d --fields=,,,,x,y,z --radius=$FEATURE_FILTER_SIZE "$NAME.rough.bin;binary=t,12d,ui;fields=,,,,x,y,z" \ 109 | | csv-shuffle --binary=t,12d,t,12d,ui --fields=t,xs,ys,zs,xn,yn,zn,x,y,z,roll,pitch,yaw,,,,,,,,,,,,,,id --output-fields=t,xs,ys,zs,xn,yn,zn,x,y,z,roll,pitch,yaw,id 110 | else 111 | csv-paste "-;binary=t,12d" "value=0;binary=ui" 112 | fi 113 | } 114 | 115 | function thin 116 | { 117 | if [ -n "$THIN" ] ; then 118 | #points-calc thin --points-per-voxel=1 --resolution=$THIN --binary=t,12d --fields=,,,,x,y,z 119 | points-calc thin --rate=$THIN --resolution=0.5 --binary=t,12d --fields=,,,,x,y,z 120 | else 121 | cat 122 | fi 123 | } 124 | 125 | if [ -n "$ROUGH" ] ; then 126 | errcho 'georeferencing and voxelising for initial rough labelling...' 127 | pv $LOG_DIR/$NAME.bin \ 128 | | csv-shuffle --binary=t,3d --fields=t,x,y,z --output-fields=t,x,y,z,x,y,z \ 129 | | points-frame --max-gap=$MAXGAP --output-frame --from="$OFFSET_STRING" --binary=t,6d --fields=t,,,,x,y,z --discard \ 130 | | points-calc thin --points-per-voxel=1 --resolution=$ROUGH --binary=t,12d --fields=,,,,x,y,z \ 131 | | csv-paste "-;binary=t,12d" "value=0;binary=ui" \ 132 | > $NAME.rough.bin 133 | 134 | errcho 'loading voxelised data in label-points. Roughly select calibration features, label them 1,2,3... use 0 for non features...' 135 | label-points "$NAME.rough.bin;binary=t,12d,ui;fields=t,,,,x,y,z,,,,,,,id" 136 | 137 | errcho 'retaining selected points only...' 138 | cat $NAME.rough.bin | csv-select --binary=t,12d,ui --fields=",,,,,,,,,,,,,id" "id;from=1" | sponge $NAME.rough.bin 139 | fi 140 | 141 | 142 | errcho 'georeferencing and thinning for fine labelling...' 143 | pv $LOG_DIR/$NAME.bin \ 144 | | csv-shuffle --binary=t,3d --fields=t,x,y,z --output-fields=t,x,y,z,x,y,z \ 145 | | points-frame --max-gap=$MAXGAP --output-frame --from="$OFFSET_STRING" --binary=t,6d --fields=t,,,,x,y,z --discard \ 146 | | points-calc thin --rate=$THIN --resolution=0.5 --binary=t,12d --fields=,,,,x,y,z \ 147 | | filter \ 148 | > temp-$NAME.features.bin 149 | 150 | 151 | errcho 'loading in label-points. Select calibration features, label them 1,2,3... use 0 for non features...' 152 | if [[ $ROUGH ]] ; then 153 | label-points "temp-$NAME.features.bin;binary=t,12d,ui;fields=t,,,,x,y,z,,,,,,,id" "$NAME.rough.bin;binary=t,12d,ui;fields=,,,,x,y,z,,,,,,,id" 154 | else 155 | label-points "temp-$NAME.features.bin;binary=t,12d,ui;fields=t,,,,x,y,z,,,,,,,id" 156 | fi 157 | 158 | 159 | 160 | errcho "forming final file, with only feature data..." 161 | pv temp-$NAME.features.bin | csv-select --binary=t,12d,ui --fields=",,,,,,,,,,,,,id" "id;from=1" > $NAME.features.bin 162 | 163 | #errcho "creating ascii version for..." 164 | #pv $NAME.features.bin | csv-from-bin t,12d,ui --precision=10 > $NAME.features.csv 165 | 166 | errcho "view final data to confirm it is appropriate for calibration" 167 | cat $NAME.features.bin | view-points --binary=t,12d,ui --fields=t,,,,x,y,z,,,,,,,id 168 | 169 | echo -e "fields=t,xs,ys,zs,xn,yn,zn,north,east,down,roll,pitch,yaw,feature\nbinary=t,12d,ui\noffset/initial=$OFFSET" | name-value-convert --to json > $NAME.features.json 170 | 171 | errcho "calibration preparation is complete, now \"load $NAME.features.csv\" OR \"bin_load('$NAME.features.bin','t,d,d,d,d,d,d,d,d,d,d,d,d,ui')\"in matlab for calibration calibrate" 172 | -------------------------------------------------------------------------------- /costfunctions/GeometricCostOfConstrainedLine.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | % 31 | % This function evaluates the geometric cost of a set of points with 32 | % respect to a line of specified direction. 33 | % 34 | % C = GeometricCostOfConstrainedLine( v, pointsXYZ ) 35 | % Where: 36 | % v is a vector in the direction of the line against which the points 37 | % swill be compared. 38 | % pointsXYZ is an N-by-3 matrix of points in a cartesian coordinate 39 | % system 40 | % C is the total cost (or standard deviation) of these points from the 41 | % plane defined above. 42 | % 43 | % If there are 0 or 1 rows in pointsXYZ (i.e. 0 or 1 cartesian points), 44 | % the total cost will always be zero. 45 | % 46 | % See also MULTISENSORMULTIREGIONCOST 47 | % 48 | 49 | function [C] = GeometricCostOfConstrainedLine( v, pointsXYZ ) 50 | 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | % Test validity of input data. 53 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 54 | 55 | [numberOfPoints, cartesianDimensions] = size(pointsXYZ); 56 | % Must be M-by-3 or empty 57 | if( cartesianDimensions ~= 3 && numberOfPoints ~= 0 ), 58 | error('RangeSensorCalibrationToolbox:GeometricCostOfConstrainedLine:InputPointsInvalidFormat', 'The points used to determine the cost with respect to a plane must be 3D cartesian, in an M-by-3 matrix.'); 59 | end 60 | 61 | % Cannot take mean/etc on empty array, so return zero if its got nothing. 62 | if( numberOfPoints < 1 ) 63 | C = 0; 64 | return 65 | end 66 | 67 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 68 | % Calculate the cost 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | pointsXYZ = RemoveMean(pointsXYZ); 72 | pointsXYZ = RemovePointOutliers2(pointsXYZ, 3); 73 | 74 | v = v ./ norm(v); 75 | covPoints = cov(pointsXYZ); 76 | [eigV, eigD] = eig( covPoints ); 77 | C = eigD(1,1) * dot( v, eigV(:,1) )^2; 78 | C = C + eigD(2,2) * dot( v, eigV(:,2) )^2; 79 | C = C + eigD(3,3) * dot( v, eigV(:,3) )^2; 80 | 81 | C = sum( diag(covPoints) ) - C; 82 | -------------------------------------------------------------------------------- /costfunctions/GeometricCostOfConstrainedPlane.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | % 31 | % This function evaluates the geometric cost of a set of points with 32 | % respect to a plane with a specified normal. 33 | % 34 | % C = GeometricCostOfConstrainedPlane( normal, pointsXYZ ) 35 | % Where: 36 | % normal is a vector normal to the plane against which the points will be 37 | % compared 38 | % pointsXYZ is an N-by-3 matrix of points in a cartesian coordinate 39 | % system 40 | % C is the total cost (or standard deviation) of these points from the 41 | % plane defined above. 42 | % 43 | % If there are 0 or 1 rows in pointsXYZ (i.e. 0 or 1 cartesian points), 44 | % the total cost will always be zero. 45 | % 46 | % See also MULTISENSORMULTIREGIONCOST 47 | % 48 | 49 | function [C] = GeometricCostOfConstrainedPlane( normal, pointsXYZ ) 50 | 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | % Test validity of input data. 53 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 54 | 55 | [numberOfPoints, cartesianDimensions] = size(pointsXYZ); 56 | % Must be M-by-3 or empty 57 | if( cartesianDimensions ~= 3 && numberOfPoints ~= 0 ), 58 | error('RangeSensorCalibrationToolbox:GeometricCostOfConstrainedPlane:InputPointsInvalidFormat', 'The points used to determine the cost with respect to a plane must be 3D cartesian, in an M-by-3 matrix.'); 59 | end 60 | 61 | % Cannot take mean/etc on empty array, so return zero if its got nothing. 62 | if( numberOfPoints < 1 ) 63 | C = 0; 64 | return 65 | end 66 | 67 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 68 | % Calculate the cost 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | pointsXYZ = RemoveMean(pointsXYZ); 72 | % pointsXYZ = RemovePointOutliers2(pointsXYZ, 3); 73 | 74 | normal = normal ./ norm(normal); 75 | 76 | [V, D] = eig( cov(pointsXYZ) ); 77 | C = D(1,1) * dot( normal, V(:,1) )^2; 78 | C = C + D(2,2) * dot( normal, V(:,2) )^2; 79 | C = C + D(3,3) * dot( normal, V(:,3) )^2; 80 | -------------------------------------------------------------------------------- /costfunctions/GeometricCostOfUnconstrainedLine.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | % 31 | % This function evaluates the geometric cost of a set of points with 32 | % respect to an unconstrained line of best fit. 33 | % 34 | % C = GeometricCostOfUnconstrainedLine( pointsXYZ ) 35 | % Where: 36 | % pointsXYZ is an N-by-3 matrix of points in a cartesian coordinate 37 | % system 38 | % C is the total cost (or standard deviation) of these points from the 39 | % plane defined above. 40 | % 41 | % If there are 0 or 1 rows in pointsXYZ (i.e. 0 or 1 cartesian points), 42 | % the total cost will always be zero. 43 | % 44 | % See also MULTISENSORMULTIREGIONCOST 45 | % 46 | 47 | function [C] = GeometricCostOfUnconstrainedLine( pointsXYZ ) 48 | 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | % Test validity of input data. 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | 53 | [numberOfPoints, cartesianDimensions] = size(pointsXYZ); 54 | % Must be M-by-3 or empty 55 | if( cartesianDimensions ~= 3 && numberOfPoints ~= 0 ), 56 | error('RangeSensorCalibrationToolbox:GeometricCostOfUnconstrainedLine:InputPointsInvalidFormat', 'The points used to determine the cost with respect to a plane must be 3D cartesian, in an M-by-3 matrix.'); 57 | end 58 | 59 | % Cannot take mean/etc on empty array, so return zero if its got nothing. 60 | if( numberOfPoints < 1 ) 61 | C = 0; 62 | return 63 | end 64 | 65 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 66 | % Calculate the cost 67 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 68 | 69 | pointsXYZ = RemoveMean(pointsXYZ); 70 | pointsXYZ = RemovePointOutliers2(pointsXYZ, 3); 71 | 72 | lambda = sort( eig(cov(pointsXYZ)) ); 73 | C = sum( lambda(1:2) ); 74 | -------------------------------------------------------------------------------- /costfunctions/GeometricCostOfUnconstrainedPlane.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | % 31 | % This function evaluates the geometric cost of a set of points with 32 | % respect to an unconstrained plane of best fit. 33 | % 34 | % C = GeometricCostOfUnconstrainedPlane( pointsXYZ ) 35 | % Where: 36 | % pointsXYZ is an N-by-3 matrix of points in a cartesian coordinate 37 | % system 38 | % C is the total cost (or standard deviation) of these points from the 39 | % plane defined above. 40 | % 41 | % If there are 0 or 1 rows in pointsXYZ (i.e. 0 or 1 cartesian points), 42 | % the total cost will always be zero. 43 | % 44 | % See also MULTISENSORMULTIREGIONCOST 45 | % 46 | 47 | function [C] = GeometricCostOfUnconstrainedPlane( pointsXYZ ) 48 | 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | % Test validity of input data. 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | 53 | [numberOfPoints, cartesianDimensions] = size(pointsXYZ); 54 | % Must be M-by-3 or empty 55 | if( cartesianDimensions ~= 3 && numberOfPoints ~= 0 ), 56 | error('RangeSensorCalibrationToolbox:GeometricCostOfUnconstrainedPlane:InputPointsInvalidFormat', 'The points used to determine the cost with respect to a plane must be 3D cartesian, in an M-by-3 matrix.'); 57 | end 58 | 59 | % Cannot take mean/etc on empty array, so return zero if its got nothing. 60 | if( numberOfPoints < 1 ) 61 | C = 0; 62 | return 63 | end 64 | 65 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 66 | % Calculate the cost 67 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 68 | 69 | pointsXYZ = RemoveMean(pointsXYZ); 70 | % pointsXYZ = RemovePointOutliers2(pointsXYZ, 3); 71 | 72 | lambda = sort( eig(cov(pointsXYZ)) ); 73 | C = lambda(1); 74 | -------------------------------------------------------------------------------- /impl/CoordinateTransform.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function [pointInBFrameXYZ] = CoordinateTransform( pointInAFrameXYZ, frameBtoAOffsetXYZRPY ) 32 | 33 | xab=frameBtoAOffsetXYZRPY(:,1); 34 | yab=frameBtoAOffsetXYZRPY(:,2); 35 | zab=frameBtoAOffsetXYZRPY(:,3); 36 | sinrollab=sin(frameBtoAOffsetXYZRPY(:,4)); 37 | cosrollab=cos(frameBtoAOffsetXYZRPY(:,4)); 38 | sinpitchab=sin(frameBtoAOffsetXYZRPY(:,5)); 39 | cospitchab=cos(frameBtoAOffsetXYZRPY(:,5)); 40 | sinyawab=sin(frameBtoAOffsetXYZRPY(:,6)); 41 | cosyawab=cos(frameBtoAOffsetXYZRPY(:,6)); 42 | xa=pointInAFrameXYZ(:,1); 43 | ya=pointInAFrameXYZ(:,2); 44 | za=pointInAFrameXYZ(:,3); 45 | 46 | pointInBFrameXYZ = zeros( size(pointInAFrameXYZ) ); 47 | 48 | pointInBFrameXYZ(:,1) = xab - ya.*(cosrollab.*sinyawab - cosyawab.*sinpitchab.*sinrollab) + za.*(sinrollab.*sinyawab + cosrollab.*cosyawab.*sinpitchab) + xa.*cospitchab.*cosyawab; 49 | pointInBFrameXYZ(:,2) = yab + ya.*(cosrollab.*cosyawab + sinpitchab.*sinrollab.*sinyawab) - za.*(cosyawab.*sinrollab - cosrollab.*sinpitchab.*sinyawab) + xa.*cospitchab.*sinyawab; 50 | pointInBFrameXYZ(:,3) = zab - xa.*sinpitchab + za.*cospitchab.*cosrollab + ya.*cospitchab.*sinrollab; 51 | 52 | %this function was created by running the following symbolic code 53 | %syms xab yab zab rollab pitchab yawab xa ya za xb yb zb Rx Ry Rz C 54 | %Rx = [1,0,0; 0,cos(rollab),-sin(rollab); 0,sin(rollab),cos(rollab) ]; 55 | %Ry = [cos(pitchab),0,sin(pitchab); 0,1,0; -sin(pitchab),0,cos(pitchab)]; 56 | %Rz = [cos(yawab),-sin(yawab),0; sin(yawab),cos(yawab),0; 0,0,1]; 57 | %C=(Rz*Ry*Rx); 58 | %T=[xab;yab;zab]; 59 | %Pa=[xa;ya;za]; 60 | %Pb = C*Pa + T; -------------------------------------------------------------------------------- /impl/MultiSensorMultiRegionCost.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function totalCost = MultiSensorMultiRegionCost( sensorToNavPose, dataSets, costFunctions ) 32 | 33 | numSensors = size(sensorToNavPose,1); 34 | numFeatures = size(dataSets,1); 35 | 36 | totalCost = 0; 37 | 38 | % Iterate each feature 39 | for f = 1:numFeatures 40 | % Initialise points for this feature to empty matrix 41 | pointsNED = zeros(0,3); 42 | % Iterate each sensor 43 | for s = 1:numSensors 44 | % Calculate cartesian points (N,E,D) for this sensor's view of the 45 | % feature 46 | if( numel(dataSets(f,s).sensor) > 0 ) 47 | pointsNED = [ pointsNED 48 | feval(dataSets(f,s).georegister, dataSets(f,s), sensorToNavPose(s,:) ) ]; 49 | end 50 | end % S over DataSets(F,:) 51 | if( size(pointsNED,1) > 0) 52 | % Feed NED points into cost function 53 | totalCost = totalCost + feval(costFunctions{f}, pointsNED); 54 | end 55 | end % F over DataSets 56 | totalCost = totalCost / numFeatures; 57 | -------------------------------------------------------------------------------- /impl/RemoveMean.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function y = RemoveMean(x) 32 | y = x - repmat(mean(x),size(x,1),1); 33 | -------------------------------------------------------------------------------- /impl/RemovePointOutliers2.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function [y] = RemovePointOutliers2(x,outlierStDev) 32 | y=RemoveMean(x); 33 | 34 | N = size(x,1); 35 | 36 | [eigvec,eigval] = eig(cov(y)); 37 | 38 | rSq = y*inv(eigvec'); 39 | rSq = rSq*inv(sqrt(eigval)); 40 | rSq = dot(rSq,rSq,2); 41 | 42 | idx = find(rSq < outlierStDev^2); 43 | y = x(idx,:); 44 | -------------------------------------------------------------------------------- /impl/SensorXYZtoWorldNED.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function [pointsNED] = SensorXYZtoWorldNED( dataSet, sensorTransformXYZRPY ) 32 | if( ~isstruct(dataSet) ) 33 | error('RangeSensorCalibrationToolbox:SensorXYZtoWorldNED:InvalidDataFormat', ... 34 | 'DataSet must be a struct array.'); 35 | end 36 | if( ~isfield(dataSet, 'sensor') || ~isfield(dataSet, 'nav') ) 37 | error('RangeSensorCalibrationToolbox:SensorXYZtoWorldNED:InvalidDataFormat', ... 38 | 'DataSet must be a struct array with the following fields: sensor, nav.'); 39 | end 40 | if( size(dataSet.nav,1) ~= size(dataSet.sensor,1) ) 41 | error('RangeSensorCalibrationToolbox:SensorXYZtoWorldNED:InvalidDataFormat', ... 42 | 'Number of rows of nav and sensor data must be equal.'); 43 | end 44 | 45 | if ( size(dataSet.nav,2) ~= 6 || size(dataSet.sensor,2) ~= 3 ) 46 | error('RangeSensorCalibrationToolbox:SensorXYZtoWorldNED:InvalidDataFormat', ... 47 | 'nav should be in the format [x y z roll pitch yaw]', ... 48 | 'and sensor should be in the format [x y z] in the sensor frame.'); 49 | end 50 | 51 | %convert from sensor frame to body frame 52 | pointsBodyXYZ = CoordinateTransform( dataSet.sensor, sensorTransformXYZRPY ); 53 | 54 | %convert from body frame to nav frame 55 | pointsNED = CoordinateTransform( pointsBodyXYZ, dataSet.nav(:,[1,2,3,4,5,6]) ); 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /impl/ValidateInputData.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function ValidateInputData( sensorToNavPose, dataSets, costFunctions, bounds ) 32 | 33 | % sensorToNavPose 34 | numSensors = size(sensorToNavPose,1); 35 | if( size(sensorToNavPose,2) ~= 6 || numSensors < 1 ) 36 | error('RangeSensorCalibrationToolbox:InvalidTransformInput', ... 37 | 'The SensorTransformsXYZRPY matrix must be an NS-by-6 matrix, with NS > 0'); 38 | end 39 | 40 | % DataSets 41 | numFeatures = size(dataSets,1); 42 | if( size(dataSets,2) < 1 || numFeatures < 1 ) 43 | error('RangeSensorCalibrationToolbox:InvalidDataInput', ... 44 | 'DATA must not be empty.'); 45 | end 46 | 47 | if( ~isstruct(dataSets) ) 48 | error('RangeSensorCalibrationToolbox:InvalidDataFormat', ... 49 | 'DATA must be a struct array.'); 50 | end 51 | 52 | if( size(dataSets,2) ~= numSensors ) 53 | error('RangeSensorCalibrationToolbox:DataSizeMismatch', ... 54 | 'The number of sensors does not match between inputs DATA and SensorTransformsXYZRPY. The number of columns in DATA should be equal to the number of rows in SensorTransformsXYZRPY.'); 55 | end 56 | 57 | if( ~isfield(dataSets, 'sensor') || ~isfield(dataSets, 'nav') ) 58 | error('RangeSensorCalibrationToolbox:InvalidDataFormat', ... 59 | 'DATA must be a struct array with the following fields: sensor, nav.'); 60 | end 61 | 62 | for S = 1:numSensors 63 | for F = 1:numFeatures 64 | % Check each element of DATA has correctly formatted fields 65 | [rM, rN] = size(dataSets(F,S).sensor); 66 | [nM, nN] = size(dataSets(F,S).nav); 67 | if( rM ~= nM ), 68 | error('RangeSensorCalibrationToolbox:InvalidDataFormat', ... 69 | 'sensor and nav in the DATA structs must have the same number of rows.'); 70 | end 71 | end 72 | end 73 | 74 | % CostFunctions 75 | if( ~iscell(costFunctions) || ~isvector(costFunctions) ) 76 | error('RangeSensorCalibrationToolbox:InvalidDataFormat', ... 77 | 'COSTFUNS must be a cell vector of function handles.'); 78 | end 79 | for F = 1:length(costFunctions) 80 | if( ~isa(costFunctions{F}, 'char') && ~(numel(costFunctions{F}) == 1 && isa(costFunctions{F}, 'function_handle') ) ) 81 | error('RangeSensorCalibrationToolbox:InvalidDataFormat', ... 82 | 'The elements of the COSTFUNS cell vector must be function handles.'); 83 | end 84 | end 85 | 86 | if( numel(costFunctions) ~= numFeatures ) 87 | error('RangeSensorCalibrationToolbox:DataSizeMismatch', ... 88 | 'The number of geometric features does not match between inputs DATA and COSTFUNS. The number of rows in DATA should be equal to the number of elements in COSTFUNS.'); 89 | end 90 | 91 | if( size(bounds) ~= size(sensorToNavPose) ) 92 | error('RangeSensorCalibrationToolbox:DataSizeMismatch:The size of bounds must equal size of initial offset'); 93 | end 94 | 95 | -------------------------------------------------------------------------------- /visualise/VisualiseDataSets.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | % Visualise a set of geometric features in the data structure as described 31 | % in OptimiseSensorPoses and MultiSensorMultiRegionCost. 32 | % 33 | % VisualiseData(SensorToNavPoses, DataSets) 34 | % 35 | % The format of each input is explained in OptimiseSensorPoses and 36 | % MultiSensorMultiRegionCost. 37 | % 38 | % A 3D plot is shown for each feature defined in the input data. If there 39 | % are multiple sets of data for each feature (i.e. multiple sensors / 40 | % columns in DataSets) then each sensor's view of the feature is coloured 41 | % in order: 42 | % blue, green, red, cyan, magenta, yellow, black 43 | % If there is only one sensor view (one column in DataSets) then the 44 | % feature is split into equal height bins and coloured in the same order 45 | % based on the elevation of the points. 46 | % 47 | % This can be useful in order to see the nature of a single feature when 48 | % first extracting the data, while the per-sensor-colouring is more useful 49 | % when visualising how well the sensors are calibrated together. 50 | % 51 | % See also OPTIMISESENSORPOSES, MULTISENSORMULTIREGIONCOST, PLOT3 52 | function VisualiseDataSets(sensorToNavPoses, dataSets) 53 | 54 | cols = { 'b.', 'g.', 'r.', 'c.', 'm.', 'y.', 'k.' }; 55 | 56 | for F = 1:size(dataSets,1), 57 | figure; 58 | clf 59 | hold on 60 | title(['Feature ' num2str(F)]); 61 | xlabel('Northing') 62 | ylabel('Easting') 63 | zlabel('Down'); 64 | if size(dataSets,2) > 1 65 | for S = 1:size(dataSets,2), 66 | pointsNED = feval(dataSets(F,S).georegister, dataSets(F,S), sensorToNavPoses(S,:)); 67 | if ( size(pointsNED,1) > 0 ) 68 | pointsNED = RemoveMean(pointsNED); 69 | % pointsNED = RemovePointOutliers2(pointsNED,3); 70 | plot3(pointsNED(:,1),pointsNED(:,2),pointsNED(:,3),cols{mod(S,length(cols))}) 71 | end 72 | end 73 | else 74 | pointsNED = feval(dataSets(F,1).georegister, dataSets(F,1), sensorToNavPoses(1,:)); 75 | if ( size(pointsNED,1) > 0 ) 76 | pointsNED = RemoveMean(pointsNED); 77 | % pointsNED = RemovePointOutliers2(pointsNED,3); 78 | maxDown = max(pointsNED(:,3)); 79 | minDown = min(pointsNED(:,3)); 80 | heights = linspace(minDown,maxDown,length(cols)+1); 81 | for C = 1:length(cols) 82 | i = find( pointsNED(:,3) > heights(C) & pointsNED(:,3) < heights(C+1) ); 83 | plot3(pointsNED(i,1),pointsNED(i,2),pointsNED(i,3),cols{C}) 84 | end 85 | end 86 | end 87 | axis equal 88 | grid on 89 | end 90 | -------------------------------------------------------------------------------- /visualise/VisualiseFeature.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function VisualiseFeature(f, s, dataSets, cal) 32 | 33 | pts = feval(dataSets(f,s).georegister, dataSets(f,s), cal); 34 | ptsMean = RemoveMean(pts); 35 | ptsOutliers = RemovePointOutliers2(pts,3); 36 | 37 | subplot(2,1,1); 38 | plot3(ptsMean(:,1), ptsMean(:,2), ptsMean(:,3),'.') 39 | 40 | subplot(2,1,2); 41 | plot3(ptsOutliers(:,1), ptsOutliers(:,2), ptsOutliers(:,3),'.') 42 | -------------------------------------------------------------------------------- /visualise/VisualiseResults.m: -------------------------------------------------------------------------------- 1 | % This file is part of calibrate-range-sensor, a utility to calibrate extrinsic range sensor offsets 2 | % Copyright (c) 2011 The University of Sydney 3 | % All rights reserved. 4 | % 5 | % Redistribution and use in source and binary forms, with or without 6 | % modification, are permitted provided that the following conditions are met: 7 | % 1. Redistributions of source code must retain the above copyright 8 | % notice, this list of conditions and the following disclaimer. 9 | % 2. Redistributions in binary form must reproduce the above copyright 10 | % notice, this list of conditions and the following disclaimer in the 11 | % documentation and/or other materials provided with the distribution. 12 | % 3. Neither the name of the University of Sydney nor the 13 | % names of its contributors may be used to endorse or promote products 14 | % derived from this software without specific prior written permission. 15 | % 16 | % NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE 17 | % GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT 18 | % HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED 19 | % WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | % MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | % BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | % WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | % OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | % IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | % 30 | 31 | function [pBefore,pAfter] = VisualiseResults( sensorTransformsBeforeXYZRPY, sensorTransformsAfterXYZRPY, dataSets, costFunctions ) 32 | 33 | disp( 'offset before calibration (m/deg):' ) 34 | disp( [sensorTransformsBeforeXYZRPY(:,[1,2,3]), rad2deg( sensorTransformsBeforeXYZRPY(:,[4,5,6]) )] ) 35 | disp( 'offset after calibration (m/deg):' ) 36 | disp( [sensorTransformsAfterXYZRPY(:,[1,2,3]), rad2deg( sensorTransformsAfterXYZRPY(:,[4,5,6]) )] ) 37 | disp( 'offset difference:' ) 38 | SensorTransformsAfterXYZRPYDiff=sensorTransformsAfterXYZRPY-sensorTransformsBeforeXYZRPY; 39 | disp( [SensorTransformsAfterXYZRPYDiff(:,[1,2,3]), rad2deg( SensorTransformsAfterXYZRPYDiff(:,[4,5,6]) )] ) 40 | 41 | nSensors = size(sensorTransformsBeforeXYZRPY,1); 42 | nFeatures = size(dataSets,1); 43 | 44 | costsBefore = zeros( nSensors, nFeatures ); 45 | costsAfter = zeros( nSensors, nFeatures ); 46 | pBefore = cell(nFeatures,nSensors); 47 | pAfter = cell(nFeatures,nSensors); 48 | for( f=1:nFeatures ) 49 | for( s=1:nSensors ) 50 | pAfter{f,s} = [feval(dataSets(f,s).georegister, dataSets(f,s), sensorTransformsAfterXYZRPY(s,:) ) ]; 51 | pBefore{f,s} = [feval(dataSets(f,s).georegister, dataSets(f,s), sensorTransformsBeforeXYZRPY(s,:) ) ]; 52 | costsAfter(s,f) = feval(costFunctions{f}, pAfter{f,s}); 53 | costsBefore(s,f) = feval(costFunctions{f}, pBefore{f,s}); 54 | end 55 | end 56 | 57 | disp( 'costs before (sd m):' ) 58 | disp( sqrt(costsBefore) ); 59 | disp( 'costs after (sd m):' ) 60 | disp( sqrt(costsAfter) ); 61 | disp( 'cost difference (sd m):' ) 62 | disp( sqrt(costsAfter)-sqrt(costsBefore) ); 63 | 64 | disp( 'mean per feature before (sd m):' ) 65 | disp( mean( sqrt(costsBefore) ) ); 66 | disp( 'mean per feature after (sd m):' ) 67 | disp( mean( sqrt(costsAfter) ) ); 68 | disp( 'mean difference per feature (sd m)' ); 69 | disp( mean( sqrt(costsAfter) ) - mean( sqrt(costsBefore) ) ); 70 | 71 | disp( 'mean per sensor before (sd m):' ) 72 | disp( mean( sqrt(costsBefore') )' ); 73 | disp( 'mean per sensor after (sd m):' ) 74 | disp( mean( sqrt(costsAfter') )' ); 75 | disp( 'mean difference per sensor (sd m)' ); 76 | disp( mean( sqrt(costsAfter') )' - mean( sqrt(costsBefore') )' ); 77 | 78 | disp( 'mean cost all features before (sd m)' ); 79 | disp( mean( mean( sqrt(costsBefore) )) ); 80 | disp( 'mean cost all features after (sd m)' ); 81 | disp( mean( mean( sqrt(costsAfter) )) ); 82 | disp( 'mean cost difference all features (sd m)' ); 83 | disp( mean( mean( sqrt(costsAfter) ))-mean( mean( sqrt(costsBefore) )) ); --------------------------------------------------------------------------------