├── .gitignore ├── BuildMexFiles.m ├── CalibPath.m ├── GenMovie.m ├── GenTforms.m ├── README.md ├── RunTests.m ├── TestBasic.m ├── genCam ├── FindCamVar.m ├── FordCamInfo.m ├── Fundmatrix.m ├── GenCam.m ├── GetCamTform.m ├── KittiCamInfo.m ├── ReadKittiTimestamps.m ├── ShrimpCamInfo.m ├── Undistort.m └── normalise2dpts.m ├── genNav ├── FordNavInfo.m ├── GenNav.m ├── KittiNavInfo.m ├── ShrimpNavInfo.m ├── deg2utm.m ├── freadstruct.m ├── quat2rot.m └── sizeof.m ├── genVel ├── FindValVar.m ├── FordVelInfo.m ├── GenVel.m ├── GenVelTimeFrac.m ├── GetVelTform.m ├── KittiVelInfo.m ├── ReadPly.m ├── ReadTimeData.m ├── ReadVelData.m ├── ShrimpVelInfo.m ├── SubVel.m ├── VelCorrect.m ├── findPointNormals.m ├── icpMex.mexw64 ├── icpMexTime.mexw64 ├── timingMatrix.mat └── velicp │ ├── CMakeCache.txt │ ├── CMakeFiles │ ├── 2.8.11.2 │ │ ├── CMakeCCompiler.cmake │ │ ├── CMakeCXXCompiler.cmake │ │ ├── CMakeDetermineCompilerABI_C.bin │ │ ├── CMakeDetermineCompilerABI_CXX.bin │ │ ├── CMakeSystem.cmake │ │ ├── CompilerIdC │ │ │ ├── CMakeCCompilerId.c │ │ │ └── a.out │ │ └── CompilerIdCXX │ │ │ ├── CMakeCXXCompilerId.cpp │ │ │ └── a.out │ ├── 2.8.12.2 │ │ ├── CMakeCCompiler.cmake │ │ ├── CMakeCXXCompiler.cmake │ │ ├── CMakeDetermineCompilerABI_C.bin │ │ ├── CMakeDetermineCompilerABI_CXX.bin │ │ ├── CMakeSystem.cmake │ │ ├── CompilerIdC │ │ │ ├── CMakeCCompilerId.c │ │ │ └── a.out │ │ └── CompilerIdCXX │ │ │ ├── CMakeCXXCompilerId.cpp │ │ │ └── a.out │ ├── CMakeDirectoryInformation.cmake │ ├── CMakeOutput.log │ ├── Makefile.cmake │ ├── Makefile2 │ ├── TargetDirectories.txt │ ├── cmake.check_cache │ ├── icp.dir │ │ ├── CXX.includecache │ │ ├── DependInfo.cmake │ │ ├── build.make │ │ ├── cmake_clean.cmake │ │ ├── depend.internal │ │ ├── depend.make │ │ ├── flags.make │ │ ├── link.txt │ │ ├── progress.make │ │ └── src │ │ │ ├── demo.cpp.o │ │ │ ├── icp.cpp.o │ │ │ ├── icpPointToPlane.cpp.o │ │ │ ├── icpPointToPoint.cpp.o │ │ │ ├── kdtree.cpp.o │ │ │ └── matrix.cpp.o │ └── progress.marks │ ├── CMakeLists.txt │ ├── Makefile │ ├── README.TXT │ ├── cmake_install.cmake │ ├── icp │ ├── matlab │ ├── demo_3d.m │ ├── icpMex.cpp │ ├── icpMex.mexa64 │ ├── icpMex.mexw64 │ ├── make.m │ ├── sparsifyMex.cpp │ ├── sparsifyMex.mexa64 │ └── sparsifyMex.mexw64 │ └── src │ ├── demo.cpp │ ├── demo.cpp~ │ ├── icp.cpp │ ├── icp.h │ ├── icpPointToPlane.cpp │ ├── icpPointToPlane.h │ ├── icpPointToPoint.cpp │ ├── icpPointToPoint.h │ ├── kdtree.cpp │ ├── kdtree.h │ ├── matrix.cpp │ └── matrix.h ├── handEye ├── ErrorEstCR.m ├── ErrorEstCR2.m ├── ErrorEstCT.m ├── ErrorEstCT2.m ├── ErrorEstCT3.m ├── ErrorEstR.m ├── ErrorEstT.m ├── Kabsch.m ├── OptR.m ├── OptT.m ├── OptTG.m ├── PlotData.m ├── ReCalProbR.m ├── RoughR.m ├── RoughT.m ├── RoughTS.m ├── SystemErrorT.m ├── SystemProbR.m ├── SystemProbT.m ├── errT.cpp ├── logpdfR.cpp ├── logpdfR.mexw64 ├── logpdfT.cpp ├── logpdfT.mexw64 ├── logpdfT2.cpp └── logpdfT2.mexw64 ├── imageMetric ├── CalcSensorOverlap.m ├── Get2DGradProject.m ├── LevImage.m ├── LevLidar.m ├── LevSub.m ├── MetricRefine.m ├── OcCut.m ├── RefineCamCam.m ├── RefineVelCam.m ├── RefineVelCamMulti.m ├── RunColourMetric.m ├── RunGomMetric.m ├── RunLevMetric.m ├── RunNMIMetric.m ├── cmaes.m ├── colourImage.cpp ├── colourImage.mexa64 ├── colourImage.mexw64 ├── evalGom.m ├── interpolateImage.m ├── interpolateImageMex.mexa64 ├── interpolateImageMex.mexw64 ├── interpolateImageUint8.m ├── interpolateImageUint8Mex.mexa64 ├── interpolateImageUint8Mex.mexw64 ├── miC.m ├── miCMex.cpp ├── miCMex.mexw64 ├── points2Image.m ├── projectLidar.m ├── projectLidarMex.mexa64 └── projectLidarMex.mexw64 ├── mex source ├── buildMex.sln ├── common.h ├── imageFromLidar │ ├── imageFromLidar.cu │ ├── imageFromLidar.vcxproj │ └── x64 │ │ └── Release │ │ └── imageFromLidarMex.mexw64 ├── imageFromLidarDist │ ├── imageFromLidarDist.cu │ └── imageFromLidarDist.vcxproj ├── imageFromLidarPan │ ├── imageFromLidarPan.cu │ └── imageFromLidarPan.vcxproj ├── interpolateImage │ ├── interpolateImage.cu │ └── interpolateImage.vcxproj ├── interpolateImageNearest │ ├── interpolateImageNear.vcxproj │ └── interpolateImageNearest.cu ├── interpolateImageUint8 │ ├── interpolateImageUint8.cu │ └── interpolateImageUint8.vcxproj ├── makefile ├── projectLidar │ ├── projectLidar.cu │ └── projectLidar.vcxproj ├── projectLidarDist │ ├── projectLidarDist.cu │ └── projectLidarDist.vcxproj ├── projectLidarPan │ ├── projectLidarPan.cu │ └── projectLidarPan.vcxproj └── x64 │ └── Release │ ├── imageFromLidarDistMex.mexw64 │ ├── imageFromLidarMex.mexw64 │ ├── imageFromLidarPanMex.mexw64 │ ├── interpolateImageMex.mexw64 │ ├── interpolateImageNearMex.mexw64 │ ├── interpolateImageUint8Mex.mexw64 │ ├── projectLidarDistMex.mexw64 │ ├── projectLidarMex.mexw64 │ └── projectLidarPanMex.mexw64 ├── misc ├── EasyScale.m ├── InvertSensorData.m ├── LoadSensorData.m ├── MyHistEq.m ├── ParSave.m ├── RejectPoints.m ├── SampleData.m ├── SampleData2.m └── UpdateMessage.m ├── pErr.cpp ├── pErr.mexw64 ├── paperPlots ├── Draw_Shrimp.m ├── MyErrorbars.m ├── Plot_1_Timing.m ├── Plot_2_Data_Vel.m ├── Plot_3_Vel_Nav.m ├── Plot_4_Nav_Cam.m ├── Plot_5_Multi.m ├── Plot_6_UpCam.m ├── Plot_7_Refine.m ├── Plot_8_Shrimp_Cams.m ├── Plot_9_All.m ├── Plot_Timing_Mag.m ├── Thumbs.db ├── boundedline.m ├── outlinebounds.m └── varChange.m ├── paperTests ├── Test1CamTiming.m ├── Test2NavCam.m ├── Test3VelNav.m ├── Test4NavCam.m ├── Test5Multi.m ├── Test6UpCam.m ├── Test7Refine.m ├── Test8ShrimpCam.m └── Test9Full.m ├── results ├── Test_1.1_Kitti_Time_0.1s.mat ├── Test_1.2_Kitti_Time_1s.mat ├── Test_1.3_Kitti_Time_5s.mat ├── Test_1.4_Shrimp_Time_0.1s.mat ├── Test_1.5_Shrimp_Time_1s.mat ├── Test_1.6_Shrimp_Time_5s.mat ├── Test_1.7_Ford_Time_0.1s.mat ├── Test_1.7_Shrimp_Time_0.1s.mat ├── Test_1.8_Ford_Time_1s.mat ├── Test_1.9_Ford_Time_5s.mat ├── Test_2.1_Kitti.mat ├── Test_2.2_Shrimp.mat ├── Test_2.3_Ford.mat ├── Test_3.10_Kitti.mat ├── Test_3.11_Shrimp.mat ├── Test_3.12_Shrimp.mat ├── Test_4.10_Kitti.mat ├── Test_5.1_Kitti.mat ├── Test_6.1.mat ├── Test_6.2.mat ├── Test_6.3.mat ├── Test_6.4.mat ├── Test_6.5.mat ├── Test_6.6.mat ├── Test_6.7.mat ├── Test_6.8.mat ├── Test_6.9.mat ├── Test_7.1.mat ├── Test_7.2.mat ├── Test_7.3.mat ├── Test_7.mat ├── Test_8.1.mat ├── Test_8.2.mat └── Test_9.1.mat ├── storedTforms ├── FordCam1Data.mat ├── FordCam2Data.mat ├── FordCam3Data.mat ├── FordCam4Data.mat ├── FordCam5Data.mat ├── FordNavData.mat ├── FordVelData.mat ├── KittiCam1Data.mat ├── KittiCam1DataBS.mat ├── KittiCam2Data.mat ├── KittiCam2DataBS.mat ├── KittiCam3Data.mat ├── KittiCam4Data.mat ├── KittiNavData.mat ├── KittiVelData.mat ├── ShrimpCam1Data.mat ├── ShrimpCam2Data.mat ├── ShrimpCam3Data.mat ├── ShrimpCam4Data.mat ├── ShrimpCam5Data.mat ├── ShrimpCam6Data.mat ├── ShrimpNavData.mat └── ShrimpVelData.mat ├── tforms ├── GenTformGrid.m ├── R2V.m ├── S2V.m ├── T2V.m ├── V2R.cpp ├── V2R.m ├── V2R.mexw64 ├── V2S.m └── V2T.m ├── timing ├── CorrectTimestamps.m ├── FindTimingOffsets.m ├── FindTimingVar.m ├── GetTimingError.m ├── IntSensorData.m ├── RandTformTimes.m ├── SensorDataSubset.m ├── TformInterp.m ├── imax.m ├── wncc.m └── xcorr2_fast.m ├── trimmedTest.m └── varApprox ├── CombEst.m ├── IndVar.m ├── IndVarVec.m ├── OptGrid.m └── lapApprox.m /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | 23 | # VS files 24 | *.opensdf 25 | *.sdf 26 | *.suo 27 | *.log 28 | *.user 29 | *.pdb 30 | *.cache 31 | *.deps 32 | *.exp 33 | *.tlog 34 | 35 | #cmaes data files 36 | *.dat 37 | 38 | #matlab autosaves 39 | *.asv 40 | -------------------------------------------------------------------------------- /BuildMexFiles.m: -------------------------------------------------------------------------------- 1 | %Builds the required mex files 2 | 3 | cd('./tforms'); 4 | mex V2R.cpp 5 | cd('../'); 6 | 7 | cd('./imageMetric'); 8 | mex colourImage.cpp 9 | cd('../'); 10 | 11 | cd('./handEye'); 12 | mex logpdfT.cpp 13 | mex logpdfR.cpp 14 | cd('../'); 15 | 16 | fprintf('Basic functions have finished building\n'); 17 | fprintf('\nNote this script will not build projectLidarMex or interpolateImageUint8Mex\n'); 18 | fprintf(' these files make use of cuda and all the linking and such needed to build is kind of tricky\n'); 19 | fprintf(' either use the provided binaries or use the provided makefiles or vsprojects in mex source\n'); 20 | fprintf('This script also will not build icp mex due to its use of boost\n'); 21 | fprintf(' either use the provided binary or use the provided makefile in genVel/velicp\n'); -------------------------------------------------------------------------------- /CalibPath.m: -------------------------------------------------------------------------------- 1 | function [] = CalibPath( set ) 2 | %CalibPath Adds or removes the Multi-Array-Calib folders from matlabs path 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % set- if true sets the path, otherwise removes it 7 | % 8 | %-------------------------------------------------------------------------- 9 | % References: 10 | %-------------------------------------------------------------------------- 11 | % This function is part of the Multi-Array-Calib toolbox 12 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 13 | % 14 | % This code was written by Zachary Taylor 15 | % zacharyjeremytaylor@gmail.com 16 | % http://www.zjtaylor.com 17 | 18 | validateattributes(set,{'logical'},{'scalar'}); 19 | pathFolders = {}; 20 | 21 | %contains functions for handling camera data and generating camera tforms 22 | pathFolders = [pathFolders,'./genCam']; 23 | 24 | %contains functions for handling velodyne data and generating vel tforms 25 | pathFolders = [pathFolders,'./genVel']; 26 | 27 | %contains functions for handling nav data and generating nav tforms 28 | pathFolders = [pathFolders,'./genNav']; 29 | 30 | %contains functions for performing hand-eye calibartion 31 | pathFolders = [pathFolders,'./handEye']; 32 | 33 | %contains functions for matching the output of overlaping sensors 34 | pathFolders = [pathFolders,'./imageMetric']; 35 | 36 | %contains transformation representations and converters 37 | pathFolders = [pathFolders,'./tforms']; 38 | 39 | %contains timing offset functions and interpolation methods 40 | pathFolders = [pathFolders,'./timing']; 41 | 42 | %contains rough variance approximations used with translation estimates 43 | pathFolders = [pathFolders,'./varApprox']; 44 | 45 | %holds precalculated transformations 46 | pathFolders = [pathFolders,'./storedTforms']; 47 | 48 | %holds results 49 | pathFolders = [pathFolders,'./results']; 50 | 51 | %holds tests run for paper 52 | pathFolders = [pathFolders,'./paperTests']; 53 | 54 | %holds plots for paper 55 | pathFolders = [pathFolders,'./paperPlots']; 56 | 57 | %holds the rest 58 | pathFolders = [pathFolders,'./misc']; 59 | 60 | for i = 1:length(pathFolders) 61 | if(set) 62 | addpath(pathFolders{i}); 63 | else 64 | rmpath(pathFolders{i}); 65 | end 66 | end 67 | 68 | end 69 | 70 | -------------------------------------------------------------------------------- /GenTforms.m: -------------------------------------------------------------------------------- 1 | % this script generates all the required transforms for the kitti and 2 | % shrimp dataset (currently wont do the camera as I am tweaking it) 3 | 4 | %% user set variables 5 | 6 | %path to data 7 | %dataPath = 'C:\Users\Zachary\Documents\Datasets\IJRR-Dataset-1\'; dataset = 'Ford'; 8 | %dataPath = 'C:\Users\Zachary\Documents\Datasets\Shrimp\high-clutter-2\'; dataset = 'Shrimp'; 9 | %dataPath = 'C:\Users\Zachary\Documents\Datasets\Kitti\2011_10_03_drive_0027_extract\'; dataset = 'Kitti'; 10 | dataPath = 'C:\Users\Zachary\Documents\Datasets\Kitti\2011_09_26_drive_0035_extract\'; dataset = 'Kitti'; 11 | 12 | %Sets if the sensor transforms will be plotted 13 | plotTforms = false; 14 | 15 | %% setup folders 16 | CalibPath(true); 17 | 18 | %% process sensors 19 | 20 | %do things in parrallel to save time 21 | for i = 2 22 | switch i 23 | case 1 24 | VelData = GenVel(dataPath, plotTforms, [], dataset); 25 | ParSave(['./storedTforms/' dataset 'VelDataC.mat'], VelData, 'velData'); 26 | case 2 27 | NavData = GenNav(dataPath, plotTforms, [], dataset); 28 | ParSave(['./storedTforms/' dataset 'NavData.mat'], NavData, 'navData'); 29 | case 3 30 | CamData = GenCam(dataPath, plotTforms, [], dataset, 1); 31 | ParSave(['./storedTforms/' dataset 'Cam1Data.mat'], CamData, 'cam1Data'); 32 | case 4 33 | CamData = GenCam(dataPath, plotTforms, [], dataset, 2); 34 | ParSave(['./storedTforms/' dataset 'Cam2Data.mat'], CamData, 'cam2Data'); 35 | case 5 36 | CamData = GenCam(dataPath, plotTforms, [], dataset, 3); 37 | ParSave(['./storedTforms/' dataset 'Cam3Data.mat'], CamData, 'cam3Data'); 38 | case 6 39 | CamData = GenCam(dataPath, plotTforms, [], dataset, 4); 40 | ParSave(['./storedTforms/' dataset 'Cam4Data.mat'], CamData, 'cam4Data'); 41 | case 7 42 | %Kitti only has 4 cameras 43 | if(~strcmpi(dataset,'Kitti')) 44 | CamData = GenCam(dataPath, plotTforms, [], dataset, 5); 45 | ParSave(['./storedTforms/' dataset 'Cam5Data.mat'], CamData, 'cam5Data'); 46 | end 47 | case 8 48 | %only shrimp has 6 cameras 49 | if(strcmpi(dataset,'Shrimp')) 50 | CamData = GenCam(dataPath, plotTforms, [], dataset, 6); 51 | ParSave(['./storedTforms/' dataset 'Cam6Data.mat'], CamData, 'cam6Data'); 52 | end 53 | otherwise 54 | errror('Parfor setup incorrectly'); 55 | end 56 | end 57 | 58 | CalibPath(false); 59 | %delete(gcp); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Array-Calib 2 | Code for Automatic Multimodal Calibration of a Sensor Array 3 | 4 | Note this code makes use of or borrows some lines from the following code 5 | 6 | libICP - icp matching code http://www.cvlibs.net/software/libicp/ 7 | 8 | Kabsch algorithm - http://www.mathworks.com/matlabcentral/fileexchange/25746-kabsch-algorithm 9 | 10 | Image undistortion code - http://stackoverflow.com/questions/12117825/how-can-i-undistort-an-image-in-matlab-using-the-known-camera-parameters 11 | 12 | CMAES optimization - https://www.lri.fr/~hansen/cmaes.m 13 | 14 | Bounds plotting - http://www.mathworks.com/matlabcentral/fileexchange/27485-boundedline--line-plots-with-shaded-error-confidence-intervals 15 | 16 | Fundemental matrix calculations - http://www.csse.uwa.edu.au/ 17 | 18 | Transformation representation - http://asrl.utias.utoronto.ca/code/barfoot_tro14.zip 19 | -------------------------------------------------------------------------------- /TestBasic.m: -------------------------------------------------------------------------------- 1 | % Basic template for tests 2 | 3 | %% user set variables 4 | 5 | %number of scans to use 6 | scansTimeRange = 100; 7 | 8 | %number of scans to combine in metric refine step 9 | numScans = 20; 10 | 11 | %samples 12 | timeSamples = 10000; 13 | 14 | metric = 'motion'; 15 | 16 | %% load sensor data 17 | CalibPath(true); 18 | %make sure to read in cameras last (due to issue with how I compensate for scale) 19 | sensorData = LoadSensorData('Kitti','Vel','Cam1'); 20 | 21 | % %% fix timestamps 22 | fprintf('Finding Timing Offsets\n'); 23 | [sensorData, offsets, varOffsets] = CorrectTimestamps(sensorData, timeSamples); 24 | fprintf('Offsets:\n'); 25 | disp(offsets); 26 | fprintf('Offset sd:\n'); 27 | disp(sqrt(varOffsets)); 28 | 29 | %% run calibration 30 | 31 | %get random contiguous scans to use 32 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 33 | 34 | %evenly sample data 35 | sData = SampleData2(sDataBase); 36 | 37 | %remove uninformative data 38 | sData = RejectPoints(sData, 10, 0.00001); 39 | 40 | %find rotation 41 | fprintf('Finding Rotation\n'); 42 | rotVec = RoughR(sData); 43 | rotVec = OptR(sData, rotVec); 44 | rotVarL = ErrorEstCR(sData, rotVec); 45 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 46 | 47 | fprintf('Rotation:\n'); 48 | disp(rotVec); 49 | fprintf('Rotation lower sd:\n'); 50 | disp(sqrt(rotVarL)); 51 | fprintf('Rotation mid sd:\n'); 52 | disp(sqrt(rotVarM)); 53 | 54 | %find camera transformation scale (only used for RoughT, OptT does its 55 | %own smarter/better thing 56 | fprintf('Finding Camera Scale\n'); 57 | sDataS = EasyScale(sData, rotVec, rotVarL,zeros(2,3),ones(2,3)); 58 | 59 | %show what we are dealing with 60 | %PlotData(sDataS,rotVec); 61 | 62 | fprintf('Finding Translation\n'); 63 | tranVec = RoughT(sDataS, rotVec); 64 | tranVec = OptT(sData, tranVec, rotVec, rotVarM); 65 | tranVarL = ErrorEstCT(sData, tranVec, rotVec, rotVarL); 66 | tranVarM = max(tranVarL,ErrorEstCT2(sData, tranVec, rotVec, rotVarM)); 67 | 68 | fprintf('Translation:\n'); 69 | disp(tranVec); 70 | fprintf('Translation lower sd:\n'); 71 | disp(sqrt(tranVarL)); 72 | fprintf('Translation mid sd:\n'); 73 | disp(sqrt(tranVarM)); 74 | 75 | %get grid of transforms 76 | fprintf('Generating transformation grid\n'); 77 | [TGrid, vTGrid] = GenTformGrid(tranVec, rotVec, tranVarM, rotVarM); 78 | 79 | %refine transforms using metrics 80 | fprintf('Refining transformations\n'); 81 | [TGridR, vTGridR] = MetricRefine(TGrid, vTGrid, sDataBase, numScans, metric); 82 | 83 | %correct for differences in grid 84 | fprintf('Combining results\n'); 85 | [finalVec, finalVar] = OptGrid(TGridR, vTGridR); 86 | -------------------------------------------------------------------------------- /genCam/Undistort.m: -------------------------------------------------------------------------------- 1 | function [ image ] = Undistort(imageD, D, K) 2 | %FORDCAMINFO Sets the directory layout, masks and intrinsics for the ford 3 | % dataset 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % imageD- distorted input image 8 | % D- 5x1 distortion vector 9 | % K-3x4 camera matrix 10 | % 11 | %-------------------------------------------------------------------------- 12 | % Outputs: 13 | %-------------------------------------------------------------------------- 14 | % I- undistorted image 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is a slightly modified version of this stackoverflow code 20 | % http://stackoverflow.com/questions/12117825/how-can-i-undistort-an-image-in-matlab-using-the-known-camera-parameters 21 | 22 | validateattributes(imageD,{'numeric'},{'2d'}); 23 | validateattributes(D,{'numeric'},{'size',[5,1]}); 24 | validateattributes(K,{'numeric'},{'size',[3,4]}); 25 | 26 | 27 | fx = K(1,1); 28 | fy = K(2,2); 29 | cx = K(1,3); 30 | cy = K(2,3); 31 | k1 = D(1); 32 | k2 = D(2); 33 | k3 = D(5); 34 | p1 = D(3); 35 | p2 = D(4); 36 | 37 | image = zeros(size(imageD)); 38 | [i, j] = find(~isnan(image)); 39 | 40 | % Xp = the xyz vals of points on the z plane 41 | Xp = K\[j i ones(length(i),1)]'; 42 | 43 | % Now we calculate how those points distort i.e forward map them through the distortion 44 | r2 = Xp(1,:).^2+Xp(2,:).^2; 45 | x = Xp(1,:); 46 | y = Xp(2,:); 47 | 48 | x = x.*(1+k1*r2 + k2*r2.^2 + k3*r2.^3) + 2*p1.*x.*y + p2*(r2 + 2*x.^2); 49 | y = y.*(1+k1*r2 + k2*r2.^2 + k3*r2.^3) + 2*p2.*x.*y + p1*(r2 + 2*y.^2); 50 | 51 | % u and v are now the distorted cooridnates 52 | u = reshape(fx*x + cx,size(image)); 53 | v = reshape(fy*y + cy,size(image)); 54 | 55 | c = class(imageD); 56 | 57 | % Now we perform a backward mapping in order to undistort the warped image coordinates 58 | image = interp2(double(imageD), u, v); 59 | 60 | image = eval([c '(image)']); 61 | 62 | end 63 | 64 | -------------------------------------------------------------------------------- /genCam/normalise2dpts.m: -------------------------------------------------------------------------------- 1 | % NORMALISE2DPTS - normalises 2D homogeneous points 2 | % 3 | % Function translates and normalises a set of 2D homogeneous points 4 | % so that their centroid is at the origin and their mean distance from 5 | % the origin is sqrt(2). This process typically improves the 6 | % conditioning of any equations used to solve homographies, fundamental 7 | % matrices etc. 8 | % 9 | % Usage: [newpts, T] = normalise2dpts(pts) 10 | % 11 | % Argument: 12 | % pts - 3xN array of 2D homogeneous coordinates 13 | % 14 | % Returns: 15 | % newpts - 3xN array of transformed 2D homogeneous coordinates. The 16 | % scaling parameter is normalised to 1 unless the point is at 17 | % infinity. 18 | % T - The 3x3 transformation matrix, newpts = T*pts 19 | % 20 | % If there are some points at infinity the normalisation transform 21 | % is calculated using just the finite points. Being a scaling and 22 | % translating transform this will not affect the points at infinity. 23 | 24 | % Peter Kovesi 25 | % School of Computer Science & Software Engineering 26 | % The University of Western Australia 27 | % pk at csse uwa edu au 28 | % http://www.csse.uwa.edu.au/~pk 29 | % 30 | % May 2003 - Original version 31 | % February 2004 - Modified to deal with points at infinity. 32 | % December 2008 - meandist calculation modified to work with Octave 3.0.1 33 | % (thanks to Ron Parr) 34 | 35 | 36 | function [newpts, T] = normalise2dpts(pts) 37 | 38 | if size(pts,1) ~= 3 39 | error('pts must be 3xN'); 40 | end 41 | 42 | % Find the indices of the points that are not at infinity 43 | finiteind = find(abs(pts(3,:)) > eps); 44 | 45 | if length(finiteind) ~= size(pts,2) 46 | warning('Some points are at infinity'); 47 | end 48 | 49 | % For the finite points ensure homogeneous coords have scale of 1 50 | pts(1,finiteind) = pts(1,finiteind)./pts(3,finiteind); 51 | pts(2,finiteind) = pts(2,finiteind)./pts(3,finiteind); 52 | pts(3,finiteind) = 1; 53 | 54 | c = mean(pts(1:2,finiteind)')'; % Centroid of finite points 55 | newp(1,finiteind) = pts(1,finiteind)-c(1); % Shift origin to centroid. 56 | newp(2,finiteind) = pts(2,finiteind)-c(2); 57 | 58 | dist = sqrt(newp(1,finiteind).^2 + newp(2,finiteind).^2); 59 | meandist = mean(dist(:)); % Ensure dist is a column vector for Octave 3.0.1 60 | 61 | scale = sqrt(2)/meandist; 62 | 63 | T = [scale 0 -scale*c(1) 64 | 0 scale -scale*c(2) 65 | 0 0 1 ]; 66 | 67 | newpts = T*pts; 68 | 69 | 70 | -------------------------------------------------------------------------------- /genNav/FordNavInfo.m: -------------------------------------------------------------------------------- 1 | function [ navData ] = FordNavInfo( path ) 2 | %FORDNAVINFO Grabs the raw data from ford nav sensor 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % path- path to where the ford dataset is stored 7 | % 8 | %-------------------------------------------------------------------------- 9 | % Outputs: 10 | %-------------------------------------------------------------------------- 11 | % navData- struct holding navigation information 12 | % 13 | %-------------------------------------------------------------------------- 14 | % References: 15 | %-------------------------------------------------------------------------- 16 | % This function is part of the Multi-Array-Calib toolbox 17 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 18 | % 19 | % This code was modified from the load_pose_applanix.m function that came 20 | % with the processing code for the ford dataset 21 | % http://robots.engin.umich.edu/SoftwareData/Ford 22 | 23 | %check inputs 24 | validateattributes(path,{'char'},{'vector'}); 25 | if(~exist(path,'dir')) 26 | error('%s is not a valid directory'); 27 | end 28 | 29 | navData.folder = [path '/Processed/']; 30 | navData.files = dir([navData.folder,'*.log']); 31 | 32 | Pose.utime = uint64(0); 33 | Pose.pos = double(zeros(3,1)); 34 | Pose.vel = double(zeros(3,1)); 35 | Pose.orientation = double(zeros(4,1)); % quaternions 36 | Pose.rotation_rate = double(zeros(3,1)); 37 | Pose.accel = double(zeros(3,1)); 38 | 39 | %read the file 40 | Pose = freadstruct([navData.folder navData.files(1).name],Pose); 41 | 42 | navData.time = Pose.utime; 43 | 44 | %sort times into chronological order 45 | [~,idx] = sort(navData.time,'ascend'); 46 | %remove double ups 47 | idx = idx([diff(navData.time(idx));1] > 0); 48 | 49 | %sort results 50 | Pose.orientation = Pose.orientation(idx,:); 51 | Pose.pos = Pose.pos(idx,:); 52 | Pose.utime = Pose.utime(idx,:); 53 | navData.time = navData.time(idx,:); 54 | 55 | navData.T_S1_Sk = zeros(length(navData.time),6); 56 | for i = 1:length(navData.time) 57 | temp = eye(4); 58 | temp(1:3,1:3) = quat2rot(Pose.orientation(i,:)); 59 | temp(1:3,4) = Pose.pos(i,:); 60 | navData.T_S1_Sk(i,:) = T2V(temp); 61 | end 62 | 63 | navData.files = repmat(navData.files,size(navData.T_S1_Sk,1),1); 64 | 65 | navData.T_Var_Skm1_Sk = repmat([0.01,0.01,0.01,0.01*pi/180,0.01*pi/180,0.01*pi/180],size(navData.T_S1_Sk,1),1); 66 | 67 | end 68 | -------------------------------------------------------------------------------- /genNav/quat2rot.m: -------------------------------------------------------------------------------- 1 | function Q = quat2rot(q) 2 | %QUAT2ROT quaternion to rotation matrix. 3 | % Q = QUAT2ROT(q) takes a 4-vector unit quaternion reprsented by q, 4 | % (i.e. q = [q0;qx;qy;qz]) and returns the corresponding [3 x 3] 5 | % orthonormal rotation matrix Q. 6 | % 7 | %----------------------------------------------------------------- 8 | % History: 9 | % Date Who What 10 | % ----------- ------- ----------------------------- 11 | % 09-10-2003 rme Created. 12 | % 09-20-2003 rme Changed to store the scalar part as the 13 | % first element instead of as the last element. 14 | 15 | q0 = q(1); 16 | qx = q(2); 17 | qy = q(3); 18 | qz = q(4); 19 | 20 | Q = zeros(3); 21 | Q(1,1) = q0^2 + qx^2 - qy^2 - qz^2; 22 | Q(1,2) = 2*(qx*qy - q0*qz); 23 | Q(1,3) = 2*(qx*qz + q0*qy); 24 | Q(2,1) = 2*(qy*qx + q0*qz); 25 | Q(2,2) = q0^2 - qx^2 + qy^2 - qz^2; 26 | Q(2,3) = 2*(qy*qz - q0*qx); 27 | Q(3,1) = 2*(qz*qx - q0*qy); 28 | Q(3,2) = 2*(qz*qy + q0*qx); 29 | Q(3,3) = q0^2 - qx^2 - qy^2 + qz^2; 30 | -------------------------------------------------------------------------------- /genNav/sizeof.m: -------------------------------------------------------------------------------- 1 | function bytes = sizeof(arg) 2 | %SIZEOF Return the size (in bytes) of a variable as stored on disk. 3 | % BYTES = SIZEOF(ARG) determines the size (in bytes) of a structure or other 4 | % (simple) variable. It is designed to correspond to the C sizeof() operator. 5 | % 6 | % Examples: 7 | % a = struct('a', int32(1), 'b', double(3)); 8 | % bytes = sizeof(a); % 4 + 8 = 12 bytes 9 | % 10 | % (c) 2007 Ryan M. Eustice 11 | % University of Michigan 12 | % eustice@umich.edu 13 | % 14 | %----------------------------------------------------------------- 15 | % History: 16 | % Date Who What 17 | % ----------- ------- ----------------------------- 18 | % 08-06-2007 RME Created and written. 19 | 20 | bytes = privateSizeOf(arg); 21 | 22 | %=============================================================================== 23 | function bytes = privateSizeOf(arg) 24 | 25 | switch (class(arg)) 26 | case {'int8' 'uint8' 'char'} 27 | bytes = 1 * numel(arg); 28 | case {'int16' 'uint16'} 29 | bytes = 2 * numel(arg); 30 | case {'int32' 'uint32' 'single'} 31 | bytes = 4 * numel(arg); 32 | case {'int64' 'uint64' 'double'} 33 | bytes = 8 * numel(arg); 34 | case {'struct'} 35 | bytes = 0; 36 | fields = fieldnames(arg); 37 | for k=1:length(fields) 38 | bytes = bytes + privateSizeOf(arg.(fields{k})); 39 | end 40 | otherwise 41 | error('MATLAB:sizeof:badtype', 'Unknown type provided as an argument.'); 42 | end 43 | -------------------------------------------------------------------------------- /genVel/FordVelInfo.m: -------------------------------------------------------------------------------- 1 | function [ velData ] = FordVelInfo( fordPath ) 2 | %FORDVELINFO Sets the directory layout and timestamps for the ford dataset 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % fordPath- path to where the ford dataset is stored 7 | % 8 | %-------------------------------------------------------------------------- 9 | % Outputs: 10 | %-------------------------------------------------------------------------- 11 | % velData- struct holding velodyne information 12 | % 13 | %-------------------------------------------------------------------------- 14 | % References: 15 | %-------------------------------------------------------------------------- 16 | % This function is part of the Multi-Array-Calib toolbox 17 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 18 | % 19 | % This code was written by Zachary Taylor 20 | % zacharyjeremytaylor@gmail.com 21 | % http://www.zjtaylor.com 22 | 23 | velData = struct; 24 | 25 | %check inputs 26 | validateattributes(fordPath,{'char'},{'vector'}); 27 | if(~exist(fordPath,'dir')) 28 | error('%s is not a valid directory'); 29 | end 30 | 31 | %set destination folder 32 | velData.folder = [fordPath '/Processed/velodyne/']; 33 | 34 | %get files 35 | velData.files = dir([velData.folder,'*.ply']); 36 | 37 | %timestamps 38 | velData.time = ReadTimeData([fordPath '/Processed/velodyne/timestamps.bin']); 39 | 40 | end 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /genVel/GenVelTimeFrac.m: -------------------------------------------------------------------------------- 1 | function [ timeFrac, valid ] = GenVelTimeFrac( xyz ) 2 | %GENVELTIMEFRAC Estimates velodyne time fraction for scans where the 3 | % timing information has been lost 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % xyz- nx3 list of 3d velodyne points, uses shrimp corrdinates (-x is 8 | % foward, +y is right, -z is up 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % timeFrac- nx1 time fraction 14 | % valid- nx1 list of valid points (ignores ends of scan) 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is part of the Multi-Array-Calib toolbox 20 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 21 | % 22 | % This code was written by Zachary Taylor 23 | % zacharyjeremytaylor@gmail.com 24 | % http://www.zjtaylor.com 25 | 26 | %check inputs 27 | validateattributes(xyz,{'numeric'},{'ncols',3}); 28 | 29 | %ensure of type double 30 | xyz = double(xyz); 31 | 32 | %project onto sphere 33 | sphere = zeros(size(xyz,1),2); 34 | sphere(:,1) = atan2(xyz(:,2), -xyz(:,1)); 35 | sphere(:,2) = atan(xyz(:,3)./ sqrt(xyz(:,2).^2 + xyz(:,1).^2)); 36 | 37 | % %discretize 38 | % sphere = 5000*sphere/(2*pi); 39 | % sphere(:,1) = sphere(:,1) + 2500; 40 | % sphere(:,2) = sphere(:,2) + 50; 41 | % sphere = ceil(sphere); 42 | % sphere = max(sphere,1); 43 | % 44 | % idx = sphere(:,2) + (sphere(:,1)-1)*400; 45 | % 46 | % %find timeFrac 47 | % load('timingMatrix.mat'); 48 | % timeFrac = timeMatrix(idx); 49 | 50 | timeFrac = 0.5*sphere(:,1)./pi; 51 | 52 | %find valid 53 | valid = true(size(timeFrac)); 54 | valid(timeFrac(:,1) < -0.45) = false; 55 | valid(timeFrac(:,1) > 0.45) = false; 56 | -------------------------------------------------------------------------------- /genVel/KittiVelInfo.m: -------------------------------------------------------------------------------- 1 | function [ velData ] = KittiVelInfo( kittiPath ) 2 | %KITTIVELINFO Sets the directory layout and timestamps for the kitti 3 | % dataset 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % kittiPath- path to where the kitti dataset is stored 8 | % 9 | %-------------------------------------------------------------------------- 10 | % Outputs: 11 | %-------------------------------------------------------------------------- 12 | % velData- struct holding velodyne information 13 | % 14 | %-------------------------------------------------------------------------- 15 | % References: 16 | %-------------------------------------------------------------------------- 17 | % This function is part of the Multi-Array-Calib toolbox 18 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 19 | % 20 | % This code was written by Zachary Taylor 21 | % zacharyjeremytaylor@gmail.com 22 | % http://www.zjtaylor.com 23 | 24 | velData = struct; 25 | 26 | %check inputs 27 | validateattributes(kittiPath,{'char'},{'vector'}); 28 | if(~exist(kittiPath,'dir')) 29 | error('%s is not a valid directory'); 30 | end 31 | 32 | %set destination folder 33 | velData.folder = [kittiPath '/velodyne_points/data/']; 34 | 35 | %get files 36 | velData.files = dir([velData.folder,'*.txt']); 37 | 38 | %timestamps 39 | [velData.time, ~, ~] = ReadKittiTimestamps([velData.folder '../']); 40 | 41 | end 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /genVel/ReadPly.m: -------------------------------------------------------------------------------- 1 | function [ data ] = ReadPly( filename ) 2 | %READPLY Reads in dataset from plyfile (WARNING WILL NOT READ GENERAL PLY 3 | %FILES, FOR THEM USE PLY_READ) 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % filename-location of ply file to read 8 | % 9 | %-------------------------------------------------------------------------- 10 | % Outputs: 11 | %-------------------------------------------------------------------------- 12 | % data- data stored in ply 13 | % 14 | %-------------------------------------------------------------------------- 15 | % References: 16 | %-------------------------------------------------------------------------- 17 | % This function is part of the Multi-Array-Calib toolbox 18 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 19 | % 20 | % This code was written by Zachary Taylor 21 | % zacharyjeremytaylor@gmail.com 22 | % http://www.zjtaylor.com 23 | 24 | %open 25 | [ fid, Msg ] = fopen (filename, 'r', 'ieee-be'); 26 | 27 | if ( fid == -1 ) 28 | error(Msg); 29 | end 30 | 31 | lineSize = 19; 32 | 33 | header = fread(fid,500,'*char')'; 34 | 35 | %get length of header 36 | key = 'end_header'; 37 | idx = strfind(header, key); 38 | header = header(1:(idx(1)+length(key))); 39 | 40 | %get length of data 41 | key = 'element vertex '; 42 | idx = strfind(header, key); 43 | numel = sscanf(header(idx(1) + length(key):end), '%g', 1); 44 | 45 | %seek to start of data 46 | fseek(fid,length(header),'bof'); 47 | 48 | %read data 49 | data.vertex.x = fread(fid,numel,'float',lineSize-4); 50 | fseek(fid,length(header)+4,'bof'); 51 | data.vertex.y = fread(fid,numel,'float',lineSize-4); 52 | fseek(fid,length(header)+8,'bof'); 53 | data.vertex.z = fread(fid,numel,'float',lineSize-4); 54 | fseek(fid,length(header)+12,'bof'); 55 | 56 | data.vertex.intensity = fread(fid,numel,'uchar',lineSize-1); 57 | fseek(fid,length(header)+13,'bof'); 58 | 59 | data.vertex.timeOffset = fread(fid,numel,'int32',lineSize-4); 60 | fseek(fid,length(header)+17,'bof'); 61 | 62 | data.vertex.beamId = fread(fid,numel,'uchar',lineSize-1); 63 | fseek(fid,length(header)+18,'bof'); 64 | 65 | data.vertex.valid = fread(fid,numel,'uchar',lineSize-1); 66 | 67 | fclose(fid); 68 | 69 | 70 | end 71 | 72 | -------------------------------------------------------------------------------- /genVel/ReadTimeData.m: -------------------------------------------------------------------------------- 1 | function [ t ] = ReadTimeData( fileName ) 2 | %READTIMEDATA Reads the timestamp data for the ford and shrimp dataset 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % fileName- path of the time filename 7 | % 8 | %-------------------------------------------------------------------------- 9 | % Outputs: 10 | %-------------------------------------------------------------------------- 11 | % t- nx1 time 12 | % 13 | %-------------------------------------------------------------------------- 14 | % References: 15 | %-------------------------------------------------------------------------- 16 | % This function is part of the Multi-Array-Calib toolbox 17 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 18 | % 19 | % This code was written by Zachary Taylor 20 | % zacharyjeremytaylor@gmail.com 21 | % http://www.zjtaylor.com 22 | 23 | %check input 24 | validateattributes(fileName,{'char'},{'vector'}); 25 | 26 | if(~exist(fileName,'file')) 27 | error('%s is not a valid file'); 28 | end 29 | 30 | fid = fopen(fileName,'r'); 31 | 32 | t = fread(fid, inf, '*uint64'); 33 | 34 | fclose(fid); 35 | 36 | end 37 | 38 | -------------------------------------------------------------------------------- /genVel/ShrimpVelInfo.m: -------------------------------------------------------------------------------- 1 | function [ velData ] = ShrimpVelInfo( shrimpPath ) 2 | %SHRIMPVELINFO Sets the directory layout and timestamps for the shrimp 3 | % dataset 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % shrimpPath- path to where the shrimp dataset is stored 8 | % 9 | %-------------------------------------------------------------------------- 10 | % Outputs: 11 | %-------------------------------------------------------------------------- 12 | % velData- struct holding velodyne information 13 | % 14 | %-------------------------------------------------------------------------- 15 | % References: 16 | %-------------------------------------------------------------------------- 17 | % This function is part of the Multi-Array-Calib toolbox 18 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 19 | % 20 | % This code was written by Zachary Taylor 21 | % zacharyjeremytaylor@gmail.com 22 | % http://www.zjtaylor.com 23 | 24 | velData = struct; 25 | 26 | %check inputs 27 | validateattributes(shrimpPath,{'char'},{'vector'}); 28 | if(~exist(shrimpPath,'dir')) 29 | error('%s is not a valid directory'); 30 | end 31 | 32 | %set destination folder 33 | velData.folder = [shrimpPath '/Processed/velodyne/']; 34 | 35 | %get files 36 | velData.files = dir([velData.folder,'*.ply']); 37 | 38 | %timestamps 39 | velData.time = ReadTimeData([shrimpPath '/Processed/velodyne/timestamps.bin']); 40 | 41 | %get every third scan (20 Hz is too much info) 42 | velData.files = velData.files(1:3:end); 43 | velData.time = velData.time(1:3:end); 44 | 45 | end 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /genVel/SubVel.m: -------------------------------------------------------------------------------- 1 | function [ subVel, idx ] = SubVel( vel, sub ) 2 | %SUBVEL subsamples velodyne taking most salient points as determined by a 3 | % difference in distance metric + some random points 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % vel- nxm set of velodyne points where m >= 3 8 | % sub- number of points to subsample down to 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % subVel- subxm set of velodyne points 14 | % idx- subx1 index of the velodyne points 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is part of the Multi-Array-Calib toolbox 20 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 21 | % 22 | % This code was written by Zachary Taylor 23 | % zacharyjeremytaylor@gmail.com 24 | % http://www.zjtaylor.com 25 | 26 | %check inputs 27 | validateattributes(vel,{'numeric'},{'2d'}); 28 | if(size(vel,2) < 3) 29 | error('vel must have atleast 3 columns'); 30 | end 31 | 32 | validateattributes(sub,{'numeric'},{'scalar','positive'}); 33 | sub = ceil(size(vel,1)/sub); 34 | 35 | %get distance 36 | dist = sqrt(sum(vel(:,1:3).^2,2)); 37 | 38 | %project onto sphere 39 | sphere = zeros(size(vel,1),2); 40 | sphere(:,1) = atan2(vel(:,2), vel(:,1)); 41 | sphere(:,2) = atan(vel(:,3)./ sqrt(vel(:,2).^2 + vel(:,1).^2)); 42 | 43 | %find closest points 44 | idx = knnsearch(sphere,sphere,'k',5); 45 | idx = idx(:,2:end); 46 | diff = abs(reshape(dist(idx),size(idx)) - repmat(dist,1,size(idx,2))); 47 | diff = max(diff,[],2)./dist; 48 | 49 | %sort by distance 50 | [~,diff] = sort(diff,'descend'); 51 | 52 | %get index of subsampled points 53 | idx = diff(1:sub); 54 | 55 | %remove close and far points 56 | dist = dist(idx); 57 | valid = and(dist < 100, dist > 3); 58 | idx = idx(valid); 59 | 60 | %subsample 61 | subVel = vel(idx,:); 62 | 63 | end 64 | 65 | -------------------------------------------------------------------------------- /genVel/VelCorrect.m: -------------------------------------------------------------------------------- 1 | function [ points ] = VelCorrect( points, tFrac, tform ) 2 | %VELCORRECT correct for motion during the recording of lidar points. 3 | % Assumes a static enviroment 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % points- nxm set of velodyne points where m >= 3 8 | % tFrac- nx1 vector of amount of time that has passed since the point was 9 | % recorded. Assumes that last frame occoured at -1 and next frame 10 | % will happen at 1 11 | % tform- 1x7 vector, tform from previous frame to this one 12 | % 13 | %-------------------------------------------------------------------------- 14 | % Outputs: 15 | %-------------------------------------------------------------------------- 16 | % points- nxm set of velodyne points ajusted for timing offset 17 | % 18 | %-------------------------------------------------------------------------- 19 | % References: 20 | %-------------------------------------------------------------------------- 21 | % This function is part of the Multi-Array-Calib toolbox 22 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 23 | % 24 | % This code was written by Zachary Taylor 25 | % zacharyjeremytaylor@gmail.com 26 | % http://www.zjtaylor.com 27 | 28 | %check inputs 29 | validateattributes(points,{'numeric'},{'2d'}); 30 | if(size(points,2) < 3) 31 | error('points must have atleast 3 columns'); 32 | end 33 | validateattributes(tFrac,{'numeric'},{'numel',size(points,1),'>=',-1,'<=',1}); 34 | validateattributes(tform,{'numeric'},{'size',[1,6]}); 35 | 36 | %ensure inputs are of type doubles 37 | points = double(points); 38 | tFrac = double(tFrac(:)); 39 | tform = double(tform); 40 | 41 | %get difference between poisitions 42 | pointsDiff = V2T(tform)*[points(:,1:3), ones(size(points,1),1)]'; 43 | pointsDiff = pointsDiff(1:3,:)'; 44 | pointsDiff = pointsDiff - points(:,1:3); 45 | 46 | %account for timing offset 47 | points(:,1:3) = points(:,1:3) - repmat(tFrac,1,3).*pointsDiff; 48 | 49 | end 50 | 51 | -------------------------------------------------------------------------------- /genVel/icpMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/icpMex.mexw64 -------------------------------------------------------------------------------- /genVel/icpMexTime.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/icpMexTime.mexw64 -------------------------------------------------------------------------------- /genVel/timingMatrix.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/timingMatrix.mat -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.11.2/CMakeCCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_C_COMPILER "/usr/bin/cc") 2 | set(CMAKE_C_COMPILER_ARG1 "") 3 | set(CMAKE_C_COMPILER_ID "GNU") 4 | set(CMAKE_C_COMPILER_VERSION "4.8.1") 5 | set(CMAKE_C_PLATFORM_ID "Linux") 6 | 7 | set(CMAKE_AR "/usr/bin/ar") 8 | set(CMAKE_RANLIB "/usr/bin/ranlib") 9 | set(CMAKE_LINKER "/usr/bin/ld") 10 | set(CMAKE_COMPILER_IS_GNUCC 1) 11 | set(CMAKE_C_COMPILER_LOADED 1) 12 | set(CMAKE_C_COMPILER_WORKS TRUE) 13 | set(CMAKE_C_ABI_COMPILED TRUE) 14 | set(CMAKE_COMPILER_IS_MINGW ) 15 | set(CMAKE_COMPILER_IS_CYGWIN ) 16 | if(CMAKE_COMPILER_IS_CYGWIN) 17 | set(CYGWIN 1) 18 | set(UNIX 1) 19 | endif() 20 | 21 | set(CMAKE_C_COMPILER_ENV_VAR "CC") 22 | 23 | if(CMAKE_COMPILER_IS_MINGW) 24 | set(MINGW 1) 25 | endif() 26 | set(CMAKE_C_COMPILER_ID_RUN 1) 27 | set(CMAKE_C_SOURCE_FILE_EXTENSIONS c) 28 | set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) 29 | set(CMAKE_C_LINKER_PREFERENCE 10) 30 | 31 | # Save compiler ABI information. 32 | set(CMAKE_C_SIZEOF_DATA_PTR "8") 33 | set(CMAKE_C_COMPILER_ABI "ELF") 34 | set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 35 | 36 | if(CMAKE_C_SIZEOF_DATA_PTR) 37 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") 38 | endif() 39 | 40 | if(CMAKE_C_COMPILER_ABI) 41 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") 42 | endif() 43 | 44 | if(CMAKE_C_LIBRARY_ARCHITECTURE) 45 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 46 | endif() 47 | 48 | 49 | 50 | 51 | set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c") 52 | set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 53 | set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.11.2/CMakeCXXCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_CXX_COMPILER "/usr/bin/c++") 2 | set(CMAKE_CXX_COMPILER_ARG1 "") 3 | set(CMAKE_CXX_COMPILER_ID "GNU") 4 | set(CMAKE_CXX_COMPILER_VERSION "4.8.1") 5 | set(CMAKE_CXX_PLATFORM_ID "Linux") 6 | 7 | set(CMAKE_AR "/usr/bin/ar") 8 | set(CMAKE_RANLIB "/usr/bin/ranlib") 9 | set(CMAKE_LINKER "/usr/bin/ld") 10 | set(CMAKE_COMPILER_IS_GNUCXX 1) 11 | set(CMAKE_CXX_COMPILER_LOADED 1) 12 | set(CMAKE_CXX_COMPILER_WORKS TRUE) 13 | set(CMAKE_CXX_ABI_COMPILED TRUE) 14 | set(CMAKE_COMPILER_IS_MINGW ) 15 | set(CMAKE_COMPILER_IS_CYGWIN ) 16 | if(CMAKE_COMPILER_IS_CYGWIN) 17 | set(CYGWIN 1) 18 | set(UNIX 1) 19 | endif() 20 | 21 | set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") 22 | 23 | if(CMAKE_COMPILER_IS_MINGW) 24 | set(MINGW 1) 25 | endif() 26 | set(CMAKE_CXX_COMPILER_ID_RUN 1) 27 | set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) 28 | set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP) 29 | set(CMAKE_CXX_LINKER_PREFERENCE 30) 30 | set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) 31 | 32 | # Save compiler ABI information. 33 | set(CMAKE_CXX_SIZEOF_DATA_PTR "8") 34 | set(CMAKE_CXX_COMPILER_ABI "ELF") 35 | set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 36 | 37 | if(CMAKE_CXX_SIZEOF_DATA_PTR) 38 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") 39 | endif() 40 | 41 | if(CMAKE_CXX_COMPILER_ABI) 42 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") 43 | endif() 44 | 45 | if(CMAKE_CXX_LIBRARY_ARCHITECTURE) 46 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 47 | endif() 48 | 49 | 50 | 51 | 52 | set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c") 53 | set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 54 | set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.11.2/CMakeDetermineCompilerABI_C.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.11.2/CMakeDetermineCompilerABI_C.bin -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.11.2/CMakeDetermineCompilerABI_CXX.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.11.2/CMakeDetermineCompilerABI_CXX.bin -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.11.2/CMakeSystem.cmake: -------------------------------------------------------------------------------- 1 | 2 | 3 | set(CMAKE_SYSTEM "Linux-3.11.0-14-generic") 4 | set(CMAKE_SYSTEM_NAME "Linux") 5 | set(CMAKE_SYSTEM_VERSION "3.11.0-14-generic") 6 | set(CMAKE_SYSTEM_PROCESSOR "x86_64") 7 | 8 | set(CMAKE_HOST_SYSTEM "Linux-3.11.0-14-generic") 9 | set(CMAKE_HOST_SYSTEM_NAME "Linux") 10 | set(CMAKE_HOST_SYSTEM_VERSION "3.11.0-14-generic") 11 | set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") 12 | 13 | set(CMAKE_CROSSCOMPILING "FALSE") 14 | 15 | set(CMAKE_SYSTEM_LOADED 1) 16 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.11.2/CompilerIdC/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.11.2/CompilerIdC/a.out -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.11.2/CompilerIdCXX/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.11.2/CompilerIdCXX/a.out -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_C_COMPILER "/usr/bin/cc") 2 | set(CMAKE_C_COMPILER_ARG1 "") 3 | set(CMAKE_C_COMPILER_ID "GNU") 4 | set(CMAKE_C_COMPILER_VERSION "4.8.2") 5 | set(CMAKE_C_PLATFORM_ID "Linux") 6 | 7 | set(CMAKE_AR "/usr/bin/ar") 8 | set(CMAKE_RANLIB "/usr/bin/ranlib") 9 | set(CMAKE_LINKER "/usr/bin/ld") 10 | set(CMAKE_COMPILER_IS_GNUCC 1) 11 | set(CMAKE_C_COMPILER_LOADED 1) 12 | set(CMAKE_C_COMPILER_WORKS TRUE) 13 | set(CMAKE_C_ABI_COMPILED TRUE) 14 | set(CMAKE_COMPILER_IS_MINGW ) 15 | set(CMAKE_COMPILER_IS_CYGWIN ) 16 | if(CMAKE_COMPILER_IS_CYGWIN) 17 | set(CYGWIN 1) 18 | set(UNIX 1) 19 | endif() 20 | 21 | set(CMAKE_C_COMPILER_ENV_VAR "CC") 22 | 23 | if(CMAKE_COMPILER_IS_MINGW) 24 | set(MINGW 1) 25 | endif() 26 | set(CMAKE_C_COMPILER_ID_RUN 1) 27 | set(CMAKE_C_SOURCE_FILE_EXTENSIONS c) 28 | set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) 29 | set(CMAKE_C_LINKER_PREFERENCE 10) 30 | 31 | # Save compiler ABI information. 32 | set(CMAKE_C_SIZEOF_DATA_PTR "8") 33 | set(CMAKE_C_COMPILER_ABI "ELF") 34 | set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 35 | 36 | if(CMAKE_C_SIZEOF_DATA_PTR) 37 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") 38 | endif() 39 | 40 | if(CMAKE_C_COMPILER_ABI) 41 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") 42 | endif() 43 | 44 | if(CMAKE_C_LIBRARY_ARCHITECTURE) 45 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 46 | endif() 47 | 48 | 49 | 50 | 51 | set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c") 52 | set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 53 | set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_CXX_COMPILER "/usr/bin/c++") 2 | set(CMAKE_CXX_COMPILER_ARG1 "") 3 | set(CMAKE_CXX_COMPILER_ID "GNU") 4 | set(CMAKE_CXX_COMPILER_VERSION "4.8.2") 5 | set(CMAKE_CXX_PLATFORM_ID "Linux") 6 | 7 | set(CMAKE_AR "/usr/bin/ar") 8 | set(CMAKE_RANLIB "/usr/bin/ranlib") 9 | set(CMAKE_LINKER "/usr/bin/ld") 10 | set(CMAKE_COMPILER_IS_GNUCXX 1) 11 | set(CMAKE_CXX_COMPILER_LOADED 1) 12 | set(CMAKE_CXX_COMPILER_WORKS TRUE) 13 | set(CMAKE_CXX_ABI_COMPILED TRUE) 14 | set(CMAKE_COMPILER_IS_MINGW ) 15 | set(CMAKE_COMPILER_IS_CYGWIN ) 16 | if(CMAKE_COMPILER_IS_CYGWIN) 17 | set(CYGWIN 1) 18 | set(UNIX 1) 19 | endif() 20 | 21 | set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") 22 | 23 | if(CMAKE_COMPILER_IS_MINGW) 24 | set(MINGW 1) 25 | endif() 26 | set(CMAKE_CXX_COMPILER_ID_RUN 1) 27 | set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) 28 | set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP) 29 | set(CMAKE_CXX_LINKER_PREFERENCE 30) 30 | set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) 31 | 32 | # Save compiler ABI information. 33 | set(CMAKE_CXX_SIZEOF_DATA_PTR "8") 34 | set(CMAKE_CXX_COMPILER_ABI "ELF") 35 | set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 36 | 37 | if(CMAKE_CXX_SIZEOF_DATA_PTR) 38 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") 39 | endif() 40 | 41 | if(CMAKE_CXX_COMPILER_ABI) 42 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") 43 | endif() 44 | 45 | if(CMAKE_CXX_LIBRARY_ARCHITECTURE) 46 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 47 | endif() 48 | 49 | 50 | 51 | 52 | set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c") 53 | set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 54 | set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.12.2/CMakeSystem.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_HOST_SYSTEM "Linux-3.13.0-24-generic") 2 | set(CMAKE_HOST_SYSTEM_NAME "Linux") 3 | set(CMAKE_HOST_SYSTEM_VERSION "3.13.0-24-generic") 4 | set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") 5 | 6 | 7 | 8 | set(CMAKE_SYSTEM "Linux-3.13.0-24-generic") 9 | set(CMAKE_SYSTEM_NAME "Linux") 10 | set(CMAKE_SYSTEM_VERSION "3.13.0-24-generic") 11 | set(CMAKE_SYSTEM_PROCESSOR "x86_64") 12 | 13 | set(CMAKE_CROSSCOMPILING "FALSE") 14 | 15 | set(CMAKE_SYSTEM_LOADED 1) 16 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.12.2/CompilerIdC/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.12.2/CompilerIdC/a.out -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/CMakeDirectoryInformation.cmake: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 2.8 3 | 4 | # Relative path conversion top directories. 5 | SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/z/Git/Multi-Array-Calib/genVel/velicp") 6 | SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/z/Git/Multi-Array-Calib/genVel/velicp") 7 | 8 | # Force unix paths in dependencies. 9 | SET(CMAKE_FORCE_UNIX_PATHS 1) 10 | 11 | 12 | # The C and CXX include file regular expressions for this directory. 13 | SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") 14 | SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") 15 | SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) 16 | SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) 17 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/TargetDirectories.txt: -------------------------------------------------------------------------------- 1 | /home/z/Git/Multi-Array-Calib/genVel/velicp/CMakeFiles/icp.dir 2 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/CXX.includecache: -------------------------------------------------------------------------------- 1 | #IncludeRegexLine: ^[ ]*#[ ]*(include|import)[ ]*[<"]([^">]+)([">]) 2 | 3 | #IncludeRegexScan: ^.*$ 4 | 5 | #IncludeRegexComplain: ^$ 6 | 7 | #IncludeRegexTransform: 8 | 9 | /home/z/Documents/libicp/src/demo.cpp 10 | iostream 11 | - 12 | icpPointToPlane.h 13 | /home/z/Documents/libicp/src/icpPointToPlane.h 14 | 15 | /home/z/Documents/libicp/src/icp.cpp 16 | icp.h 17 | /home/z/Documents/libicp/src/icp.h 18 | 19 | /home/z/Documents/libicp/src/icp.h 20 | stdio.h 21 | - 22 | string.h 23 | - 24 | stdlib.h 25 | - 26 | iostream 27 | - 28 | vector 29 | - 30 | matrix.h 31 | /home/z/Documents/libicp/src/matrix.h 32 | kdtree.h 33 | /home/z/Documents/libicp/src/kdtree.h 34 | 35 | /home/z/Documents/libicp/src/icpPointToPlane.cpp 36 | icpPointToPlane.h 37 | /home/z/Documents/libicp/src/icpPointToPlane.h 38 | 39 | /home/z/Documents/libicp/src/icpPointToPlane.h 40 | icp.h 41 | /home/z/Documents/libicp/src/icp.h 42 | 43 | /home/z/Documents/libicp/src/icpPointToPoint.cpp 44 | icpPointToPoint.h 45 | /home/z/Documents/libicp/src/icpPointToPoint.h 46 | 47 | /home/z/Documents/libicp/src/icpPointToPoint.h 48 | icp.h 49 | /home/z/Documents/libicp/src/icp.h 50 | 51 | /home/z/Documents/libicp/src/kdtree.cpp 52 | kdtree.h 53 | /home/z/Documents/libicp/src/kdtree.h 54 | stdio.h 55 | - 56 | algorithm 57 | - 58 | iostream 59 | - 60 | 61 | /home/z/Documents/libicp/src/kdtree.h 62 | vector 63 | - 64 | algorithm 65 | - 66 | boost/multi_array.hpp 67 | - 68 | boost/array.hpp 69 | - 70 | 71 | /home/z/Documents/libicp/src/matrix.cpp 72 | matrix.h 73 | /home/z/Documents/libicp/src/matrix.h 74 | math.h 75 | - 76 | 77 | /home/z/Documents/libicp/src/matrix.h 78 | stdio.h 79 | - 80 | string.h 81 | - 82 | stdlib.h 83 | - 84 | iostream 85 | - 86 | vector 87 | - 88 | stdint.h 89 | - 90 | 91 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/DependInfo.cmake: -------------------------------------------------------------------------------- 1 | # The set of languages for which implicit dependencies are needed: 2 | SET(CMAKE_DEPENDS_LANGUAGES 3 | "CXX" 4 | ) 5 | # The set of files for implicit dependencies of each language: 6 | SET(CMAKE_DEPENDS_CHECK_CXX 7 | "/home/z/Git/Multi-Array-Calib/genVel/velicp/src/demo.cpp" "/home/z/Git/Multi-Array-Calib/genVel/velicp/CMakeFiles/icp.dir/src/demo.cpp.o" 8 | "/home/z/Git/Multi-Array-Calib/genVel/velicp/src/icp.cpp" "/home/z/Git/Multi-Array-Calib/genVel/velicp/CMakeFiles/icp.dir/src/icp.cpp.o" 9 | "/home/z/Git/Multi-Array-Calib/genVel/velicp/src/icpPointToPlane.cpp" "/home/z/Git/Multi-Array-Calib/genVel/velicp/CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o" 10 | "/home/z/Git/Multi-Array-Calib/genVel/velicp/src/icpPointToPoint.cpp" "/home/z/Git/Multi-Array-Calib/genVel/velicp/CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o" 11 | "/home/z/Git/Multi-Array-Calib/genVel/velicp/src/kdtree.cpp" "/home/z/Git/Multi-Array-Calib/genVel/velicp/CMakeFiles/icp.dir/src/kdtree.cpp.o" 12 | "/home/z/Git/Multi-Array-Calib/genVel/velicp/src/matrix.cpp" "/home/z/Git/Multi-Array-Calib/genVel/velicp/CMakeFiles/icp.dir/src/matrix.cpp.o" 13 | ) 14 | SET(CMAKE_CXX_COMPILER_ID "GNU") 15 | 16 | # Targets to which this target links. 17 | SET(CMAKE_TARGET_LINKED_INFO_FILES 18 | ) 19 | 20 | # The include file search paths: 21 | SET(CMAKE_C_TARGET_INCLUDE_PATH 22 | "src" 23 | ) 24 | SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) 25 | SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) 26 | SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH}) 27 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/cmake_clean.cmake: -------------------------------------------------------------------------------- 1 | FILE(REMOVE_RECURSE 2 | "CMakeFiles/icp.dir/src/demo.cpp.o" 3 | "CMakeFiles/icp.dir/src/kdtree.cpp.o" 4 | "CMakeFiles/icp.dir/src/icp.cpp.o" 5 | "CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o" 6 | "CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o" 7 | "CMakeFiles/icp.dir/src/matrix.cpp.o" 8 | "icp.pdb" 9 | "icp" 10 | ) 11 | 12 | # Per-language clean rules from dependency scanning. 13 | FOREACH(lang CXX) 14 | INCLUDE(CMakeFiles/icp.dir/cmake_clean_${lang}.cmake OPTIONAL) 15 | ENDFOREACH(lang) 16 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/depend.internal: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 2.8 3 | 4 | CMakeFiles/icp.dir/src/demo.cpp.o 5 | /home/z/Documents/libicp/src/demo.cpp 6 | /home/z/Documents/libicp/src/icp.h 7 | /home/z/Documents/libicp/src/icpPointToPlane.h 8 | /home/z/Documents/libicp/src/kdtree.h 9 | /home/z/Documents/libicp/src/matrix.h 10 | CMakeFiles/icp.dir/src/icp.cpp.o 11 | /home/z/Documents/libicp/src/icp.cpp 12 | /home/z/Documents/libicp/src/icp.h 13 | /home/z/Documents/libicp/src/kdtree.h 14 | /home/z/Documents/libicp/src/matrix.h 15 | CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o 16 | /home/z/Documents/libicp/src/icp.h 17 | /home/z/Documents/libicp/src/icpPointToPlane.cpp 18 | /home/z/Documents/libicp/src/icpPointToPlane.h 19 | /home/z/Documents/libicp/src/kdtree.h 20 | /home/z/Documents/libicp/src/matrix.h 21 | CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o 22 | /home/z/Documents/libicp/src/icp.h 23 | /home/z/Documents/libicp/src/icpPointToPoint.cpp 24 | /home/z/Documents/libicp/src/icpPointToPoint.h 25 | /home/z/Documents/libicp/src/kdtree.h 26 | /home/z/Documents/libicp/src/matrix.h 27 | CMakeFiles/icp.dir/src/kdtree.cpp.o 28 | /home/z/Documents/libicp/src/kdtree.cpp 29 | /home/z/Documents/libicp/src/kdtree.h 30 | CMakeFiles/icp.dir/src/matrix.cpp.o 31 | /home/z/Documents/libicp/src/matrix.cpp 32 | /home/z/Documents/libicp/src/matrix.h 33 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/depend.make: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 2.8 3 | 4 | CMakeFiles/icp.dir/src/demo.cpp.o: src/demo.cpp 5 | CMakeFiles/icp.dir/src/demo.cpp.o: src/icp.h 6 | CMakeFiles/icp.dir/src/demo.cpp.o: src/icpPointToPlane.h 7 | CMakeFiles/icp.dir/src/demo.cpp.o: src/kdtree.h 8 | CMakeFiles/icp.dir/src/demo.cpp.o: src/matrix.h 9 | 10 | CMakeFiles/icp.dir/src/icp.cpp.o: src/icp.cpp 11 | CMakeFiles/icp.dir/src/icp.cpp.o: src/icp.h 12 | CMakeFiles/icp.dir/src/icp.cpp.o: src/kdtree.h 13 | CMakeFiles/icp.dir/src/icp.cpp.o: src/matrix.h 14 | 15 | CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o: src/icp.h 16 | CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o: src/icpPointToPlane.cpp 17 | CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o: src/icpPointToPlane.h 18 | CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o: src/kdtree.h 19 | CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o: src/matrix.h 20 | 21 | CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o: src/icp.h 22 | CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o: src/icpPointToPoint.cpp 23 | CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o: src/icpPointToPoint.h 24 | CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o: src/kdtree.h 25 | CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o: src/matrix.h 26 | 27 | CMakeFiles/icp.dir/src/kdtree.cpp.o: src/kdtree.cpp 28 | CMakeFiles/icp.dir/src/kdtree.cpp.o: src/kdtree.h 29 | 30 | CMakeFiles/icp.dir/src/matrix.cpp.o: src/matrix.cpp 31 | CMakeFiles/icp.dir/src/matrix.cpp.o: src/matrix.h 32 | 33 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/flags.make: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 2.8 3 | 4 | # compile CXX with /usr/bin/c++ 5 | CXX_FLAGS = -O3 -DNDEBUG -I/home/z/Git/Multi-Array-Calib/genVel/velicp/src 6 | 7 | CXX_DEFINES = 8 | 9 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/link.txt: -------------------------------------------------------------------------------- 1 | /usr/bin/c++ -O3 -DNDEBUG CMakeFiles/icp.dir/src/demo.cpp.o CMakeFiles/icp.dir/src/kdtree.cpp.o CMakeFiles/icp.dir/src/icp.cpp.o CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o CMakeFiles/icp.dir/src/matrix.cpp.o -o icp -rdynamic 2 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/progress.make: -------------------------------------------------------------------------------- 1 | CMAKE_PROGRESS_1 = 1 2 | CMAKE_PROGRESS_2 = 2 3 | CMAKE_PROGRESS_3 = 3 4 | CMAKE_PROGRESS_4 = 4 5 | CMAKE_PROGRESS_5 = 5 6 | CMAKE_PROGRESS_6 = 6 7 | 8 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/src/demo.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/icp.dir/src/demo.cpp.o -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/src/icp.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/icp.dir/src/icp.cpp.o -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/icp.dir/src/icpPointToPlane.cpp.o -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/icp.dir/src/icpPointToPoint.cpp.o -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/src/kdtree.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/icp.dir/src/kdtree.cpp.o -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/icp.dir/src/matrix.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/CMakeFiles/icp.dir/src/matrix.cpp.o -------------------------------------------------------------------------------- /genVel/velicp/CMakeFiles/progress.marks: -------------------------------------------------------------------------------- 1 | 6 2 | -------------------------------------------------------------------------------- /genVel/velicp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # project 2 | cmake_minimum_required (VERSION 2.6) 3 | project (libicp) 4 | 5 | # directories 6 | set (LIBICP_SRC_DIR src) 7 | FIND_PATH(BOOST_DIR "boost") 8 | 9 | # include directory 10 | include_directories("${LIBICP_SRC_DIR}") 11 | include_directories("${BOOST_DIR}") 12 | 13 | # sources 14 | FILE(GLOB LIBICP_SRC_FILES "src/*.cpp") 15 | 16 | # make release version 17 | set(CMAKE_BUILD_TYPE Release) 18 | 19 | # build demo program 20 | add_executable(icp ${LIBICP_SRC_FILES}) 21 | 22 | -------------------------------------------------------------------------------- /genVel/velicp/cmake_install.cmake: -------------------------------------------------------------------------------- 1 | # Install script for directory: /home/z/Git/Multi-Array-Calib/genVel/velicp 2 | 3 | # Set the install prefix 4 | IF(NOT DEFINED CMAKE_INSTALL_PREFIX) 5 | SET(CMAKE_INSTALL_PREFIX "/usr/local") 6 | ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) 7 | STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") 8 | 9 | # Set the install configuration name. 10 | IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) 11 | IF(BUILD_TYPE) 12 | STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" 13 | CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") 14 | ELSE(BUILD_TYPE) 15 | SET(CMAKE_INSTALL_CONFIG_NAME "Release") 16 | ENDIF(BUILD_TYPE) 17 | MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") 18 | ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) 19 | 20 | # Set the component getting installed. 21 | IF(NOT CMAKE_INSTALL_COMPONENT) 22 | IF(COMPONENT) 23 | MESSAGE(STATUS "Install component: \"${COMPONENT}\"") 24 | SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") 25 | ELSE(COMPONENT) 26 | SET(CMAKE_INSTALL_COMPONENT) 27 | ENDIF(COMPONENT) 28 | ENDIF(NOT CMAKE_INSTALL_COMPONENT) 29 | 30 | # Install shared libraries without execute permission? 31 | IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) 32 | SET(CMAKE_INSTALL_SO_NO_EXE "1") 33 | ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) 34 | 35 | IF(CMAKE_INSTALL_COMPONENT) 36 | SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") 37 | ELSE(CMAKE_INSTALL_COMPONENT) 38 | SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt") 39 | ENDIF(CMAKE_INSTALL_COMPONENT) 40 | 41 | FILE(WRITE "/home/z/Git/Multi-Array-Calib/genVel/velicp/${CMAKE_INSTALL_MANIFEST}" "") 42 | FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES}) 43 | FILE(APPEND "/home/z/Git/Multi-Array-Calib/genVel/velicp/${CMAKE_INSTALL_MANIFEST}" "${file}\n") 44 | ENDFOREACH(file) 45 | -------------------------------------------------------------------------------- /genVel/velicp/icp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/icp -------------------------------------------------------------------------------- /genVel/velicp/matlab/demo_3d.m: -------------------------------------------------------------------------------- 1 | % Copyright 2011. All rights reserved. 2 | % Institute of Measurement and Control Systems 3 | % Karlsruhe Institute of Technology, Germany 4 | 5 | % This file is part of libicp. 6 | % Authors: Andreas Geiger 7 | 8 | % libicp is free software; you can redistribute it and/or modify it under the 9 | % terms of the GNU General Public License as published by the Free Software 10 | % Foundation; either version 3 of the License, or any later version. 11 | 12 | % libicp is distributed in the hope that it will be useful, but WITHOUT ANY 13 | % WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | % PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | % You should have received a copy of the GNU General Public License along with 17 | % libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | % Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | 20 | % libicp demo file for MATLAB 21 | 22 | dbstop error; clear variables; close all; 23 | disp('================================'); 24 | 25 | % create model 26 | [X,Y] = meshgrid(-2:.25:2,-2:.25:2); 27 | Z = 5 * X .* exp(-X.^2-Y.^2); 28 | M = [X(:) Y(:) Z(:)]'; 29 | 30 | % transform model yielding the template 31 | rx = 0.3; 32 | Tr = [1 0 0 -2; 0 cos(rx) -sin(rx) 1;0 sin(rx) cos(rx) 0;0 0 0 1]; 33 | T = Tr(1:3,1:3)*M + Tr(1:3,4)*ones(1,size(M,2)); 34 | 35 | % fit template to model 36 | % - init with identity transformation (eye(4)) 37 | % - no outlier rejection step (-1) 38 | % - use point-to-plane fitting 39 | Tr_fit = icpMex(M,T,eye(4),-1,'point_to_plane'); 40 | T_fit = Tr_fit(1:3,1:3)*T + Tr_fit(1:3,4)*ones(1,size(T,2)); 41 | 42 | % plot 43 | figure,axis equal,hold on; ms=8; lw=2; fs=16; 44 | plot3(M(1,:),M(2,:),M(3,:),'or','MarkerSize',ms,'LineWidth',lw); 45 | plot3(T(1,:),T(2,:),T(3,:),'sg','MarkerSize',ms,'LineWidth',lw); 46 | plot3(T_fit(1,:),T_fit(2,:),T_fit(3,:),'xb','MarkerSize',ms,'LineWidth',lw); 47 | legend('model','template','fitted template','Location','NorthEast'); 48 | set(gca,'FontSize',fs); 49 | view(20,20); 50 | -------------------------------------------------------------------------------- /genVel/velicp/matlab/icpMex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/matlab/icpMex.mexa64 -------------------------------------------------------------------------------- /genVel/velicp/matlab/icpMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/matlab/icpMex.mexw64 -------------------------------------------------------------------------------- /genVel/velicp/matlab/make.m: -------------------------------------------------------------------------------- 1 | % Copyright 2011. All rights reserved. 2 | % Institute of Measurement and Control Systems 3 | % Karlsruhe Institute of Technology, Germany 4 | 5 | % This file is part of libicp. 6 | % Authors: Andreas Geiger 7 | 8 | % libicp is free software; you can redistribute it and/or modify it under the 9 | % terms of the GNU General Public License as published by the Free Software 10 | % Foundation; either version 3 of the License, or any later version. 11 | 12 | % libicp is distributed in the hope that it will be useful, but WITHOUT ANY 13 | % WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | % PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | % You should have received a copy of the GNU General Public License along with 17 | % libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | % Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | 20 | % make file for building mex MATLAB wrappers 21 | 22 | mex('icpMex.cpp','../src/icp.cpp','../src/icpPointToPoint.cpp',... 23 | '../src/icpPointToPlane.cpp','../src/kdtree.cpp',... 24 | '../src/matrix.cpp','-I../src','-IC:\boost'); 25 | mex('sparsifyMex.cpp','../src/kdtree.cpp','-I../src','-IC:\boost'); 26 | -------------------------------------------------------------------------------- /genVel/velicp/matlab/sparsifyMex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/matlab/sparsifyMex.mexa64 -------------------------------------------------------------------------------- /genVel/velicp/matlab/sparsifyMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/genVel/velicp/matlab/sparsifyMex.mexw64 -------------------------------------------------------------------------------- /genVel/velicp/src/demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libicp. 7 | Authors: Andreas Geiger 8 | 9 | libicp is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | // Demo program showing how libicp can be used 23 | 24 | #include 25 | #include "icpPointToPlane.h" 26 | 27 | using namespace std; 28 | 29 | int main (int argc, char** argv) { 30 | 31 | // define a 3 dim problem with 10000 model points 32 | // and 10000 template points: 33 | int32_t dim = 3; 34 | int32_t num = 10000; 35 | 36 | // allocate model and template memory 37 | double* M = (double*)calloc(3*num,sizeof(double)); 38 | double* T = (double*)calloc(3*num,sizeof(double)); 39 | 40 | // set model and template points 41 | cout << endl << "Creating model with 10000 points ..." << endl; 42 | cout << "Creating template by shifting model by (1,1,1) ..." << endl; 43 | int32_t k=0; 44 | for (double x=-2; x<2; x+=0.04) { 45 | for (double y=-2; y<2; y+=0.04) { 46 | double z=5*x*exp(-x*x-y*y); 47 | M[k*3+0] = x; 48 | M[k*3+1] = y; 49 | M[k*3+2] = z; 50 | T[k*3+0] = x-1; 51 | T[k*3+1] = y-1; 52 | T[k*3+2] = z-1; 53 | k++; 54 | } 55 | } 56 | 57 | // start with identity as initial transformation 58 | // in practice you might want to use some kind of prediction here 59 | Matrix R = Matrix::eye(3); 60 | Matrix t(3,1); 61 | 62 | // run point-to-plane ICP (-1 = no outlier threshold) 63 | cout << endl << "Running ICP (point-to-plane, no outliers)" << endl; 64 | //IcpPointToPlane icp(M,num,dim); 65 | //icp.fit(T,num,R,t,-1); 66 | 67 | // results 68 | cout << endl << "Transformation results:" << endl; 69 | cout << "R:" << endl << R << endl << endl; 70 | cout << "t:" << endl << t << endl << endl; 71 | 72 | // free memory 73 | free(M); 74 | free(T); 75 | 76 | // success 77 | return 0; 78 | } 79 | 80 | -------------------------------------------------------------------------------- /genVel/velicp/src/demo.cpp~: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libicp. 7 | Authors: Andreas Geiger 8 | 9 | libicp is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | // Demo program showing how libicp can be used 23 | 24 | #include 25 | #include "icpPointToPlane.h" 26 | 27 | using namespace std; 28 | 29 | int main (int argc, char** argv) { 30 | 31 | // define a 3 dim problem with 10000 model points 32 | // and 10000 template points: 33 | int32_t dim = 3; 34 | int32_t num = 10000; 35 | 36 | // allocate model and template memory 37 | double* M = (double*)calloc(3*num,sizeof(double)); 38 | double* T = (double*)calloc(3*num,sizeof(double)); 39 | 40 | // set model and template points 41 | cout << endl << "Creating model with 10000 points ..." << endl; 42 | cout << "Creating template by shifting model by (1,1,1) ..." << endl; 43 | int32_t k=0; 44 | for (double x=-2; x<2; x+=0.04) { 45 | for (double y=-2; y<2; y+=0.04) { 46 | double z=5*x*exp(-x*x-y*y); 47 | M[k*3+0] = x; 48 | M[k*3+1] = y; 49 | M[k*3+2] = z; 50 | T[k*3+0] = x-1; 51 | T[k*3+1] = y-1; 52 | T[k*3+2] = z-1; 53 | k++; 54 | } 55 | } 56 | 57 | // start with identity as initial transformation 58 | // in practice you might want to use some kind of prediction here 59 | Matrix R = Matrix::eye(3); 60 | Matrix t(3,1); 61 | 62 | // run point-to-plane ICP (-1 = no outlier threshold) 63 | cout << endl << "Running ICP (point-to-plane, no outliers)" << endl; 64 | IcpPointToPlane icp(M,num,dim); 65 | icp.fit(T,num,R,t,-1); 66 | 67 | // results 68 | cout << endl << "Transformation results:" << endl; 69 | cout << "R:" << endl << R << endl << endl; 70 | cout << "t:" << endl << t << endl << endl; 71 | 72 | // free memory 73 | free(M); 74 | free(T); 75 | 76 | // success 77 | return 0; 78 | } 79 | 80 | -------------------------------------------------------------------------------- /genVel/velicp/src/icpPointToPlane.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | libicp is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #ifndef ICP_POINT_TO_PLANE_H 22 | #define ICP_POINT_TO_PLANE_H 23 | 24 | #include "icp.h" 25 | 26 | class IcpPointToPlane : public Icp { 27 | 28 | public: 29 | 30 | IcpPointToPlane (double *M,const int32_t M_num,const int32_t dim,const int32_t num_neighbors=10,const double flatness=5.0) : Icp(M,M_num,dim) { 31 | M_normal = computeNormals(num_neighbors,flatness); 32 | } 33 | 34 | virtual ~IcpPointToPlane () { 35 | delete M_normal; 36 | } 37 | 38 | private: 39 | 40 | double fitStep (double *T,double *time,const int32_t T_num,Matrix &R,Matrix &t,const std::vector &active); 41 | std::vector getInliers (double *T,double *time,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist); 42 | 43 | // utility functions to compute normals from the model tree 44 | void computeNormal (const kdtree::KDTreeResultVector &neighbors,double *M_normal,const double flatness); 45 | double* computeNormals (const int32_t num_neighbors,const double flatness); 46 | 47 | // normals of model points 48 | double *M_normal; 49 | }; 50 | 51 | #endif // ICP_POINT_TO_PLANE_H 52 | -------------------------------------------------------------------------------- /genVel/velicp/src/icpPointToPoint.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | libicp is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #ifndef ICP_POINT_TO_POINT_H 22 | #define ICP_POINT_TO_POINT_H 23 | 24 | #include "icp.h" 25 | 26 | class IcpPointToPoint : public Icp { 27 | 28 | public: 29 | 30 | IcpPointToPoint (double *M,const int32_t M_num,const int32_t dim) : Icp(M,M_num,dim) {} 31 | virtual ~IcpPointToPoint () {} 32 | 33 | private: 34 | 35 | double fitStep (double *T,double *time,const int32_t T_num,Matrix &R,Matrix &t,const std::vector &active); 36 | std::vector getInliers (double *T,double *time,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist); 37 | }; 38 | 39 | #endif // ICP_POINT_TO_POINT_H 40 | -------------------------------------------------------------------------------- /handEye/ErrorEstCR.m: -------------------------------------------------------------------------------- 1 | function [ varVec ] = ErrorEstCR( sensorData, rotVec ) 2 | %ERRORESTR estimate cramer rao lower bound for error variance 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % estVec- nx3 matrix of rotations for each sensor 8 | % step- step between test points for numercial differentiation 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % varVec- nx3 matrix containing rotational variance 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(sensorData,{'cell'},{'vector'}); 27 | for i = 1:length(sensorData) 28 | validateattributes(sensorData{i},{'struct'},{}); 29 | end 30 | validateattributes(rotVec,{'numeric'},{'size',[length(sensorData),3]}); 31 | 32 | %pull usful info out of sensorData 33 | RData = zeros(size(sensorData{1}.T_Skm1_Sk,1),3,length(sensorData)); 34 | vRData = RData; 35 | 36 | for i = 1:length(sensorData) 37 | RData(:,:,i) = sensorData{i}.T_Skm1_Sk(:,4:6); 38 | vRData(:,:,i) = sensorData{i}.T_Var_Skm1_Sk(:,4:6); 39 | end 40 | 41 | rotVec = rotVec(2:end,:); 42 | varVec = zeros(size(rotVec)); 43 | 44 | steps = [1,0.1,0.01,0.001,0.0001,0.00001,0.000001,0.0000001]; 45 | for i = 1:length(rotVec(:)) 46 | out = zeros(length(steps),3); 47 | for j = 1:3 48 | for k = 1:length(steps) 49 | temp = rotVec; 50 | temp(i) = temp(i) + steps(k)*(j-2); 51 | out(k,j) = SystemProbR(RData, vRData, temp, false); 52 | end 53 | end 54 | out(:,1) = min(out(:,1),out(:,3)); 55 | out(:,3) = out(:,1); 56 | out = (steps.^2)'./diff(out,2,2); 57 | varVec(i) = max(out); 58 | end 59 | 60 | varVec = [0,0,0;varVec]; 61 | varVec = sqrt(varVec.^2); 62 | 63 | end 64 | 65 | -------------------------------------------------------------------------------- /handEye/ErrorEstCT.m: -------------------------------------------------------------------------------- 1 | function [ varVec ] = ErrorEstCT( sensorData, tranVec, rotVec, rotVar ) 2 | %OPTR Optimize translation based on inital guess 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % estVec- nx3 matrix of rotations for each sensor 8 | % rotVec- nx3 matrix of rotations for each sensor 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % outVec- nx3 matrix of the translation for each sensor 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(sensorData,{'cell'},{'vector'}); 27 | for i = 1:length(sensorData) 28 | validateattributes(sensorData{i},{'struct'},{}); 29 | end 30 | validateattributes(tranVec,{'numeric'},{'size',[length(sensorData),3]}); 31 | validateattributes(rotVec,{'numeric'},{'size',[length(sensorData),3]}); 32 | 33 | %pull usful info out of sensorData 34 | TData = zeros(size(sensorData{1}.T_Skm1_Sk,1),6,length(sensorData)); 35 | vTData = TData; 36 | s = zeros(length(sensorData),1); 37 | 38 | for i = 1:length(sensorData) 39 | TData(:,:,i) = sensorData{i}.T_Skm1_Sk; 40 | vTData(:,:,i) = sensorData{i}.T_Var_Skm1_Sk; 41 | s(i) = strcmpi(sensorData{i}.type,'camera'); 42 | end 43 | 44 | tranVec = tranVec(2:end,:); 45 | varVec = zeros(size(tranVec)); 46 | 47 | steps = [1,0.1,0.01,0.001,0.0001,0.00001,0.000001,0.0000001]; 48 | for i = 1:length(tranVec(:)) 49 | out = zeros(length(steps),3); 50 | for j = 1:3 51 | for k = 1:length(steps) 52 | temp = tranVec; 53 | temp(i) = temp(i) + steps(k)*(j-2); 54 | out(k,j) = SystemProbT(TData, vTData, s, temp, rotVec, rotVar, false); 55 | end 56 | end 57 | out(:,1) = min(out(:,1),out(:,3)); 58 | out(:,3) = out(:,1); 59 | out = (steps.^2)'./diff(out,2,2); 60 | varVec(i) = max(out); 61 | end 62 | 63 | varVec = [0,0,0;varVec]; 64 | varVec = sqrt(varVec.^2); 65 | 66 | end 67 | 68 | -------------------------------------------------------------------------------- /handEye/ErrorEstR.m: -------------------------------------------------------------------------------- 1 | function [ rotVar ] = ErrorEstR3( sensorData, rotVec ) 2 | %OPTR Optimize translation based on inital guess 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % estVec- nx3 matrix of rotations for each sensor 8 | % rotVec- nx3 matrix of rotations for each sensor 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % outVec- nx3 matrix of the translation for each sensor 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(sensorData,{'cell'},{'vector'}); 27 | for i = 1:length(sensorData) 28 | validateattributes(sensorData{i},{'struct'},{}); 29 | end 30 | validateattributes(rotVec,{'numeric'},{'size',[length(sensorData),3]}); 31 | 32 | %pull usful info out of sensorData 33 | RData = zeros(size(sensorData{1}.T_Skm1_Sk,1),3,length(sensorData)); 34 | vRData = RData; 35 | 36 | for i = 1:length(sensorData) 37 | RData(:,:,i) = sensorData{i}.T_Skm1_Sk(:,4:6); 38 | vRData(:,:,i) = sensorData{i}.T_Var_Skm1_Sk(:,4:6); 39 | end 40 | 41 | runFunc = @(RData, vRData) findRot(RData, vRData, rotVec(2:end,:)); 42 | 43 | [~,rotVar] = IndVarVec(0.01, runFunc, RData, vRData); 44 | 45 | rotVar = [0,0,0;rotVar]; 46 | 47 | end 48 | 49 | function [rotVec] = findRot(RData, vRData, rotVec) 50 | 51 | rotVec = fminsearch(@(rotVec) SystemProbR(RData, vRData, rotVec, false),rotVec); 52 | 53 | end 54 | -------------------------------------------------------------------------------- /handEye/ErrorEstT.m: -------------------------------------------------------------------------------- 1 | function [ tranVar ] = ErrorEstT3( sensorData, tranVec, rotVec, rotVar ) 2 | %OPTR Optimize translation based on inital guess 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % estVec- nx3 matrix of rotations for each sensor 8 | % rotVec- nx3 matrix of rotations for each sensor 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % outVec- nx3 matrix of the translation for each sensor 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(sensorData,{'cell'},{'vector'}); 27 | for i = 1:length(sensorData) 28 | validateattributes(sensorData{i},{'struct'},{}); 29 | end 30 | validateattributes(tranVec,{'numeric'},{'size',[length(sensorData),3]}); 31 | validateattributes(rotVec,{'numeric'},{'size',[length(sensorData),3]}); 32 | 33 | %pull useful info out of sensorData 34 | TData = zeros(size(sensorData{i}.T_Skm1_Sk,1),6,length(sensorData)); 35 | vTData = TData; 36 | s = zeros(length(sensorData),1); 37 | 38 | for i = 1:length(sensorData) 39 | TData(:,:,i) = sensorData{i}.T_Skm1_Sk; 40 | vTData(:,:,i) = sensorData{i}.T_Var_Skm1_Sk; 41 | s(i) = strcmpi(sensorData{i}.type,'camera'); 42 | end 43 | 44 | runFunc = @(TData, vTData, rotVec, rotVar) findTran(TData, vTData, s, tranVec(2:end,:), rotVec, rotVar); 45 | 46 | [~,tranVar] = IndVarVec(0.01, runFunc, TData, vTData, rotVec, rotVar); 47 | 48 | tranVar = [0,0,0;tranVar]; 49 | 50 | end 51 | 52 | function [t] = findTran(TData, vTData, s, t, rotVec, rotVar) 53 | t = fminsearch(@(t) SystemProbT(TData, vTData, s, t, rotVec, rotVar, false),t); 54 | 55 | end 56 | -------------------------------------------------------------------------------- /handEye/OptR.m: -------------------------------------------------------------------------------- 1 | function outVec = OptR( sensorData, estVec ) 2 | %OPTR Optimize rotation based on inital guess 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % estVec- nx3 matrix of rotations for each sensor 8 | % 9 | %-------------------------------------------------------------------------- 10 | % Outputs: 11 | %-------------------------------------------------------------------------- 12 | % outVec- nx3 matrix of the rotation for each sensor 13 | % 14 | %-------------------------------------------------------------------------- 15 | % References: 16 | %-------------------------------------------------------------------------- 17 | % This function is part of the Multi-Array-Calib toolbox 18 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 19 | % 20 | % This code was written by Zachary Taylor 21 | % zacharyjeremytaylor@gmail.com 22 | % http://www.zjtaylor.com 23 | 24 | %check inputs 25 | validateattributes(sensorData,{'cell'},{'vector'}); 26 | for i = 1:length(sensorData) 27 | validateattributes(sensorData{i},{'struct'},{}); 28 | end 29 | validateattributes(estVec,{'numeric'},{'size',[length(sensorData),3]}); 30 | 31 | %pull usful info out of sensorData 32 | RData = zeros(size(sensorData{1}.T_Skm1_Sk,1),3,length(sensorData)); 33 | vRData = RData; 34 | 35 | for i = 1:length(sensorData) 36 | RData(:,:,i) = sensorData{i}.T_Skm1_Sk(:,4:6); 37 | vRData(:,:,i) = sensorData{i}.T_Var_Skm1_Sk(:,4:6); 38 | end 39 | 40 | %refine rotation estimate and record result 41 | options = optimset('MaxFunEvals',100000,'MaxIter',5000); 42 | outVec = fminsearch(@(estVec) SystemProbR(RData, vRData, estVec, false),estVec(2:end,:), options); 43 | outVec = [0,0,0;outVec]; 44 | 45 | end 46 | 47 | -------------------------------------------------------------------------------- /handEye/OptT.m: -------------------------------------------------------------------------------- 1 | function [ tranVec ] = OptT( sensorData, estVec, rotVec, rotVar ) 2 | %OPTR Optimize translation based on inital guess 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % estVec- nx3 matrix of rotations for each sensor 8 | % rotVec- nx3 matrix of rotations for each sensor 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % outVec- nx3 matrix of the translation for each sensor 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(sensorData,{'cell'},{'vector'}); 27 | for i = 1:length(sensorData) 28 | validateattributes(sensorData{i},{'struct'},{}); 29 | end 30 | validateattributes(estVec,{'numeric'},{'size',[length(sensorData),3]}); 31 | validateattributes(rotVec,{'numeric'},{'size',[length(sensorData),3]}); 32 | 33 | %pull usful info out of sensorData 34 | TData = zeros(size(sensorData{1}.T_Skm1_Sk,1),6,length(sensorData)); 35 | vTData = TData; 36 | s = zeros(length(sensorData),1); 37 | 38 | for i = 1:length(sensorData) 39 | TData(:,:,i) = sensorData{i}.T_Skm1_Sk; 40 | vTData(:,:,i) = sensorData{i}.T_Var_Skm1_Sk; 41 | s(i) = strcmpi(sensorData{i}.type,'camera'); 42 | end 43 | 44 | %refine translation estimate and record result 45 | options = optimset('MaxFunEvals',100000,'MaxIter',5000); 46 | estVec = estVec(2:end,1:3); 47 | tranVec = fminsearch(@(estVec) SystemProbT(TData, vTData, s, estVec, rotVec, rotVar, false),estVec, options); 48 | 49 | %[ err, verr ] = SystemErrorT( TData, vTData, s, tranVec, rotVec, rotVar ); 50 | 51 | tranVec = [0,0,0;tranVec]; 52 | 53 | end 54 | 55 | -------------------------------------------------------------------------------- /handEye/OptTG.m: -------------------------------------------------------------------------------- 1 | function [ tranVec ] = OptT( sensorData, estVec, rotVec, rotVar ) 2 | %OPTR Optimize translation based on inital guess 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % estVec- nx3 matrix of rotations for each sensor 8 | % rotVec- nx3 matrix of rotations for each sensor 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % outVec- nx3 matrix of the translation for each sensor 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(sensorData,{'cell'},{'vector'}); 27 | for i = 1:length(sensorData) 28 | validateattributes(sensorData{i},{'struct'},{}); 29 | end 30 | validateattributes(estVec,{'numeric'},{'size',[length(sensorData),3]}); 31 | validateattributes(rotVec,{'numeric'},{'size',[length(sensorData),3]}); 32 | 33 | %pull usful info out of sensorData 34 | TData = zeros(size(sensorData{1}.T_Skm1_Sk,1),6,length(sensorData)); 35 | vTData = TData; 36 | s = zeros(length(sensorData),1); 37 | 38 | for i = 1:length(sensorData) 39 | TData(:,:,i) = sensorData{i}.T_Skm1_Sk; 40 | vTData(:,:,i) = sensorData{i}.T_Var_Skm1_Sk; 41 | s(i) = strcmpi(sensorData{i}.type,'camera'); 42 | end 43 | 44 | %refine translation estimate and record result 45 | options = optimset('MaxFunEvals',100000,'MaxIter',5000); 46 | estVec = estVec(2:end,1:3); 47 | tranVec = fminsearch(@(estVec) SystemProbTG(gpuArray(TData), gpuArray(vTData), s, estVec, rotVec, rotVar),estVec, options); 48 | 49 | tranVec = [0,0,0;tranVec]; 50 | 51 | end 52 | 53 | -------------------------------------------------------------------------------- /handEye/PlotData.m: -------------------------------------------------------------------------------- 1 | function [] = PlotData( sensorData, rotVec, fig ) 2 | %PlotData plots sensor data for easy visualization 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % 8 | %-------------------------------------------------------------------------- 9 | % Optional Inputs: 10 | %-------------------------------------------------------------------------- 11 | % fig- figure handle to plot to 12 | % 13 | %-------------------------------------------------------------------------- 14 | % References: 15 | %-------------------------------------------------------------------------- 16 | % This function is part of the Multi-Array-Calib toolbox 17 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 18 | % 19 | % This code was written by Zachary Taylor 20 | % zacharyjeremytaylor@gmail.com 21 | % http://www.zjtaylor.com 22 | 23 | %check inputs 24 | validateattributes(sensorData,{'cell'},{'vector'}); 25 | for i = 1:length(sensorData) 26 | validateattributes(sensorData{i},{'struct'},{}); 27 | end 28 | if(nargin > 2) 29 | validateattributes(fig,{'Figure'},{}); 30 | else 31 | fig = figure; 32 | end 33 | 34 | points = cell(size(sensorData)); 35 | leg = 'legend('; 36 | 37 | cmap = hsv(length(sensorData)); 38 | 39 | set(0, 'CurrentFigure', fig); 40 | hold on; 41 | 42 | %generate points 43 | for i = 1:length(sensorData) 44 | R = V2R(rotVec(i,:)); 45 | points{i} = zeros(size(sensorData{i}.T_Skm1_Sk,1),3); 46 | temp= eye(4); 47 | adj = eye(4); 48 | adj(1:3,1:3) = R; 49 | for j = 1:size(sensorData{i}.T_Skm1_Sk,1) 50 | temp = temp*V2T(sensorData{i}.T_Skm1_Sk(j,:)); 51 | points{i}(j,:) = temp(1:3,4); 52 | end 53 | 54 | temp = adj*[points{i},ones(size(points{i},1),1)]'; 55 | points{i} = temp(1:3,:)'; 56 | 57 | plot3(points{i}(:,1),points{i}(:,2),points{i}(:,3),'Color',cmap(i,:)); 58 | leg =[leg, '''', 'Sensor ', num2str(i),' ', sensorData{i}.type, '''']; 59 | if(i == length(sensorData)) 60 | leg = [leg ');']; 61 | else 62 | leg = [leg ',']; 63 | end 64 | 65 | end 66 | 67 | axis equal; 68 | eval(leg); 69 | drawnow; 70 | 71 | -------------------------------------------------------------------------------- /handEye/ReCalProbR.m: -------------------------------------------------------------------------------- 1 | function [ prob ] = ReCalProbR( RData, vRData, estVec, retVecFlag ) 2 | %SYSTEMPROBR uses variance to find a measure of the systems probablity of 3 | % being correct (lower score == better) 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % sensorData- nx1 cell containing sensor data sturcts 8 | % estVec- (n-1)x3 matrix of rotations for each sensor (1st sensor info 9 | % not given as it will just be 0,0,0) 10 | % 11 | %-------------------------------------------------------------------------- 12 | % Outputs: 13 | %-------------------------------------------------------------------------- 14 | % prob- measure of system likelihood (note not an actual probabilty) 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is part of the Multi-Array-Calib toolbox 20 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 21 | % 22 | % This code was written by Zachary Taylor 23 | % zacharyjeremytaylor@gmail.com 24 | % http://www.zjtaylor.com 25 | 26 | %check inputs 27 | validateattributes(RData,{'double'},{'3d','ncols',3}); 28 | validateattributes(vRData,{'double'},{'size',size(RData)}); 29 | validateattributes(estVec,{'numeric'},{'size',[size(RData,3)-1,3]}); 30 | 31 | %set first element to zeros 32 | estVec = [0,0,0;estVec]; 33 | 34 | %find probablity of system 35 | prob = 0; 36 | for a = 1:size(RData,3) 37 | for b = 1:size(RData,3) 38 | %ensure no repeats 39 | if(a < b) 40 | %get rotation and variance 41 | VA = vRData(:,:,a)'; 42 | VB = vRData(:,:,b)'; 43 | 44 | RA = RData(:,:,a)'; 45 | RB = RData(:,:,b)'; 46 | 47 | %find position error 48 | temp = logpdfR(RA,RB,VA,VB,estVec(a,:),estVec(b,:)); 49 | 50 | %add error 51 | prob= prob + temp; 52 | end 53 | end 54 | end 55 | 56 | prob = prob / (size(RData,3)*size(RData,3)); 57 | 58 | if(~retVecFlag) 59 | prob = -sum(prob); 60 | end 61 | 62 | end 63 | 64 | -------------------------------------------------------------------------------- /handEye/RoughR.m: -------------------------------------------------------------------------------- 1 | function [ estVec ] = RoughR( sensorData ) 2 | %ROUGHR finds an approximation to R using weighted least squares 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % 8 | %-------------------------------------------------------------------------- 9 | % Outputs: 10 | %-------------------------------------------------------------------------- 11 | % estVec- nx3 matrix of the rotation for each sensor 12 | % 13 | %-------------------------------------------------------------------------- 14 | % References: 15 | %-------------------------------------------------------------------------- 16 | % This function is part of the Multi-Array-Calib toolbox 17 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 18 | % 19 | % This code was written by Zachary Taylor 20 | % zacharyjeremytaylor@gmail.com 21 | % http://www.zjtaylor.com 22 | 23 | %check inputs 24 | validateattributes(sensorData,{'cell'},{'vector'}); 25 | for i = 1:length(sensorData) 26 | validateattributes(sensorData{i},{'struct'},{}); 27 | end 28 | 29 | estVec = zeros(size(sensorData,1),3); 30 | for i = 2:size(sensorData,1) 31 | %takes maximum variance and convert to weighting 32 | weight = 1./sqrt((max(sensorData{1}.T_Var_Skm1_Sk(:,4:6),[],2) + max(sensorData{i}.T_Var_Skm1_Sk(:,4:6),[],2))); 33 | weight = weight(:)'; 34 | 35 | %use Kabsch to find rotation estimate 36 | [temp,~] = Kabsch(sensorData{i}.T_Skm1_Sk(:,4:6)',sensorData{1}.T_Skm1_Sk(:,4:6)',weight); 37 | estVec(i,:) = R2V(temp); 38 | end 39 | 40 | end 41 | 42 | -------------------------------------------------------------------------------- /handEye/SystemProbR.m: -------------------------------------------------------------------------------- 1 | function [ prob ] = SystemProbR( RData, vRData, estVec, retVecFlag ) 2 | %SYSTEMPROBR uses variance to find a measure of the systems probablity of 3 | % being correct (lower score == better) 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % sensorData- nx1 cell containing sensor data sturcts 8 | % estVec- (n-1)x3 matrix of rotations for each sensor (1st sensor info 9 | % not given as it will just be 0,0,0) 10 | % 11 | %-------------------------------------------------------------------------- 12 | % Outputs: 13 | %-------------------------------------------------------------------------- 14 | % prob- measure of system likelihood (note not an actual probabilty) 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is part of the Multi-Array-Calib toolbox 20 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 21 | % 22 | % This code was written by Zachary Taylor 23 | % zacharyjeremytaylor@gmail.com 24 | % http://www.zjtaylor.com 25 | 26 | %check inputs 27 | validateattributes(RData,{'double'},{'3d','ncols',3}); 28 | validateattributes(vRData,{'double'},{'size',size(RData)}); 29 | validateattributes(estVec,{'numeric'},{'size',[size(RData,3)-1,3]}); 30 | 31 | %set first element to zeros 32 | estVec = [0,0,0;estVec]; 33 | 34 | %find probablity of system 35 | prob = 0; 36 | for a = 1:size(RData,3) 37 | for b = 1:size(RData,3) 38 | %ensure no repeats 39 | if(a < b) 40 | %get rotation and variance 41 | VA = vRData(:,:,a)'; 42 | VB = vRData(:,:,b)'; 43 | 44 | RA = RData(:,:,a)'; 45 | RB = RData(:,:,b)'; 46 | 47 | %find position error 48 | temp = logpdfR(RA,RB,VA,VB,estVec(a,:),estVec(b,:)); 49 | 50 | [~,idx] = sort(temp,'descend'); 51 | idx = idx([ceil(size(temp,1)*0.75):size(temp,1)]); 52 | temp(idx) = 0; 53 | 54 | %v = 4.65; 55 | %idx = temp < v; 56 | %idx2 = temp < -v; 57 | %temp(idx) = temp(idx).*(1-(temp(idx)/v).^2).^2; 58 | %temp(idx2) = 0; 59 | 60 | %add error 61 | prob= prob + temp; 62 | end 63 | end 64 | end 65 | 66 | prob = prob / (size(RData,3)*size(RData,3)); 67 | 68 | if(~retVecFlag) 69 | prob = -sum(prob); 70 | end 71 | 72 | end 73 | 74 | -------------------------------------------------------------------------------- /handEye/logpdfR.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/handEye/logpdfR.mexw64 -------------------------------------------------------------------------------- /handEye/logpdfT.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/handEye/logpdfT.mexw64 -------------------------------------------------------------------------------- /handEye/logpdfT2.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/handEye/logpdfT2.mexw64 -------------------------------------------------------------------------------- /imageMetric/Get2DGradProject.m: -------------------------------------------------------------------------------- 1 | function [ x, y ] = Get2DGradProject( in, tform ) 2 | %GET2DGRADPROJECT Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | in = double(in); 6 | 7 | out = in(:,1:4); 8 | out(:,4) = 1; 9 | out = (tform*out')'; 10 | out = [out(:,1:3), in(:,4:end)]; 11 | 12 | %project points onto sphere 13 | sphere = zeros(size(out,1),2); 14 | sphere(:,1) = atan2(out(:,1), out(:,3)); 15 | sphere(:,2) = atan(out(:,2)./ sqrt(out(:,1).^2 + out(:,3).^2)); 16 | 17 | %create image from sphere 18 | sphere = sphere - repmat(min(sphere),size(sphere,1),1); 19 | sphere = sphere ./ repmat(max(sphere(:)),size(sphere,1),2); 20 | 21 | lim = 0.5*prod(max(sphere)); 22 | imMax = sqrt(size(sphere,1)/lim); 23 | sphere = sphere*imMax; 24 | 25 | %interpolate 26 | [xq,yq] = meshgrid(1:max(sphere(:,1)), 1:max(sphere(:,2))); 27 | img = griddata(sphere(:,1),sphere(:,2),out(:,4),xq,yq); 28 | img(~isfinite(img(:))) = 0; 29 | 30 | %G = fspecial('gaussian',[50 50],1); 31 | %img = imfilter(img,G,'same'); 32 | 33 | %find gradients 34 | [x,y] = gradient(img); 35 | 36 | %interpolate back 37 | x = interp2(xq,yq,x,sphere(:,1),sphere(:,2)); 38 | y = interp2(xq,yq,y,sphere(:,1),sphere(:,2)); 39 | 40 | x(~isfinite(x(:))) = 0; 41 | y(~isfinite(y(:))) = 0; 42 | 43 | end 44 | 45 | -------------------------------------------------------------------------------- /imageMetric/LevImage.m: -------------------------------------------------------------------------------- 1 | function [ out ] = LevImage(in) 2 | %gets distance to each point 3 | in = double(in); 4 | 5 | E = zeros(size(in,1)-2, size(in,2)-2); 6 | D = zeros(size(in,1)-2, size(in,2)-2); 7 | 8 | for x = 1:3 9 | for y = 1:3 10 | E = max(E, abs(in(y:size(in,1)+y-3,x:size(in,2)+x-3) - in(2:size(in,1)-1, 2:size(in,2)-1))); 11 | end 12 | end 13 | 14 | s = 50; 15 | 16 | x = 0; 17 | for y = -s:s 18 | dxL = max(0,x); 19 | dxU = min(size(E,2)-1, size(E,2)-1+x); 20 | 21 | dyL = max(0,y); 22 | dyU = min(size(E,1)-1, size(E,1)-1+y); 23 | 24 | temp = E(dyL+1:dyU+1, dxL+1:dxU+1).*(0.98.^(max(abs(x),abs(y)))); 25 | 26 | D((dyL-y+1):(dyU-y+1), (dxL-x+1):(dxU-x+1)) = max(D((dyL-y+1):(dyU-y+1), (dxL-x+1):(dxU-x+1)), temp); 27 | 28 | end 29 | 30 | y = 0; 31 | for x = -s:s 32 | 33 | dxL = max(0,x); 34 | dxU = min(size(E,2)-1, size(E,2)-1+x); 35 | 36 | dyL = max(0,y); 37 | dyU = min(size(E,1)-1, size(E,1)-1+y); 38 | 39 | temp = E(dyL+1:dyU+1, dxL+1:dxU+1).*(0.98.^(max(abs(x),abs(y)))); 40 | 41 | D((dyL-y+1):(dyU-y+1), (dxL-x+1):(dxU-x+1)) = max(D((dyL-y+1):(dyU-y+1), (dxL-x+1):(dxU-x+1)), temp); 42 | 43 | end 44 | 45 | D = (2*E + D)/3; 46 | 47 | out = zeros(size(in)); 48 | 49 | out(2:size(in,1)-1,2:size(in,2)-1) = D; 50 | 51 | end 52 | 53 | -------------------------------------------------------------------------------- /imageMetric/LevLidar.m: -------------------------------------------------------------------------------- 1 | function [ points, valid ] = LevLidar(cloud) 2 | %gets distance to each point 3 | 4 | dist = sqrt(sum(cloud(:,1:3).^2,2)); 5 | 6 | diff = max(abs(dist(1:end-2)-dist(2:end-1)),abs(dist(2:end-1)-dist(3:end))); 7 | diff = [0;diff;0]; 8 | 9 | valid = diff > 0.3; 10 | points = [cloud(valid,1:3),sqrt(diff(valid))]; 11 | 12 | % y = 0.5; 13 | % 14 | % sphere = zeros(size(cloud,1),2); 15 | % sphere(:,1) = atan2(cloud(:,1), cloud(:,3)); 16 | % sphere(:,2) = atan(cloud(:,2)./ sqrt(cloud(:,1).^2 + cloud(:,3).^2)); 17 | % sphere(:,3) = sqrt(sum(cloud(:,1:3).^2,2)); 18 | % 19 | % idx = knnsearch(sphere(:,1:2),sphere(:,1:2),'k',9); 20 | % 21 | % dis = abs(sphere(:,3) - mean(reshape(sphere(idx(:),3),size(idx)),2)); 22 | % 23 | % cloud = [cloud, dis]; 24 | % points = cloud(dis > 0.3,:); 25 | % 26 | % %remove centre points to stop wrap round issue 27 | % points = points(abs(points(:,2))> 0.2,:); 28 | 29 | 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /imageMetric/LevSub.m: -------------------------------------------------------------------------------- 1 | function [ vel ] = SubVel( vel ) 2 | %SUBVEL subsamples velodyne taking most salient points as determined by a 3 | % difference in distance metric + some random points 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % vel- nxm set of velodyne points where m >= 3 8 | % sub- number of points to subsample down to 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % subVel- subxm set of velodyne points 14 | % idx- subx1 index of the velodyne points 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is part of the Multi-Array-Calib toolbox 20 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 21 | % 22 | % This code was written by Zachary Taylor 23 | % zacharyjeremytaylor@gmail.com 24 | % http://www.zjtaylor.com 25 | 26 | %check inputs 27 | validateattributes(vel,{'numeric'},{'2d'}); 28 | if(size(vel,2) < 3) 29 | error('vel must have atleast 3 columns'); 30 | end 31 | 32 | %get distance 33 | dist = sqrt(sum(vel(:,1:3).^2,2)); 34 | 35 | %project onto sphere 36 | sphere = zeros(size(vel,1),2); 37 | sphere(:,1) = atan2(vel(:,2), vel(:,1)); 38 | sphere(:,2) = atan(vel(:,3)./ sqrt(vel(:,2).^2 + vel(:,1).^2)); 39 | 40 | %find closest points 41 | idx = knnsearch(sphere,sphere,'k',5); 42 | idx = idx(:,2:end); 43 | diff = abs(reshape(dist(idx),size(idx)) - repmat(dist,1,size(idx,2))); 44 | diff = max(diff,[],2); 45 | 46 | vel = [vel, diff]; 47 | 48 | end 49 | 50 | -------------------------------------------------------------------------------- /imageMetric/MetricRefine.m: -------------------------------------------------------------------------------- 1 | function [ TGridR, vTGridR ] = MetricRefine( TGrid, vTGrid, sensorData, numScans, metric ) 2 | %METRICREFINE Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | TGridR = cell(length(sensorData)); 6 | vTGridR = cell(length(sensorData)); 7 | 8 | for i = 1:size(TGrid,1); 9 | for j = 1:size(vTGrid,1); 10 | if(i= 3 8 | % sub- number of points to subsample down to 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % subVel- subxm set of velodyne points 14 | % idx- subx1 index of the velodyne points 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is part of the Multi-Array-Calib toolbox 20 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 21 | % 22 | % This code was written by Zachary Taylor 23 | % zacharyjeremytaylor@gmail.com 24 | % http://www.zjtaylor.com 25 | 26 | %check inputs 27 | validateattributes(vel,{'numeric'},{'2d'}); 28 | if(size(vel,2) < 3) 29 | error('vel must have atleast 3 columns'); 30 | end 31 | 32 | %get distance 33 | dist = sqrt(sum(vel(:,1:3).^2,2)); 34 | 35 | %project onto sphere 36 | sphere = zeros(size(vel,1),2); 37 | sphere(:,1) = atan2(vel(:,2), vel(:,1)); 38 | sphere(:,2) = atan(vel(:,3)./ sqrt(vel(:,2).^2 + vel(:,1).^2)); 39 | 40 | %find closest points 41 | idx = knnsearch(sphere,sphere,'k',50); 42 | idx = idx(:,2:end); 43 | diff = reshape(dist(idx),size(idx)) - repmat(dist,1,size(idx,2)); 44 | diff = min(diff,[],2)./dist; 45 | 46 | vel = vel(diff > -0.1,:); 47 | 48 | end 49 | 50 | -------------------------------------------------------------------------------- /imageMetric/RunColourMetric.m: -------------------------------------------------------------------------------- 1 | function [ error ] = RunColourMetric( tform, K, scans, images, updateRate, avVals ) 2 | %RUNFLOWMETRIC Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | persistent tnow; 6 | persistent run; 7 | if(isempty(tnow)) 8 | tnow = now; 9 | run = 0; 10 | end 11 | 12 | T = inv(V2T(tform')); 13 | T = gpuArray(single(T)); 14 | 15 | ims = size(images{1}); 16 | 17 | error = zeros(size(scans,1),1); 18 | 19 | n = 0; 20 | for i = 1:size(scans,1) 21 | [pro1, valid1] = projectLidar(T, K, scans{i}(:,1:3), ims(1:2)); 22 | [pro2, valid2] = projectLidar(T, K, scans{i}(:,4:6), ims(1:2)); 23 | 24 | %dist = sqrt(sum(scans{i}(:,1:3).^2,2)); 25 | 26 | valid = and(valid1,valid2); 27 | pro1c = pro1(valid,:); 28 | pro2c = pro2(valid,:); 29 | 30 | %dist = dist(valid); 31 | 32 | pro1 = interpolateImage(images{i,1}, pro1c(:,1:2)); 33 | pro2 = interpolateImage(images{i,2}, pro2c(:,1:2)); 34 | 35 | valid = and(pro1>0,pro2>0); 36 | 37 | pro1 = single(pro1(valid,:)); 38 | pro2 = single(pro2(valid,:)); 39 | 40 | %dist = dist(valid); 41 | 42 | err = abs((pro1-pro2));%./(pro1+pro2); 43 | 44 | error(i) = gather(sum(err)); 45 | n = n + length(err); 46 | end 47 | 48 | error = sum(error); 49 | if avVals 50 | error = error/n; 51 | else 52 | error = error*size(scans,1)/n; 53 | end 54 | 55 | if(isnan(error)) 56 | error = 100; 57 | end 58 | 59 | run = run+1; 60 | i = 1; 61 | 62 | if((now - tnow) > updateRate/(3600*24)) 63 | 64 | %display image 65 | disp1 = points2Image( gather([scans{i}(:,1:3),scans{i}(:,7:9)]), ims(1:2), gather(K), gather(T), 3, 0.3, true, repmat(double(gather(images{i,1}))/255,1,1,3) ); 66 | disp1 = imresize(disp1, 1000/max(size(disp1))); 67 | 68 | %imwrite(disp,['./gen/' num2str(run,'%08d'),'.jpg']); 69 | imshow(disp1); 70 | 71 | t = gather(T); 72 | [r1,r2,r3] = dcm2angle(t(1:3,1:3)); t = [180*[r1,r2,r3]/pi,t(1,4),t(2,4),t(3,4)]; 73 | text = sprintf('R: %2.2f P: %2.2f, Y: %2.2f, X: %1.2f, Y: %1.2f, Z: %1.2f, Err: %2.3f, Run: %i\n',t(1),t(2),t(3),t(4),t(5),t(6),error,run); 74 | xlabel(text); 75 | 76 | %set(gcf,'units','normalized','outerposition',[0.25 0.25 0.5 0.5]) 77 | 78 | drawnow; 79 | tnow = now; 80 | end 81 | 82 | -------------------------------------------------------------------------------- /imageMetric/RunGomMetric.m: -------------------------------------------------------------------------------- 1 | function [ error ] = RunGomMetric( tform, K, scans, images, updateRate ) 2 | %RUNFLOWMETRIC Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | persistent tnow; 6 | persistent run; 7 | if(isempty(tnow)) 8 | tnow = now; 9 | run = 0; 10 | end 11 | 12 | T = inv(V2T(tform')); 13 | T = gpuArray(single(T)); 14 | 15 | ims = size(images{1}); 16 | 17 | error = zeros(size(scans,1),1); 18 | for i = 1:size(scans,1) 19 | [pro, valid] = projectLidar(T, K, scans{i}(:,1:3), ims(1:2)); 20 | 21 | A = interpolateImage(images{i}, pro(valid,1:2)); 22 | B = scans{i}(valid,4:5); 23 | 24 | err = evalGom(A,B); 25 | 26 | error(i) = gather(err); 27 | end 28 | 29 | error = -mean(error(:),1); 30 | 31 | run = run+1; 32 | i = 1; 33 | if((now - tnow) > updateRate/(3600*24)) 34 | 35 | %display image 36 | im = sqrt(sum((gather(images{i,1})).^2,3)); 37 | im = double(repmat(im,1,1,3)); 38 | disp = points2Image( gather([scans{i}(:,1:3),scans{i}(:,7:9)]), ims(1:2), gather(K), gather(T), 3, 0.3, true, im); 39 | disp = imresize(disp, 1000/max(size(disp))); 40 | imshow(disp); 41 | 42 | t = gather(T); 43 | [r1,r2,r3] = dcm2angle(t(1:3,1:3)); t = [180*[r1,r2,r3]/pi,t(1,4),t(2,4),t(3,4)]; 44 | text = sprintf('R: %2.2f P: %2.2f, Y: %2.2f, X: %1.2f, Y: %1.2f, Z: %1.2f, Err: %2.3f, Run: %i\n',t(1),t(2),t(3),t(4),t(5),t(6),error,run); 45 | xlabel(text); 46 | 47 | drawnow; 48 | tnow = now; 49 | end 50 | 51 | -------------------------------------------------------------------------------- /imageMetric/RunLevMetric.m: -------------------------------------------------------------------------------- 1 | function [ error ] = RunLevMetric( tform, K, scans, images, updateRate ) 2 | %RUNFLOWMETRIC Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | persistent tnow; 6 | persistent run; 7 | if(isempty(tnow)) 8 | tnow = now; 9 | run = 0; 10 | end 11 | 12 | T = inv(V2T(tform')); 13 | T = gpuArray(single(T)); 14 | 15 | ims = size(images{1}); 16 | 17 | error = zeros(size(scans,1),1); 18 | for i = 1:size(scans,1) 19 | [pro, valid] = projectLidar(T, K, scans{i}(:,1:3), ims(1:2)); 20 | 21 | pro = interpolateImage(images{i}, pro(valid,1:2)); 22 | depth = scans{i}(valid,4); 23 | 24 | err = pro .* depth; 25 | err = sum(err); 26 | 27 | error(i) = gather(err); 28 | end 29 | 30 | error = -mean(error(:),1); 31 | 32 | run = run+1; 33 | i = 1; 34 | if((now - tnow) > updateRate/(3600*24)) 35 | 36 | %display image 37 | disp = points2Image( gather([scans{i}(:,1:3),scans{i}(:,7:9)]), ims(1:2), gather(K), gather(T), 3, 0.3, true, repmat(double(gather(images{i,1}))/255,1,1,3) ); 38 | disp = imresize(disp, 1000/max(size(disp))); 39 | imshow(disp); 40 | 41 | t = gather(T); 42 | [r1,r2,r3] = dcm2angle(t(1:3,1:3)); t = [180*[r1,r2,r3]/pi,t(1,4),t(2,4),t(3,4)]; 43 | text = sprintf('R: %2.2f P: %2.2f, Y: %2.2f, X: %1.2f, Y: %1.2f, Z: %1.2f, Err: %2.3f, Run: %i\n',t(1),t(2),t(3),t(4),t(5),t(6),error,run); 44 | xlabel(text); 45 | 46 | 47 | drawnow; 48 | tnow = now; 49 | end 50 | 51 | -------------------------------------------------------------------------------- /imageMetric/RunNMIMetric.m: -------------------------------------------------------------------------------- 1 | function [ error ] = RunNMIMetric( tform, K, scans, images, updateRate ) 2 | %RUNFLOWMETRIC Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | persistent tnow; 6 | persistent run; 7 | if(isempty(tnow)) 8 | tnow = now; 9 | run = 0; 10 | end 11 | 12 | T = inv(V2T(tform')); 13 | T = gpuArray(single(T)); 14 | 15 | ims = size(images{1}); 16 | 17 | error = zeros(size(scans,1),1); 18 | for i = 1:size(scans,1) 19 | [pro, valid] = projectLidar(T, K, scans{i}(:,1:3), ims(1:2)); 20 | 21 | A = interpolateImage(images{i}, pro(valid,1:2)); 22 | B = scans{i}(valid,4); 23 | 24 | err = miC(gather(A),gather(B),true,50); 25 | 26 | error(i) = err; 27 | end 28 | 29 | error = -mean(error(:),1); 30 | 31 | run = run+1; 32 | i = 1; 33 | if((now - tnow) > updateRate/(3600*24)) 34 | 35 | %display image 36 | im = gather(images{i,1}); 37 | im = double(repmat(im,1,1,3)); 38 | disp = points2Image( gather([scans{i}(:,1:3),scans{i}(:,7:9)]), ims(1:2), gather(K), gather(T), 3, 0.3, true, im); 39 | disp = imresize(disp, 1000/max(size(disp))); 40 | imshow(disp); 41 | 42 | t = gather(T); 43 | [r1,r2,r3] = dcm2angle(t(1:3,1:3)); t = [180*[r1,r2,r3]/pi,t(1,4),t(2,4),t(3,4)]; 44 | text = sprintf('R: %2.2f P: %2.2f, Y: %2.2f, X: %1.2f, Y: %1.2f, Z: %1.2f, Err: %2.3f, Run: %i\n',t(1),t(2),t(3),t(4),t(5),t(6),error,run); 45 | xlabel(text); 46 | 47 | drawnow; 48 | tnow = now; 49 | end 50 | 51 | -------------------------------------------------------------------------------- /imageMetric/colourImage.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/colourImage.mexa64 -------------------------------------------------------------------------------- /imageMetric/colourImage.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/colourImage.mexw64 -------------------------------------------------------------------------------- /imageMetric/evalGom.m: -------------------------------------------------------------------------------- 1 | function [ error ] = evalGom( A, B ) 2 | 3 | z = or(or((A(:,1) == 0), (A(:,2) == 0)),or((B(:,1) == 0),(B(:,2) == 0))); 4 | 5 | e1 = abs(A(:,1).*B(:,1) + A(:,2).*B(:,2)); 6 | e1(z) = 0; 7 | e2 = sqrt(A(:,1).^2 + A(:,2).^2).*sqrt(B(:,1).^2 + B(:,2).^2) ; 8 | 9 | error = sum(e1)/(sum(e2)); 10 | 11 | error = gather(error); 12 | end 13 | 14 | -------------------------------------------------------------------------------- /imageMetric/interpolateImage.m: -------------------------------------------------------------------------------- 1 | function [ intVals ] = interpolateImage(image, points) 2 | %PROJECTLIDAR Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | %% check inputs 6 | if(~existsOnGPU(image)) 7 | error('image must be on GPU'); 8 | end 9 | if(~strcmp(classUnderlying(image),'single')) 10 | error('image must be of type single'); 11 | end 12 | 13 | if(size(points,2)< 2) 14 | error('points must be atleast x by 2'); 15 | end 16 | if(~existsOnGPU(points)) 17 | error('lidar must be on GPU'); 18 | end 19 | if(~strcmp(classUnderlying(points),'single')) 20 | error('points must be of type single'); 21 | end 22 | 23 | intVals = interpolateImageMex(image, points); 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /imageMetric/interpolateImageMex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/interpolateImageMex.mexa64 -------------------------------------------------------------------------------- /imageMetric/interpolateImageMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/interpolateImageMex.mexw64 -------------------------------------------------------------------------------- /imageMetric/interpolateImageUint8.m: -------------------------------------------------------------------------------- 1 | function [ intVals ] = interpolateImageUint8(image, points) 2 | %PROJECTLIDAR Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | %% check inputs 6 | if(~existsOnGPU(image)) 7 | error('image must be on GPU'); 8 | end 9 | if(~strcmp(classUnderlying(image),'uint8')) 10 | error('image must be of type uint8'); 11 | end 12 | 13 | if(size(points,2)< 2) 14 | error('points must be atleast x by 2'); 15 | end 16 | if(~existsOnGPU(points)) 17 | error('lidar must be on GPU'); 18 | end 19 | if(~strcmp(classUnderlying(points),'single')) 20 | error('points must be of type single'); 21 | end 22 | 23 | intVals = interpolateImageUint8Mex(image, points); 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /imageMetric/interpolateImageUint8Mex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/interpolateImageUint8Mex.mexa64 -------------------------------------------------------------------------------- /imageMetric/interpolateImageUint8Mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/interpolateImageUint8Mex.mexw64 -------------------------------------------------------------------------------- /imageMetric/miC.m: -------------------------------------------------------------------------------- 1 | function [ mi ] = miC( A, B, normal, bins ) 2 | % MIC A C implementation to calculate the mutual information 3 | % between two matricies. 4 | % 5 | % Required Inputs: 6 | % A- matrix with all points in the range 0 to 1 7 | % B- matrix the same size as A with all points in the range 0 to 1 8 | % 9 | % Optional Inputs: 10 | % normal- true for normalized mutual information, false for standard 11 | % (default false) 12 | % bins- number of bins to use in calculating MI. 13 | % (default 110, see performance notes for reason) 14 | % 15 | % Outputs: 16 | % mi- mi of A and B 17 | % 18 | % Performance Notes: 19 | % For a large speed gain pass A and B in as gpuArrays 20 | % Due to how GPUs divide work the runtime will decrease as bins increases 21 | % until (4*bins)^2 is larger then the shared memory, a which point the 22 | % runtime will roughly scale by (ceil((4*bins)^2/(shared memory in bytes)). 23 | % Note for most GPUs the shared memory is 48kB resulting in the optimal bin 24 | % number from a runtime point of view being 110. 25 | % The performance will also increase the closer to equally distributed 26 | % the histograms are. 27 | % 28 | % References- 29 | % This code was used in generating the results for the journal paper 30 | % Multi-modal sensor calibration using a gradient orientation measure 31 | % http://www.zjtaylor.com/welcome/download_pdf?pdf=JFR2013.pdf as well 32 | % as several of my other publications 33 | % 34 | % This code was written by Zachary Taylor 35 | % zacharyjeremytaylor@gmail.com 36 | % http://www.zjtaylor.com 37 | 38 | %% check inputs 39 | if(isnumeric(A)) 40 | A = single(A); 41 | else 42 | error('A must be numeric'); 43 | end 44 | 45 | if(isnumeric(B)) 46 | B = single(B); 47 | else 48 | error('B must be numeric'); 49 | end 50 | 51 | if(numel(A) ~= numel(B)) 52 | error('A and B must contain the same number of elements'); 53 | end 54 | 55 | if(nargin < 3) 56 | normal = []; 57 | end 58 | if(isempty(normal)) 59 | normal = false; 60 | else 61 | validateattributes(normal, {'logical'},{'scalar'}); 62 | end 63 | 64 | if(nargin < 4) 65 | bins = []; 66 | end 67 | if(isempty(bins)) 68 | bins = 255; 69 | else 70 | validateattributes(bins, {'numeric'},{'scalar','integer','positive'}); 71 | end 72 | 73 | mi = miCMex(A, B, normal, uint32(bins)); 74 | 75 | end 76 | 77 | -------------------------------------------------------------------------------- /imageMetric/miCMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/miCMex.mexw64 -------------------------------------------------------------------------------- /imageMetric/projectLidar.m: -------------------------------------------------------------------------------- 1 | function [ projectedLidar, validPoints ] = projectLidar( tform, cam, lidar, imageSize ) 2 | %PROJECTLIDAR Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | %% check inputs 6 | if(~isequal(size(tform),[4,4])) 7 | error('tform must be 4 by 4'); 8 | end 9 | if(~existsOnGPU(tform)) 10 | error('tform must be on GPU'); 11 | end 12 | if(~strcmp(classUnderlying(tform),'single')) 13 | error('tform must be of type single'); 14 | end 15 | 16 | if(~isequal(size(cam),[3,4])) 17 | error('cam must be 3 by 4'); 18 | end 19 | if(~existsOnGPU(cam)) 20 | error('cam must be on GPU'); 21 | end 22 | if(~strcmp(classUnderlying(cam),'single')) 23 | error('cam must be of type single'); 24 | end 25 | 26 | if(size(lidar,2)< 3) 27 | error('lidar must be atleast x by 3'); 28 | end 29 | if(~existsOnGPU(lidar)) 30 | error('lidar must be on GPU'); 31 | end 32 | if(~strcmp(classUnderlying(lidar),'single')) 33 | error('lidar must be of type single'); 34 | end 35 | 36 | if(~isequal(size(imageSize),[1,2])) 37 | error('imageSize must be 1 by 2'); 38 | end 39 | imageSize = uint32(imageSize); 40 | 41 | [validPoints, projectedLidar] = projectLidarMex(tform, cam, lidar(:,1:3), imageSize); 42 | projectedLidar = [projectedLidar, lidar(:,4:end)]; 43 | %projectedLidar = projectedLidar(validPoints,:); 44 | 45 | end 46 | 47 | -------------------------------------------------------------------------------- /imageMetric/projectLidarMex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/projectLidarMex.mexa64 -------------------------------------------------------------------------------- /imageMetric/projectLidarMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/imageMetric/projectLidarMex.mexw64 -------------------------------------------------------------------------------- /mex source/common.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #include 5 | 6 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445923078164062 7 | 8 | #define MATLAB 9 | #define MATGPU 10 | #define CUDA_ERROR_CHECK 11 | 12 | #ifdef MATLAB 13 | #include 14 | #define printf mexPrintf 15 | #endif 16 | 17 | #ifdef MATGPU 18 | #include 19 | #endif 20 | 21 | #define BLOCK_SIZE 512 22 | 23 | #define CudaSafeCall( err ) __cudaSafeCall( err, FILE, __LINE__ ) 24 | #define CudaCheckError() __cudaCheckError( FILE, __LINE__ ) 25 | 26 | #define FILE (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) 27 | 28 | __inline size_t gridSize(size_t num){ 29 | if(num == 0){ 30 | return 1; 31 | } 32 | else 33 | { 34 | return (size_t)ceil(((float)(num))/((float)(BLOCK_SIZE))); 35 | } 36 | }; 37 | 38 | inline void __cudaSafeCall( cudaError err, const char *file, const int line ) 39 | { 40 | #ifdef CUDA_ERROR_CHECK 41 | if ( cudaSuccess != err ) 42 | { 43 | printf("CUDA Function Error at %s:%i : %s\n", 44 | file, line, cudaGetErrorString( err ) ); 45 | cudaDeviceReset(); 46 | } 47 | #endif 48 | 49 | return; 50 | } 51 | 52 | inline void __cudaCheckError( const char *file, const int line ) 53 | { 54 | #ifdef CUDA_ERROR_CHECK 55 | cudaError errCuda = cudaGetLastError(); 56 | if ( cudaSuccess != errCuda ){ 57 | std::ostringstream err; err << "CUDA Kernel Error at " << file << ":" << line << " : " << cudaGetErrorString(errCuda); 58 | mexErrMsgTxt(err.str().c_str()); 59 | cudaDeviceReset(); 60 | } 61 | 62 | // More careful checking. However, this will affect performance. 63 | // Comment away if needed. 64 | errCuda = cudaDeviceSynchronize(); 65 | if( cudaSuccess != errCuda ){ 66 | printf("CUDA Kernel Error with sync failed at %s:%i : %s\n", 67 | file, line, cudaGetErrorString( errCuda ) ); 68 | cudaDeviceReset(); 69 | } 70 | #endif 71 | 72 | return; 73 | } 74 | 75 | #endif //COMMON_H 76 | -------------------------------------------------------------------------------- /mex source/imageFromLidar/x64/Release/imageFromLidarMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/imageFromLidar/x64/Release/imageFromLidarMex.mexw64 -------------------------------------------------------------------------------- /mex source/interpolateImageNearest/interpolateImageNearest.cu: -------------------------------------------------------------------------------- 1 | /* function for projecting lidar points 2 | * 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "../common.h" 10 | 11 | 12 | __global__ void NearInterpolateKernel(const float* const imageIn, 13 | float* const out, 14 | const size_t height, 15 | const size_t width, 16 | const float* const x, 17 | const float* const y, 18 | const size_t numPoints){ 19 | 20 | unsigned int i = blockDim.x * blockIdx.x + threadIdx.x; 21 | 22 | if(i >= numPoints){ 23 | return; 24 | } 25 | 26 | if((x[i] < 0) || (y[i] < 0) || (x[i] >= (width-1)) || (y[i] >= (height-1))){ 27 | out[i] = 0; 28 | return; 29 | } 30 | 31 | int xF = round(x[i]); 32 | int yF = round(y[i]); 33 | 34 | //nearest neighbour interpolate 35 | out[i] = imageIn[yF + xF*height]; 36 | } 37 | 38 | void mexFunction(int nlhs, mxArray *plhs[], 39 | int nrhs, mxArray const *prhs[]) 40 | { 41 | //initialize the MathWorks GPU API. 42 | mxInitGPU(); 43 | 44 | //read data 45 | mxGPUArray const * image = mxGPUCreateFromMxArray(prhs[0]); 46 | mxGPUArray const * points = mxGPUCreateFromMxArray(prhs[1]); 47 | size_t imageWidth = mxGPUGetDimensions(image)[1]; 48 | size_t imageHeight = mxGPUGetDimensions(image)[0]; 49 | size_t numPoints = mxGPUGetDimensions(points)[0]; 50 | size_t imageDepth = 1; 51 | if(mxGPUGetNumberOfDimensions(image) > 2){ 52 | imageDepth = mxGPUGetDimensions(image)[2]; 53 | } 54 | 55 | //create pointers from data 56 | float* imagePtr = (float*)(mxGPUGetDataReadOnly(image)); 57 | float* xPtr = (float*)(mxGPUGetDataReadOnly(points)); 58 | float* yPtr = &(xPtr[numPoints]); 59 | 60 | //create output 61 | mwSize outSize[] = {numPoints,imageDepth}; 62 | mxGPUArray* out = mxGPUCreateGPUArray(2, outSize, mxSINGLE_CLASS, mxREAL, MX_GPU_DO_NOT_INITIALIZE); 63 | float* outPtr = (float*)(mxGPUGetDataReadOnly(out)); 64 | 65 | //run and get ouputs 66 | for(size_t i = 0; i < imageDepth; i++){ 67 | float* imageLayerPtr = &(imagePtr[imageHeight*imageWidth*i]); 68 | float* outLayerPtr = &(outPtr[numPoints*i]); 69 | NearInterpolateKernel<<>>(imageLayerPtr, outLayerPtr, imageHeight, imageWidth, xPtr, yPtr, numPoints); 70 | CudaCheckError(); 71 | } 72 | plhs[0] = mxGPUCreateMxArrayOnGPU(out); 73 | 74 | //destroy reference structures 75 | mxGPUDestroyGPUArray(points); 76 | mxGPUDestroyGPUArray(image); 77 | mxGPUDestroyGPUArray(out); 78 | } 79 | -------------------------------------------------------------------------------- /mex source/makefile: -------------------------------------------------------------------------------- 1 | MEXSUFFIX = mexa64 2 | MATLABHOME = /usr/local/MATLAB/R2013b 3 | CUDAHOME = /usr/local/cuda-6.0 4 | MEX = g++ 5 | NVCC = nvcc 6 | 7 | CFLAGS = -fPIC -pthread -DMATLAB_MEX_FILE -ansi -D_GNU_SOURCE -fno-omit-frame-pointer -pthread -O3 -DNDEBUG 8 | NVCCFLAGS = -O3 -arch sm_30 -use_fast_math -Xcompiler -fpic -cudart=static 9 | 10 | LIBS = -lmx -lmex -lmat -lm -lmwgpu -lcudart 11 | LIBPATH = -L$(MATLABHOME)/bin/glnxa64 -L$(CUDAHOME)/lib64 12 | INCLUDE = -I$(MATLABHOME)/extern/include -I$(MATLABHOME)/toolbox/distcomp/gpu/extern/include -Icommon 13 | MEXFLAGS = -shared -Wl,-rpath-link,$(CUDAHOME)/lib64 14 | 15 | PROJECTS = projectLidar projectLidarDist projectLidarPan interpolateImage interpolateImageNearest interpolateImageUint8 imageFromLidar imageFromLidarDist imageFromLidarPan 16 | 17 | all: $(PROJECTS) 18 | 19 | imageFromLidar: ./imageFromLidar/imageFromLidarBase.o ./imageFromLidar/imageFromLidarLink.o 20 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 21 | 22 | imageFromLidarDist: ./imageFromLidarDist/imageFromLidarDistBase.o ./imageFromLidarDist/imageFromLidarDistLink.o 23 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 24 | 25 | imageFromLidarPan: ./imageFromLidarPan/imageFromLidarPanBase.o ./imageFromLidarPan/imageFromLidarPanLink.o 26 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 27 | 28 | projectLidar: ./projectLidar/projectLidarBase.o ./projectLidar/projectLidarLink.o 29 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 30 | 31 | projectLidarDist: ./projectLidarDist/projectLidarDistBase.o ./projectLidarDist/projectLidarDistLink.o 32 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 33 | 34 | projectLidarPan: ./projectLidarPan/projectLidarPanBase.o ./projectLidarPan/projectLidarPanLink.o 35 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 36 | 37 | interpolateImage: ./interpolateImage/interpolateImageBase.o ./interpolateImage/interpolateImageLink.o 38 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 39 | 40 | interpolateImageNearest: ./interpolateImageNearest/interpolateImageNearestBase.o ./interpolateImageNearest/interpolateImageNearestLink.o 41 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 42 | 43 | interpolateImageUint8: ./interpolateImageUint8/interpolateImageUint8Base.o ./interpolateImageUint8/interpolateImageUint8Link.o 44 | $(MEX) -o $@Mex.$(MEXSUFFIX) $^ $() $(MEXFLAGS) $(LIBPATH) $(LIBS) 45 | 46 | %Base.o: %.cu 47 | $(NVCC) $(NVCCFLAGS) $(INCLUDE) -rdc=true -o $@ -dc $< 48 | %Link.o: %Base.o 49 | $(NVCC) $(NVCCFLAGS) $(INCLUDE) -dlink $< -o $@ 50 | 51 | clean: 52 | rm -f *.o 53 | -------------------------------------------------------------------------------- /mex source/x64/Release/imageFromLidarDistMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/imageFromLidarDistMex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/imageFromLidarMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/imageFromLidarMex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/imageFromLidarPanMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/imageFromLidarPanMex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/interpolateImageMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/interpolateImageMex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/interpolateImageNearMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/interpolateImageNearMex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/interpolateImageUint8Mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/interpolateImageUint8Mex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/projectLidarDistMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/projectLidarDistMex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/projectLidarMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/projectLidarMex.mexw64 -------------------------------------------------------------------------------- /mex source/x64/Release/projectLidarPanMex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/mex source/x64/Release/projectLidarPanMex.mexw64 -------------------------------------------------------------------------------- /misc/EasyScale.m: -------------------------------------------------------------------------------- 1 | function [ sensorData ] = EasyScale( sensorData, rotVec, rotVar, tranVec, tranVar ) 2 | 3 | %divide into camera and non-camera sensors 4 | nonCamData = {}; 5 | nonCamIdx = {}; 6 | camData = {}; 7 | camIdx = {}; 8 | 9 | for i = 1:length(sensorData) 10 | if(strcmp(sensorData{i}.type,'camera')) 11 | camData = [camData,sensorData{i}]; 12 | camIdx = [camIdx,i]; 13 | else 14 | nonCamData = [nonCamData,sensorData{i}]; 15 | nonCamIdx = [nonCamIdx,i]; 16 | end 17 | end 18 | 19 | %if all cameras ext 20 | if(isempty(nonCamIdx)) 21 | return; 22 | end 23 | 24 | for i = 1:length(camData) 25 | 26 | %get joint rotation 27 | R = zeros(length(nonCamData),3); 28 | vR = zeros(length(nonCamData),3); 29 | for k = 1:length(nonCamData) 30 | output = @(r1,r2) R2V(V2R(r1)*V2R(r2))'; 31 | [R(k,:),vR(k,:)] = IndVar(0.001, output, rotVec(camIdx{i},:),rotVar(camIdx{i},:),rotVec(nonCamIdx{k},:),rotVar(nonCamIdx{k},:)); 32 | end 33 | 34 | tA = nonCamData{1}.T_Skm1_Sk(:,1:3); 35 | vtA = nonCamData{1}.T_Var_Skm1_Sk(:,1:3); 36 | tB = camData{i}.T_Skm1_Sk(:,1:3); 37 | vtB = camData{i}.T_Var_Skm1_Sk(:,1:3); 38 | 39 | RA = nonCamData{1}.T_Skm1_Sk(:,4:6); 40 | vRA = nonCamData{1}.T_Var_Skm1_Sk(:,4:6); 41 | RB = camData{i}.T_Skm1_Sk(:,4:6); 42 | vRB = camData{i}.T_Var_Skm1_Sk(:,4:6); 43 | 44 | for j = 1:size(camData{i}.T_Skm1_Sk,1) 45 | %estimate scale 46 | [s,sV] = IndVar(0.001,@findScale,R(1,:),vR(1,:),tranVec(2,:),tranVar(2,:),RA(j,:),vRA(j,:),tA(j,:),vtA(j,:),RB(j,:),vRB(j,:),tB(j,:),vtB(j,:)); 47 | 48 | %form single estimate 49 | sV = 1./sV; 50 | s = sum(s.*sV); 51 | sV = 1./sum(sV); 52 | s = s.*sV; 53 | 54 | sensorData{camIdx{i}}.T_Var_Skm1_Sk(j,1:3) = sV.*(tB(j,:).^2) + (s.^2).*vtB(j,:); 55 | sensorData{camIdx{i}}.T_Skm1_Sk(j,1:3) = s.*tB(j,:); 56 | end 57 | end 58 | 59 | warning('on','MATLAB:nearlySingularMatrix'); 60 | 61 | end 62 | 63 | function [scale] = findScale(R,t,RA,tA,RB,tB) 64 | 65 | scale = zeros(6,1); 66 | scale(1:3) = (V2R(R)'*((V2R(RA)-eye(3))*t' + tA'))./tB'; 67 | scale(4:6) = (V2R(R)'*tA' - (eye(3)-V2R(RB))*V2R(R)'*t')./tB'; 68 | end -------------------------------------------------------------------------------- /misc/InvertSensorData.m: -------------------------------------------------------------------------------- 1 | function [ sensorData ] = InvertSensorData( sensorData ) 2 | 3 | if(iscell(sensorData)) 4 | for i=1:length(sensorData) 5 | sensorData{i} = InvData(sensorData{i}); 6 | end 7 | elseif(isstruct(sensorData)) 8 | sensorData = InvData(sensorData); 9 | else 10 | error('sensorData must be a struct of cell of structs'); 11 | end 12 | 13 | end 14 | 15 | 16 | function [ sData ] = InvData( sData ) 17 | 18 | for i = 2:size(sData.T_Skm1_Sk,1) 19 | sData.T_Skm1_Sk(i,:) = T2V(inv(V2T(sData.T_Skm1_Sk(i,:)))); 20 | sData.T_S1_Sk(i,:) = T2V(V2T(sData.T_S1_Sk(i-1,:))*V2T(sData.T_Skm1_Sk(i,:))); 21 | end 22 | 23 | end -------------------------------------------------------------------------------- /misc/LoadSensorData.m: -------------------------------------------------------------------------------- 1 | function [ sensorData ] = LoadSensorData( dataset, sensor, varargin ) 2 | %LOADSENSORDATA Automates the loading of stored sensor data for testing 3 | % their performance 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % dataset- mx1 char array holding the name of the dataset to load 8 | % sensor- nx1 char array holding the name of the 1st sensor to load from 9 | % the dataset 10 | % 11 | %-------------------------------------------------------------------------- 12 | % Optional Inputs: 13 | %-------------------------------------------------------------------------- 14 | % sensor2- px1 char array holding the name of the 2nd sensor to load from 15 | % the dataset 16 | % sensorn- qx1 char array holding the name of the nth sensor to load from 17 | % the dataset (no limit to the number of sensor inputs of this form) 18 | % 19 | %-------------------------------------------------------------------------- 20 | % Outputs: 21 | %-------------------------------------------------------------------------- 22 | % sensorData- mx1 cell containing the sensor information 23 | % 24 | %-------------------------------------------------------------------------- 25 | % References: 26 | %-------------------------------------------------------------------------- 27 | % This function is part of the Multi-Array-Calib toolbox 28 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 29 | % 30 | % This code was written by Zachary Taylor 31 | % zacharyjeremytaylor@gmail.com 32 | % http://www.zjtaylor.com 33 | 34 | %check inputs 35 | validateattributes(dataset,{'char'},{'vector'}); 36 | sensors = {sensor,varargin{:}}; 37 | for i = 1:length(sensors) 38 | validateattributes(sensors{i},{'char'},{'vector'}); 39 | end 40 | 41 | %convert to lowercase first letter capitalized 42 | dataset = lower(dataset); 43 | dataset(1) = upper(dataset(1)); 44 | for i = 1:length(sensors) 45 | sensors{i} = lower(sensors{i}); 46 | sensors{i}(1) = upper(sensors{i}(1)); 47 | end 48 | 49 | %read in sensor data 50 | sensorData = cell(length(sensors),1); 51 | for i = 1:length(sensors) 52 | in = load(['./StoredTforms/' dataset sensors{i} 'Data.mat']); 53 | name = fieldnames(in); 54 | sensorData{i} = in.(name{1}); 55 | end 56 | 57 | end 58 | 59 | -------------------------------------------------------------------------------- /misc/MyHistEq.m: -------------------------------------------------------------------------------- 1 | function [ out ] = MyHistEq( in ) 2 | %MYHISTEQ evenly distributes data between 0 and 1, note does not prevserve 3 | % equivalence 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % in- data to equalize 8 | % 9 | %-------------------------------------------------------------------------- 10 | % Outputs: 11 | %-------------------------------------------------------------------------- 12 | % out- equalized data 13 | % 14 | %-------------------------------------------------------------------------- 15 | % References: 16 | %-------------------------------------------------------------------------- 17 | % This function is part of the Multi-Array-Calib toolbox 18 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 19 | % 20 | % This code was written by Zachary Taylor 21 | % zacharyjeremytaylor@gmail.com 22 | % http://www.zjtaylor.com 23 | 24 | validateattributes(in,{'numeric'},{}); 25 | 26 | in = double(in); 27 | in = in - min(in(:)); 28 | 29 | nz = find(in); 30 | 31 | in(nz) = in(nz) - min(in(nz)); 32 | in(nz) = in(nz) / max(in(nz)); 33 | 34 | [~,temp] = sort(in(nz)); 35 | 36 | out = (0:1/(length(in(nz))-1):1)'; 37 | in(nz(temp)) = out; 38 | 39 | out = in; 40 | 41 | end 42 | 43 | -------------------------------------------------------------------------------- /misc/ParSave.m: -------------------------------------------------------------------------------- 1 | function [] = ParSave(fileName, data, variableName) 2 | %PARSAVE Allows threads to save variables without waiting 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % fileName- file to save variable to 7 | % data- holds the data being saved 8 | % variableName- name the data will take when its loaded into matlab 9 | % 10 | %-------------------------------------------------------------------------- 11 | % References: 12 | %-------------------------------------------------------------------------- 13 | % This function is part of the Multi-Array-Calib toolbox 14 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 15 | % 16 | % I found this code somewhere on stackoverflow (I forget where) 17 | % zacharyjeremytaylor@gmail.com 18 | % http://www.zjtaylor.com 19 | 20 | validateattributes(fileName,{'char'},{'vector'}); 21 | validateattributes(variableName,{'char'},{'vector'}); 22 | 23 | eval(sprintf('%s = data;', variableName)); 24 | eval(sprintf('save(fileName, ''%s'');',variableName)); 25 | end 26 | 27 | -------------------------------------------------------------------------------- /misc/SampleData.m: -------------------------------------------------------------------------------- 1 | function [ sensorData ] = SampleData( sensorData, samples ) 2 | %SAMPLEDATA Uniformly samples data 3 | %-------------------------------------------------------------------------- 4 | % Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % samples - scalar, number of points to sample the data at (uniformly 8 | % distributed) 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % sensorData- nx1cell containing sensor data sturcts 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(samples,{'numeric'},{'scalar','positive','integer','nonzero'}); 27 | validateattributes(sensorData,{'cell'},{'vector'}); 28 | for i = 1:length(sensorData) 29 | validateattributes(sensorData{i},{'struct'},{}); 30 | end 31 | 32 | addpath('./timing'); 33 | 34 | %get interpolation points 35 | tMin = 0; 36 | tMax = inf; 37 | for i = 1:length(sensorData) 38 | tMin = max(tMin,sensorData{i}.time(1)); 39 | tMax = min(tMax,sensorData{i}.time(end)); 40 | end 41 | 42 | %turn points into times 43 | times = tMin:(tMax-tMin)/(samples):tMax; 44 | 45 | %interpolate at set times 46 | sensorData = IntSensorData(sensorData, times); 47 | 48 | end 49 | -------------------------------------------------------------------------------- /misc/SampleData2.m: -------------------------------------------------------------------------------- 1 | function [ sensorData ] = SampleData2( sensorData ) 2 | %SAMPLEDATA2 Uniformly samples data at rate of slowest sensor 3 | %-------------------------------------------------------------------------- 4 | % Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- nx1 cell containing sensor data sturcts 7 | % samples - scalar, number of points to sample the data at (uniformly 8 | % distributed) 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % sensorData- nx1cell containing sensor data sturcts 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | %check inputs 26 | validateattributes(sensorData,{'cell'},{'vector'}); 27 | for i = 1:length(sensorData) 28 | validateattributes(sensorData{i},{'struct'},{}); 29 | end 30 | 31 | addpath('./timing'); 32 | 33 | %get interpolation points 34 | tMin = 0; 35 | tMax = inf; 36 | for i = 1:length(sensorData) 37 | tMin = max(tMin,sensorData{i}.time(1)); 38 | tMax = min(tMax,sensorData{i}.time(end)); 39 | end 40 | 41 | %get number of points in range 42 | samples = inf; 43 | for i = 1:length(sensorData) 44 | temp = sum(and(sensorData{i}.time >= tMin, sensorData{i}.time <= tMax)); 45 | if(temp < samples) 46 | samples = temp; 47 | end 48 | end 49 | 50 | %turn points into times 51 | times = tMin:(tMax-tMin)/(samples):tMax; 52 | 53 | %interpolate at set times 54 | sensorData = IntSensorData(sensorData, times); 55 | 56 | end 57 | -------------------------------------------------------------------------------- /misc/UpdateMessage.m: -------------------------------------------------------------------------------- 1 | function [] = UpdateMessage( message, varargin ) 2 | %UPDATEMESSAGE Removes the last message and prints a new one (functions the 3 | % same as printf when passing inputs 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % Message- the message to print (operates the same as fprintf) 8 | % 9 | %-------------------------------------------------------------------------- 10 | % References: 11 | %-------------------------------------------------------------------------- 12 | % This function is part of the Multi-Array-Calib toolbox 13 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 14 | % 15 | % This code was written by Zachary Taylor 16 | % zacharyjeremytaylor@gmail.com 17 | % http://www.zjtaylor.com 18 | 19 | persistent messLength 20 | if(isempty(messLength)) 21 | fprintf(' '); 22 | messLength = 0; 23 | end 24 | 25 | validateattributes(message,{'char'},{'vector'}); 26 | 27 | %remove last message 28 | %fprintf(repmat('\b', 1, messLength)); 29 | message = [message '\n']; 30 | 31 | %setup new message 32 | toPrint = cell(length(varargin) + 2,1); 33 | toPrint{1} = 'sprintf(message'; 34 | for i = 1:length(varargin) 35 | toPrint{i+1} = [', varargin{' num2str(i) '}']; 36 | end 37 | toPrint{end} = ');'; 38 | 39 | %print message 40 | toPrint = eval(cell2mat(toPrint')); 41 | fprintf(toPrint); 42 | 43 | %record length 44 | messLength = length(toPrint); 45 | 46 | -------------------------------------------------------------------------------- /pErr.cpp: -------------------------------------------------------------------------------- 1 | /*Small program to quickly get probablity as matlab cant loop to save itself 2 | * call using cProb(err, VA, VB, rot) 3 | */ 4 | 5 | #include "mex.h" 6 | #include "matrix.h" 7 | #define _USE_MATH_DEFINES 8 | #include 9 | 10 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 11 | 12 | //check inputs 13 | if (nrhs != 3 || (nlhs != 1 && nlhs != 0)) { 14 | mexErrMsgIdAndTxt("CPROB:BadNArgs", 15 | "Need 3 inputs and 1 output."); 16 | } 17 | 18 | //gets size of variables 19 | const int lenA = mxGetDimensions(prhs[0])[1]; 20 | const int lenB = mxGetDimensions(prhs[1])[1]; 21 | 22 | //get value of input variables 23 | float* AIn = (float*) mxGetData(prhs[0]); 24 | float* BIn = (float*) mxGetData(prhs[1]); 25 | float* F = (float*) mxGetData(prhs[2]); 26 | 27 | //setup outputs 28 | plhs[0] = mxCreateNumericMatrix(lenA, 1, mxSINGLE_CLASS, mxREAL); 29 | float* out = (float*) mxGetData(plhs[0]); 30 | 31 | for(int a = 0; a < lenA; a++){ 32 | float err = 100000; 33 | float* A = &AIn[2*a]; 34 | float* B; 35 | for(int b = 0; b < lenB; b++){ 36 | 37 | B = &BIn[2*b]; 38 | 39 | float temp = B[0]*(A[0]*F[0] + A[1]*F[1] + F[2]) + B[1]*(A[0]*F[3] + A[1]*F[4] + F[5]) + A[0]*F[6] + A[1]*F[7] + F[8]; 40 | //temp *= sqrt((A[0] - B[0])*(A[0] - B[0]) + (A[1] - B[1])*(A[1] - B[1])); 41 | if(abs(temp) < err){ 42 | err = abs(temp); 43 | out[a] = b; 44 | } 45 | } 46 | B = &BIn[2*(int)(out[a])]; 47 | out[a] = B[0]*(A[0]*F[0] + A[1]*F[1] + F[2]) + B[1]*(A[0]*F[3] + A[1]*F[4] + F[5]) + A[0]*F[6] + A[1]*F[7] + F[8]; 48 | out[a] = abs(out[a]); 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /pErr.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/pErr.mexw64 -------------------------------------------------------------------------------- /paperPlots/MyErrorbars.m: -------------------------------------------------------------------------------- 1 | function [] = MyErrorbars( res, sd, colour ) 2 | %MYERRORBARS Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | 6 | plot(res,[colour '.'],'MarkerSize',20); 7 | 8 | for i = 1:length(res) 9 | line([i,i],[res(i)-sd(i),res(i)+sd(i)],'color',colour); 10 | end 11 | 12 | -------------------------------------------------------------------------------- /paperPlots/Plot_1_Timing.m: -------------------------------------------------------------------------------- 1 | data = {}; 2 | 3 | %load the data 4 | data{1} = load('../results/Test_1.1_Kitti_Time_0.1s.mat'); 5 | data{2} = load('../results/Test_1.2_Kitti_Time_1s.mat'); 6 | data{3} = load('../results/Test_1.3_Kitti_Time_5s.mat'); 7 | data{4} = load('../results/Test_1.4_Shrimp_Time_0.1s.mat'); 8 | data{5} = load('../results/Test_1.5_Shrimp_Time_1s.mat'); 9 | data{6} = load('../results/Test_1.6_Shrimp_Time_5s.mat'); 10 | data{7} = load('../results/Test_1.7_Ford_Time_0.1s.mat'); 11 | data{8} = load('../results/Test_1.8_Ford_Time_1s.mat'); 12 | data{9} = load('../results/Test_1.9_Ford_Time_5s.mat'); 13 | 14 | %average data 15 | y = cell(size(data)); 16 | for i = 1:length(data) 17 | data{i} = data{i}.results; 18 | data{i}.Error = mean(data{i}.Error(2:end,:,1:2:end),1); 19 | 20 | data{i}.Error = abs(data{i}.Error); 21 | y{i} = reshape(data{i}.Error,size(data{i}.Error,1)*size(data{i}.Error,2),[]); 22 | end 23 | 24 | x = data{1}.TimeLength(1:2:end); 25 | 26 | h = figure; 27 | 28 | subplot(3,1,1); 29 | boxplot(y{1},x,'datalim',[0.0001,inf]) 30 | set(gca,'YScale','log') 31 | title('0.1 Seconds Random Offset'); 32 | ax = gca; 33 | ax.YTick = [0.0001,0.001,0.01,0.1,1,10]; 34 | 35 | subplot(3,1,2); 36 | boxplot(y{2},x,'datalim',[0.0001,inf]) 37 | set(gca,'YScale','log') 38 | title('1 Second Random Offset'); 39 | ylabel('Final Timing Offset (s)'); 40 | ax = gca; 41 | ax.YTick = [0.0001,0.001,0.01,0.1,1,10]; 42 | 43 | subplot(3,1,3); 44 | boxplot(y{3},x,'datalim',[0.0001,inf]) 45 | set(gca,'YScale','log') 46 | title('5 Seconds Random Offset'); 47 | xlabel('Length of Sensor Data used (s)'); 48 | ax = gca; 49 | ax.YTick = [0.0001,0.001,0.01,0.1,1,10]; 50 | 51 | set(h, 'Position', [100, 100, 707, 1000]); -------------------------------------------------------------------------------- /paperPlots/Plot_2_Data_Vel.m: -------------------------------------------------------------------------------- 1 | %Plots the velocity of the different vechiles over their datasets 2 | 3 | %load the data 4 | CalibPath(true); 5 | f = LoadSensorData('Ford','Nav'); 6 | k = LoadSensorData('Kitti','Nav'); 7 | s = LoadSensorData('Shrimp','Nav'); 8 | 9 | %get timing info 10 | ft = (f{1}.time- min(f{1}.time))/1000000; 11 | kt = double(k{1}.time- min(k{1}.time))/1000000; 12 | st = double(s{1}.time- min(s{1}.time))/1000000; 13 | 14 | %get time difference 15 | fD = repmat([0.01;diff(ft)],1,6); 16 | kD = repmat([0.01;diff(kt)],1,6); 17 | sD = repmat([0.01;diff(st)],1,6); 18 | 19 | %get angular and linear velocity 20 | f = (f{1}.T_Skm1_Sk)./fD; 21 | k = (k{1}.T_Skm1_Sk)./kD; 22 | s = (s{1}.T_Skm1_Sk)./sD; 23 | 24 | %median filter so it graphs nicely 25 | f = medfilt1(f,100); 26 | k = medfilt1(k,100); 27 | s = medfilt1(s,100); 28 | 29 | %% graph results 30 | subplot(3,2,1); 31 | hold on; 32 | plot(ft,f(:,1),'r-'); 33 | plot(kt,k(:,1),'b-'); 34 | plot(st,s(:,1),'g-'); 35 | 36 | subplot(3,2,3); 37 | hold on; 38 | plot(ft,f(:,2),'r-'); 39 | plot(kt,k(:,2),'b-'); 40 | plot(st,s(:,2),'g-'); 41 | 42 | subplot(3,2,5); 43 | hold on; 44 | plot(ft,f(:,3),'r-'); 45 | plot(kt,k(:,3),'b-'); 46 | plot(st,s(:,3),'g-'); 47 | 48 | subplot(3,2,2); 49 | hold on; 50 | plot(ft,f(:,4),'r-'); 51 | plot(kt,k(:,4),'b-'); 52 | plot(st,s(:,4),'g-'); 53 | 54 | subplot(3,2,4); 55 | hold on; 56 | plot(ft,f(:,5),'r-'); 57 | plot(kt,k(:,5),'b-'); 58 | plot(st,s(:,5),'g-'); 59 | 60 | subplot(3,2,6); 61 | hold on; 62 | plot(ft,f(:,6),'r-'); 63 | plot(kt,k(:,6),'b-'); 64 | plot(st,s(:,6),'g-'); 65 | 66 | -------------------------------------------------------------------------------- /paperPlots/Plot_9_All.m: -------------------------------------------------------------------------------- 1 | addpath('../tforms'); 2 | 3 | %load the data 4 | data = load('./results/Test_9.1.mat'); 5 | 6 | %10_03 7 | velCam1 = [0.291804720076481,-0.0114055066485929,-0.0562394126383353,-1.21437856632943,1.20575835034676,-1.20140449070730]; 8 | velCam2 = [0.292034456780211,-0.548572251398307,-0.0605822662128782,-1.19262401861693,1.24394317283683,-1.20667887644520]; 9 | velCam3 = [0.287988792734513,0.0481207596442812,-0.0572301603112048,-1.21927554848229,1.20959078381303,-1.20612910053213]; 10 | velCam4 = [0.287103485048149,-0.485304279180288,-0.0584847671355261,-1.19258234104363,1.23385881496779,-1.20595346907977]; 11 | 12 | gt = [velCam1;velCam2;velCam3;velCam4]; 13 | 14 | fail = false(100,1); 15 | res = zeros(100,6); 16 | sd = zeros(100,6); 17 | t = zeros(100,2); 18 | 19 | r = 1:4; 20 | 21 | for i = 1:100 22 | 23 | [data.results{i}.final, data.results{i}.finalVar] = OptGrid(data.results{i}.ref, data.results{i}.vref); 24 | 25 | temp = abs(data.results{i}.final(2:end,:)-gt); 26 | res(i,:) = mean(temp(r,:),1); 27 | 28 | if(any(abs(data.results{i}.final(:))>3)) 29 | fail(i) = true; 30 | end 31 | 32 | temp = zeros(4,3); 33 | for j = 1:4 34 | [temp(j,1),temp(j,2),temp(j,3)] = dcm2angle(V2R(data.results{i}.final(j+1,4:6))/V2R(gt(j,4:6))); 35 | end 36 | res(i,4:6) = mean(abs(temp(r,:))*180/pi,1); 37 | 38 | sd(i,:) = sqrt(mean(data.results{i}.finalVar(2:end,:))); 39 | temp = zeros(4,3); 40 | for j = 1:4 41 | [~,temp(j,:)] = varChange(data.results{i}.final(j+1,4:6),sqrt(data.results{i}.finalVar(j+1,4:6)),gt(j,4:6)); 42 | end 43 | 44 | sd(i,4:6) = mean(temp(r,:),1); 45 | end 46 | 47 | res = res(~fail,:); 48 | sd = sd(~fail,:); 49 | 50 | mean(res) 51 | %% plot 52 | 53 | subplot(3,1,1); 54 | MyErrorbars(res(:,1), sd(:,1), 'r') 55 | axis([0,100,0,0.5]) 56 | title('X Error'); 57 | 58 | subplot(3,1,2); 59 | MyErrorbars(res(:,2), sd(:,2), 'g') 60 | axis([0,100,0,0.5]) 61 | title('Y Error'); 62 | ylabel('Calibration Error (m)'); 63 | 64 | subplot(3,1,3); 65 | MyErrorbars(res(:,3), sd(:,3), 'b') 66 | axis([0,100,0,0.5]) 67 | xlabel('Run'); 68 | title('Z Error'); 69 | 70 | figure 71 | subplot(3,1,1); 72 | MyErrorbars(res(:,4), sd(:,4), 'r') 73 | axis([0,100,0,2]) 74 | title('Roll Error'); 75 | 76 | subplot(3,1,2); 77 | MyErrorbars(res(:,5), sd(:,5), 'g') 78 | axis([0,100,0,2]) 79 | ylabel('Calibration Error (degrees)'); 80 | title('Yaw Error'); 81 | 82 | subplot(3,1,3); 83 | MyErrorbars(res(:,6), sd(:,6), 'b') 84 | axis([0,100,0,2]) 85 | xlabel('Run'); 86 | title('Pitch Error'); 87 | 88 | figure 89 | labels = {'X','Y','Z','Roll','Pitch','Yaw'}; 90 | boxplot(res./sd,labels); 91 | ylabel('estimated std from ground truth'); 92 | -------------------------------------------------------------------------------- /paperPlots/Plot_Timing_Mag.m: -------------------------------------------------------------------------------- 1 | data = {}; 2 | 3 | %load the data 4 | data{1} = load('../storedTforms/KittiCam1Data.mat'); 5 | data{2} = load('../storedTforms/ShrimpCam1Data.mat'); 6 | data{3} = load('../storedTforms/FordCam1Data.mat'); 7 | 8 | y = cell(size(data)); 9 | x = y; 10 | for i = 1:length(data) 11 | y{i} = sqrt(sum(data{i}.cam1Data.T_Skm1_Sk(:,4:6).^2,2)); 12 | x{i} = double(data{i}.cam1Data.time); 13 | x{i} = (x{i} - x{i}(1))/1000000; 14 | y{i} = y{i}./median(diff(x{i})); 15 | y{i}(y{i} > 1) = 0; 16 | 17 | %y{i} = [0;diff(y{i})]; 18 | y{i} = abs(y{i}); 19 | end 20 | 21 | subplot(3,1,1); 22 | plot(x{1},y{1}); 23 | axis([0 600 -0.1 0.8]) 24 | title('Kitti dataset'); 25 | subplot(3,1,2); 26 | plot(x{2},y{2}); 27 | axis([0 600 -0.1 0.8]) 28 | title('Shrimp dataset'); 29 | ylabel('Rotational Speed (rad/s)'); 30 | subplot(3,1,3); 31 | plot(x{3},y{3}); 32 | axis([0 600 -0.1 0.8]) 33 | title('Ford dataset'); 34 | xlabel('Time (s)'); -------------------------------------------------------------------------------- /paperPlots/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/paperPlots/Thumbs.db -------------------------------------------------------------------------------- /paperPlots/outlinebounds.m: -------------------------------------------------------------------------------- 1 | function hnew = outlinebounds(hl, hp) 2 | %OUTLINEBOUNDS Outline the patch of a boundedline 3 | % 4 | % hnew = outlinebounds(hl, hp) 5 | % 6 | % This function adds an outline to the patch objects created by 7 | % boundedline, matching the color of the central line associated with each 8 | % patch. 9 | % 10 | % Input variables: 11 | % 12 | % hl: handles to line objects from boundedline 13 | % 14 | % hp: handles to patch objects from boundedline 15 | % 16 | % Output variables: 17 | % 18 | % hnew: handle to new line objects 19 | 20 | % Copyright 2012 Kelly Kearney 21 | 22 | 23 | hnew = zeros(size(hl)); 24 | for il = 1:numel(hp) 25 | col = get(hl(il), 'color'); 26 | xy = get(hp(il), {'xdata','ydata'}); 27 | ax = ancestor(hl(il), 'axes'); 28 | 29 | hnew(il) = line(xy{1}, xy{2}, 'parent', ax, 'linestyle', '-', 'color', col); 30 | end 31 | 32 | -------------------------------------------------------------------------------- /paperPlots/varChange.m: -------------------------------------------------------------------------------- 1 | function [ out, outV ] = varChange( a, va, gt ) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | out = zeros(100,1); 6 | for i = 1:100 7 | as = a + randn(size(a)).*va; 8 | [out(i,1),out(i,2),out(i,3)] = dcm2angle(V2R(as)/V2R(gt)); 9 | out(i,:) = (out(i,:))*180/pi; 10 | end 11 | 12 | outV = std(out); 13 | 14 | [r,p,y] = dcm2angle(V2R(a)); 15 | out = abs([r,p,y])*180/pi; 16 | 17 | -------------------------------------------------------------------------------- /paperTests/Test1CamTiming.m: -------------------------------------------------------------------------------- 1 | function [ err, var ] = Test1CamTiming( sensorData, offsetMag, timeSamples ) 2 | %TEST1CAMTIMING Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | baseOffset = 2*offsetMag*1000000*(rand(length(sensorData),1)-0.5); 6 | baseOffset(1) = 0; 7 | 8 | %add timing offset 9 | for i = 1:length(sensorData) 10 | sensorData{i}.time = sensorData{i}.time + baseOffset(i); 11 | end 12 | 13 | % fix timestamps 14 | [~, offsets, var] = CorrectTimestamps(sensorData, timeSamples); 15 | 16 | %find error 17 | var = var(:,1)/(1000000*1000000); 18 | err = (offsets(:,1) - baseOffset)/1000000; 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /paperTests/Test3VelNav.m: -------------------------------------------------------------------------------- 1 | function [ calib ] = Test3VelNav( scansTimeRange, dataset ) 2 | 3 | %% load sensor data 4 | CalibPath(true); 5 | %make sure to read in cameras last (due to issue with how I compensate for scale) 6 | sensorData = LoadSensorData(dataset,'Vel','Nav'); 7 | 8 | %% run calibration 9 | %[sensorData, offsets, varOffsets] = CorrectTimestamps(sensorData, 10000); 10 | 11 | %get random contiguous scans to use 12 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 13 | 14 | %evenly sample data 15 | sData = SampleData2(sDataBase); 16 | 17 | %remove uninformative data 18 | sDataE = RejectPoints(sData, 1000, 1e-10); 19 | 20 | sDataE{1}.T_Var_Skm1_Sk(:) = 1; 21 | sDataE{1}.T_Var_S1_Sk = cumsum(sDataE{1}.T_Var_Skm1_Sk); 22 | sDataE{2}.T_Var_Skm1_Sk(:) = 1; 23 | sDataE{2}.T_Var_S1_Sk = cumsum(sDataE{2}.T_Var_Skm1_Sk); 24 | 25 | sData = RejectPoints(sData, 1000, 0.00001); 26 | 27 | % sData{1}.T_Var_Skm1_Sk(:) = medfilt1(sData{1}.T_Var_Skm1_Sk(:),10); 28 | % sData{1}.T_Var_S1_Sk = cumsum(sData{1}.T_Var_Skm1_Sk); 29 | % sData{2}.T_Var_Skm1_Sk(:) = medfilt1(sData{2}.T_Var_Skm1_Sk(:),10); 30 | % sData{2}.T_Var_S1_Sk = cumsum(sData{2}.T_Var_Skm1_Sk); 31 | 32 | %find rotation 33 | rotVec = RoughR(sDataE); 34 | calib.rotR = rotVec; 35 | rotVec = RoughR(sData); 36 | rotVec = OptR(sData, rotVec); 37 | calib.rot = rotVec; 38 | rotVarL = ErrorEstCR(sData, rotVec); 39 | calib.rotCR = rotVarL; 40 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 41 | calib.rotVar = rotVarM; 42 | 43 | tranVec = RoughT(sDataE, calib.rotR); 44 | calib.tranR = tranVec; 45 | tranVec = RoughT(sData, rotVec); 46 | tranVec = OptT(sData, tranVec, rotVec, rotVarM); 47 | calib.tran = tranVec; 48 | tranVarL = ErrorEstCT(sData, tranVec, rotVec, rotVarL); 49 | calib.tranCR = tranVarL; 50 | tranVarM = max(tranVarL,ErrorEstCT2(sData, tranVec, rotVec, rotVarM)); 51 | calib.tranVar = tranVarM; 52 | 53 | -------------------------------------------------------------------------------- /paperTests/Test4NavCam.m: -------------------------------------------------------------------------------- 1 | function [ calib ] = Test4NavCam( scansTimeRange, dataset ) 2 | 3 | %% load sensor data 4 | CalibPath(true); 5 | %make sure to read in cameras last (due to issue with how I compensate for scale) 6 | sensorData = LoadSensorData(dataset,'Nav','Cam1'); 7 | 8 | %% run calibration 9 | [sensorData, offsets, varOffsets] = CorrectTimestamps(sensorData, 10000); 10 | 11 | %get random contiguous scans to use 12 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 13 | 14 | %evenly sample data 15 | sData = SampleData2(sDataBase); 16 | 17 | %remove uninformative data 18 | sData = RejectPoints(sData, 100, 0.00001); 19 | 20 | sDataE = sData; 21 | sDataE{1}.T_Var_Skm1_Sk(:) = 1; 22 | sDataE{1}.T_Var_S1_Sk = cumsum(sDataE{1}.T_Var_Skm1_Sk); 23 | sDataE{2}.T_Var_Skm1_Sk(:) = 1; 24 | sDataE{2}.T_Var_S1_Sk = cumsum(sDataE{2}.T_Var_Skm1_Sk); 25 | 26 | %find rotation 27 | rotVec = RoughR(sDataE); 28 | calib.rotR = rotVec; 29 | rotVec = RoughR(sData); 30 | rotVec = OptR(sData, rotVec); 31 | calib.rot = rotVec; 32 | rotVarL = ErrorEstCR(sData, rotVec); 33 | calib.rotCR = rotVarL; 34 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 35 | calib.rotVar = rotVarM; 36 | 37 | estVecT = RoughTS(sData, rotVec); 38 | calib.tranRT = estVecT; 39 | 40 | tranVec = RoughT(sDataE, calib.rotR); 41 | calib.tranR = tranVec; 42 | tranVec = RoughT(sData, rotVec); 43 | tranVec = OptT(sData, tranVec, rotVec, rotVarM); 44 | calib.tran = tranVec; 45 | tranVarL = ErrorEstCT(sData, tranVec, rotVec, rotVarL); 46 | calib.tranCR = tranVarL; 47 | tranVarM = max(tranVarL,ErrorEstCT2(sData, tranVec, rotVec, rotVarM)); 48 | calib.tranVar = tranVarM; 49 | 50 | -------------------------------------------------------------------------------- /paperTests/Test5Multi.m: -------------------------------------------------------------------------------- 1 | function [ calib ] = Test5Multi( scansTimeRange, dataset ) 2 | 3 | %% load sensor data 4 | CalibPath(true); 5 | %make sure to read in cameras last (due to issue with how I compensate for scale) 6 | sensorData = LoadSensorData(dataset,'Vel','Nav','Cam1','Cam2'); 7 | 8 | %% run calibration 9 | 10 | %get random contiguous scans to use 11 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 12 | 13 | %evenly sample data 14 | sData = SampleData2(sDataBase); 15 | 16 | %remove uninformative data 17 | sData = RejectPoints(sData, 100, 0.00001); 18 | 19 | %find rotation 20 | rotVec = RoughR(sData); 21 | calib.rotR = rotVec; 22 | for i = 2:4 23 | rotVec([1,i],:) = OptR(sData([1,i]), rotVec([1,i],:)); 24 | end 25 | calib.rotI = rotVec; 26 | rotVec = OptR(sData, rotVec); 27 | calib.rot = rotVec; 28 | rotVarL = ErrorEstCR(sData, rotVec); 29 | calib.rotCR = rotVarL; 30 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 31 | calib.rotVar = rotVarM; 32 | 33 | tranVec = RoughT(sData, rotVec); 34 | calib.tranR = tranVec; 35 | for i = 2:4 36 | tranVec([1,i],:) = OptT(sData([1,i]), tranVec([1,i],:), rotVec([1,i],:), rotVarM([1,i],:)); 37 | end 38 | calib.tranI = tranVec; 39 | tranVec = OptT(sData, tranVec, rotVec, rotVarM); 40 | calib.tran = tranVec; 41 | tranVarL = ErrorEstCT(sData, tranVec, rotVec, rotVarL); 42 | calib.tranCR = tranVarL; 43 | tranVarM = max(tranVarL,ErrorEstCT2(sData, tranVec, rotVec, rotVarM)); 44 | calib.tranVar = tranVarM; 45 | 46 | -------------------------------------------------------------------------------- /paperTests/Test6UpCam.m: -------------------------------------------------------------------------------- 1 | function [ calib ] = Test6UpCam( scansTimeRange ) 2 | 3 | try 4 | timeSamples = 10000; 5 | 6 | %% load sensor data 7 | CalibPath(true); 8 | %make sure to read in cameras last (due to issue with how I compensate for scale) 9 | sensorData = LoadSensorData('Shrimp','Cam1','Cam6'); 10 | 11 | %% run calibration 12 | 13 | %get random contiguous scans to use 14 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 15 | 16 | % %find time offset 17 | % [~, offsets, varOffsets] = CorrectTimestamps(sDataBase, timeSamples); 18 | % calib.tOff = offsets; 19 | % calib.tV = varOffsets; 20 | 21 | %evenly sample data 22 | sData = SampleData2(sDataBase); 23 | 24 | %remove uninformative data 25 | sData = RejectPoints(sData, 1000, 0.0000001); 26 | calib.n = length(sData{1}.time); 27 | 28 | %find rotation 29 | rotVec = RoughR(sData); 30 | calib.rotR = rotVec; 31 | calib.rotI = rotVec; 32 | rotVec = OptR(sData, rotVec); 33 | calib.rot = rotVec; 34 | rotVarL = ErrorEstCR(sData, rotVec); 35 | calib.rotCR = rotVarL; 36 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 37 | calib.rotVar = rotVarM; 38 | catch 39 | calib.err = 1; 40 | end 41 | end -------------------------------------------------------------------------------- /paperTests/Test7Refine.m: -------------------------------------------------------------------------------- 1 | function [ calib ] = Test7Refine( scansTimeRange, dataset ) 2 | 3 | %% load sensor data 4 | CalibPath(true); 5 | %make sure to read in cameras last (due to issue with how I compensate for scale) 6 | sensorData = LoadSensorData(dataset,'Vel','Cam1'); 7 | 8 | %% run calibration 9 | 10 | %get random contiguous scans to use 11 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 12 | 13 | %evenly sample data 14 | sData = SampleData2(sDataBase); 15 | 16 | %remove uninformative data 17 | sData = RejectPoints(sData, 100, 0.00001); 18 | 19 | %find rotation 20 | rotVec = RoughR(sData); 21 | rotVec = OptR(sData, rotVec); 22 | calib.rot = rotVec; 23 | rotVarL = ErrorEstCR(sData, rotVec); 24 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 25 | calib.rotVar = rotVarM; 26 | 27 | tranVec = RoughT(sData, rotVec); 28 | calib.tranR = tranVec; 29 | tranVec = OptT(sData, tranVec, rotVec, rotVarM); 30 | calib.tran = tranVec; 31 | tranVarL = ErrorEstCT(sData, tranVec, rotVec, rotVarL); 32 | tranVarM = max(tranVarL,ErrorEstCT2(sData, tranVec, rotVec, rotVarM)); 33 | calib.tranVar = tranVarM; 34 | 35 | %get grid of transforms 36 | fprintf('Generating transformation grid\n'); 37 | [TGrid, vTGrid] = GenTformGrid(tranVec, rotVec, tranVarM, rotVarM); 38 | 39 | noBase = TGrid; 40 | noSol = vTGrid; 41 | noSol{1,2}(:) = 10000; 42 | numScans = 25; 43 | 44 | %refine transforms using metrics 45 | fprintf('Refining transformations\n'); 46 | [TGridR, ~] = MetricRefine(TGrid, vTGrid, sDataBase, numScans, 'nmi'); 47 | calib.NMIConRef = S2V(TGridR{1,2}); 48 | 49 | [TGridR, ~] = MetricRefine(noBase, noSol, sDataBase, numScans, 'nmi'); 50 | calib.NMIRef = S2V(TGridR{1,2}); 51 | 52 | [TGridR, ~] = MetricRefine(TGrid, vTGrid, sDataBase, numScans, 'lev'); 53 | calib.LevConRef = S2V(TGridR{1,2}); 54 | 55 | [TGridR, ~] = MetricRefine(noBase, noSol, sDataBase, numScans, 'lev'); 56 | calib.LevRef = S2V(TGridR{1,2}); 57 | 58 | [TGridR, ~] = MetricRefine(TGrid, vTGrid, sDataBase, numScans, 'gom'); 59 | calib.GOMconRef = S2V(TGridR{1,2}); 60 | 61 | [TGridR, ~] = MetricRefine(noBase, noSol, sDataBase, numScans, 'gom'); 62 | calib.GOMRef = S2V(TGridR{1,2}); 63 | 64 | [TGridR, ~] = MetricRefine(TGrid, vTGrid, sDataBase, numScans, 'motion'); 65 | calib.MotionConRef = S2V(TGridR{1,2}); 66 | 67 | [TGridR, ~] = MetricRefine(noBase, noSol, sDataBase, numScans, 'motion'); 68 | calib.MotionRef = S2V(TGridR{1,2}); 69 | 70 | 71 | -------------------------------------------------------------------------------- /paperTests/Test8ShrimpCam.m: -------------------------------------------------------------------------------- 1 | function [ calib ] = Test8ShrimpCam( scansTimeRange ) 2 | 3 | %% load sensor data 4 | CalibPath(true); 5 | %make sure to read in cameras last (due to issue with how I compensate for scale) 6 | sensorData = LoadSensorData('Shrimp','Cam1','Cam2','Cam3','Cam4','Cam5'); 7 | 8 | %% run calibration 9 | 10 | %get random contiguous scans to use 11 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 12 | 13 | %evenly sample data 14 | sData = SampleData2(sDataBase); 15 | 16 | %remove uninformative data 17 | sData = RejectPoints(sData, 10, 0.0000001); 18 | 19 | %find rotation 20 | rotVec = RoughR(sData); 21 | calib.rotR = rotVec; 22 | for i = 2:5 23 | rotVec([1,i],:) = OptR(sData([1,i]), rotVec([1,i],:)); 24 | end 25 | calib.rotI = rotVec; 26 | rotVec = OptR(sData, rotVec); 27 | calib.rot = rotVec; 28 | rotVarL = ErrorEstCR(sData, rotVec); 29 | calib.rotCR = rotVarL; 30 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 31 | calib.rotVar = rotVarM; 32 | 33 | end -------------------------------------------------------------------------------- /paperTests/Test9Full.m: -------------------------------------------------------------------------------- 1 | function [ calib ] = Test9Full( scansTimeRange, dataset ) 2 | 3 | %% load sensor data 4 | CalibPath(true); 5 | %make sure to read in cameras last (due to issue with how I compensate for scale) 6 | sensorData = LoadSensorData(dataset,'Vel','Cam1','Cam2','Cam3','Cam4'); 7 | 8 | %% run calibration 9 | 10 | %get random contiguous scans to use 11 | sDataBase = RandTformTimes(sensorData, scansTimeRange); 12 | 13 | [sDataBase, ~, ~] = CorrectTimestamps(sensorData, 10000); 14 | 15 | %evenly sample data 16 | sData = SampleData2(sDataBase); 17 | 18 | %remove uninformative data 19 | sData = RejectPoints(sData, 10, 0.00001); 20 | 21 | %find rotation 22 | rotVec = RoughR(sData); 23 | rotVec = OptR(sData, rotVec); 24 | calib.rot = rotVec; 25 | rotVarL = ErrorEstCR(sData, rotVec); 26 | rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec)); 27 | calib.rotVar = rotVarM; 28 | 29 | tranVec = RoughT(sData, rotVec); 30 | calib.tranR = tranVec; 31 | tranVec = OptT(sData, tranVec, rotVec, rotVarM); 32 | calib.tran = tranVec; 33 | tranVarL = ErrorEstCT(sData, tranVec, rotVec, rotVarL); 34 | tranVarM = max(tranVarL,ErrorEstCT2(sData, tranVec, rotVec, rotVarM)); 35 | calib.tranVar = tranVarM; 36 | 37 | %get grid of transforms 38 | fprintf('Generating transformation grid\n'); 39 | [TGrid, vTGrid] = GenTformGrid(tranVec, rotVec, tranVarM, rotVarM); 40 | 41 | numScans = 25; 42 | 43 | %refine transforms using metrics 44 | [TGridR, vTGridR] = MetricRefine(TGrid, vTGrid, sDataBase, numScans, 'motion'); 45 | calib.ref = TGridR; 46 | calib.vref = vTGridR; 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /results/Test_1.1_Kitti_Time_0.1s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.1_Kitti_Time_0.1s.mat -------------------------------------------------------------------------------- /results/Test_1.2_Kitti_Time_1s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.2_Kitti_Time_1s.mat -------------------------------------------------------------------------------- /results/Test_1.3_Kitti_Time_5s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.3_Kitti_Time_5s.mat -------------------------------------------------------------------------------- /results/Test_1.4_Shrimp_Time_0.1s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.4_Shrimp_Time_0.1s.mat -------------------------------------------------------------------------------- /results/Test_1.5_Shrimp_Time_1s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.5_Shrimp_Time_1s.mat -------------------------------------------------------------------------------- /results/Test_1.6_Shrimp_Time_5s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.6_Shrimp_Time_5s.mat -------------------------------------------------------------------------------- /results/Test_1.7_Ford_Time_0.1s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.7_Ford_Time_0.1s.mat -------------------------------------------------------------------------------- /results/Test_1.7_Shrimp_Time_0.1s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.7_Shrimp_Time_0.1s.mat -------------------------------------------------------------------------------- /results/Test_1.8_Ford_Time_1s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.8_Ford_Time_1s.mat -------------------------------------------------------------------------------- /results/Test_1.9_Ford_Time_5s.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_1.9_Ford_Time_5s.mat -------------------------------------------------------------------------------- /results/Test_2.1_Kitti.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_2.1_Kitti.mat -------------------------------------------------------------------------------- /results/Test_2.2_Shrimp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_2.2_Shrimp.mat -------------------------------------------------------------------------------- /results/Test_2.3_Ford.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_2.3_Ford.mat -------------------------------------------------------------------------------- /results/Test_3.10_Kitti.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_3.10_Kitti.mat -------------------------------------------------------------------------------- /results/Test_3.11_Shrimp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_3.11_Shrimp.mat -------------------------------------------------------------------------------- /results/Test_3.12_Shrimp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_3.12_Shrimp.mat -------------------------------------------------------------------------------- /results/Test_4.10_Kitti.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_4.10_Kitti.mat -------------------------------------------------------------------------------- /results/Test_5.1_Kitti.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_5.1_Kitti.mat -------------------------------------------------------------------------------- /results/Test_6.1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.1.mat -------------------------------------------------------------------------------- /results/Test_6.2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.2.mat -------------------------------------------------------------------------------- /results/Test_6.3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.3.mat -------------------------------------------------------------------------------- /results/Test_6.4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.4.mat -------------------------------------------------------------------------------- /results/Test_6.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.5.mat -------------------------------------------------------------------------------- /results/Test_6.6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.6.mat -------------------------------------------------------------------------------- /results/Test_6.7.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.7.mat -------------------------------------------------------------------------------- /results/Test_6.8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.8.mat -------------------------------------------------------------------------------- /results/Test_6.9.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_6.9.mat -------------------------------------------------------------------------------- /results/Test_7.1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_7.1.mat -------------------------------------------------------------------------------- /results/Test_7.2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_7.2.mat -------------------------------------------------------------------------------- /results/Test_7.3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_7.3.mat -------------------------------------------------------------------------------- /results/Test_7.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_7.mat -------------------------------------------------------------------------------- /results/Test_8.1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_8.1.mat -------------------------------------------------------------------------------- /results/Test_8.2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_8.2.mat -------------------------------------------------------------------------------- /results/Test_9.1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/results/Test_9.1.mat -------------------------------------------------------------------------------- /storedTforms/FordCam1Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/FordCam1Data.mat -------------------------------------------------------------------------------- /storedTforms/FordCam2Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/FordCam2Data.mat -------------------------------------------------------------------------------- /storedTforms/FordCam3Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/FordCam3Data.mat -------------------------------------------------------------------------------- /storedTforms/FordCam4Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/FordCam4Data.mat -------------------------------------------------------------------------------- /storedTforms/FordCam5Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/FordCam5Data.mat -------------------------------------------------------------------------------- /storedTforms/FordNavData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/FordNavData.mat -------------------------------------------------------------------------------- /storedTforms/FordVelData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/FordVelData.mat -------------------------------------------------------------------------------- /storedTforms/KittiCam1Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiCam1Data.mat -------------------------------------------------------------------------------- /storedTforms/KittiCam1DataBS.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiCam1DataBS.mat -------------------------------------------------------------------------------- /storedTforms/KittiCam2Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiCam2Data.mat -------------------------------------------------------------------------------- /storedTforms/KittiCam2DataBS.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiCam2DataBS.mat -------------------------------------------------------------------------------- /storedTforms/KittiCam3Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiCam3Data.mat -------------------------------------------------------------------------------- /storedTforms/KittiCam4Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiCam4Data.mat -------------------------------------------------------------------------------- /storedTforms/KittiNavData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiNavData.mat -------------------------------------------------------------------------------- /storedTforms/KittiVelData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/KittiVelData.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpCam1Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpCam1Data.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpCam2Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpCam2Data.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpCam3Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpCam3Data.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpCam4Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpCam4Data.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpCam5Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpCam5Data.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpCam6Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpCam6Data.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpNavData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpNavData.mat -------------------------------------------------------------------------------- /storedTforms/ShrimpVelData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZacharyTaylor/Multi-Array-Calib/d3a57d03896adbcec48e72dd9c4ef2b748003a4d/storedTforms/ShrimpVelData.mat -------------------------------------------------------------------------------- /tforms/GenTformGrid.m: -------------------------------------------------------------------------------- 1 | function [ TGrid, vTGrid ] = GenTformGrid(tranVec, rotVec, tranVar, rotVar) 2 | %GENTFORMGRID Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | TGrid = cell(size(tranVec,1)); 6 | vTGrid = cell(size(tranVec,1)); 7 | 8 | for i = 1:size(TGrid,1); 9 | for j = 1:size(vTGrid,1); 10 | if(i v) = 0; 64 | %out = err.*(1-(err/v).^2).^2; 65 | 66 | end -------------------------------------------------------------------------------- /timing/RandTformTimes.m: -------------------------------------------------------------------------------- 1 | function [ sensorData ] = RandTformTimes( sensorData, timeLength ) 2 | %RANDTFORMTIMES gets a random contiguous section of sensor data of length 3 | % timeLength 4 | %-------------------------------------------------------------------------- 5 | % Required Inputs: 6 | %-------------------------------------------------------------------------- 7 | % sensorData- a nx1 cell containing sensor data sturcts 8 | % timeLength- length of required data in seconds 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % sensorData- a nx1 cell containing sensor data sturcts 14 | % 15 | %-------------------------------------------------------------------------- 16 | % References: 17 | %-------------------------------------------------------------------------- 18 | % This function is part of the Multi-Array-Calib toolbox 19 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 20 | % 21 | % This code was written by Zachary Taylor 22 | % zacharyjeremytaylor@gmail.com 23 | % http://www.zjtaylor.com 24 | 25 | validateattributes(sensorData,{'cell'},{'vector'}); 26 | validateattributes(timeLength,{'numeric'},{'positive','scalar'}); 27 | 28 | 29 | timeLength = 1000000*timeLength; 30 | 31 | startTime = 0; 32 | endTime = inf; 33 | for i = 1:length(sensorData) 34 | startTime = max(startTime,sensorData{i}.time(1)); 35 | endTime = min(endTime,sensorData{i}.time(end)); 36 | end 37 | 38 | dataLength = endTime - startTime; 39 | 40 | if(timeLength > dataLength) 41 | error('Not enough data for set time'); 42 | end 43 | 44 | startT = startTime + uint64(rand(1)*double(dataLength-timeLength)); 45 | endT = startT + timeLength; 46 | 47 | for i = 1:length(sensorData) 48 | valid = and(sensorData{i}.time > startT, sensorData{i}.time < endT); 49 | valid = find(valid); 50 | sensorData{i} = SensorDataSubset(sensorData{i}, valid); 51 | end 52 | 53 | end 54 | 55 | -------------------------------------------------------------------------------- /timing/SensorDataSubset.m: -------------------------------------------------------------------------------- 1 | function [ sensorData ] = SensorDataSubset( sensorData, idx ) 2 | %SENSORDATASUBSET selects subset of sensorData structure 3 | %-------------------------------------------------------------------------- 4 | % Inputs: 5 | %-------------------------------------------------------------------------- 6 | % sensorData- either a nx1 cell containing sensor data sturcts 7 | % or a single sensor data struct 8 | % idx- index of data points in the structs to keep 9 | % 10 | %-------------------------------------------------------------------------- 11 | % Outputs: 12 | %-------------------------------------------------------------------------- 13 | % sensorData- either a nx1 cell containing sensor data sturcts 14 | % or a single sensor data struct 15 | % 16 | %-------------------------------------------------------------------------- 17 | % References: 18 | %-------------------------------------------------------------------------- 19 | % This function is part of the Multi-Array-Calib toolbox 20 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 21 | % 22 | % This code was written by Zachary Taylor 23 | % zacharyjeremytaylor@gmail.com 24 | % http://www.zjtaylor.com 25 | 26 | %check inputs 27 | validateattributes(idx,{'numeric'},{'integer','positive','nonzero','vector'}); 28 | 29 | if(iscell(sensorData)) 30 | for i=1:length(sensorData) 31 | sensorData{i} = FindSubset(sensorData{i}, idx); 32 | end 33 | elseif(isstruct(sensorData)) 34 | sensorData = FindSubset(sensorData, idx); 35 | else 36 | error('sensorData must be a struct of cell of structs'); 37 | end 38 | 39 | end 40 | 41 | 42 | function [ sData ] = FindSubset( sData, idx ) 43 | 44 | if(length(sData.files) > 1) 45 | sData.files = sData.files(idx,:); 46 | end 47 | 48 | sData.time = sData.time(idx,:); 49 | sData.T_Skm1_Sk = sData.T_Skm1_Sk(idx,:); 50 | sData.T_S1_Sk = sData.T_S1_Sk(idx,:); 51 | sData.T_Var_Skm1_Sk = sData.T_Var_Skm1_Sk(idx,:); 52 | sData.T_Var_S1_Sk = sData.T_Var_S1_Sk(idx,:); 53 | end 54 | 55 | -------------------------------------------------------------------------------- /timing/TformInterp.m: -------------------------------------------------------------------------------- 1 | function [ Tout ] = TformInterp( T, interVal ) 2 | %TFORMINTERP Interpolate between eye(4) and a tform matrix T using slerp 3 | %-------------------------------------------------------------------------- 4 | % Required Inputs: 5 | %-------------------------------------------------------------------------- 6 | % T- 4x4 tform matrix 7 | % interVal- ratio to interpolate to (0 to 1), can also extrapolate 8 | % 9 | %-------------------------------------------------------------------------- 10 | % Outputs: 11 | %-------------------------------------------------------------------------- 12 | % Tout- 4x4 output tform matrix 13 | % 14 | %-------------------------------------------------------------------------- 15 | % References: 16 | %-------------------------------------------------------------------------- 17 | % This function is part of the Multi-Array-Calib toolbox 18 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 19 | % 20 | % This code was written by Zachary Taylor 21 | % zacharyjeremytaylor@gmail.com 22 | % http://www.zjtaylor.com 23 | 24 | %check inputs 25 | validateattributes(T,{'numeric'},{'size',[4,4]}); 26 | validateattributes(interVal,{'numeric'},{'scalar'}); 27 | 28 | %setup output 29 | Tout = eye(4); 30 | 31 | %interpolate translation 32 | Tout(1:3,4) = interVal*T(1:3,4); 33 | 34 | %interpolate rotation 35 | quat1 = [1,0,0,0]; 36 | quat2 = dcm2quat(T(1:3,1:3)); 37 | 38 | theta = acos(dot(quat1,quat2)); 39 | 40 | if(theta == 0) 41 | return; 42 | end 43 | 44 | A = sin((1-interVal)*theta)/sin(theta); 45 | B = sin(interVal*theta)/sin(theta); 46 | 47 | out = A*quat1 + B*quat2; 48 | 49 | Tout(1:3,1:3) = quat2dcm(out); 50 | 51 | end 52 | 53 | -------------------------------------------------------------------------------- /timing/xcorr2_fast.m: -------------------------------------------------------------------------------- 1 | function cross_corr = xcorr2_fast(T,A,RetSizeOpt) 2 | % function cross_corr = xcorr2_fast(T,A,RetSizeOpt) 3 | % image correlation that switches between freq and spatial domains 4 | % automatically for speed. 5 | % T - Template or Image 6 | % A - Template or Image 7 | % RetSizeOpt - 'valid', 'same','full' - see doc on conv2 for details 8 | % default is 'full' to be compatible with the original matlab xcorr_fast 9 | % that I found buried in another mathworks function somewhere. 10 | % cross_corr - response image - size depends on RetSizeOpt 11 | if(~exist('RetSizeOpt','var')) 12 | RetSizeOpt = 'full'; 13 | end 14 | if(numel(T) > numel(A)) 15 | Tmp = A; 16 | A=T; 17 | T=Tmp; 18 | Tmp=[]; 19 | end 20 | T_size = size(T); 21 | A_size = size(A); 22 | outsize = A_size + T_size - 1; 23 | 24 | % figure out when to use spatial domain vs. freq domain 25 | conv_time = time_conv2(T_size,A_size); % 1 conv2 26 | fft_time = 3*time_fft2(outsize); % 2 fft2 + 1 ifft2 27 | 28 | if (conv_time < fft_time) 29 | cross_corr = conv2(A,rot90(T,2),RetSizeOpt); 30 | RetSizeOpt=''; 31 | else 32 | cross_corr = freqxcorr(T,A,outsize); 33 | end 34 | switch(lower(RetSizeOpt)) 35 | case 'same' 36 | TmpltRadius=floor(T_size/2); 37 | cross_corr = cross_corr(TmpltRadius(1)+1:TmpltRadius(1)+A_size(1),TmpltRadius(2)+1:TmpltRadius(2)+A_size(2)); 38 | case 'valid' 39 | OutImgSize = A_size - T_size + 1; 40 | cross_corr = cross_corr(T_size(1):T_size(1)+OutImgSize(1)-1,T_size(2):T_size(2)+OutImgSize(2)-1); 41 | end 42 | %------------------------------- 43 | % Function freqxcorr 44 | % 45 | function xcorr_ab = freqxcorr(a,b,outsize) 46 | 47 | % calculate correlation in frequency domain 48 | Fa = fft2(rot90(a,2),outsize(1),outsize(2)); 49 | Fb = fft2(b,outsize(1),outsize(2)); 50 | xcorr_ab = real(ifft2(Fa .* Fb)); 51 | 52 | function time = time_conv2(obssize,refsize) 53 | K = 2.7e-8; 54 | % convolution time = K*prod(obssize)*prod(refsize) 55 | time = K*prod(obssize)*prod(refsize); 56 | 57 | 58 | %------------------------------- 59 | % Function time_fft2 60 | % 61 | function time = time_fft2(outsize) 62 | 63 | % time a frequency domain convolution by timing two one-dimensional ffts 64 | 65 | R = outsize(1); 66 | S = outsize(2); 67 | 68 | % Tr = time_fft(R); 69 | % K_fft = Tr/(R*log(R)); 70 | 71 | % K_fft was empirically calculated by the 2 commented-out lines above. 72 | K_fft = 3.3e-7; 73 | Tr = K_fft*R*log(R); 74 | 75 | if S==R 76 | Ts = Tr; 77 | else 78 | % Ts = time_fft(S); % uncomment to estimate explicitly 79 | Ts = K_fft*S*log(S); 80 | end 81 | 82 | time = S*Tr + R*Ts; 83 | 84 | -------------------------------------------------------------------------------- /varApprox/CombEst.m: -------------------------------------------------------------------------------- 1 | function [ out, outV ] = CombEst( varargin ) 2 | %COMBEST combines a series of estimates + variance into a single esitmate 3 | %-------------------------------------------------------------------------- 4 | % Inputs: 5 | %-------------------------------------------------------------------------- 6 | % Requires an even number of inputs of the following type 7 | % val- nxm array of estimated values 8 | % var- nxm corrosponding variance 9 | % 10 | % Example calls 11 | % CombEst(valA,varA) 12 | % CombEst(valA,varA,valB,varB) 13 | % CombEst(valA,varA,valB,varB,valC,varC,...) 14 | % 15 | %-------------------------------------------------------------------------- 16 | % Outputs: 17 | %-------------------------------------------------------------------------- 18 | % val- nxm array of combined values 19 | % var- nxm corrosponding variance 20 | % 21 | %-------------------------------------------------------------------------- 22 | % References: 23 | %-------------------------------------------------------------------------- 24 | % This function is part of the Multi-Array-Calib toolbox 25 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 26 | % 27 | % This code was written by Zachary Taylor 28 | % zacharyjeremytaylor@gmail.com 29 | % http://www.zjtaylor.com 30 | 31 | %check inputs 32 | for i = 1:ceil(length(varargin)/2) 33 | validateattributes(varargin{2*i-1},{'double'},{'size',size(varargin{1})}); 34 | validateattributes(varargin{2*i},{'double'},{'size',size(varargin{1})}); 35 | end 36 | 37 | base = varargin(1:2:end); 38 | baseV = varargin(2:2:end); 39 | 40 | out = zeros(size(base{1})); 41 | outV = out; 42 | 43 | %combine 44 | for i = 1:length(base) 45 | baseV{i} = 1./baseV{i}; 46 | out = out + base{i}.*baseV{i}; 47 | outV = outV + baseV{i}; 48 | end 49 | 50 | out = out./outV; 51 | outV = 1./outV; 52 | 53 | end 54 | 55 | -------------------------------------------------------------------------------- /varApprox/IndVar.m: -------------------------------------------------------------------------------- 1 | function [ val, var ] = IndVar(offset, func, varargin ) 2 | %INDVAR Finds variance of a function given input variance and assuming the 3 | % inputs have independent effect and the output is gaussian 4 | %-------------------------------------------------------------------------- 5 | % Inputs: 6 | %-------------------------------------------------------------------------- 7 | % offset- offset to apply when testing variance (0.01 usually works well) 8 | % func- handle to function to find variance of, note all inputs to this 9 | % function must be of type double 10 | % 11 | % Requires the inputs to function func + their variance 12 | % For example if func is called by func(A,B) 13 | % IndVar would be called as IndVar(offset,@func,A,varA,B,varB) 14 | % 15 | %-------------------------------------------------------------------------- 16 | % Outputs: 17 | %-------------------------------------------------------------------------- 18 | % val- output of func 19 | % var- variance of function output 20 | % 21 | %-------------------------------------------------------------------------- 22 | % References: 23 | %-------------------------------------------------------------------------- 24 | % This function is part of the Multi-Array-Calib toolbox 25 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 26 | % 27 | % This code was written by Zachary Taylor 28 | % zacharyjeremytaylor@gmail.com 29 | % http://www.zjtaylor.com 30 | 31 | %check inputs 32 | validateattributes(offset,{'double'},{'scalar'}); 33 | validateattributes(func,{'function_handle'},{}); 34 | for i = 1:ceil(length(varargin)/2) 35 | validateattributes(varargin{2*i-1},{'double'},{}); 36 | validateattributes(varargin{2*i},{'double'},{'size',size(varargin{2*i})}); 37 | end 38 | 39 | base = varargin(1:2:end); 40 | baseV = varargin(2:2:end); 41 | val = func(base{:}); 42 | 43 | var = zeros(size(val)); 44 | for i = 1:length(base) 45 | for j = 1:length(base{i}) 46 | %sample at offset 47 | base{i}(j) = base{i}(j) + offset; 48 | temp = func(base{:}); 49 | base{i}(j) = base{i}(j) - offset; 50 | 51 | %combine variance 52 | if(isfinite(baseV{i}(j))) 53 | temp = baseV{i}(j).*((temp-val)./offset).^2; 54 | else 55 | %handle inf variance 56 | temp = temp-val; 57 | temp(temp~=0) = inf; 58 | end 59 | var = var + temp; 60 | end 61 | end 62 | 63 | end 64 | 65 | -------------------------------------------------------------------------------- /varApprox/IndVarVec.m: -------------------------------------------------------------------------------- 1 | function [ val, var ] = IndVarVec(offset, func, varargin ) 2 | %INDVAR Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | base = varargin(1:2:end); 6 | baseV = varargin(2:2:end); 7 | val = func(varargin{:}); 8 | 9 | var = zeros(size(val)); 10 | for i = 1:length(base) 11 | for j = 1:size(base{i},2) 12 | %for k = 1:size(base{i},3) 13 | base{i}(:,j,:) = base{i}(:,j,:) + sqrt(baseV{i}(:,j,:)).*offset; 14 | joint = cell(size(varargin)); 15 | joint(1:2:end) = base; 16 | joint(2:2:end) = baseV; 17 | temp = func(joint{:}); 18 | base{i}(:,j,:) = base{i}(:,j,:) - sqrt(baseV{i}(:,j,:)).*offset; 19 | 20 | temp = ((temp-val)./offset).^2; 21 | var = var + temp; 22 | %end 23 | end 24 | end 25 | 26 | end 27 | 28 | -------------------------------------------------------------------------------- /varApprox/OptGrid.m: -------------------------------------------------------------------------------- 1 | function [ finalVec, finalVar ] = OptGrid( TGrid, vTGrid ) 2 | %OPTGRID converts an inconsistent grid of transformations to a vector of 3 | % transforamtions wrt sensor 1. 4 | %-------------------------------------------------------------------------- 5 | % Inputs: 6 | %-------------------------------------------------------------------------- 7 | % TGrid- nxn cell of sensor transformations. TGrid(i,j) is the 8 | % transformation from sensor i to sensor j. Transforms are stored in 9 | % scaled vector form. Any unknown transformation is left empty 10 | % vTGrid- nxn cell of the transforamiotn variances 11 | % 12 | %-------------------------------------------------------------------------- 13 | % Outputs: 14 | %-------------------------------------------------------------------------- 15 | % finalVec- nx6 array of sensor transoforamation vectors wrt sensor 1 16 | % finalVar- nx6 array of corrosponding variances 17 | % 18 | %-------------------------------------------------------------------------- 19 | % References: 20 | %-------------------------------------------------------------------------- 21 | % This function is part of the Multi-Array-Calib toolbox 22 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 23 | % 24 | % This code was written by Zachary Taylor 25 | % zacharyjeremytaylor@gmail.com 26 | % http://www.zjtaylor.com 27 | 28 | %check inputs 29 | validateattributes(TGrid,{'cell'},{'square'}); 30 | validateattributes(vTGrid,{'cell'},{'size',size(TGrid)}); 31 | 32 | for i = 1:length(TGrid(:)) 33 | if(~isempty(TGrid{i})) 34 | validateattributes(TGrid{i},{'double'},{'size',[1,7]}); 35 | validateattributes(vTGrid{i},{'double'},{'size',[1,7]}); 36 | end 37 | end 38 | 39 | %setup output 40 | finalVec = zeros(size(TGrid,1),6); 41 | finalVar = inf*ones(size(TGrid,1),6); 42 | 43 | %conversion function 44 | findVec = @(A,B) T2V(V2T(S2V(A))*V2T(S2V(B))); 45 | 46 | for i = 1:size(TGrid,1); 47 | TGrid{i,i} = zeros(1,7); 48 | vTGrid{i,i} = zeros(1,7); 49 | end 50 | 51 | for i = 1:size(TGrid,1); 52 | for j = 1:size(vTGrid,1); 53 | if(i < j) 54 | %find sol and variance 55 | [A,VA] = IndVar(0.01,findVec,TGrid{1,i},vTGrid{1,i},TGrid{i,j},vTGrid{i,j}); 56 | %combine 57 | [finalVec(j,:),finalVar(j,:)] = CombEst(finalVec(j,:),finalVar(j,:),A,VA); 58 | end 59 | end 60 | end 61 | 62 | %handle cases of 0 and inf variance 63 | finalVar(1,:) = 0; 64 | finalVar(~isfinite(finalVec)) = inf; 65 | finalVec(~isfinite(finalVec)) = 0; 66 | 67 | end 68 | 69 | -------------------------------------------------------------------------------- /varApprox/lapApprox.m: -------------------------------------------------------------------------------- 1 | function [ val, var ] = IndVar(offset, func, varargin ) 2 | %INDVAR Finds variance of a function given input variance and assuming the 3 | % inputs have independent effect and the output is gaussian 4 | %-------------------------------------------------------------------------- 5 | % Inputs: 6 | %-------------------------------------------------------------------------- 7 | % offset- offset to apply when testing variance (0.01 usually works well) 8 | % func- handle to function to find variance of, note all inputs to this 9 | % function must be of type double 10 | % 11 | % Requires the inputs to function func + their variance 12 | % For example if func is called by func(A,B) 13 | % IndVar would be called as IndVar(offset,@func,A,varA,B,varB) 14 | % 15 | %-------------------------------------------------------------------------- 16 | % Outputs: 17 | %-------------------------------------------------------------------------- 18 | % val- output of func 19 | % var- variance of function output 20 | % 21 | %-------------------------------------------------------------------------- 22 | % References: 23 | %-------------------------------------------------------------------------- 24 | % This function is part of the Multi-Array-Calib toolbox 25 | % https://github.com/ZacharyTaylor/Multi-Array-Calib 26 | % 27 | % This code was written by Zachary Taylor 28 | % zacharyjeremytaylor@gmail.com 29 | % http://www.zjtaylor.com 30 | 31 | %check inputs 32 | validateattributes(offset,{'double'},{'scalar'}); 33 | validateattributes(func,{'function_handle'},{}); 34 | for i = 1:ceil(length(varargin)/2) 35 | validateattributes(varargin{2*i-1},{'double'},{}); 36 | validateattributes(varargin{2*i},{'double'},{'size',size(varargin{2*i})}); 37 | end 38 | 39 | base = varargin(1:2:end); 40 | baseV = varargin(2:2:end); 41 | val = func(base{:}); 42 | 43 | var = zeros(size(val)); 44 | for i = 1:length(base) 45 | for j = 1:length(base{i}) 46 | %sample at offset 47 | base{i}(j) = base{i}(j) + offset; 48 | temp = func(base{:}); 49 | base{i}(j) = base{i}(j) - offset; 50 | 51 | %combine variance 52 | if(isfinite(baseV{i}(j))) 53 | temp = baseV{i}(j).*((temp-val)./offset).^2; 54 | else 55 | %handle inf variance 56 | temp = temp-val; 57 | temp(temp~=0) = inf; 58 | end 59 | var = var + temp; 60 | end 61 | end 62 | 63 | end 64 | 65 | --------------------------------------------------------------------------------