보고서 작성에 따른 코드 저장 - 라바콘 인지 과정

clear; close all; clc;
rosshutdown;
rosinit('http://localhost:11311')
tftree = rostf;
pause(3);

% Parameter//============================================================
load('wheelbase.mat');

Ld = 1.0; v = 1.5; delta = 0.3;
roi = [0, 7, -4, 4, -2, 4];

clusterThreshold = 1.0; % Cluster distance
cuboidTreshold = 0.01; % Ignore smaller than 0.003 cuboid (cone: 0.0215) 
waypointTreshold = 3.0; % make a waypoint before 3m 

pp = controllerPurePursuit;
pp.LookaheadDistance = Ld; % m
pp.DesiredLinearVelocity = v; % m/s    
pp.MaxAngularVelocity = delta; % rad/s    

L = 1.04;

% init//==================================================================
waypoints = [];
markerIdPath = 0;
markerIdClusters = 0;
params = lidarParameters('OS1Gen1-64',512);

lidarSub = rossubscriber('/lidar/points',"DataFormat", "struct");

[pubClusters, markerArrayMsg] = rospublisher('/clusters_marker', 'visualization_msgs/MarkerArray','DataFormat','struct');

pubPath = rospublisher('/path_marker', 'visualization_msgs/Marker','DataFormat','struct');

ekfSub = rossubscriber('/odometry/filtered_map',"DataFormat", "struct");
modelSub = rossubscriber('/gazebo/model_states',"DataFormat", "struct");

client = rossvcclient('/Activate_yolo','cob_object_detection_msgs/DetectObjects','DataFormat','struct');
request_l = rosmessage(client);
request_l.ObjectName.Data = 'left';
request_r = rosmessage(client);
request_r.ObjectName.Data = 'right';

load("camera1_tform.mat");  
camera1_tform = tform;
tformCamera1 = invert(camera1_tform);
load("camera2_tform.mat");
camera2_tform = tform;
tformCamera2 = invert(camera2_tform);

load("camera1_Params.mat");
cameraParam_r = cameraParams;
load("camera2_Params.mat");
cameraParam_l = cameraParams;

% ekf or gt
EKF = true;

velPub = rospublisher('/pp_vel/matlab','geometry_msgs/Twist','DataFormat','struct');
LpPub = rospublisher('/LookAheadPoint','geometry_msgs/Pose','DataFormat','struct');

while true % ctrl + c to stop
    tic;

    if EKF
        [vehiclePose, curr_v, curr_w] = updateVehiclePose(ekfSub, tftree);
    else
        [vehiclePose, curr_v, curr_w] = updateVehiclePose_GT(modelSub, tftree);
    end
    

    if isempty(pp.Waypoints) || norm(worldWaypoints(end,:)-[vehiclePose(1), vehiclePose(2)]) < waypointTreshold  % Considering only x and y for the distance
        disp("Make new waypoints");
        try
            lidarData = receive(lidarSub);
            bboxData_r = call(client, request_r);
            bboxData_l = call(client, request_l);

            roiPtCloud = preprocess_lidar_data(lidarData, params, roi);
            
            [y_coneBboxs_l, b_coneBboxs_l] = extractConesBboxs(bboxData_l.ObjectList);
            [y_coneBboxs_r, b_coneBboxs_r] = extractConesBboxs(bboxData_r.ObjectList);
            
            [bboxesLidar_l,~,boxesUsed_l] = bboxCameraToLidar([y_coneBboxs_l; b_coneBboxs_l],roiPtCloud,cameraParam_l,tformCamera2,'ClusterThreshold',clusterThreshold);
            [bboxesLidar_r,~,boxesUsed_r] = bboxCameraToLidar([y_coneBboxs_r; b_coneBboxs_r],roiPtCloud,cameraParam_r,tformCamera1,'ClusterThreshold',clusterThreshold);

            [y_coneBboxesLidar_l, b_coneBboxesLidar_l] = splitConesBboxes(y_coneBboxs_l,bboxesLidar_l,boxesUsed_l);
            [y_coneBboxesLidar_r, b_coneBboxesLidar_r] = splitConesBboxes(y_coneBboxs_r,bboxesLidar_r,boxesUsed_r);
            
            innerConePosition = extractConePositions(cuboidTreshold, b_coneBboxesLidar_l, b_coneBboxesLidar_r);
            outerConePosition = extractConePositions(cuboidTreshold, y_coneBboxesLidar_l, y_coneBboxesLidar_r);

            innerConePosition = unique_rows(innerConePosition);
            outerConePosition = unique_rows(outerConePosition);

            [innerConePosition, outerConePosition] = match_array_lengths(innerConePosition, outerConePosition);

            waypoints = generate_waypoints_del(innerConePosition, outerConePosition);
            worldWaypoints = transformWaypointsToOdom(waypoints, vehiclePose);
            

            [markerArrayMsg, markerIdClusters] = generate_clusters_marker(innerConePosition, outerConePosition, 'base_footprint', markerIdClusters);
            send(pubClusters, markerArrayMsg);

            [pathMarkerMsg, markerIdPath] = generate_path_marker(worldWaypoints, 'erp42_base', markerIdPath);
            send(pubPath, pathMarkerMsg);
     
            pp.Waypoints = worldWaypoints;
        catch
            disp("Fail to make new waypoints");
            continue; % 다음 while문 반복으로 넘어감
        end
        pause(1)
    end

    [v, w, Lp] = pp(vehiclePose);    
    alpha = atan2((Lp(2) - vehiclePose(2)), (Lp(1) - vehiclePose(1)-0.5));

    velMsg = rosmessage(velPub);
    velMsg.Linear.X = v;
    velMsg.Angular.Z = w;
    send(velPub,velMsg);

    LpMsg = rosmessage(LpPub);
    LpMsg.Position.X = Lp(1);
    LpMsg.Position.Y = Lp(2);
    LpMsg.Orientation.Z = alpha;
    send(LpPub,LpMsg);

    % [pub, msg] = publish_twist_command(v, w, '/cmd_vel');
    % send(pub, msg);
    toc;
