Add parameter for heuristic-based max-clique problem

Author: lajoiepy

cpp/examples/robust_distributed_optimization_example_2robots.cpp

@@ -33,6 +33,7 @@ int main(int argc, char *argv[]) {
  double pcm_threshold = 0.99; // confidence probability for the pairwise consistency computation.
  bool use_covariance = false; // use covariance in dataset file.
  bool use_PCM = false; // Use pairwise consistency maximization.
+ bool use_heuristics = true; // Use heuristics-based algorithm for the max-clique solver.
 
  try {
  // Parse program options
@@ -61,7 +62,8 @@ int main(int argc, char *argv[]) {
  ("confidence, c", po::value<double>(&pcm_threshold), "confidence probability for the pairwise consistency computation (default: 0.99)")
  ("useCovariance, i", po::value<bool>(&use_covariance), "use covariance in dataset file (default: false)")
  ("debug, d", po::value<bool>(&debug), "debug (default: false)")
- ("usePCM", po::value<bool>(&use_PCM), "use pairwise consistency maximization (default: true)");
+ ("usePCM", po::value<bool>(&use_PCM), "use pairwise consistency maximization (default: true)")
+ ("useHeuristics", po::value<bool>(&use_heuristics), "use heuristics-based algorithm for the max-clique solver. (default: true)");
 
  po::variables_map vm;
  try {
@@ -89,7 +91,7 @@ int main(int argc, char *argv[]) {
  use_XY, use_OP, debug, priorModel, model,
  max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
  gamma, use_flagged_init, update_type, use_between_noise,
- use_chr_less_full_graph, use_landmarks, pcm_threshold, use_covariance, use_PCM);
+ use_chr_less_full_graph, use_landmarks, pcm_threshold, use_covariance, use_PCM, use_heuristics);
 
  return 0;
 }

cpp/include/distributed_mapper/distributed_mapper_utils.h

@@ -106,6 +106,7 @@ distributedOptimizer(std::vector< boost::shared_ptr<DistributedMapper> >& dist_m
  const double& pcm_threshold = 0.99,
  const bool& use_covariance = false,
  const bool& use_pcm = true,
+ const bool& use_heuristics = true,
  boost::optional<std::vector<gtsam::GraphAndValues>&> graph_and_values_vec = boost::none,
  boost::optional<std::vector<gtsam::Values>&> rotation_trace = boost::none,
  boost::optional<std::vector<gtsam::Values>&> pose_trace = boost::none,

cpp/include/distributed_mapper/run_distributed_mapper.h

@@ -27,5 +27,5 @@ namespace distributed_mapper {
  const size_t& max_iter, const double& rotation_estimate_change_threshold, const double& pose_estimate_change_threshold,
  const double& gamma, const bool& use_flagged_init, const distributed_mapper::DistributedMapper::UpdateType& update_type,
  const bool& use_between_noise, const bool& use_chr_less_full_graph, const bool& use_landmarks, const double& pcm_threshold, const bool& use_covariance,
- const bool& use_PCM);
+ const bool& use_PCM, const bool& use_heuristics);
 }

cpp/include/pairwise_consistency_maximization/distributed_pcm/distributed_pcm.h

@@ -30,7 +30,8 @@ namespace distributed_pcm {
  */
  static std::pair<int, int> solveCentralized(std::vector< boost::shared_ptr<distributed_mapper::DistributedMapper> >& dist_mappers,
  std::vector<gtsam::GraphAndValues>& graph_and_values_vector,
- const double& pcm_threshold, const bool& use_covariance);
+ const double& pcm_threshold, const bool& use_covariance, 
+ const bool& use_heuristics);
 
  /**
  * \brief Function that solves the pairwise consistency maximization according to the current constraints with limited information
@@ -46,7 +47,8 @@ namespace distributed_pcm {
  robot_measurements::RobotLocalMap& robot_local_map,
  const graph_utils::Trajectory& other_robot_trajectory,
  const double& pcm_threshold,
- const bool& is_prior_added);
+ const bool& is_prior_added, 
+ const bool& use_heuristics);
 
  private:
 
@@ -61,15 +63,17 @@ namespace distributed_pcm {
  const std::map<std::pair<char, char>,graph_utils::Transforms>& separators_transforms_by_pair,
  std::vector< boost::shared_ptr<distributed_mapper::DistributedMapper> >& dist_mappers,
  std::vector<gtsam::GraphAndValues>& graph_and_values_vector,
- const double& pcm_threshold);
+ const double& pcm_threshold,
+ const bool& use_heuristics);
 
  static std::pair<std::pair<int, int>, std::pair<std::set<std::pair<gtsam::Key, gtsam::Key>>, std::set<std::pair<gtsam::Key, gtsam::Key>>>>
  executePCMDecentralized(const int& other_robot_id, robot_measurements::RobotLocalMap& robot_local_map,
  const robot_measurements::RobotLocalMap& other_robot_local_info,
  boost::shared_ptr<distributed_mapper::DistributedMapper>& dist_mapper,
  gtsam::GraphAndValues& local_graph_and_values,
  const double& pcm_threshold,
- const bool& is_prior_added);
+ const bool& is_prior_added,
+ const bool& use_heuristics);
 
  static bool isSeparatorToBeRejected(const std::vector<int>& max_clique, const int& separtor_id, const graph_utils::Transforms& separators_transforms,
  const graph_utils::LoopClosures& loop_closures, boost::shared_ptr<distributed_mapper::DistributedMapper>& dist_mapper);

cpp/include/pairwise_consistency_maximization/global_map/global_map.h

@@ -42,7 +42,8 @@ namespace global_map {
  GlobalMap(const robot_measurements::RobotLocalMap& robot1_local_map,
  const robot_measurements::RobotLocalMap& robot2_local_map,
  const robot_measurements::RobotMeasurements& interrobot_measurements,
- const double& pcm_threshold);
+ const double& pcm_threshold,
+ const bool& use_heuristics);
 
  /**
  * \brief Function that solves the global maps according to the current constraints
@@ -54,6 +55,8 @@ namespace global_map {
  private:
  pairwise_consistency::PairwiseConsistency pairwise_consistency_; ///< Pairwise consistency solver.
 
+ bool use_heuristics_; ///< if true: uses the heuristics-based max-clique solver (faster), if false: uses the exact algorithm (slower) 
+
  }; 
 }
 

cpp/src/distributed_mapper/distributed_mapper_utils.cpp

@@ -494,6 +494,7 @@ distributedOptimizer(std::vector< boost::shared_ptr<DistributedMapper> >& dist_m
  const double& pcm_threshold,
  const bool& use_covariance,
  const bool& use_pcm,
+ const bool& use_heuristics,
  boost::optional<std::vector<gtsam::GraphAndValues>&> graph_and_values_vec,
  boost::optional<std::vector<gtsam::Values>&> rotation_trace,
  boost::optional<std::vector<gtsam::Values>&> pose_trace,
@@ -516,7 +517,7 @@ distributedOptimizer(std::vector< boost::shared_ptr<DistributedMapper> >& dist_m
 
  if (use_pcm && contains_odometry) {
  auto max_clique_info = distributed_pcm::DistributedPCM::solveCentralized(dist_mappers, graph_and_values_vec.get(),
- pcm_threshold, use_covariance);
+ pcm_threshold, use_covariance, use_heuristics);
  max_clique_size = max_clique_info.first;
  }
 

cpp/src/distributed_mapper/run_distributed_mapper.cpp

@@ -15,7 +15,8 @@ std::tuple<double, double, int> runDistributedMapper(const size_t& nr_robots, co
  const size_t& max_iter, const double& rotation_estimate_change_threshold, const double& pose_estimate_change_threshold,
  const double& gamma, const bool& use_flagged_init, const distributed_mapper::DistributedMapper::UpdateType& update_type,
  const bool& use_between_noise, const bool& use_chr_less_full_graph, const bool& use_landmarks, const double& pcm_threshold, const bool& use_covariance,
- const bool& use_PCM) {
+ const bool& use_PCM,
+ const bool& use_heuristics) {
 
  vector <GraphAndValues> graph_and_values_vec; // vector of all graphs and initials
 
@@ -114,7 +115,7 @@ std::tuple<double, double, int> runDistributedMapper(const size_t& nr_robots, co
  gamma, rotation_estimate_change_threshold,
  pose_estimate_change_threshold,
  use_flagged_init, use_landmarks, debug, true,
- pcm_threshold, use_covariance, use_PCM,
+ pcm_threshold, use_covariance, use_PCM, use_heuristics,
  graph_and_values_vec,
  rotation_trace, pose_trace, subgraph_rotation_trace,
  subgraph_pose_trace, rotation_vector_values_trace);

cpp/src/pairwise_consistency_maximization/distributed_pcm/distributed_pcm.cpp

@@ -6,7 +6,7 @@ namespace distributed_pcm {
 
  std::pair<int, int> DistributedPCM::solveCentralized(std::vector< boost::shared_ptr<distributed_mapper::DistributedMapper> >& dist_mappers,
  std::vector<gtsam::GraphAndValues>& graph_and_values_vector,
- const double& pcm_threshold, const bool& use_covariance) {
+ const double& pcm_threshold, const bool& use_covariance, const bool& use_heuristics) {
 
  std::vector<graph_utils::LoopClosures> separators_by_robot;
  std::vector<graph_utils::Transforms> transforms_by_robot;
@@ -23,7 +23,7 @@ namespace distributed_pcm {
 
  auto max_clique_info = executePCMCentralized(roboti, robotj, transforms_by_robot, separators_by_robot,
  separators_transforms_by_pair, dist_mappers,
- graph_and_values_vector, pcm_threshold);
+ graph_and_values_vector, pcm_threshold, use_heuristics);
 
  total_max_clique_sizes += max_clique_info.first;
  total_outliers_rejected += max_clique_info.second;
@@ -40,15 +40,16 @@ namespace distributed_pcm {
  robot_measurements::RobotLocalMap& robot_local_map,
  const graph_utils::Trajectory& other_robot_trajectory,
  const double& pcm_threshold,
- const bool& is_prior_added) {
+ const bool& is_prior_added, 
+ const bool& use_heuristics) {
 
  graph_utils::Transforms empty_transforms;
  auto other_robot_local_info = robot_measurements::RobotLocalMap(other_robot_trajectory, empty_transforms, robot_local_map.getLoopClosures());
 
  // Apply PCM for each pair of robots
  auto max_clique_info = executePCMDecentralized(other_robot_id, robot_local_map, other_robot_local_info,
  dist_mapper, local_graph_and_values,
- pcm_threshold, is_prior_added);
+ pcm_threshold, is_prior_added, use_heuristics);
 
  return max_clique_info;
  }
@@ -119,13 +120,14 @@ namespace distributed_pcm {
  const std::map<std::pair<char, char>,graph_utils::Transforms>& separators_transforms_by_pair,
  std::vector< boost::shared_ptr<distributed_mapper::DistributedMapper> >& dist_mappers,
  std::vector<gtsam::GraphAndValues>& graph_and_values_vector,
- const double& pcm_threshold){
+ const double& pcm_threshold,
+ const bool& use_heuristics){
  auto roboti_local_map = robot_measurements::RobotLocalMap(transforms_by_robot[roboti], separators_by_robot[roboti]);
  auto robotj_local_map = robot_measurements::RobotLocalMap(transforms_by_robot[robotj], separators_by_robot[robotj]);
  auto roboti_robotj_separators_transforms = separators_transforms_by_pair.at(std::make_pair(dist_mappers[roboti]->robotName(),dist_mappers[robotj]->robotName()));
  auto interrobot_measurements = robot_measurements::InterRobotMeasurements(roboti_robotj_separators_transforms, dist_mappers[roboti]->robotName(), dist_mappers[robotj]->robotName());
 
- auto global_map = global_map::GlobalMap(roboti_local_map, robotj_local_map, interrobot_measurements, pcm_threshold);
+ auto global_map = global_map::GlobalMap(roboti_local_map, robotj_local_map, interrobot_measurements, pcm_threshold, use_heuristics);
  auto max_clique_info = global_map.pairwiseConsistencyMaximization();
  std::vector<int> max_clique = max_clique_info.first;
 
@@ -174,7 +176,8 @@ namespace distributed_pcm {
  boost::shared_ptr<distributed_mapper::DistributedMapper>& dist_mapper,
  gtsam::GraphAndValues& local_graph_and_values,
  const double& pcm_threshold,
- const bool& is_prior_added){
+ const bool& is_prior_added,
+ const bool& use_heuristics){
 
  graph_utils::Transforms roboti_robotj_separators_transforms;
  for (const auto& transform : robot_local_map.getTransforms().transforms) {
@@ -188,7 +191,7 @@ namespace distributed_pcm {
  auto interrobot_measurements = robot_measurements::InterRobotMeasurements(roboti_robotj_separators_transforms, 
  dist_mapper->robotName(), ((char) other_robot_id + 97));
 
- auto global_map = global_map::GlobalMap(robot_local_map, other_robot_local_info, interrobot_measurements, pcm_threshold);
+ auto global_map = global_map::GlobalMap(robot_local_map, other_robot_local_info, interrobot_measurements, pcm_threshold, use_heuristics);
  auto max_clique_info = global_map.pairwiseConsistencyMaximization();
  std::vector<int> max_clique = max_clique_info.first;
 

cpp/src/pairwise_consistency_maximization/global_map/global_map.cpp

@@ -20,11 +20,12 @@ const std::string GlobalMap::CONSISTENCY_LOOP_CLOSURES_FILE_NAME = std::string(G
 GlobalMap::GlobalMap(const robot_measurements::RobotLocalMap& robot1_local_map,
  const robot_measurements::RobotLocalMap& robot2_local_map,
  const robot_measurements::RobotMeasurements& interrobot_measurements,
- const double& pcm_threshold):
+ const double& pcm_threshold,
+ const bool& use_heuristics):
  pairwise_consistency_(robot1_local_map.getTransforms(), robot2_local_map.getTransforms(), 
  interrobot_measurements.getTransforms(), interrobot_measurements.getLoopClosures(),
  robot1_local_map.getTrajectory(), robot2_local_map.getTrajectory(),
- robot1_local_map.getNbDegreeFreedom(), pcm_threshold){}
+ robot1_local_map.getNbDegreeFreedom(), pcm_threshold), use_heuristics_(use_heuristics) {}
 
 
 std::pair<std::vector<int>, int> GlobalMap::pairwiseConsistencyMaximization() {
@@ -39,6 +40,7 @@ std::pair<std::vector<int>, int> GlobalMap::pairwiseConsistencyMaximization() {
  gio.readGraph(consistency_matrix_file);
  int max_clique_size = 0;
  std::vector<int> max_clique_data;
+
  //max_clique_size = FMC::maxClique(gio, max_clique_size, max_clique_data);
  max_clique_size = FMC::maxCliqueHeu(gio, max_clique_data);
 

cpp/tests/test_distributed_mapper.cpp

@@ -83,7 +83,7 @@ distributedEstimation(size_t nrRobots, string dataPath, string traceFile, Values
  int max_clique_size = 0;
  vector< Values > estimates = distributedOptimizer(distMappers, maxIter, max_clique_size, DistributedMapper::incUpdate,
  gamma, rotationEstimateChangeThreshold, poseEstimateChangeThreshold,
- useFlaggedInit, false, false, contains_odometry, 0.99, false, true,
+ useFlaggedInit, false, false, contains_odometry, 0.99, false, true, true,
  boost::none, rotationTrace, poseTrace,
  subgraphRotationTrace, subgraphPoseTrace, rotationVectorValuesTrace);