refactor(object_merger)!: fix namespace and directory structure

perception/object_merger/CMakeLists.txt

@@ -18,18 +18,18 @@ include_directories(
     ${EIGEN3_INCLUDE_DIR}
 )
 
-ament_auto_add_library(object_association_merger SHARED
-  src/object_association_merger/data_association/data_association.cpp
-  src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
-  src/object_association_merger/node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/association/data_association.cpp
+  src/association/solver/mu_successive_shortest_path_wrapper.cpp
+  src/object_association_merger_node.cpp
 )
 
-target_link_libraries(object_association_merger
+target_link_libraries(${PROJECT_NAME}
   Eigen3::Eigen
 )
 
-rclcpp_components_register_node(object_association_merger
-  PLUGIN "object_association::ObjectAssociationMergerNode"
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::object_merger::ObjectAssociationMergerNode"
   EXECUTABLE object_association_merger_node
 )
 

perception/object_merger/include/object_merger/data_association/data_association.hpp → perception/object_merger/include/autoware_object_merger/association/data_association.hpp

@@ -16,21 +16,25 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#ifndef OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-#define OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-
-#include <list>
-#include <memory>
-#include <unordered_map>
-#include <vector>
+#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_
+#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_
 
 #define EIGEN_MPL2_ONLY
-#include "object_merger/data_association/solver/gnn_solver.hpp"
+
+#include "autoware_object_merger/association/solver/gnn_solver.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+
+#include <list>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+namespace autoware::object_merger
+{
 
 class DataAssociation
 {
@@ -40,7 +44,7 @@ class DataAssociation
   Eigen::MatrixXd max_rad_matrix_;
   Eigen::MatrixXd min_iou_matrix_;
   const double score_threshold_;
-  std::unique_ptr<gnn_solver::GnnSolverInterface> gnn_solver_ptr_;
+  std::unique_ptr<autoware::object_merger::gnn_solver::GnnSolverInterface> gnn_solver_ptr_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -56,4 +60,6 @@ class DataAssociation
   virtual ~DataAssociation() {}
 };
 
-#endif  // OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
+}  // namespace autoware::object_merger
+
+#endif  // AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_

perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp → perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver.hpp

@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
-#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
 
-#include "object_merger/data_association/solver/gnn_solver_interface.hpp"
-#include "object_merger/data_association/solver/mu_successive_shortest_path.hpp"
-#include "object_merger/data_association/solver/successive_shortest_path.hpp"
+#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp"
+#include "autoware_object_merger/association/solver/mu_ssp.hpp"
+#include "autoware_object_merger/association/solver/ssp.hpp"
 
-#endif  // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#endif  // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_

perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp → perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver_interface.hpp

@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
-#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::object_merger::gnn_solver
 {
 class GnnSolverInterface
 {
@@ -30,6 +30,6 @@ class GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) = 0;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::object_merger::gnn_solver
 
-#endif  // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#endif  // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_

perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp → perception/object_merger/include/autoware_object_merger/association/solver/mu_ssp.hpp

@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
-#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_
+#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_
 
-#include "object_merger/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::object_merger::gnn_solver
 {
 class MuSSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class MuSSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::object_merger::gnn_solver
 
-#endif  // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_

perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp → perception/object_merger/include/autoware_object_merger/association/solver/ssp.hpp

@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
-#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_
+#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_
 
-#include "object_merger/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::object_merger::gnn_solver
 {
 class SSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class SSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::object_merger::gnn_solver
 
-#endif  // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_

perception/object_merger/package.xml

@@ -22,7 +22,6 @@
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
-  <depend>tier4_perception_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

perception/object_merger/src/object_association_merger/data_association/data_association.cpp → perception/object_merger/src/association/data_association.cpp

@@ -12,11 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "object_merger/data_association/data_association.hpp"
+#include "autoware_object_merger/association/data_association.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
-#include "object_merger/data_association/solver/gnn_solver.hpp"
-#include "object_merger/utils/utils.hpp"
+#include "autoware_object_merger/association/solver/gnn_solver.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
 
 #include <algorithm>
@@ -47,6 +46,9 @@
 }
 }  // namespace
 
+namespace autoware::object_merger
+{
+
 DataAssociation::DataAssociation(
   std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
   std::vector<double> max_rad_vector, std::vector<double> min_iou_vector)
@@ -77,98 +79,100 @@
     min_iou_matrix_ = min_iou_matrix_tmp.transpose();
   }
 
-  gnn_solver_ptr_ = std::make_unique<gnn_solver::MuSSP>();
+  gnn_solver_ptr_ = std::make_unique<autoware::object_merger::gnn_solver::MuSSP>();
 }
 
 void DataAssociation::assign(
   const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
   std::unordered_map<int, int> & reverse_assignment)
 {
   std::vector<std::vector<double>> score(src.rows());
   for (int row = 0; row < src.rows(); ++row) {
     score.at(row).resize(src.cols());
     for (int col = 0; col < src.cols(); ++col) {
       score.at(row).at(col) = src(row, col);
     }
   }
   // Solve
   gnn_solver_ptr_->maximizeLinearAssignment(score, &direct_assignment, &reverse_assignment);
 
   for (auto itr = direct_assignment.begin(); itr != direct_assignment.end();) {
     if (src(itr->first, itr->second) < score_threshold_) {
       itr = direct_assignment.erase(itr);
       continue;
     } else {
       ++itr;
     }
   }
   for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end();) {
     if (src(itr->second, itr->first) < score_threshold_) {
       itr = reverse_assignment.erase(itr);
       continue;
     } else {
       ++itr;
     }
   }
 }
 
 Eigen::MatrixXd DataAssociation::calcScoreMatrix(
   const autoware_perception_msgs::msg::DetectedObjects & objects0,
   const autoware_perception_msgs::msg::DetectedObjects & objects1)
 {
   Eigen::MatrixXd score_matrix =
     Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size());
   for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) {
     const autoware_perception_msgs::msg::DetectedObject & object1 =
       objects1.objects.at(objects1_idx);
     const std::uint8_t object1_label =
       object_recognition_utils::getHighestProbLabel(object1.classification);
 
     for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) {
       const autoware_perception_msgs::msg::DetectedObject & object0 =
         objects0.objects.at(objects0_idx);
       const std::uint8_t object0_label =
         object_recognition_utils::getHighestProbLabel(object0.classification);
 
       double score = 0.0;
       if (can_assign_matrix_(object1_label, object0_label)) {
         const double max_dist = max_dist_matrix_(object1_label, object0_label);
         const double dist = autoware::universe_utils::calcDistance2d(
           object0.kinematics.pose_with_covariance.pose.position,
           object1.kinematics.pose_with_covariance.pose.position);
 
         bool passed_gate = true;
         // dist gate
         if (passed_gate) {
           if (max_dist < dist) passed_gate = false;
         }
         // angle gate
         if (passed_gate) {
           const double max_rad = max_rad_matrix_(object1_label, object0_label);
           const double angle = getFormedYawAngle(
             object0.kinematics.pose_with_covariance.pose.orientation,
             object1.kinematics.pose_with_covariance.pose.orientation, false);
           if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle))
             passed_gate = false;
         }
         // 2d iou gate
         if (passed_gate) {
           const double min_iou = min_iou_matrix_(object1_label, object0_label);
           const double min_union_iou_area = 1e-2;
           const double iou =
             object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area);
           if (iou < min_iou) passed_gate = false;
         }
 
         // all gate is passed
         if (passed_gate) {
           score = (max_dist - std::min(dist, max_dist)) / max_dist;
           if (score < score_threshold_) score = 0.0;
         }
       }
       score_matrix(objects1_idx, objects0_idx) = score;
     }
   }
 
   return score_matrix;
 }