end



%%
function [vehiclePose, curr_v, curr_w] = updateVehiclePose(ekfStatesSub, tftree)

    ekfMsg = receive(ekfStatesSub);
   
    robotPose = ekfMsg.Pose.Pose;
    quat = [robotPose.Orientation.W, robotPose.Orientation.X, robotPose.Orientation.Y, robotPose.Orientation.Z];
    euler = quat2eul(quat);
    yaw = euler(1);
    vehiclePose = [robotPose.Position.X; robotPose.Position.Y; yaw];
    
    curr_v = sqrt(ekfMsg.Twist.Twist.Linear.X.^2 + ekfMsg.Twist.Twist.Linear.Y.^2);
    curr_w = ekfMsg.Twist.Twist.Angular.Z;

    % TF 메시지 생성 및 설정
    tfStampedMsg = rosmessage('geometry_msgs/TransformStamped');
    tfStampedMsg.ChildFrameId = 'base_link';
    tfStampedMsg.Header.FrameId = 'erp42_base';
    tfStampedMsg.Header.Stamp = rostime('now');
    tfStampedMsg.Transform.Translation.X = vehiclePose(1);
    tfStampedMsg.Transform.Translation.Y = vehiclePose(2);
    tfStampedMsg.Transform.Rotation.Z = sin(vehiclePose(3)/2);
    tfStampedMsg.Transform.Rotation.W = cos(vehiclePose(3)/2);

    % TF 브로드캐스팅
    sendTransform(tftree, tfStampedMsg);
end

function [vehiclePose, curr_v, curr_w] = updateVehiclePose_GT(modelSub, tftree)
    modelMsg = receive(modelSub);
   
    robotIndex = find(strcmp(modelMsg.Name, 'robot'));  
    robotPose = modelMsg.Pose(robotIndex);
    robotTwist = modelMsg.Twist(robotIndex);
    quat = [robotPose.Orientation.W, robotPose.Orientation.X, robotPose.Orientation.Y, robotPose.Orientation.Z];
    euler = quat2eul(quat);
    yaw = euler(1);
    vehiclePose = [robotPose.Position.X; robotPose.Position.Y; yaw];
    
    curr_v = sqrt(robotTwist.Linear.X.^2 + robotTwist.Linear.Y.^2);
    curr_w = robotTwist.Angular.Z;

    % TF 메시지 생성 및 설정
    tfStampedMsg = rosmessage('geometry_msgs/TransformStamped');
    tfStampedMsg.ChildFrameId = 'base_link';
    tfStampedMsg.Header.FrameId = 'erp42_base';
    tfStampedMsg.Header.Stamp = rostime('now');
    tfStampedMsg.Transform.Translation.X = vehiclePose(1);
    tfStampedMsg.Transform.Translation.Y = vehiclePose(2);
    tfStampedMsg.Transform.Rotation.Z = sin(vehiclePose(3)/2);
    tfStampedMsg.Transform.Rotation.W = cos(vehiclePose(3)/2);

    % TF 브로드캐스팅
    sendTransform(tftree, tfStampedMsg);
end


function conePosition = extractConePositions(cuboidTreshold, coneBboxesLidar_l, coneBboxesLidar_r)
    % Extract xlen, ylen, zlen from the bounding boxes
    volumes_l = prod(coneBboxesLidar_l(:, 4:6), 2);
    volumes_r = prod(coneBboxesLidar_r(:, 4:6), 2);

    % Find indices where volumes are smaller than cuboidThreshold
    indices_l = volumes_l > cuboidTreshold;
    indices_r = volumes_r > cuboidTreshold;

    % Combine the inner cone positions from left and right into a single array
    conePosition = [[coneBboxesLidar_l(indices_l, 1) + 0.6, coneBboxesLidar_l(indices_l, 2)]; 
                    [coneBboxesLidar_r(indices_r, 1) + 0.6, coneBboxesLidar_r(indices_r, 2)]];
