Function calculate_cost

Function Documentation

arma::fvec calculate_cost(const std::vector<std::array<float, 3>> &points, const std::vector<std::vector<int>> &configurations, ConeType cone_type, const std::array<float, 2> &vehicle_position, const std::array<float, 2> &vehicle_direction, bool return_individual_costs)

Calculates the total or individual costs for each cone configuration.

Parameters:

  • points – Matrix of cone positions.

  • configurations – Matrix of cone index configurations.

  • cone_type – Type of cones (LEFT or RIGHT).

  • vehicle_position – Current vehicle position (not used currently).

  • vehicle_direction – Current vehicle direction.

  • return_individual_costs – If true, returns individual cost components; otherwise, returns the total cost.

Returns:

Arma vector of floats containing either the total costs or individual costs.