Function insert_virtual_cones_to_existing

Function Documentation

std::pair<arma::fmat, arma::ivec> insert_virtual_cones_to_existing(const arma::fmat &other_side_cones, const arma::fmat &other_side_virtual_cones, const arma::ivec &match_idx_original, const arma::fvec &car_pos)

Inserts virtual cones into an existing set of cones based on proximity and angle constraints.

This function takes two sets of cones (real and virtual) from the opposite side, along with the position of the car, and inserts the virtual cones into the existing cones in an optimized manner. The cones are first sorted based on their proximity to the existing cones, and then the virtual cones are inserted where necessary, either between existing cones or at the end based on angle constraints. After inserting the virtual cones, the function filters out cones that would create small angles (< 85 degrees) between consecutive cones.

Note

This function assumes that other_side_cones and other_side_virtual_cones are matrices where each row represents a cone in 2D or 3D space (depending on the context). The car_pos vector is assumed to represent the position of the car in a 2D or 3D coordinate system.

Parameters:

  • other_side_cones – A matrix of cones representing the existing cones on the opposite side.

  • other_side_virtual_cones – A matrix of cones representing the virtual cones to be inserted.

  • match_idx_original – A vector of indices that match the original cones, used for sorting and insertion.

  • car_pos – A row vector representing the position of the car.

Throws:

std::logic_error – Throws an exception if an unreachable code block is encountered during the insertion of cones.

Returns:

A matrix of cones after inserting virtual cones and filtering out those creating small angles, and a vector indicating which cones are virtual (1 if virtual, 0 if real).