end


function [y_coneBboxesLidar, b_coneBboxesLidar] = splitConesBboxes(y_coneBboxs,bboxesLidar,boxesUsed)
    % y_cone의 개수만 계산
    numY_cone = sum(boxesUsed(1:size(y_coneBboxs,1)));
    
    % bboxesLidar에서 y_cone와 b_cone의 bbox 분류
    y_coneBboxesLidar = bboxesLidar(1:numY_cone, :);
    b_coneBboxesLidar = bboxesLidar(numY_cone+1:end, :);
end



function odomWaypoints = transformWaypointsToOdom(waypoints, vehiclePoseInOdom)
    % Initialize transformed waypoints
    odomWaypoints = zeros(size(waypoints));

    % Extract the vehicle's yaw angle
    theta = vehiclePoseInOdom(3);

    % Create the 2D rotation matrix
    R = [cos(theta), -sin(theta);
         sin(theta), cos(theta)];

    % Transform each waypoint
    for i = 1:size(waypoints,1)
        % Rotate the waypoint considering the vehicle's yaw
        rotatedPoint = R * waypoints(i,:)';

        % Translate considering the vehicle's position in the odom frame
        odomWaypoints(i,:) = rotatedPoint' + vehiclePoseInOdom(1:2)';
    end
end


function [y_coneBboxs, b_coneBboxs] = extractConesBboxs(bboxData)
    % BoundingBoxes_ 대신 Detections의 길이로 메모리 공간을 미리 할당
    numBboxes = numel(bboxData.Detections);

    % y_cone 및 b_cone에 대한 임시 저장 공간
    temp_y_coneBboxs = zeros(numBboxes, 4);
    temp_b_coneBboxs = zeros(numBboxes, 4);

    y_count = 0;
    b_count = 0;

    for i = 1:numBboxes
        currentBbox = bboxData.Detections(i, 1).Mask.Roi;
        
        % 변경된 데이터 형식에 따라 BoundingBoxes_ 대신 Mask.Roi 사용
        x = currentBbox.X;
        y = currentBbox.Y;
        w = currentBbox.Width;
        h = currentBbox.Height;
        
        if strcmp(bboxData.Detections(i, 1).Label, 'y_cone')
            y_count = y_count + 1;
            temp_y_coneBboxs(y_count, :) = [x, y, w, h];
        else
            b_count = b_count + 1;
            temp_b_coneBboxs(b_count, :) = [x, y, w, h];
        end
    end

    % 최종 결과
    y_coneBboxs = temp_y_coneBboxs(1:y_count, :);
    b_coneBboxs = temp_b_coneBboxs(1:b_count, :);
end

function roiPtCloud = preprocess_lidar_data(lidarData, params, roi)
    xyzData = rosReadXYZ(lidarData);
    ptCloud = pointCloud(xyzData);

    ptCloudOrg = pcorganize(ptCloud, params);

    groundPtsIdx = segmentGroundFromLidarData(ptCloudOrg);
    nonGroundPtCloud = select(ptCloudOrg, ~groundPtsIdx, 'OutputSize', 'full');

    indices = findPointsInROI(nonGroundPtCloud, roi);
    roiPtCloud = select(nonGroundPtCloud, indices);

    roiPtCloud = pcdenoise(roiPtCloud, 'PreserveStructure', true);
end

function [centers, innerConePosition, outerConePosition] = process_clusters(roiPtCloud)
    [labels, numClusters] = pcsegdist(roiPtCloud, 0.3);

    xData = roiPtCloud.Location(:,1);
    yData = roiPtCloud.Location(:,2);

    clf;
    hold on;
    centers = [];
    innerConePosition = [];
    outerConePosition = [];
    for i = 1:numClusters
        idx = labels == i;
        clusterPoints = [xData(idx), yData(idx), roiPtCloud.Location(idx,3)];

        if size(clusterPoints, 1) >= 20
            [~, maxZIdx] = max(clusterPoints(:,3));
            center = clusterPoints(maxZIdx, 1:2);
            centers = [centers; center];

            if center(2)<0
                innerConePosition=[innerConePosition; center(1), center(2)];
            else
                outerConePosition=[outerConePosition; center(1), center(2)];
            end
            scatter(center(1), -center(2), "red","filled");
        end
    end
end

function uniqueArray = unique_rows(array)
    [~, uniqueIdx] = unique(array, 'rows');
    uniqueArray = array(uniqueIdx, :);
    uniqueArray = sortrows(uniqueArray);
end

function [out1, out2] = match_array_lengths(arr1, arr2)
    len1 = size(arr1, 1); % Get the number of rows
    len2 = size(arr2, 1); % Get the number of rows

    if len1 > len2
        out1 = arr1(1:len2, :); % Keep only the first len2 rows
        out2 = arr2;
    elseif len2 > len1
        out1 = arr1;
        out2 = arr2(1:len1, :); % Keep only the first len1 rows
    else
        out1 = arr1;
        out2 = arr2;
    end