+
+}  // namespace autoware::object_merger

perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp → perception/object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "object_merger/data_association/solver/mu_successive_shortest_path.hpp"
+#include "autoware_object_merger/association/solver/mu_ssp.hpp"
 
 #include <mussp/mussp.h>
 
@@ -24,7 +24,7 @@
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::object_merger::gnn_solver
 {
 void MuSSP::maximizeLinearAssignment(
   const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
@@ -38,4 +38,4 @@ void MuSSP::maximizeLinearAssignment(
   // Solve DA by muSSP
   solve_muSSP(cost, direct_assignment, reverse_assignment);
 }
-}  // namespace gnn_solver
+}  // namespace autoware::object_merger::gnn_solver

perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp → perception/object_merger/src/association/solver/successive_shortest_path.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "object_merger/data_association/solver/successive_shortest_path.hpp"
+#include "autoware_object_merger/association/solver/ssp.hpp"
 
 #include <algorithm>
 #include <cassert>
@@ -22,7 +22,7 @@
 #include <utility>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::object_merger::gnn_solver
 {
 struct ResidualEdge
 {
@@ -43,312 +43,312 @@
   }
 };
 
 void SSP::maximizeLinearAssignment(
   const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
   std::unordered_map<int, int> * reverse_assignment)
 {
   // NOTE: Need to set as default arguments
   bool sparse_cost = true;
   // bool sparse_cost = false;
 
   // Hyperparameters
   // double MAX_COST = 6;
   const double MAX_COST = 10;
   const double INF_DIST = 10000000;
   const double EPS = 1e-5;
 
   // When there is no agents or no tasks, terminate
   if (cost.size() == 0 || cost.at(0).size() == 0) {
     return;
   }
 
   // Construct a bipartite graph from the cost matrix
   int n_agents = cost.size();
   int n_tasks = cost.at(0).size();
 
   int n_dummies;
   if (sparse_cost) {
     n_dummies = n_agents;
   } else {
     n_dummies = 0;
   }
 
   int source = 0;
   int sink = n_agents + n_tasks + 1;
   int n_nodes = n_agents + n_tasks + n_dummies + 2;
 
   // // Print cost matrix
   // std::cout << std::endl;
   // for (int agent = 0; agent < n_agents; agent++)
   // {
   //   for (int task = 0; task < n_tasks; task++)
   //   {
   //     std::cout << cost.at(agent).at(task) << " ";
   //   }
   //   std::cout << std::endl;
   // }
 
   // std::chrono::system_clock::time_point start_time, end_time;
   // start_time = std::chrono::system_clock::now();
 
   // Adjacency list of residual graph (index: nodes)
   //     - 0: source node
   //     - {1, ...,  n_agents}: agent nodes
   //     - {n_agents+1, ...,  n_agents+n_tasks}: task nodes
   //     - n_agents+n_tasks+1: sink node
   //     - {n_agents+n_tasks+2, ..., n_agents+n_tasks+1+n_agents}:
   //       dummy node (when sparse_cost is true)
   std::vector<std::vector<ResidualEdge>> adjacency_list(n_nodes);
 
   // Reserve memory
   for (int v = 0; v < n_nodes; ++v) {
     if (v == source) {
       // Source
       adjacency_list.at(v).reserve(n_agents);
     } else if (v <= n_agents) {
       // Agents
       adjacency_list.at(v).reserve(n_tasks + 1 + 1);
     } else if (v <= n_agents + n_tasks) {
       // Tasks
       adjacency_list.at(v).reserve(n_agents + 1);
     } else if (v == sink) {
       // Sink
       adjacency_list.at(v).reserve(n_tasks + n_dummies);
     } else {
       // Dummies
       adjacency_list.at(v).reserve(2);
     }
   }
 
   // Add edges form source
   for (int agent = 0; agent < n_agents; ++agent) {
     // From source to agent
     adjacency_list.at(source).emplace_back(agent + 1, 1, 0, 0, adjacency_list.at(agent + 1).size());
     // From agent to source
     adjacency_list.at(agent + 1).emplace_back(
       source, 0, 0, 0, adjacency_list.at(source).size() - 1);
   }
 
   // Add edges from agents
   for (int agent = 0; agent < n_agents; ++agent) {
     for (int task = 0; task < n_tasks; ++task) {
       if (!sparse_cost || cost.at(agent).at(task) > EPS) {
         // From agent to task
         adjacency_list.at(agent + 1).emplace_back(
           task + n_agents + 1, 1, MAX_COST - cost.at(agent).at(task), 0,
           adjacency_list.at(task + n_agents + 1).size());
 
         // From task to agent
         adjacency_list.at(task + n_agents + 1)
           .emplace_back(
             agent + 1, 0, cost.at(agent).at(task) - MAX_COST, 0,
             adjacency_list.at(agent + 1).size() - 1);
       }
     }
   }
 
   // Add edges form tasks
   for (int task = 0; task < n_tasks; ++task) {
     // From task to sink
     adjacency_list.at(task + n_agents + 1)
       .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size());
 
     // From sink to task
     adjacency_list.at(sink).emplace_back(
       task + n_agents + 1, 0, 0, 0, adjacency_list.at(task + n_agents + 1).size() - 1);
   }
 
   // Add edges from dummy
   if (sparse_cost) {
     for (int agent = 0; agent < n_agents; ++agent) {
       // From agent to dummy
       adjacency_list.at(agent + 1).emplace_back(
         agent + n_agents + n_tasks + 2, 1, MAX_COST, 0,
         adjacency_list.at(agent + n_agents + n_tasks + 2).size());
 
       // From dummy to agent
       adjacency_list.at(agent + n_agents + n_tasks + 2)
         .emplace_back(agent + 1, 0, -MAX_COST, 0, adjacency_list.at(agent + 1).size() - 1);
 
       // From dummy to sink
       adjacency_list.at(agent + n_agents + n_tasks + 2)
         .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size());
 
       // From sink to dummy
       adjacency_list.at(sink).emplace_back(
         agent + n_agents + n_tasks + 2, 0, 0, 0,
         adjacency_list.at(agent + n_agents + n_tasks + 2).size() - 1);
     }
   }
 
   // Maximum flow value
   const int max_flow = std::min(n_agents, n_tasks);
 
   // Feasible potentials
   std::vector<double> potentials(n_nodes, 0);
 
   // Shortest path lengths
   std::vector<double> distances(n_nodes, INF_DIST);
 
   // Whether previously visited the node or not
   std::vector<bool> is_visited(n_nodes, false);
 
   // Parent node (<prev_node, edge_index>)
   std::vector<std::pair<int, int>> prev_values(n_nodes);
 
   for (int i = 0; i < max_flow; ++i) {
     // Initialize priority queue (<distance, node>)
     std::priority_queue<
       std::pair<double, int>, std::vector<std::pair<double, int>>,
       std::greater<std::pair<double, int>>>
       p_queue;
 
     // Reset all trajectory states
     if (i > 0) {
       std::fill(distances.begin(), distances.end(), INF_DIST);
       std::fill(is_visited.begin(), is_visited.end(), false);
     }
 
     // Start trajectory from the source node
     p_queue.push(std::make_pair(0, source));
     distances.at(source) = 0;
 
     while (!p_queue.empty()) {
       // Get the next element
       std::pair<double, int> cur_elem = p_queue.top();
       // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl;
       p_queue.pop();
 
       double cur_node_dist = cur_elem.first;
       int cur_node = cur_elem.second;
 
       // If already visited node, skip and continue
       if (is_visited.at(cur_node)) {
         continue;
       }
       assert(cur_node_dist == distances.at(cur_node));
 
       // Mark as visited
       is_visited.at(cur_node) = true;
       // Update potential
       potentials.at(cur_node) += cur_node_dist;
 
       // When reached to the sink node, terminate.
       if (cur_node == sink) {
         break;
       }
 
       // Loop over the incident nodes(/edges)
       for (auto it_incident_edge = adjacency_list.at(cur_node).cbegin();
            it_incident_edge != adjacency_list.at(cur_node).cend(); it_incident_edge++) {
         // If the node is not visited and have capacity to increase flow, visit.
         if (!is_visited.at(it_incident_edge->dst) && it_incident_edge->capacity > 0) {
           // Calculate reduced cost
           double reduced_cost =
             it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst);
           assert(reduced_cost >= 0);
           if (distances.at(it_incident_edge->dst) > reduced_cost) {
             distances.at(it_incident_edge->dst) = reduced_cost;
             prev_values.at(it_incident_edge->dst) =
               std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin());
             // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl;
             p_queue.push(std::make_pair(reduced_cost, it_incident_edge->dst));
           }
         }
       }
     }
 
     // Shortest path length to sink is greater than MAX_COST,
     // which means no non-dummy routes left, terminate
     if (potentials.at(sink) >= MAX_COST) {
       break;
     }
 
     // Update potentials of unvisited nodes
     for (int v = 0; v < n_nodes; ++v) {
       if (!is_visited.at(v)) {
         potentials.at(v) += distances.at(sink);
       }
     }
     // //Print potentials
     // for (int v = 0; v < n_nodes; ++v)
     // {
     //   std::cout << potentials.at(v) << ", ";
     // }
     // std::cout << std::endl;
 
     // Increase/decrease flow and capacity along the shortest path from the source to the sink
     int v = sink;
     int prev_v;
     while (v != source) {
       ResidualEdge & e_forward =
         adjacency_list.at(prev_values.at(v).first).at(prev_values.at(v).second);
       assert(e_forward.dst == v);
       ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse);
       prev_v = e_backward.dst;
 
       if (e_backward.flow == 0) {
         // Increase flow
         // State A
         assert(e_forward.capacity == 1);
         assert(e_forward.flow == 0);
         assert(e_backward.capacity == 0);
         assert(e_backward.flow == 0);
 
         e_forward.capacity -= 1;
         e_forward.flow += 1;
         e_backward.capacity += 1;
 
         // State B
         assert(e_forward.capacity == 0);
         assert(e_forward.flow == 1);
         assert(e_backward.capacity == 1);
         assert(e_backward.flow == 0);
       } else {
         // Decrease flow
         // State B
         assert(e_forward.capacity == 1);
         assert(e_forward.flow == 0);
         assert(e_backward.capacity == 0);
         assert(e_backward.flow == 1);
 
         e_forward.capacity -= 1;
         e_backward.capacity += 1;
         e_backward.flow -= 1;
 
         // State A
         assert(e_forward.capacity == 0);
         assert(e_forward.flow == 0);
         assert(e_backward.capacity == 1);
         assert(e_backward.flow == 0);
       }
 
       v = prev_v;
     }
 
 #ifndef NDEBUG
     // Check if the potentials are feasible potentials
     for (int v = 0; v < n_nodes; ++v) {
       for (auto it_incident_edge = adjacency_list.at(v).cbegin();
            it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) {
         if (it_incident_edge->capacity > 0) {
           double reduced_cost =
             it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst);
           assert(reduced_cost >= 0);
         }
       }
     }
 #endif
   }
 
   // Output
   for (int agent = 0; agent < n_agents; ++agent) {
     for (auto it_incident_edge = adjacency_list.at(agent + 1).cbegin();
          it_incident_edge != adjacency_list.at(agent + 1).cend(); ++it_incident_edge) {
       int task = it_incident_edge->dst - (n_agents + 1);
 
       // If the flow value is 1 and task is not dummy, assign the task to the agent.
       if (it_incident_edge->flow == 1 && 0 <= task && task < n_tasks) {
         (*direct_assignment)[agent] = task;
         (*reverse_assignment)[task] = agent;
         break;
@@ -367,4 +367,4 @@
   }
 #endif
 }
-}  // namespace gnn_solver
+}  // namespace autoware::object_merger::gnn_solver