Function combine_and_sort_virtual_with_real

Function Documentation

std::tuple<arma::fmat, arma::ivec> combine_and_sort_virtual_with_real(const arma::fmat &other_side_cones, const arma::fmat &other_side_virtual_cones, const arma::ivec &match_idx_original, const arma::fvec &car_pos, const arma::fvec &car_dir)

Combines real and virtual cones, sorts them, and identifies virtual cones based on distance from real cones.

This function combines two sets of cones (real and virtual), inserts the virtual cones into the existing set of real cones, and sorts them. It then calculates the Euclidean distance between the combined cones and the real cones to determine which cones are virtual. A cone is considered virtual if it is sufficiently far from all real cones, as defined by an epsilon value.

See also

insert_virtual_cones_to_existing, my_cdist_sq_euclidean

Note

This function assumes that other_side_cones and other_side_virtual_cones are matrices where each row represents a cone in a multi-dimensional space. The car_pos vector represents the position of the car, which is used when inserting virtual cones.

Parameters:

  • other_side_cones – A matrix of real cones (rows = points, cols = dimensions), type arma::fmat.

  • other_side_virtual_cones – A matrix of virtual cones (rows = points, cols = dimensions), type arma::fmat.

  • match_idx_original – A vector of indices that match the original cones, type arma::ivec.

  • car_pos – A matrix representing the position of the car (used for insertion), type arma::fvec.

  • car_dir – A matrix representing the direction of the car (currently not used), type arma::fvec.

Returns:

A tuple containing:

  • A matrix of sorted and combined cones (real and virtual), type arma::mat.

  • A boolean vector indicating whether each cone is virtual (1 if virtual, 0 if real), type arma::ivec.