end

function waypoints = generate_waypoints_del(innerConePosition, outerConePosition)
    [m,nc] = size(innerConePosition); % size of the inner/outer cone positions data
    kockle_coords = zeros(2*m,nc); % initiate a P matrix consisting of inner and outer coordinates
    kockle_coords(1:2:2*m,:) = innerConePosition;
    kockle_coords(2:2:2*m,:) = outerConePosition; % merge the inner and outer coordinates with alternate values
    xp = []; % create an empty numeric xp vector to store the planned x coordinates after each iteration
    yp = []; 

    
    interv=size(innerConePosition,1)*2;
    %step 1 : delaunay triangulation
    tri=delaunayTriangulation(kockle_coords);
    pl=tri.Points;
    cl=tri.ConnectivityList;
    [mc, nc]=size(pl);
		    
    % inner and outer constraints when the interval is even
    if rem(interv,2) == 0
     cIn = [2 1;(1:2:mc-3)' (3:2:(mc))'; (mc-1) mc];
     cOut = [(2:2:(mc-2))' (4:2:mc)'];
    else
    % inner and outer constraints when the interval is odd
    cIn = [2 1;(1:2:mc-2)' (3:2:(mc))'; (mc-1) mc];
    cOut = [(2:2:(mc-2))' (4:2:mc)'];
    end
    
		    %step 2 : 외부 삼각형 거
    C = [cIn;cOut];
    TR=delaunayTriangulation(pl,C);
    TRC=TR.ConnectivityList;
    TL=isInterior(TR);
    TC =TR.ConnectivityList(TL,:);
    [~, pt]=sort(sum(TC,2));
    TS=TC(pt,:);
    TO=triangulation(TS,pl);
		    
		    %step 3 : 중간 waypoint 생성
    xPo=TO.Points(:,1);
    yPo=TO.Points(:,2);
    E=edges(TO);
    iseven=rem(E,2)==0;
    Eeven=E(any(iseven,2),:);
    isodd=rem(Eeven,2)~=0;
    Eodd=Eeven(any(isodd,2),:);
    xmp=((xPo((Eodd(:,1))) + xPo((Eodd(:,2))))/2);
    ymp=((yPo((Eodd(:,1))) + yPo((Eodd(:,2))))/2);
    Pmp=[xmp ymp];
    waypoints = Pmp;

	% 	    %step 4 : waypoint 보간
    % distancematrix = squareform(pdist(Pmp));
    % distancesteps = zeros(length(Pmp)-1,1);
    % for j = 2:length(Pmp)
    %    distancesteps(j-1,1) = distancematrix(j,j-1);
    % end
    % totalDistance = sum(distancesteps); % total distance travelled
    % distbp = cumsum([0; distancesteps]); % distance for each waypoint
    % gradbp = linspace(0,totalDistance,100);
    % xq = interp1(distbp,xmp,gradbp,'spline'); % interpolate x coordinates
    % yq = interp1(distbp,ymp,gradbp,'spline'); % interpolate y coordinates
    % xp = [xp xq]; % store obtained x midpoints after each iteration
    % yp = [yp yq]; % store obtained y midpoints after each iteration
    % 
	% 	    % step 5 : 최종 waypoint 생성
    % waypoints=[xp', yp'];
end

function waypoints = generate_waypoints(innerConePosition, outerConePosition)
	%go_traingulation
    
    [m,nc] = size(innerConePosition); % size of the inner/outer cone positions data
    kockle_coords = zeros(2*m,nc); % initiate a P matrix consisting of inner and outer coordinates
    kockle_coords(1:2:2*m,:) = innerConePosition;
    kockle_coords(2:2:2*m,:) = outerConePosition;
    xp=[];
    yp=[];

    midpoints=zeros(size(kockle_coords, 1)-1 , size(kockle_coords,2));

    for i=1:size(kockle_coords, 1) -1
        midpoints(i,1)=(kockle_coords(i,1)+kockle_coords(i+1,1)) /2;
        midpoints(i,2)=(kockle_coords(i,2)+kockle_coords(i+1,2)) /2;
    end
    waypoints = midpoints;
    
    % distancematrix = squareform(pdist(midpoints));
    % distancesteps = zeros(length(midpoints)-1,1);
    % for j = 2:length(midpoints)
    %     distancesteps(j-1,1) = distancematrix(j,j-1);
    % end
    % totalDistance = sum(distancesteps); % total distance travelled
    % distbp = cumsum([0; distancesteps]); % distance for each waypoint
    % gradbp = linspace(0,totalDistance,100);
    % xq = interp1(distbp,midpoints(:,1),gradbp,'spline'); % interpolate x coordinates
    % yq = interp1(distbp,midpoints(:,2),gradbp,'spline'); % interpolate y coordinates
    % xp = [xp xq]; % store obtained x midpoints after each iteration
    % yp = [yp yq]; % store obtained y midpoints after each iteration
    % 
    % waypoints=[xp', yp'];
