Method getCostMatrix

swarm_exploration/active_perception/src/hgrid.cpp:277–338

}

void HGrid::getCostMatrix(const vector<Eigen::Vector3d>& positions,
    const vector<Eigen::Vector3d>& velocities, const vector<vector<int>>& first_ids,
    const vector<vector<int>>& second_ids, const vector<int>& grid_ids, Eigen::MatrixXd& mat) {
  // first_ids and second_ids are drone_num x 1-4 vectors

  // Fill the cost matrix
  const int drone_num = positions.size();
  const int grid_num = grid_ids.size();
  const int dimen = 1 + drone_num + grid_num;
  mat = Eigen::MatrixXd::Zero(dimen, dimen);

  // std::cout << "First id: ";
  // for (auto ids : first_ids)
  //   for (auto id : ids)
  //     std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // std::cout << "Second id: ";
  // for (auto ids : second_ids)
  //   for (auto id : ids)
  //     std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // Virtual depot to drones
  for (int i = 0; i < drone_num; ++i) {
    mat(0, 1 + i) = -1000;
    mat(1 + i, 0) = 1000;
  }
  // Virtual depot to grid
  for (int i = 0; i < grid_num; ++i) {
    mat(0, 1 + drone_num + i) = 1000;
    mat(1 + drone_num + i, 0) = 0;
  }
  // Costs between drones
  for (int i = 0; i < drone_num; ++i) {
    for (int j = 0; j < drone_num; ++j) {
      mat(1 + i, 1 + j) = 10000;
    }
  }

  // Costs from drones to grid
  for (int i = 0; i < drone_num; ++i) {
    for (int j = 0; j < grid_num; ++j) {
      double cost = getCostDroneToGrid(positions[i], grid_ids[j], first_ids[i]);
      mat(1 + i, 1 + drone_num + j) = cost;
      mat(1 + drone_num + j, 1 + i) = 0;
    }
  }
  // Costs between grid
  for (int i = 0; i < grid_num; ++i) {
    for (int j = i + 1; j < grid_num; ++j) {
      double cost = getCostGridToGrid(grid_ids[i], grid_ids[j], first_ids, second_ids, drone_num);
      mat(1 + drone_num + i, 1 + drone_num + j) = cost;
      mat(1 + drone_num + j, 1 + drone_num + i) = cost;
    }
  }

  // Diag