├── README.md └── Software └── matlab ├── GNN.m ├── PlayPointCloud.m ├── ROSbag_destruct.m ├── epoch2datetime.m ├── euclidDist.m ├── euclidDist2PointClouds.m ├── filterPC.m ├── filterTracks.m ├── filterVLP16.m ├── findLim.m ├── fixFileName.m ├── genImages.m ├── merge_ptCloud.m ├── merge_ptCloud_tf.m ├── movementPtCl.m ├── run_main.m ├── shrink_box.m ├── smooth3DPoints_med.m ├── smooth3DPoints_sgolay.m ├── stitch.m ├── utils └── geom3d │ ├── geom3d-demos │ └── html │ │ ├── demoDrawTubularMesh.png │ │ ├── demoDrawTubularMesh_01.png │ │ ├── demoDrawTubularMesh_02.png │ │ ├── demoDrawTubularMesh_03.png │ │ ├── demoDrawTubularMesh_04.png │ │ ├── demoGeom3d.png │ │ ├── demoGeom3d_01.png │ │ ├── demoGeom3d_02.png │ │ ├── demoGeom3d_03.png │ │ ├── demoInertiaEllipsoid.png │ │ ├── demoInertiaEllipsoid_01.png │ │ ├── demoInertiaEllipsoid_02.png │ │ ├── demoInertiaEllipsoid_03.png │ │ ├── demoRevolutionSurface.png │ │ ├── demoRevolutionSurface_01.png │ │ ├── demoRevolutionSurface_02.png │ │ ├── drawSoccerBall.png │ │ ├── drawSoccerBall_01.png │ │ ├── drawSoccerBall_02.png │ │ └── drawSoccerBall_03.png │ └── meshes3d-demos │ └── html │ ├── demoPolyhedra.png │ ├── demoPolyhedra_01.png │ ├── demoPolyhedra_02.png │ ├── demoPolyhedra_03.png │ ├── demoPolyhedra_04.png │ ├── demoPolyhedra_05.png │ ├── demoPolyhedra_06.png │ ├── demoPolyhedra_07.png │ ├── demoPolyhedra_08.png │ ├── demoPolyhedra_09.png │ ├── demoTriangulateFaces.png │ ├── demoTriangulateFaces_01.png │ ├── demoTriangulateFaces_02.png │ ├── demoVoronoiCell.png │ ├── demoVoronoiCell_01.png │ ├── demoVoronoiCell_02.png │ └── demoVoronoiCell_03.png ├── velodyneExtract.m ├── voxelClassification.m ├── voxelMerge.m └── voxelMovement.m /README.md: -------------------------------------------------------------------------------- 1 | # LiDARBicycleDetection 2 | Code and documents to support the Thesis: Progress Towards LiDAR Based Bicycle Detection in Urban Environments 3 | 4 | The rosbag data for this code can be downloaded from https://www.coe.neu.edu/fieldrobotics/2017_bicycle_detection/ 5 | **Note:** The rosbag that you download are compressed, you might need to decompress it using `rosbag decompress filename` to run it at the correct speed. 6 | -------------------------------------------------------------------------------- /Software/matlab/GNN.m: -------------------------------------------------------------------------------- 1 | function [points,adjacency_tracks,target]=GNN(target,points,... 2 | method,max_linking_distance,max_gap_closing) 3 | 4 | addpath('utils/SimpleTracker/'); 5 | 6 | if nargin<3 7 | %method = 'NearestNeighbor'; 8 | method = 'Hungarian'; 9 | end 10 | if nargin<4 11 | max_linking_distance = 1.5; 12 | end 13 | if nargin<5 14 | max_gap_closing = 1; 15 | end 16 | 17 | for j=1:length(target(end).class) 18 | tmp(j,:) = target(end).class(j).center; 19 | end 20 | 21 | if isempty(points) 22 | points{1}(1:j,:) = tmp; 23 | else 24 | points{end+1}(1:j,:) = tmp; 25 | end 26 | 27 | [~, adjacency_tracks] = simpletracker(points,... 28 | 'MaxLinkingDistance', max_linking_distance, ... 29 | 'MaxGapClosing', max_gap_closing, ... 30 | 'Method',method,'Debug', false); 31 | 32 | end -------------------------------------------------------------------------------- /Software/matlab/PlayPointCloud.m: -------------------------------------------------------------------------------- 1 | clc; clear; 2 | 3 | zoom = 1; 4 | save = 0; 5 | rate = 10; % frame rate of output video 6 | breakpoint = 0; 7 | 8 | folder = uigetdir('.','Select PCD Folder'); 9 | if ~folder 10 | disp('No Folder Selected... Exiting!'); 11 | return; 12 | end 13 | [parent,~]=fileparts(folder); 14 | [~,subDir]=fileparts(parent); 15 | d = dir(folder); 16 | d = d(3:end); 17 | 18 | xlim=[0,1]; ylim=[0,1]; zlim=[0,1]; 19 | 20 | if save 21 | if zoom 22 | fname = fullfile(parent,[subDir '_zoom.avi']); 23 | else 24 | fname = fullfile(parent,[subDir '.avi']); 25 | end 26 | disp(['Saving file: ' fname]); 27 | v = VideoWriter(fname); 28 | v.FrameRate = rate; 29 | open(v); 30 | end 31 | 32 | for i=1:length(d) 33 | h = figure(10); ax = axes('Parent',h); set(h,'Color','black'); 34 | if i==1 35 | set(h, 'Position', get(0, 'Screensize')); 36 | end 37 | fname = fullfile(folder,d(i).name); 38 | ptCloud = pcread(fname); 39 | 40 | if zoom 41 | %roi = [-8 8 -8 8 -2 2]; % bike circle 42 | %roi = [-5 10 -6 6 -2 2]; % extra bike circle 43 | %roi = [-15 15 -20 20 -2 2]; % multibike 44 | roi = [-40 40 -10 15 -2 5]; % road 45 | end 46 | 47 | indices = findPointsInROI(ptCloud, roi); 48 | ptCloud = select(ptCloud, indices); 49 | 50 | [xlim, ylim, zlim] = findLim(ptCloud,xlim,ylim,zlim); 51 | pcshow(ptCloud,'MarkerSize',40); 52 | 53 | if zoom 54 | %view(160, 20); % bike circle 55 | view([-90,35]); % rear view 56 | %view([-180,35]); % road side view 57 | else 58 | view(-90, 30); 59 | end 60 | axis([xlim ylim -2 2]); 61 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 62 | %xlabel('X (meters)'); ylabel('Y (meters)'); 63 | %title(['Frame: ' num2str(i)]); 64 | drawnow; 65 | 66 | if exist('v','var') 67 | F = getframe(h); 68 | writeVideo(v,F); 69 | end 70 | 71 | if i == breakpoint 72 | keyboard; 73 | end 74 | clf; 75 | end 76 | if exist('v','var') 77 | close(v); 78 | end 79 | disp('Completed!!!'); 80 | -------------------------------------------------------------------------------- /Software/matlab/ROSbag_destruct.m: -------------------------------------------------------------------------------- 1 | function [folder]=ROSbag_destruct(FileName,chunk_size) 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % ROSbag_destruct - 9/18/17 - Antonio Rufo 4 | % Script extracts contents of ROS bag file. 5 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 6 | 7 | % str = 'C:\Users\Thinkpad-Rufo\Documents\MATLAB\Robotics\matlab_gen\msggen'; 8 | % addpath(str); 9 | % savepath; 10 | % rosgenmsg(str); 11 | if nargin<2 12 | chunk_size = 1000; 13 | end 14 | 15 | if nargin<1 16 | % set step size of files (100 is the safest) (10000 is faster but crashes) 17 | chunk_size = 100; 18 | %Change for your specific path 19 | PathName = '/home/antoniorufo/catkin_ws/'; 20 | addpath(PathName); 21 | 22 | [FileName,PathName] = uigetfile('*','Get rosbag Log File',... 23 | PathName); %GUI to open desired bag file 24 | if FileName == 0 25 | disp('No File Selected... Exiting!'); 26 | return; 27 | end 28 | else 29 | [PathName,name,ext] = fileparts(FileName); 30 | FileName = [name ext]; 31 | end 32 | 33 | folder = FileName(1:end-4); % Create ouput folder 34 | if ~isdir(folder) 35 | mkdir(folder); 36 | end 37 | 38 | disp('Opening bag file'); 39 | tic; 40 | bag = rosbag(fullfile(PathName,FileName)); 41 | toc 42 | 43 | %Find all unique Topic names and row locations 44 | [names,~,row]=unique(bag.MessageList.Topic); 45 | 46 | topicList = bag.AvailableTopics; 47 | fname = [folder filesep FileName(1:end-4) '_TopicList']; 48 | save(fname,'topicList'); % save file containing only topic list 49 | 50 | tic; 51 | for i=1:length(names) 52 | 53 | try 54 | tab = bag.MessageList(row==i,:); %Extract messages only from specific topic 55 | str = char(tab.Topic(1)); % Extract topics name as string 56 | bagSel = select(bag,'Topic',str); % Select topics string from bag 57 | ts = timeseries(bagSel); 58 | 59 | str = strrep(str, filesep, '_'); 60 | disp(['Topic: ' str ' has ' num2str(height(tab)) ' msgs']); 61 | 62 | if strcmp('_ns1_velodyne_points',str) || strcmp('_ns2_velodyne_points',str) || strcmp('_rosout',str) 63 | 64 | % else 65 | % continue; 66 | end 67 | if height(tab)>chunk_size % Handel topics with more messages then chunk size 68 | for j=1:chunk_size:height(tab)-chunk_size %iterate through chunks 69 | step = chunk_size - 1; 70 | data = readMessages(bagSel,j:j+step); %extract section of message 71 | time = ts.Time(j:j+step); 72 | fname = [folder filesep FileName(1:end-4) '_BAG' str '_'... 73 | num2str(j) '_to_' num2str(j+step)]; 74 | disp(['Saving data for Topic: ' str ' from '... 75 | num2str(j) ' to ' num2str(j+step)... 76 | ' ' datestr(time(1)) ' to ' datestr(time(end))]); 77 | save(fname,'data','time'); 78 | 79 | end 80 | j = j+1; 81 | data = readMessages(bagSel,j+step:height(tab)); %extract end of message 82 | time = ts.Time(j+step:height(tab)); 83 | fname = [folder filesep FileName(1:end-4) '_BAG' str '_'... 84 | num2str(j+step) '_to_' num2str(height(tab))]; 85 | disp(['Saving data for Topic: ' str ' from '... 86 | num2str(j+step) ' to ' num2str(height(tab))... 87 | ' ' datestr(time(1)) ' to ' datestr(time(end))]); 88 | save(fname,'data','time'); 89 | else 90 | time = ts.Time; 91 | data = readMessages(bagSel); %Extract all data in topic 92 | fname = [folder filesep FileName(1:end-4) '_BAG' str]; 93 | disp(['Saving all data for Topic: ' str]); 94 | save(fname,'data','time'); 95 | end 96 | catch e 97 | disp(['Topic: ' str ' ' e.message]); 98 | err{i} = ['Topic: ' str ' ' e.message]; 99 | %keyboard; 100 | end 101 | 102 | end 103 | 104 | if exist('err','var') 105 | save([folder filesep 'errors.mat'],'err','e'); 106 | end 107 | toc; 108 | 109 | end -------------------------------------------------------------------------------- /Software/matlab/epoch2datetime.m: -------------------------------------------------------------------------------- 1 | function [time_matlab,time_matlab_string]=epoch2datetime(epoch,zone) 2 | 3 | time_reference = datenum('1970', 'yyyy'); 4 | time_matlab = time_reference + epoch / 86400; % seconds in day 5 | time_matlab = time_matlab - hours(zone); 6 | time_matlab_string = datestr(time_matlab, 'yyyymmdd HH:MM:SS.FFF'); 7 | 8 | end -------------------------------------------------------------------------------- /Software/matlab/euclidDist.m: -------------------------------------------------------------------------------- 1 | function distMat = euclidDist(reference,sample) 2 | % calculate the euclidean distance for every point in sample point cloud to 3 | % the closest point in the reference point cloud (number of columns must 4 | % match) 5 | 6 | % INPUT: 7 | % reference = P x N matrix 8 | % sample = P x N matrix 9 | % OUTPUT: 10 | % distMat = P x N matrix 11 | 12 | distMat = zeros(size(sample,1),1); 13 | for row_idx = 1:size(sample,1) 14 | diff = reference-repmat(sample(row_idx,:),size(reference,1),1); 15 | D = sqrt(sum(diff.^2,2)); 16 | distMat(row_idx) = min(D); 17 | end -------------------------------------------------------------------------------- /Software/matlab/euclidDist2PointClouds.m: -------------------------------------------------------------------------------- 1 | function distMat = euclidDist2PointClouds(ptCloudA,ptCloudB) 2 | % calculate the euclidean distance for every point in sample point cloud to 3 | % the closest point in the reference point cloud (number of columns must 4 | % match) 5 | 6 | % INPUT: 7 | % ptCloudA = Point Cloud A 8 | % ptCloudB = Point Cloud B 9 | % OUTPUT: 10 | % distMat = P x N matrix 11 | 12 | 13 | reference = ptCloudA.Location; % reference = M x N matrix 14 | sample = ptCloudB.Location; % sample = P x N matrix 15 | 16 | 17 | distMat = zeros(size(sample,1),1); 18 | for row_idx = 1:size(sample,1) 19 | diff = reference-repmat(sample(row_idx,:),size(reference,1),1); 20 | D = sqrt(sum(diff.^2,2)); 21 | distMat(row_idx) = min(D); 22 | end -------------------------------------------------------------------------------- /Software/matlab/filterPC.m: -------------------------------------------------------------------------------- 1 | function [ptCloudOut]=filterPC(ptCloudIn) 2 | 3 | ptCloudIn = removeInvalidPoints(ptCloudIn); 4 | ptCloudOut = pcdenoise(ptCloudIn); 5 | 6 | 7 | end -------------------------------------------------------------------------------- /Software/matlab/filterTracks.m: -------------------------------------------------------------------------------- 1 | function [adjTracks_out,trackLen_out]=filterTracks(adjTracks,... 2 | trackLen_old) 3 | 4 | n_tracks = numel(adjTracks); 5 | trackLen = zeros(n_tracks,1); 6 | adjTracks_out = {}; 7 | count = 1; 8 | 9 | for i = 1 : n_tracks 10 | seltrack = adjTracks{i}; 11 | if length(seltrack)>3 % track must be longer than 3 points 12 | trackLen(count) = length(seltrack); % store length of track 13 | adjTracks_out{count} = adjTracks{i}; % store track points 14 | count = count + 1; 15 | end 16 | end 17 | 18 | % find difference in track lengths 19 | difflen = length(trackLen) - length(trackLen_old); 20 | for i=1:difflen % add zeros if dimm mismatch 21 | if difflen>0 22 | trackLen_old = vertcat(trackLen_old,0); 23 | else 24 | keyobard; 25 | trackLen = vertcat(trackLen,0); 26 | end 27 | end 28 | 29 | trackLen_out = trackLen; 30 | 31 | end -------------------------------------------------------------------------------- /Software/matlab/filterVLP16.m: -------------------------------------------------------------------------------- 1 | function [ptCloud] = filterVLP16(ptCloudIn, display) 2 | 3 | if nargin<2 4 | display = 0; 5 | end 6 | 7 | maxDistance = .5; % Set the maximum point-to-plane distance in meters 8 | referenceVector = [0,0,1]; 9 | maxAngularDistance = 0.5; 10 | 11 | ptCloudIn = removeInvalidPoints(ptCloudIn); 12 | ptCloudIn = pcdenoise(ptCloudIn); 13 | ptCloudIn.Normal = pcnormals(ptCloudIn); 14 | 15 | medZ = median(ptCloudIn.ZLimits); 16 | 17 | if abs(diff(ptCloudIn.ZLimits)) < 15 18 | % remove Ceiling 19 | Z = [(ptCloudIn.ZLimits(2) - 0.5), ptCloudIn.ZLimits(2)]; 20 | roi = [-inf,inf;-inf,inf;Z]; 21 | sampleIndices = findPointsInROI(ptCloudIn,roi); 22 | 23 | [modelP1,inlierIndices,outlierIndices] = pcfitplane(ptCloudIn,... 24 | maxDistance,referenceVector,'SampleIndices',sampleIndices); 25 | 26 | plane = select(ptCloudIn,inlierIndices); 27 | ptCloud = select(ptCloudIn,outlierIndices); 28 | 29 | else 30 | ptCloud = ptCloudIn; 31 | end 32 | 33 | % remove ground 34 | medZ = median(ptCloudIn.ZLimits); 35 | minLim = medZ - diff(ptCloudIn.ZLimits); 36 | Z = [minLim, minLim + 0.1]; 37 | % Z = [ptCloud.ZLimits(1), (ptCloud.ZLimits(1) +0.5)]; % set next z limit 38 | roi = [-inf,inf;-inf,inf;Z]; 39 | sampleIndices = findPointsInROI(ptCloud,roi); 40 | 41 | % [modelP2,inlierIndices,outlierIndices] = pcfitplane(ptCloud,... 42 | % maxDistance,referenceVector,maxAngularDistance); 43 | [modelP2,inlierIndices,outlierIndices] = pcfitplane(ptCloud,... 44 | maxDistance,referenceVector,'SampleIndices',sampleIndices); 45 | 46 | if ~isempty(inlierIndices) 47 | plane1 = select(ptCloud,inlierIndices); 48 | ptCloud = select(ptCloud,outlierIndices); 49 | end 50 | 51 | if display 52 | figure(10); 53 | 54 | if exist('plane','var') 55 | subplot(2,2,1); pcshow(plane); title('Ceiling Plane'); 56 | end 57 | if exist('plane1','var') 58 | subplot(2,2,2); pcshow(plane1); 59 | end 60 | title('Floor Plane'); 61 | 62 | subplot(2,2,3:4); pcshow(ptCloudIn); hold on; 63 | 64 | if exist('modelP1','var') 65 | plot(modelP1); 66 | end 67 | if exist('modelP2','var') 68 | plot(modelP2); 69 | end 70 | hold off; 71 | 72 | title('Floor & Ceiling models'); xlabel('X (m)'); ylabel('Y (m)'); 73 | 74 | figure(20); 75 | pcshow(ptCloud); title('Filtered Point Cloud'); 76 | xlabel('X (m)'); ylabel('Y (m)'); 77 | drawnow; 78 | end 79 | 80 | end -------------------------------------------------------------------------------- /Software/matlab/findLim.m: -------------------------------------------------------------------------------- 1 | function [xlim, ylim, zlim]=findLim(ptCloud,xlim,ylim,zlim) 2 | maxX = max(ptCloud.XLimits); 3 | minX = min(ptCloud.XLimits); 4 | 5 | maxY = max(ptCloud.YLimits); 6 | minY = min(ptCloud.YLimits); 7 | 8 | maxZ = max(ptCloud.ZLimits); 9 | minZ = min(ptCloud.ZLimits); 10 | 11 | if maxX > xlim(2) 12 | xlim(2) = maxX; 13 | end 14 | if minX < xlim(1) 15 | xlim(1) = minX; 16 | end 17 | 18 | if maxY > ylim(2) 19 | ylim(2) = maxY; 20 | end 21 | if minY < ylim(1) 22 | ylim(1) = minY; 23 | end 24 | 25 | if maxZ > zlim(2) 26 | zlim(2) = maxZ; 27 | end 28 | if minZ < zlim(1) 29 | zlim(1) = minZ; 30 | end 31 | 32 | end -------------------------------------------------------------------------------- /Software/matlab/fixFileName.m: -------------------------------------------------------------------------------- 1 | function fixFileName(folder) 2 | 3 | if nargin<1 4 | folder = uigetdir('.','Select PCD Folder'); 5 | if ~folder 6 | disp('No Folder Selected... Exiting!'); 7 | return; 8 | end 9 | end 10 | 11 | d = dir(folder); 12 | d = d(3:end); 13 | 14 | leadZeros = num2str(floor(length(d)/10)); 15 | leadZeros = length(leadZeros)+1; 16 | 17 | disp(['Renaming ' num2str(length(d)) ' file(s) in folder: ' folder]); 18 | disp(['Leading Zeros: ' num2str(leadZeros)]); 19 | 20 | for ii=1:length(d) 21 | 22 | [~,file,ext] = fileparts(d(ii).name); 23 | k = strfind(file,'_'); 24 | 25 | strNum = sprintf(['%0' num2str(leadZeros) 'd'],... 26 | str2num(file(k(end)+1:end))); 27 | 28 | source = fullfile(folder,d(ii).name); 29 | destination = fullfile(folder,[file(1:k(end)) strNum ext]); 30 | 31 | [status,msg,~] = movefile(source,destination); 32 | disp(['Moving ' d(ii).name ' to ' [file(1:k(end)) strNum ext]]); 33 | 34 | if status~=1 35 | disp(['Failed @ file: ' d(ii).name]); 36 | disp([' ' msg]) 37 | end 38 | 39 | end 40 | disp('Completed!!!'); 41 | 42 | end 43 | 44 | -------------------------------------------------------------------------------- /Software/matlab/genImages.m: -------------------------------------------------------------------------------- 1 | close all; clc; clear; 2 | addpath(genpath('./utils')); 3 | 4 | %% Single Bike 5 | 6 | frame = 50; 7 | fname = ['./2017-09-18-20-09-08/pcd/velodyne_points_frame_0' num2str(frame) '.pcd']; 8 | disp(['Loading: ' fname]); 9 | 10 | frameB = 75; 11 | fnameB = ['./2017-09-18-20-11-50/pcd/velodyne_points_frame_0' num2str(frameB) '.pcd']; 12 | disp(['Loading: ' fnameB]); 13 | 14 | pc_close = pcread(fname); 15 | pc_close = filterPC(pc_close); 16 | 17 | pc_far = pcread(fnameB); 18 | pc_far = filterPC(pc_far); 19 | 20 | indices = findPointsInROI(pc_close, [-10 10 -10 10 -inf inf]); 21 | pc_close = select(pc_close, indices); 22 | 23 | h = figure(101); ax = axes('Parent',h); set(h,'Color','black'); 24 | % subplot(2,2,1); 25 | pcshow(pc_close,'MarkerSize',20); 26 | title('Point Cloud - Small Circle - Counter Clockwise'); 27 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 28 | % axis([-50 50 -50 50 -inf inf]); 29 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 30 | %saveas(h,'H1.fig'); 31 | 32 | indices = findPointsInROI(pc_close, [-1 2 -1 3 -inf 4]); 33 | pc_close = select(pc_close, indices); 34 | 35 | h = figure(102); ax = axes('Parent',h); set(h,'Color','black'); 36 | % subplot(2,2,3); 37 | pcshow(pc_close,'MarkerSize',40); title('Zoomed'); 38 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 39 | view([-88,10]); 40 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 41 | %saveas(h,'H2.fig'); 42 | 43 | h = figure(103); ax = axes('Parent',h); set(h,'Color','black'); 44 | % subplot(2,2,2); 45 | pcshow(pc_far,'MarkerSize',10); 46 | title('Point Cloud - Large Circle - Clockwise'); 47 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 48 | axis([-50 50 -50 50 -inf inf]); 49 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 50 | %saveas(h,'H3.fig'); 51 | 52 | indices = findPointsInROI(pc_far, [5.5 7 -2 0.5 -inf 2]); 53 | pc_far = select(pc_far, indices); 54 | 55 | h = figure(104); ax = axes('Parent',h); set(h,'Color','black'); 56 | % subplot(2,2,4); 57 | pcshow(pc_far,'MarkerSize',40); title('Zoomed'); 58 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 59 | view([-88,10]); 60 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 61 | %saveas(h,'H4.fig'); 62 | 63 | %% Multi - Bike 64 | 65 | frame = 20; 66 | fname = ['./2017-11-02-11-20-21_2/pcd_merged/merged_velodyne_points_frame_0' num2str(frame) '.pcd']; 67 | disp(['Loading: ' fname]); 68 | 69 | ptCloud = pcread(fname); 70 | ptCloud = filterPC(ptCloud); 71 | 72 | indices = findPointsInROI(ptCloud, [-40 40 -20 20 -inf 2]); 73 | ptCloud = select(ptCloud, indices); 74 | 75 | h = figure(105); ax = axes('Parent',h); set(h,'Color','black'); 76 | pcshow(ptCloud,'MarkerSize',40); title('Zoomed'); 77 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 78 | view([-90,90]); 79 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 80 | %saveas(h,'H5.fig'); 81 | 82 | %% Driving Route 83 | 84 | frame = 340; 85 | fname = ['./Columbus_drive2/2017-10-18-17-33-13_0/pcd_merged/merged_velodyne_points_frame_0' num2str(frame) '.pcd']; 86 | disp(['Loading: ' fname]); 87 | 88 | ptCloud = pcread(fname); 89 | ptCloud = filterPC(ptCloud); 90 | 91 | indices = findPointsInROI(ptCloud, [8 10.5 -4 -2.5 -inf 2]); 92 | pc_zoom = select(ptCloud, indices); 93 | 94 | indices = findPointsInROI(ptCloud, [-15 15 -10 10 -inf 2]); 95 | ptCloud = select(ptCloud, indices); 96 | 97 | h = figure(106); ax = axes('Parent',h); set(h,'Color','black'); 98 | pcshow(ptCloud,'MarkerSize',20); title('Zoomed'); 99 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 100 | view([-88,10]); 101 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 102 | %saveas(h,'H6.fig'); 103 | 104 | h = figure(107); ax = axes('Parent',h); set(h,'Color','black'); 105 | pcshow(pc_zoom,'MarkerSize',80); title('Zoomed'); 106 | view([-88,10]); 107 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 108 | %saveas(h,'H7.fig'); 109 | 110 | %% Octree 111 | % close all; 112 | 113 | doplot3 = @(p,varargin)scatter3(p(:,1),p(:,2),p(:,3),varargin{:}); 114 | binCapacity = 100; 115 | % style = 'equal'; 116 | style = 'weighted'; 117 | 118 | frame = 30; 119 | fname = ['./2017-09-18-20-09-08/pcd/velodyne_points_frame_0' num2str(frame) '.pcd']; 120 | disp(['Loading: ' fname]); 121 | 122 | ptCloud = pcread(fname); 123 | ptCloud = filterPC(ptCloud); 124 | xyzPoints = ptCloud.Location; 125 | 126 | ot = OcTree(xyzPoints,'style',style,'binCapacity',binCapacity); 127 | ot.shrink; 128 | ot.findroi; 129 | 130 | h = figure(108); ax = axes('Parent',h); set(h,'Color','white'); 131 | title(['Octree Frame: ' num2str(frame)]); 132 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 133 | boxH = ot.plot; 134 | cols = lines(ot.BinCount); 135 | for i = 1:ot.BinCount 136 | set(boxH(i),'Color',cols(i,:),'LineWidth', 2); 137 | %doplot3(xyzPoints(ot.PointBins==i,:),'.'); 138 | end 139 | %axis([-4 1 -4 2 -1 0.5]); 140 | % axis([-5 5 -5 5 -2 2]); 141 | view([0,0]); 142 | %set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 143 | %saveas(h,'H8.fig'); 144 | 145 | %% Density and State Estimation 146 | 147 | density_thresh = 20; 148 | 149 | frame = 10; 150 | fname = ['./2017-09-18-20-09-08/pcd/velodyne_points_frame_0' num2str(frame) '.pcd']; 151 | disp(['Loading: ' fname]); 152 | 153 | ptCloudA = pcread(fname); 154 | ptCloudA = filterPC(ptCloudA); 155 | 156 | [bbox]=voxelMovement(ptCloudA,ptCloud,ot,density_thresh); 157 | idx = find(bbox(:,1)~=0); 158 | 159 | C = ones(size(ot.Points)); 160 | C(:,1) = .5; 161 | C(:,2) = .5; 162 | C(:,3) = .5; 163 | 164 | for i=1:length(idx) 165 | indices = findPointsInROI(ptCloud, ot.roi(idx(i),:)); 166 | C(indices,1) = 1; 167 | C(indices,2) = 0; 168 | C(indices,3) = 0; 169 | end 170 | h = figure(109); ax = axes('Parent',h); set(h,'Color','black'); 171 | pcshow(xyzPoints,C,'MarkerSize',80); 172 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 173 | view([-90,90]); 174 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 175 | %saveas(h,'H9.fig'); 176 | 177 | 178 | %% Voxel Merging 179 | h = figure(110); ax = axes('Parent',h); set(h,'Color','black'); 180 | for i=1:length(idx) 181 | 182 | binMinMax = ot.BinBoundaries(idx(i),:); 183 | pts = cat(1, binMinMax([... 184 | 1 2 3; 4 2 3; 4 5 3; 1 5 3; 1 2 3;... 185 | 1 2 6; 4 2 6; 4 5 6; 1 5 6; 1 2 6; 1 2 3]),... 186 | nan(1,3), binMinMax([4 2 3; 4 2 6]),... 187 | nan(1,3), binMinMax([4 5 3; 4 5 6]),... 188 | nan(1,3), binMinMax([1 5 3; 1 5 6])); 189 | h = plot3(pts(:,1),pts(:,2),pts(:,3)); hold on; 190 | set(h,'Color',cols(i,:),... 191 | 'LineWidth',2); 192 | end 193 | pcshow(xyzPoints,C,'MarkerSize',80); 194 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 195 | axis([-5 5 -5 5 -2 2]); 196 | view([-90,90]); 197 | set(h,'Color','k'); 198 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 199 | %saveas(h,'H10.fig'); 200 | 201 | [ot,bbox]=voxelMerge(ot,bbox); 202 | idx = find(bbox(:,1)~=0); 203 | 204 | h = figure(111); ax = axes('Parent',h); set(h,'Color','black'); 205 | for i=1:length(idx) 206 | 207 | binMinMax = ot.BinBoundaries(idx(i),:); 208 | pts = cat(1, binMinMax([... 209 | 1 2 3; 4 2 3; 4 5 3; 1 5 3; 1 2 3;... 210 | 1 2 6; 4 2 6; 4 5 6; 1 5 6; 1 2 6; 1 2 3]),... 211 | nan(1,3), binMinMax([4 2 3; 4 2 6]),... 212 | nan(1,3), binMinMax([4 5 3; 4 5 6]),... 213 | nan(1,3), binMinMax([1 5 3; 1 5 6])); 214 | h = plot3(pts(:,1),pts(:,2),pts(:,3)); hold on; 215 | set(h,'Color',cols(i,:),... 216 | 'LineWidth',2); 217 | end 218 | pcshow(xyzPoints,C,'MarkerSize',80); 219 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 220 | axis([-5 5 -5 5 -2 2]); 221 | view([-90,90]); 222 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 223 | %saveas(h,'H11.fig'); 224 | 225 | %% Classification 226 | 227 | C = ones(size(ot.Points)); 228 | C(:,1) = .5; 229 | C(:,2) = .5; 230 | C(:,3) = .5; 231 | 232 | [ot,bbox,target]=voxelClassification(ot,bbox); 233 | 234 | h = figure(112); ax = axes('Parent',h); set(h,'Color','black'); 235 | for i=1:length(target) 236 | if ~isempty(target(i).roi) 237 | indices = findPointsInROI(ptCloud,target(i).roi ); 238 | 239 | C(indices,1) = target(i).color(1); 240 | C(indices,2) = target(i).color(2); 241 | C(indices,3) = target(i).color(3); 242 | 243 | binMinMax = ot.BinBoundaries(target(i).index,:); 244 | pts = cat(1, binMinMax([... 245 | 1 2 3; 4 2 3; 4 5 3; 1 5 3; 1 2 3;... 246 | 1 2 6; 4 2 6; 4 5 6; 1 5 6; 1 2 6; 1 2 3]),... 247 | nan(1,3), binMinMax([4 2 3; 4 2 6]),... 248 | nan(1,3), binMinMax([4 5 3; 4 5 6]),... 249 | nan(1,3), binMinMax([1 5 3; 1 5 6])); 250 | h = plot3(pts(:,1),pts(:,2),pts(:,3)); hold on; 251 | set(h,'Color',cols(i,:),... 252 | 'LineWidth',2); 253 | end 254 | end 255 | pcshow(xyzPoints,C,'MarkerSize',80); 256 | xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); 257 | % axis([-5 5 -5 5 -2 2]); 258 | view([-90,90]); 259 | set(ax,'Color',[0 0 0],'DataAspectRatio',[1 1 1]); 260 | %saveas(h,'H12.fig'); 261 | -------------------------------------------------------------------------------- /Software/matlab/merge_ptCloud.m: -------------------------------------------------------------------------------- 1 | function [corrected,raw]=merge_ptCloud(ptCloudA,ptCloudB,tf,gridStep) 2 | 3 | if nargin<4 4 | gridStep = 0.1; 5 | end 6 | 7 | trans_port_base = tf{1}.Transforms(2,1).Transform.Translation; 8 | trans_port_base = [trans_port_base.X trans_port_base.Y trans_port_base.Z]; 9 | quate_port_base = tf{1}.Transforms(2,1).Transform.Rotation; 10 | 11 | trans_star_base = tf{1}.Transforms(4,1).Transform.Translation; 12 | trans_star_base = [trans_star_base.X trans_star_base.Y trans_star_base.Z]; 13 | quate_star_base = tf{1}.Transforms(4,1).Transform.Rotation; 14 | 15 | trans = trans_star_base - trans_port_base; 16 | rot.X = quate_star_base.X - quate_port_base.X; 17 | rot.Y = quate_star_base.Y - quate_port_base.Y; 18 | rot.Z = quate_star_base.Z - quate_port_base.Z; 19 | 20 | trans = [1 0 0 0;... 21 | 0 1 0 0;... 22 | 0 0 1 0;... 23 | trans 1]; 24 | rotX = [1 0 0 0;... 25 | 0 cos(rot.X) sin(rot.X) 0;... 26 | 0 -sin(rot.X) cos(rot.X) 0;... 27 | 0 0 0 1]; 28 | rotY = [cos(rot.Y) 0 -sin(rot.Y) 0;... 29 | 0 1 0 0;... 30 | sin(rot.Y) 0 cos(rot.Y) 0;... 31 | 0 0 0 1]; 32 | rotZ = [cos(rot.Y) sin(rot.Y) 0 0;... 33 | -sin(rot.Y) cos(rot.Y) 0 0;... 34 | 0 0 1 0;... 35 | 0 0 0 1]; 36 | 37 | A = trans * rotX * rotY * rotZ; 38 | tform = affine3d(A); 39 | % roughAlign = pctransform(ptCloudB,tform); 40 | 41 | % tformB = pcregrigid(roughAlign,ptCloudA,'Extrapolate',true); 42 | aligned = pctransform(ptCloudB,tform); 43 | 44 | raw = pcmerge(ptCloudA, ptCloudB, gridStep); 45 | corrected = pcmerge(ptCloudA, aligned, gridStep); 46 | 47 | end 48 | 49 | -------------------------------------------------------------------------------- /Software/matlab/merge_ptCloud_tf.m: -------------------------------------------------------------------------------- 1 | function [corrected,raw]=merge_ptCloud_tf(ptCLoudA,ptCloudB,tf,gridStep) 2 | 3 | if nargin<4 4 | gridStep = 0.1; 5 | end 6 | 7 | rot = tf{1}.Transforms.Transform.Rotation; 8 | trans = tf{1}.Transforms.Transform.Translation; 9 | trans = [trans.X trans.Y trans.Z]; 10 | 11 | trans = [1 0 0 0;... 12 | 0 1 0 0;... 13 | 0 0 1 0;... 14 | trans 1]; 15 | rotX = [1 0 0 0;... 16 | 0 cos(rot.X) sin(rot.X) 0;... 17 | 0 -sin(rot.X) cos(rot.X) 0;... 18 | 0 0 0 1]; 19 | rotY = [cos(rot.Y) 0 -sin(rot.Y) 0;... 20 | 0 1 0 0;... 21 | sin(rot.Y) 0 cos(rot.Y) 0;... 22 | 0 0 0 1]; 23 | rotZ = [cos(rot.Y) sin(rot.Y) 0 0;... 24 | -sin(rot.Y) cos(rot.Y) 0 0;... 25 | 0 0 1 0;... 26 | 0 0 0 1]; 27 | 28 | A = trans * rotX * rotY * rotZ; 29 | tform = affine3d(A); 30 | aligned = pctransform(ptCloudB,tform); 31 | 32 | raw = pcmerge(ptCLoudA, ptCloudB, gridStep); 33 | corrected = pcmerge(ptCLoudA, aligned, gridStep); 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /Software/matlab/movementPtCl.m: -------------------------------------------------------------------------------- 1 | function [ptCloud_mps,binCount_mps,edges_mps,... 2 | ptCloud_dist,binCount_dist,edges_dist] = movementPtCl(X,Y,Z,distMat,L) 3 | 4 | binSize_mps = 0.1; % bin size in meters per second 5 | binSize_dist = 0.1; % bin size in meters 6 | 7 | mps = (distMat./L); 8 | 9 | numBins = round(range(mps))/binSize_mps; 10 | [locs_mps,edges_mps] = discretize(mps,numBins); 11 | % figure; histogram(mps,numBins); 12 | 13 | for i=1:length(edges_mps) 14 | idx = find(locs_mps==i); 15 | 16 | % mps_loc{i} = [X(idx) Y(idx) Z(idx)]; 17 | 18 | % binLocs_mps{:,i} = mps(idx); 19 | binCount_mps(i) = length(idx); 20 | end 21 | 22 | numBins = round(range(distMat))/binSize_dist; 23 | [locs_dist,edges_dist] = discretize(distMat,numBins); 24 | % figure; histogram(distMat,numBins); 25 | 26 | for i=1:length(edges_dist) 27 | idx = find(locs_dist==i); 28 | 29 | % dist_loc{i} = [X(idx) Y(idx) Z(idx)]; 30 | 31 | % binLocs_dist{:,i} = distMat(idx); 32 | binCount_dist(i) = length(idx); 33 | end 34 | 35 | rm = find(binCount_dist==max(binCount_dist)); 36 | locs_dist(locs_dist==rm) = 0; 37 | 38 | xyzPoints = []; 39 | for i=1:length(edges_dist) 40 | idx = find(locs_dist==i); 41 | xyzPoints = vertcat(xyzPoints,[X(idx) Y(idx) Z(idx)]); 42 | end 43 | 44 | ptCloud_dist = pointCloud(xyzPoints); 45 | 46 | rm = find(binCount_mps==max(binCount_mps)); 47 | locs_mps(locs_mps==rm) = 0; 48 | locs_mps(locs_mps==rm+1) = 0; 49 | 50 | 51 | xyzPoints = []; 52 | for i=2:length(edges_mps) 53 | idx = find(locs_mps==i); 54 | xyzPoints = vertcat(xyzPoints,[X(idx) Y(idx) Z(idx)]); 55 | end 56 | 57 | ptCloud_mps = pointCloud(xyzPoints); 58 | 59 | end -------------------------------------------------------------------------------- /Software/matlab/run_main.m: -------------------------------------------------------------------------------- 1 | close all; clear; clc; 2 | 3 | save = 1; 4 | 5 | [rawDir]=velodyneExtract(); 6 | 7 | [mergedDir]=stitch(); 8 | fixFileName(mergedDir); 9 | % fixFileName(rawDir); mergedDir=rawDir; 10 | 11 | [parent,~]=fileparts(mergedDir); 12 | [~,subDir]=fileparts(parent); 13 | d = dir(mergedDir); 14 | d = d(3:end); 15 | % [~,idx] = sort([d.datenum]); 16 | % [~,idx] = sort([d.name]); 17 | 18 | % for i=1:length(d) 19 | % fname = fullfile(mergedDir, d(i).name); 20 | % ptCloud = pcread(fname); 21 | % 22 | % [ptcFilt] = filterVLP16(ptCloud, 0); 23 | % 24 | % outDir = fullfile(parent,'pcd_filtered'); 25 | % if ~isdir(outDir) 26 | % mkdir(outDir); 27 | % end 28 | % if save 29 | % fname = strrep(fname, 'merged', 'filtered'); 30 | % fname = strrep(fname, '/pcd', '/pcd_filtered'); 31 | % pcwrite(ptcFilt, fname, 'Encoding', 'compressed'); 32 | % disp(['Writing file: ' fname]); 33 | % end 34 | % end 35 | % fixFileName(outDir); 36 | -------------------------------------------------------------------------------- /Software/matlab/shrink_box.m: -------------------------------------------------------------------------------- 1 | function [box]=shrink_box(pts,box) 2 | 3 | if min(pts(:,1))>box(1) 4 | % keyboard; 5 | box(1) = min(pts(:,1)); 6 | end 7 | if max(pts(:,1))box(3) 12 | % keyboard; 13 | box(3) = min(pts(:,2)); 14 | end 15 | if max(pts(:,2))box(5) 20 | % keyboard; 21 | box(5) = min(pts(:,3)); 22 | end 23 | if max(pts(:,3))=10 11 | polynomialOrder = 4; 12 | elseif L>13 13 | polynomialOrder = 5; 14 | elseif L>100 15 | polynomialOrder = 6; 16 | end 17 | 18 | if isempty(smoothPoints) 19 | smoothPoints(:,1)=medfilt1(points(:,1),polynomialOrder); 20 | smoothPoints(:,2)=medfilt1(points(:,2),polynomialOrder); 21 | smoothPoints(:,3)=medfilt1(points(:,3),polynomialOrder); 22 | end 23 | end 24 | 25 | -------------------------------------------------------------------------------- /Software/matlab/smooth3DPoints_sgolay.m: -------------------------------------------------------------------------------- 1 | function [smoothPoints]=smooth3DPoints_sgolay(points) 2 | 3 | smoothPoints = []; 4 | L = length(points); 5 | 6 | if L<3 7 | smoothPoints = points; 8 | elseif L<5 9 | windowWidth = 3; 10 | polynomialOrder = 1; 11 | elseif L<11 12 | windowWidth = 5; 13 | polynomialOrder = 2; 14 | else 15 | windowWidth = 11; 16 | polynomialOrder = 3; 17 | end 18 | 19 | % elseif L<10 20 | % windowWidth = 3; 21 | % polynomialOrder = 1; 22 | % elseif L>=10 23 | % windowWidth = ceil(L*0.3); 24 | % polynomialOrder = 1; 25 | % elseif L>13 26 | % windowWidth = ceil(L*0.3); 27 | % polynomialOrder = 1; 28 | % elseif L>100 29 | % windowWidth = 30; 30 | % polynomialOrder = 1; 31 | % end 32 | % 33 | % if mod(windowWidth,2) == 0 34 | % windowWidth = windowWidth - 1; 35 | % end 36 | 37 | if isempty(smoothPoints) 38 | smoothPoints(:,1)=sgolayfilt(points(:,1),polynomialOrder, windowWidth); 39 | smoothPoints(:,2)=sgolayfilt(points(:,2),polynomialOrder, windowWidth); 40 | smoothPoints(:,3)=sgolayfilt(points(:,3),polynomialOrder, windowWidth); 41 | end 42 | end 43 | 44 | -------------------------------------------------------------------------------- /Software/matlab/stitch.m: -------------------------------------------------------------------------------- 1 | function [outDir]=stitch(folder,display,save,zoom) 2 | close all; clc; clear; 3 | 4 | calFrame = 1; % what frame is used to calibrate transforms 5 | zone = 5; % timezone difference 6 | 7 | if nargin<4 8 | zoom = 0; % zoom in output? 9 | end 10 | if nargin<3 11 | save = 1; % Save output? 12 | end 13 | if nargin<2 14 | display = 0; % display output? 15 | end 16 | if nargin<1 17 | folder = uigetdir('.','Select PCD Folder'); 18 | if ~folder 19 | disp('No Folder Selected... Exiting!'); 20 | return; 21 | end 22 | end 23 | 24 | [parent,~]=fileparts(folder); 25 | [~,subDir]=fileparts(parent); 26 | d = dir(folder); 27 | d = d(3:end); 28 | [~,idx] = sort([d.datenum]); 29 | 30 | tf = load([parent filesep subDir '_tf_static.mat']); 31 | % tf = load('/home/antoniorufo/bicycleDetection/catkin_ws/matlab/2017-10-18-17-41-30_0/2017-10-18-17-41-30_0_tf_static.mat'); 32 | % tf = load([parent filesep subDir '_tf.mat']); % old method 33 | timesPort = load([parent filesep 'port_velodyne_points_time.mat']); 34 | timesStar = load([parent filesep 'starboard_velodyne_points_time.mat']); 35 | 36 | if mod(numel(idx),2) 37 | disp(['Uneven number of PCd files detected... fix directory '... 38 | folder ' and rerun']); 39 | keyboard; 40 | end 41 | numF = numel(idx)/2; 42 | port_files = cell(1,numF); starboard_files = cell(1,numF); 43 | portT_string = cell(1,numF); starT_string = cell(1,numF); 44 | 45 | for i=1:numel(idx) 46 | %disp(d(idx(i)).name); 47 | if strfind(d(idx(i)).name,'port') 48 | port_files{i} = [folder filesep d(idx(i)).name]; 49 | [portT(i,:),portT_string{i}]=epoch2datetime(timesPort.ts.Time(i),zone); 50 | ii=i; 51 | elseif strfind(d(idx(i)).name,'starboard') 52 | starboard_files{i-ii} = [folder filesep d(idx(i)).name]; 53 | [starT(i-ii,:),starT_string{i-ii}]=epoch2datetime(timesStar.ts.Time(i-ii),zone); 54 | end 55 | end 56 | 57 | figure(4); 58 | plot(portT); hold on; title('Velodyne Timestamps'); 59 | plot(starT); hold off; 60 | portT = duration(portT,'Format','s'); 61 | starT = duration(starT,'Format','s'); 62 | figure(5); 63 | plot(portT-starT); title('Velodyne Mismatch'); 64 | figure(6); 65 | plot(diff(portT)); hold on; title('Velodyne time skips'); 66 | plot(diff(starT)); hold off; 67 | 68 | if display 69 | xlim=[0,1]; ylim=[0,1]; zlim=[0,1]; 70 | player = pcplayer(xlim,ylim,zlim); 71 | end 72 | 73 | for i=1:numel(port_files) 74 | port = pcread(port_files{i}); 75 | starboard = pcread(starboard_files{i}); 76 | 77 | [corr,raw]=merge_ptCloud(port,starboard,tf.data,0.01); 78 | %s[corr,raw]=merge_ptCloud_tf(port,starboard,tf.data,0.01); %old method 79 | 80 | if display 81 | [xlim, ylim, zlim] = findLim(corr,xlim,ylim,zlim); 82 | if zoom 83 | player.Axes.XLim = [-15 15]; 84 | player.Axes.YLim = [-15 15]; 85 | player.Axes.ZLim = zlim; 86 | set(player.Axes,'view',[-90 90]); 87 | else 88 | player.Axes.XLim = xlim; 89 | player.Axes.YLim = ylim; 90 | player.Axes.ZLim = zlim; 91 | set(player.Axes,'view',[-90 30]); 92 | end 93 | player.Axes.Title.String = ['Frame: ' num2str(i)]; 94 | view(player,corr); drawnow; 95 | end 96 | 97 | outDir = fullfile(parent,'pcd_merged'); 98 | if ~isdir(outDir) 99 | mkdir(outDir); 100 | end 101 | if save 102 | port_files{i} = strrep(port_files{i}, 'port', 'merged'); 103 | loc = strfind(port_files{i},'merged'); 104 | file = fullfile(outDir,port_files{i}(loc:end)); 105 | pcwrite(corr, file, 'Encoding', 'compressed'); 106 | disp(['Writing file: ' port_files{i}(loc:end)]); 107 | end 108 | 109 | if i==calFrame 110 | h(1) = figure(2); 111 | pcshow(raw); 112 | title(['Raw Frame: ' num2str(i) ' - Merged']); 113 | axis([-12 12 -12 12 min(raw.ZLimits) max(raw.ZLimits)]); 114 | view([-90 90]); drawnow; 115 | h(2) = figure(3); 116 | pcshow(corr); 117 | title(['Corrected Frame: ' num2str(i) ' - Merged']); 118 | axis([-12 12 -12 12 min(corr.ZLimits) max(corr.ZLimits)]); 119 | view([-90 90]); drawnow; 120 | 121 | %disp('Press enter to continue.....'); 122 | %pause; 123 | end 124 | end 125 | clearvars -except outDir 126 | end 127 | 128 | 129 | -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_03.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoDrawTubularMesh_04.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoGeom3d_03.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoInertiaEllipsoid_03.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoRevolutionSurface.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoRevolutionSurface.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoRevolutionSurface_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoRevolutionSurface_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/demoRevolutionSurface_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/demoRevolutionSurface_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/geom3d-demos/html/drawSoccerBall_03.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_03.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_04.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_05.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_05.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_06.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_06.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_07.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_07.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_08.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_08.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_09.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoPolyhedra_09.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoTriangulateFaces.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoTriangulateFaces.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoTriangulateFaces_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoTriangulateFaces_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoTriangulateFaces_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoTriangulateFaces_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell_01.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell_02.png -------------------------------------------------------------------------------- /Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/neufieldrobotics/LiDARBicycleDetection/0becb0c761c41f830d50e8ef80917ffc12005063/Software/matlab/utils/geom3d/meshes3d-demos/html/demoVoronoiCell_03.png -------------------------------------------------------------------------------- /Software/matlab/velodyneExtract.m: -------------------------------------------------------------------------------- 1 | function [outDir]=velodyneExtract(FileName,topics) 2 | 3 | format = 'pcd'; 4 | encoding = 'compressed'; 5 | 6 | if nargin<2 7 | topics = {'/tf_static','/tf','/ns1/velodyne_points','/ns2/velodyne_points','/imu/imu','/vehicle/gps/fix'}; 8 | % topics = {'/velodyne_points'}; 9 | % topics = {'/imu/imu','/vehicle/gps/fix'}; 10 | % topics = {'/vehicle/imu/data_raw','/vehicle/gps/fix'}; 11 | % topics = {'/tf_static'}; 12 | end 13 | 14 | if nargin<1 15 | %Change for your specific path 16 | PathName = '/home/antoniorufo/bicycleDetection/catkin_ws/'; 17 | addpath(PathName); 18 | 19 | [FileName,PathName] = uigetfile('*','Get rosbag Log File',... 20 | PathName); %GUI to open desired bag file 21 | if FileName == 0 22 | disp('No File Selected... Exiting!'); 23 | return; 24 | end 25 | else 26 | [PathName,name,ext] = fileparts(FileName); 27 | FileName = [name ext]; 28 | end 29 | 30 | folder = FileName(1:end-4); % Create ouput folder 31 | if ~isdir(folder) 32 | mkdir(folder); 33 | end 34 | 35 | disp('Opening bag file'); 36 | tic; 37 | bag = rosbag(fullfile(PathName,FileName)); 38 | toc 39 | 40 | topicList = bag.AvailableTopics; 41 | fname = [folder filesep FileName(1:end-4) '_TopicList']; 42 | save(fname,'topicList'); % save file containing only topic list 43 | 44 | if isempty(topics) 45 | topics = topicList.Row; 46 | end 47 | 48 | for i=1:length(topics) 49 | % try 50 | bagSel = select(bag,'Topic',topics{i}); % Select topics string from bag 51 | msgH = height(bagSel.MessageList); 52 | 53 | str = strrep(topics{i}, filesep, '_'); 54 | fnameB = strrep(fname, '_TopicList', str); 55 | disp(['Topic: ' topics{i} ' has ' num2str(msgH) ' msgs']); 56 | 57 | if strcmp(topics{i},'/ns1/velodyne_points') || strcmp(topics{i},'/ns2/velodyne_points') || strcmp(topics{i},'/velodyne_points') 58 | xlim=[0,1]; ylim=[0,1]; zlim=[0,1]; 59 | player = pcplayer(xlim,ylim,zlim); 60 | 61 | j=1; 62 | while j2.5 || centerZ > 3 38 | class(i).index = NaN; 39 | class(i).center = [NaN,NaN,NaN]; 40 | class(i).dimm = [NaN NaN NaN]; 41 | class(i).type = 'noise'; 42 | class(i).color = [1 1 1]; 43 | bbox(idx(i),:) = [0 0 0 0 0 0]; 44 | elseif sizeX<=0.3 || sizeY<=0.3 || sizeZ<=0.5 || centerZ>=3.05 45 | class(i).index = NaN; 46 | class(i).center = [NaN,NaN,NaN]; 47 | class(i).dimm = [NaN NaN NaN]; 48 | class(i).type = 'noise'; 49 | class(i).color = [1 1 1]; 50 | bbox(idx(i),:) = [0 0 0 0 0 0]; 51 | elseif (sizeX<=2 && sizeY<=2 && sizeZ<=2.5) || (sizeY<=2 && sizeX<=2 && sizeZ<=2.5) 52 | class(i).type = 'bike/person'; 53 | class(i).color = [1 0 0]; % red 54 | class(i).roi = box; 55 | elseif (sizeX<=2 && sizeY<=2 && sizeZ>=1) || (sizeY<=2 && sizeX<=2 && sizeZ>=1) 56 | class(i).type = 'car'; 57 | class(i).color = [0 1 0]; % green 58 | class(i).roi = box; 59 | elseif (sizeX<=25 && sizeY<=2 && sizeZ>=1) || (sizeY<=2 && sizeX<=25 && sizeZ>=1) 60 | class(i).type = 'Large Vehicle'; 61 | class(i).color = [0 0 1]; % blue 62 | class(i).roi = box; 63 | elseif sizeX>25 || sizeY>25 || sizeZ>=4.2 || sizeZ<=1 64 | class(i).index = NaN; 65 | class(i).center = [NaN,NaN,NaN]; 66 | class(i).dimm = [NaN NaN NaN]; 67 | class(i).type = 'noise'; 68 | class(i).color = [1 1 1]; 69 | bbox(idx(i),:) = [0 0 0 0 0 0]; 70 | else 71 | class(i).type = 'unknown'; 72 | class(i).color = [0 1 1]; 73 | class(i).roi = box; 74 | %bbox(idx(i),:) = 0; 75 | end 76 | end 77 | 78 | end -------------------------------------------------------------------------------- /Software/matlab/voxelMerge.m: -------------------------------------------------------------------------------- 1 | function [refTree,bbox]=voxelMerge(refTree,bbox) 2 | 3 | idx = find(bbox(:,1)~=0); 4 | if numel(idx)<2 5 | return; 6 | end 7 | 8 | for i=1:numel(idx) 9 | for j=i+1:numel(idx) 10 | boxB = bbox(idx(j),:); 11 | boxA = bbox(idx(i),:); 12 | cubeA = [boxA(1),boxA(3),boxA(5),... 13 | boxA(2)-boxA(1),boxA(4)-boxA(3),boxA(6)-boxA(5)]; 14 | cubeB = [boxB(1),boxB(3),boxB(5),... 15 | boxB(2)-boxB(1),boxB(4)-boxB(3),boxB(6)-boxB(5)]; 16 | 17 | centXA = (boxB(1) + boxB(2))/2; 18 | centYA = (boxB(3) + boxB(4))/2; 19 | centZA = (boxB(5) + boxB(6))/2; 20 | centXB = (boxA(1) + boxA(2))/2; 21 | centYB = (boxA(3) + boxA(4))/2; 22 | centZB = (boxA(5) + boxA(6))/2; 23 | 24 | dist = euclidDist([centXA,centYA,centZA],[centXB,centYB,centZB]); 25 | area = cubeint(cubeA,cubeB); 26 | 27 | if abs(dist)<= 1.5 || (area>0 && abs(dist)<=2) 28 | minX = min(boxB(1),boxA(1)); 29 | maxX = max(boxB(2),boxA(2)); 30 | minY = min(boxB(3),boxA(3)); 31 | maxY = max(boxB(4),boxA(4)); 32 | minZ = min(boxB(5),boxA(5)); 33 | maxZ = max(boxB(6),boxA(6)); 34 | 35 | bbox(idx(j),:) = [minX,maxX,minY,maxY,minZ,maxZ]; 36 | bbox(idx(i),:) = 0; 37 | refTree.BinBoundaries(idx(j),:) = [minX,minY,minZ,maxX,maxY,maxZ]; 38 | 39 | refTree.PointBins(refTree.PointBins==idx(i)) = idx(j); 40 | break; 41 | end 42 | end 43 | end 44 | end -------------------------------------------------------------------------------- /Software/matlab/voxelMovement.m: -------------------------------------------------------------------------------- 1 | function [bbox]=voxelMovement(ptCloudA,ptCloudB,refTree,thresh) 2 | 3 | if nargin<4 4 | thresh = 20; 5 | end 6 | 7 | voxelDensityA = zeros(refTree.BinCount,1); 8 | voxelDensityB = zeros(refTree.BinCount,1); 9 | voxelStateA = zeros(refTree.BinCount,1); 10 | voxelStateB = zeros(refTree.BinCount,1); 11 | bbox = zeros(size(refTree.roi)); 12 | 13 | for i=1:refTree.BinCount 14 | 15 | indicesA = findPointsInROI(ptCloudA, refTree.roi(i,:)); 16 | indicesB = findPointsInROI(ptCloudB, refTree.roi(i,:)); 17 | 18 | if ~isempty(indicesA) 19 | voxelDensityA(i) = length(indicesA); 20 | end 21 | if ~isempty(indicesB) 22 | voxelDensityB(i) = length(indicesB); 23 | end 24 | 25 | if voxelDensityA(i)>thresh 26 | voxelStateA(i) = 1; % occupied voxel 27 | end 28 | 29 | if voxelDensityB(i)>thresh 30 | voxelStateB(i) = 1; % occupied voxel 31 | end 32 | 33 | if voxelStateA(i)==0 && voxelStateB(i)==1 34 | bbox(i,:) = refTree.roi(i,:); 35 | % elseif voxelStateA(i)==1 && voxelStateB(i)==0 36 | % bbox(i,:) = refTree.roi(i,:); 37 | end 38 | end 39 | 40 | % figure(102); 41 | % plot(1:i,voxelDensityA,1:i,voxelDensityB); 42 | % title('Density'); 43 | % figure(101); 44 | % plot(1:i,voxelStateA,1:i,voxelStateB); 45 | % title('State'); 46 | 47 | end --------------------------------------------------------------------------------