end

function [markerArrayMsg, markerID] = generate_clusters_marker(b_coneCluster, y_coneCluster, frameId, markerID)
    % Concatenate the clusters for easier looping
    combinedClusters = [b_coneCluster; y_coneCluster];
    clusterColors = [repmat([0 0 1], size(b_coneCluster, 1), 1); % Blue for b_coneCluster
                     repmat([1 1 0], size(y_coneCluster, 1), 1)]; % Yellow for y_coneCluster

    markerArrayMsg = rosmessage('visualization_msgs/MarkerArray','DataFormat','struct');
    
    for i = 1:height(combinedClusters)
        markerMsg = rosmessage('visualization_msgs/Marker','DataFormat','struct');
        markerMsg.Header.FrameId = frameId;
        markerMsg.Id = int32(markerID); % Cast 'markerID' to int32
        markerID = markerID + 1;  % Increment markerID by 1 for each new marker
        markerMsg.Type = int32(3);
        markerMsg.Action = int32(0);
        markerMsg.Pose.Position.X = double(combinedClusters(i,1));
        markerMsg.Pose.Position.Y = double(combinedClusters(i,2));
        markerMsg.Pose.Position.Z = 0;
        markerMsg.Pose.Orientation.W = 1.0;
        markerMsg.Scale.X = 0.3;
        markerMsg.Scale.Y = 0.3;
        markerMsg.Scale.Z = 0.5;
        
        % Set Color
        markerMsg.Color.R = single(clusterColors(i, 1));
        markerMsg.Color.G = single(clusterColors(i, 2));
        markerMsg.Color.B = single(clusterColors(i, 3));
        markerMsg.Color.A = single(0.5);
        
        markerArrayMsg.Markers(i) = markerMsg;
    end
end


function [markerMsg, markerID] = generate_path_marker(waypoints, frameId, markerID)
    markerMsg = rosmessage('visualization_msgs/Marker','DataFormat','struct');
    markerMsg.Header.FrameId = frameId;
    markerMsg.Id = int32(markerID);  % Cast 'markerID' to int32
    markerID = markerID + 1;  % Increment markerID by 1 for each new marker
    markerMsg.Type = int32(4); % LINE_STRIP
    markerMsg.Action = int32(0);
    markerMsg.Pose.Orientation.W = 1.0;
    markerMsg.Scale.X = 0.05;  % Specify a suitable scale
    markerMsg.Color.R = single(1.0); % red
    markerMsg.Color.G = single(0.0); % green
    markerMsg.Color.B = single(0.0); % blue
    markerMsg.Color.A = single(1.0); % alpha

    % Add the waypoints to the points array of the marker
    for i = 1:size(waypoints, 1)
        point = rosmessage('geometry_msgs/Point','DataFormat','struct');
        point.X = double(waypoints(i, 1));
        point.Y = double(waypoints(i, 2));
        point.Z = 0;
        markerMsg.Points(i) = point;
    end
end



function [pub, msg] = publish_twist_command(v, w, topicName)
    pub = rospublisher(topicName, 'geometry_msgs/Twist','DataFormat','struct');
    msg = rosmessage(pub);
    msg.Linear.X = v;
    msg.Angular.Z = w;
end

 

