km.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2020 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <unistd.h>

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <set>
#include <string>
#include <utility>
#include <vector>

#include <Eigen/Core>

#define USR_INF 999999999999

namespace apollo {
namespace v2x {
namespace ft {

class KMkernal {
 public:
  KMkernal() = default;
  ~KMkernal() = default;
  template <typename T>
  bool GetKMResult(const T& association_mat,
                   std::vector<std::pair<int, int>>* match_cps,
                   bool need_reverse = false);

 private:
  int u_size_;
  int v_size_;
  double* ex_u_;
  double* ex_v_;
  int* v_matched_;
  double* v_slack_;
  std::set<int> used_u_;
  std::set<int> used_v_;
  template <typename T>
  bool FindCP(const T& mat, int i);
};
template <typename T>
bool KMkernal::GetKMResult(const T& association_mat,
                           std::vector<std::pair<int, int>>* match_cps,
                           bool need_reverse) {
  match_cps->clear();
  u_size_ = association_mat.rows();
  v_size_ = association_mat.cols();
  if (u_size_ > v_size_) return false;
  ex_u_ = new double[u_size_];
  ex_v_ = new double[v_size_];
  v_matched_ = new int[v_size_];
  std::fill(v_matched_, v_matched_ + v_size_, -1);
  memset(ex_v_, 0, v_size_ * sizeof(double));
  for (int i = 0; i < u_size_; ++i) {
    ex_u_[i] = association_mat(i, 0);
    for (int j = 1; j < v_size_; ++j) {
      ex_u_[i] = std::max(static_cast<float>(ex_u_[i]), association_mat(i, j));
    }
  }
  for (int i = 0; i < u_size_; ++i) {
    if (ex_u_[i] <= 0) {
      if (need_reverse)
        match_cps->push_back(std::make_pair(-1, i));
      else
        match_cps->push_back(std::make_pair(i, -1));
      continue;
    }
    v_slack_ = new double[v_size_];
    std::fill(v_slack_, v_slack_ + v_size_, USR_INF);
    while (1) {
      used_u_.clear();
      used_v_.clear();
      if (FindCP(association_mat, i)) break;
      double d = USR_INF;
      for (int j = 0; j < v_size_; ++j)
        if (used_v_.find(j) == used_v_.end()) d = std::min(d, v_slack_[j]);
      for (auto it = used_u_.begin(); it != used_u_.end(); it++) {
        ex_u_[*it] -= d;
      }
      for (int j = 0; j < v_size_; ++j) {
        if (used_v_.find(j) != used_v_.end())
          ex_v_[j] += d;
        else
          v_slack_[j] -= d;
      }
    }
    delete[] v_slack_;
  }
  if (need_reverse) {
    for (int j = 0; j < v_size_; ++j) {
      if (v_matched_[j] == -1) {
        match_cps->push_back(std::make_pair(j, -1));
      } else if (association_mat(v_matched_[j], j) > 0) {
        match_cps->push_back(std::make_pair(j, v_matched_[j]));
      } else {
        match_cps->push_back(std::make_pair(-1, v_matched_[j]));
        match_cps->push_back(std::make_pair(j, -1));
      }
    }
  } else {
    for (int j = 0; j < v_size_; ++j) {
      if (v_matched_[j] == -1) {
        match_cps->push_back(std::make_pair(-1, j));
      } else if (association_mat(v_matched_[j], j) > 0) {
        match_cps->push_back(std::make_pair(v_matched_[j], j));
      } else {
        match_cps->push_back(std::make_pair(v_matched_[j], -1));
        match_cps->push_back(std::make_pair(-1, j));
      }
    }
  }
  delete[] ex_u_;
  delete[] ex_v_;
  delete[] v_matched_;
  return true;
}

template <typename T>
bool KMkernal::FindCP(const T& mat, int i) {
  used_u_.insert(i);
  for (int j = 0; j < v_size_; ++j) {
    if (used_v_.find(j) != used_v_.end()) {
      continue;
    }
    double gap = ex_u_[i] + ex_v_[j] - mat(i, j);
    if (gap <= 0) {
      // res = 0;
      used_v_.insert(j);
      bool match_success = v_matched_[j] == -1 || FindCP(mat, v_matched_[j]);
      if (match_success) {
        v_matched_[j] = i;
        return true;
      }
    } else {
      v_slack_[j] = std::min(v_slack_[j], gap);
    }
  }
  return false;
}

}  // namespace ft
}  // namespace v2x
}  // namespace apollo