function [bboxesLidar, objectIndices, bboxesUsed] = bboxCameraToLidar(bboxesCamera, ptCloudIn, intrinsics, tform, varargin)
%bboxCameraToLidar Estimates 3-D bounding boxes in lidar from 2-D bounding
% boxes in Camera
%
%   bboxesLidar = bboxCameraToLidar(bboxesCamera, ptCloudIn, intrinsics, tform)
%   returns 3-D bounding boxes, bboxesLidar in the point cloud, ptCloudIn
%   using 2-D bounding boxes, bboxesCamera in an image. bboxesLidar is an
%   array of cuboids of size N-by-9, where N is the number of cuboids
%   detected. bboxesCamera is an M-by-4 matrix, where M represents the
%   number of 2-D bounding boxes. intrinsics is a cameraIntrinsics object.
%   tform is a rigidtform3d object representing the transformation from the
%   camera to lidar.
%
%   [..., indices] = bboxCameraToLidar(...) additionally returns a cell
%   array of indices of points that are inside 3-D bounding box.
%
%   [..., boxesUsed] = bboxCameraToLidar(...) additionally returns a
%   logical vector of the same length as number of bboxesCamera where a
%   value of true indicates that a bounding box in the point cloud was
%   detected corresponding to the bounding box in an image.
%
%   [...] = bboxCameraToLidar(..., Name, Value) specifies additional
%   name-value pair arguments described below.
%
%   'ClusterThreshold'          Minimum Euclidean distance between points
%                               from two different clusters, specified as
%                               a positive scalar.
%
%                               Default: 1
%
%   'MaxDetectionRange'         Detection range in world units from the
%                               sensor to define the search range for
%                               bounding box detection.
%
%                               Default: (0, Inf]
%
%   Notes
%   -----
%   - The point cloud data and image data are assumed to be time synced
%     approximately.
%
%   - The point cloud is assumed to be in vehicle coordinate system, where
%     positive x axis is along the motion of ego vehicle.
%
%   - 3-D bounding box is a cuboid specified as
%     [xctr, yctr, zctr, xlen, ylen, zlen, xrot, yrot, zrot].
%     * [xctr, yctr, zctr] represents the cuboid center.
%     * [xlen, ylen, zlen] represents dimension along x, y, z axes.
%     * [xrot, yrot, zrot] represents rotations in degrees along x, y, z axes.
%
%   - 2-D bounding box of camera is a rectangle specified as
%     [x y width height]. [x y] is the upper-left corner of the rectangle.
%
%   - The 2-D bounding boxes are used for generating 3-D frustums in the
%     point cloud. Euclidean clustering is performed on these frustum-based
%     ROI regions to segment the possible object with the maximum number
%     of points.
%
%   - If the bounding box is smaller than expected, increase the
%     ClusterThreshold value.
%
%   Class Support
%   -------------
%   ptCloudIn must be a pointCloud object. intrinsics must be a
%   cameraIntrinsics object. tform must be a rigidtform3d
%   object.
%
%   Example: Detect and visualize bounding box in point cloud
%   ---------------------------------------------------------
%   % Load data
%   path = fullfile(toolboxdir('lidar'),'lidardata', 'lcc', ...
%           'bboxGT.mat');
%   gt = load(path);
%   im = gt.im;
%   pc = gt.pc;
%
%   intrinsics = gt.cameraParams;
%
%   % Load camera to lidar transformation matrix
%   tform = gt.camToLidar;
%
%   % Load ground truth rectangles.
%   bboxImage = gt.box;
%
%   % Draw image bounding box
%   annotatedImage = insertObjectAnnotation(im,'Rectangle', bboxImage, 'Vehicle');
%   figure;
%   imshow(annotatedImage);
%
%   % Estimate the bounding box in lidar
%   [bboxLidar, indices] = bboxCameraToLidar(bboxImage, pc, intrinsics, ...
%    tform, 'ClusterThreshold', 1);
%
%   % Draw detected cuboid bounding box
%   figure;
%   pcshow(pc.Location, pc.Location(:,3));
%   showShape('cuboid', bboxLidar, 'Opacity',0.5, 'Color', 'yellow');
%
%   See also estimateLidarCameraTransform, projectLidarPointsOnImage,
%   pcsegdist, cuboidModel, pcfitcuboid

%   Copyright 2020-2022 The MathWorks, Inc.

%#codegen
narginchk(4,8);

[camerabboxesCorner, intrinsicMatrix, clusterThreshold, maxDetectionRange, idx] = ...
    validateAndParseInputs(bboxesCamera, ptCloudIn, intrinsics, tform, varargin{:});

% Find the corners for bounding boxes in image
numBboxes = size(camerabboxesCorner, 3);
lidarbboxesCorner = zeros(8, 3, numBboxes);

for i = 1:numBboxes
    cornersCamera = camerabboxesCorner(:, :, i);
    
    % Transform bounding boxes from camera to lidar as frustums
    lidarbboxesCorner(:, :, i) = ...
        lidar.internal.calibration.findFrustumCorners(cornersCamera, ...
        intrinsicMatrix, tform, maxDetectionRange);
end

% Find indices of points inside the frustum
frustumIndices = extractIndicesInsideFrustum(lidarbboxesCorner, ptCloudIn);

% Extract cuboid parameters from the points inside frustum
[bboxesLidar, objectIndices, bboxesUsed] = ...
    extractCuboids(ptCloudIn, frustumIndices, clusterThreshold);
bboxesUsed = idx & bboxesUsed;

end

%--------------------------------------------------------------------------
function [bboxesCornersCamera, intrinsicMatrix, clusterThreshold,...
    maxDetectionRange, idx] = validateAndParseInputs(bboxesCamera, ptCloudIn,...
    intrinsics, tform, varargin)
% Parse and validate input

% Validate rectangles
validateattributes(bboxesCamera, {'single','double'}, ...
    {'real','nonsparse', 'ncols', 4,'finite'}, mfilename, 'bboxesCamera', 1);

% Validate point cloud
validateattributes(ptCloudIn, {'pointCloud'}, {'scalar'}, mfilename, 'ptCloudIn', 2);

xlims = ptCloudIn.XLimits;
ylims = ptCloudIn.YLimits;
zlims = ptCloudIn.ZLimits;

% Validate point cloud x range
validateattributes(xlims, {'double', 'single'}, {'real','nonsparse', 'finite', 'numel', 2}, mfilename, 'ptCloudIn.Xlimits');

% Validate point cloud y range
validateattributes(ylims, {'double', 'single'}, {'real','nonsparse', 'finite', 'numel', 2}, mfilename, 'ptCloudIn.Ylimits');

% Validate point cloud z range
validateattributes(zlims, {'double', 'single'}, {'real','nonsparse', 'finite', 'numel', 2}, mfilename, 'ptCloudIn.Zlimits');

% Validate camera intrinsics
validateattributes(intrinsics, {'cameraIntrinsics', 'cameraParameters'}, {'scalar'}, mfilename, 'intrinsics', 3);

% Validate camera-lidar transform
validateattributes(tform, {'rigidtform3d', 'rigid3d'}, {'scalar'}, mfilename, 'tform', 4);

intrinsicMatrix = intrinsics.IntrinsicMatrix;

% Convert rectangles from [x, y, l, w] to [x1, y1;x2, y2;x3, y3;x4, y4]
% where [xi, yi] represents the corner of the rectangles.
numBboxes = size(bboxesCamera, 1);
bboxesCornersCamera = zeros(4, 2, numBboxes);
j = 1;
idx = true(numBboxes, 1);
for i = 1:numBboxes
    bbox = bboxesCamera(i, :);
    
    % Check if the bounding box lies within the image dimension
    bboxCorners = getCornersFromBbox(bbox);
    if checkCameraCorners(bboxCorners, intrinsicMatrix)
        bboxesCornersCamera(:, :, j) = bboxCorners;
        j = j + 1;
    else
        idx(i) = false;
    end
end
newPc = pctransform(ptCloudIn, tform.invert());
xRange = newPc.ZLimits; %#ok<NASGU>

if (newPc.ZLimits(2) < 0.01)
    xRange = [newPc.ZLimits(2), 0.01];
else
    xRange = [0.01, newPc.ZLimits(2)];
end

% Validate XRange
validateattributes(xRange, {'single', 'double'}, ...
    {'real', 'nonsparse', 'finite', 'nonnan', 'numel', 2}, mfilename);

[clusterThreshold, maxDetectionRange] = validateAndParseOptionalArguments(xRange, varargin{:});
end

%--------------------------------------------------------------------------
function [threshold, maxRange] = validateAndParseOptionalArguments(xRange, varargin)
% Validate and parse optional inputs
if isSimMode()
    % Setup parser
    parser = inputParser;
    parser.CaseSensitive = false;
    parser.FunctionName  = mfilename;
    
    parser.addParameter('ClusterThreshold', 1);
    parser.addParameter('MaxDetectionRange', xRange);
    
    %Parse input
    parser.parse(varargin{:});
    
    threshold = checkThreshold(parser.Results.ClusterThreshold);
    maxRange = checkMaxDetectionRange(parser.Results.MaxDetectionRange, xRange);
    
else
    pvPairs = struct(...
        'ClusterThreshold',   uint32(0), ...
        'MaxDetectionRange', uint32(0));
    defaults = struct(...
        'ClusterThreshold',   1, ...
        'MaxDetectionRange', [0.001 inf]);
    popt = struct(...
        'CaseSensitivity', false, ...
        'StructExpand',    true,  ...
        'PartialMatching', true);
    
    optarg = eml_parse_parameter_inputs(pvPairs, popt, varargin{...
        1:end});
    
    threshold = eml_get_parameter_value(optarg.ClusterThreshold, ...
        defaults.ClusterThreshold, varargin{:});
    threshold = checkThreshold(threshold);
    
    maxRange = eml_get_parameter_value(optarg.MaxDetectionRange, ...
        defaults.MaxDetectionRange, varargin{:});
    maxRange = checkMaxDetectionRange(maxRange, xRange);
end
end

%--------------------------------------------------------------------------
function clusterThresholdOut = checkThreshold(clusterThreshold)
% Validate cluster threshold value
validateattributes(clusterThreshold, {'single','double'}, ...
    {'nonnan', 'nonsparse', 'scalar', 'real', 'positive', 'finite'}, ...
    'bboxCameraToLidar', 'clusterThreshold');
clusterThresholdOut = clusterThreshold;
end

%--------------------------------------------------------------------------
function maxRangeOut = checkMaxDetectionRange(defaultRange, maxRange)
% Validate detection range
validateattributes(defaultRange, {'single', 'double'}, ...
    { 'real', 'nonsparse', 'nonnan', 'positive', 'numel', 2}, 'bboxCameraToLidar', 'MaxDetectionRange');
% Check if the value is finite
if isfinite(defaultRange(2))
    maxRange(2) = defaultRange(2);
end
coder.internal.errorIf((maxRange(1) > maxRange(2) || defaultRange(1)==0 || ...
    defaultRange(1) > defaultRange(2) || eq(defaultRange(1), defaultRange(2))),...
    'lidar:imageToLidar:invalidRange');
maxRangeOut = maxRange;
end

%--------------------------------------------------------------------------
function tf = checkCameraCorners(bbox, intrinsics)
cx = intrinsics(3, 1);
cy = intrinsics(3, 2);
imageSize = [2*cx 2*cy];
checkBbox = zeros(4,2,'logical');
for i = 1:4
    checkBbox(i,:) = bbox(i,:) < imageSize;
end
if (sum(checkBbox(:, 1)) == 4 && size(find(checkBbox(:, 2)), 1))
    tf = true;
else
    tf = false;
end
end

%--------------------------------------------------------------------------
function frustumIndices = extractIndicesInsideFrustum(cornersLidar, ptCloudIn)
% EXTRACTINDICESINSIDEFRUSTUM Extracts indices of points using frustum
%   points in point cloud.
numBboxes = size(cornersLidar, 3);
frustumIndices =  cell(1, numBboxes);

for bboxesCount = 1:numBboxes
    bboxLidar = cornersLidar(:, :, bboxesCount);
%     pts = ptCloudIn.Location;
    % Check if the point cloud is organized
    if (ndims(ptCloudIn.Location) == 3)
        pts = reshape(ptCloudIn.Location, [], 3);
    else
        pts = ptCloudIn.Location;
    end        
    frustumIndices{bboxesCount} = ...
        lidar.internal.findPointsInFrustum(bboxLidar, pts);
end
end

%--------------------------------------------------------------------------
function cornersCamera = getCornersFromBbox(bbox)
% GETCORNERSFROMBBOX Returns the four corners from the rectangle defined as
%   [x, y, l, w]. The corners are arranged in the following fashion
%            (1)---------------(2)
%             |                 |
%             |                 |
%             |                 |
%             |                 |
%            (4)---------------(3)
%
cornersCamera = zeros(4, 2);
cornersCamera(1, 1:2) = bbox(1:2);
cornersCamera(2, 1:2) = bbox(1:2) + [bbox(3), 0];
cornersCamera(3, 1:2) = bbox(1:2) + bbox(3:4);
cornersCamera(4, 1:2) = bbox(1:2) + [0, bbox(4)];
end

%--------------------------------------------------------------------------
function [bboxLidar, indices, bboxesUsed] = extractCuboids(ptCloudIn, frustumIndicesCell, clusterThreshold)
%EXTRACTCUBOIDS Fits a cuboid, bboxLidar in the point cloud.
numBbox = numel(frustumIndicesCell);
indices = cell(numBbox, 1);
bboxLidar = zeros(numBbox, 9);
bboxesUsed = zeros(numBbox,1,'logical');

for countBbox = 1:numBbox
    frustumIndices = frustumIndicesCell{countBbox};
    tmpPc = select(ptCloudIn, frustumIndices, 'OutputSize', 'Full');
    [bbox, objectIndices] = ...
        lidar.internal.getLidarBbox(tmpPc, clusterThreshold);
    if ~isempty(bbox)
        bboxLidar(countBbox, :) = bbox;
        indices{countBbox} = objectIndices;
        bboxesUsed(countBbox) = true;
    else
        indices{countBbox} = [];
    end    
end
bboxLidar = bboxLidar(bboxesUsed', :);
end

%--------------------------------------------------------------------------
function flag = isSimMode()
flag = isempty(coder.target);
end

 

%PROD Product of elements.
%   P = PROD(X) is the product of the elements of the vector X. If X is a
%   matrix, P is a row vector with the product over each column. For 
%   N-D arrays, PROD(X) operates on the first non-singleton dimension.
%
%   PROD(X,"all") computes the product of all elements of X.
%
%   PROD(X,DIM) operates along the dimension DIM.
%
%   PROD(X,VECDIM) operates on the dimensions specified in the vector 
%   VECDIM. For example, PROD(X,[1 2]) operates on the elements contained
%   in the first and second dimensions of X.
%
%   PROD(...,OUTTYPE) specifies the type in which the product is 
%   performed, and the type of P. Available options are:
%
%   "double"    -  P has class double for any input X
%   "native"    -  P has the same class as X
%   "default"   -  If X is floating point, that is double or single,
%                  P has the same class as X. If X is not floating point, 
%                  P has class double.
%
%   PROD(...,NANFLAG) specifies how NaN values are treated:
%
%   "includemissing" / "includenan" -
%                  (default) The product of a vector containing NaN values
%                  is also NaN.
%   "omitmissing" / "omitnan"       -
%                  The product of a vector containing NaN values
%                  is the product of all its non-NaN elements. If all 
%                  elements are NaN, the result is 1.
%
%   Examples:
%       X = [0 1 2; 3 4 5]
%       prod(X,1)
%       prod(X,2)
%
%       X = int8([5 5 5 5])
%       prod(X)              % returns double(625), accumulates in double
%       prod(X,"native")     % returns int8(127), because it accumulates in
%                            % int8, but overflows and saturates.
%
%   See also SUM, CUMPROD, DIFF.

%   Copyright 1984-2023 The MathWorks, Inc.

%   Built-in function.