search

MIT-SPARK / Kimera-RPGO

Robust Pose Graph Optimization
BSD 2-Clause "Simplified" License
474 stars 133 forks source link

bad results or wrong config with the optimized G2o #84

Closed JokerJohn closed 1 year ago

JokerJohn commented 1 year ago

Hi，Yun Chang！ Thanks for your great work. I have not carefully read the algorithm principles of related papers, but I want to quickly apply it to my LIO SLAM system. The following three pictures are my scene (single robot with many closed loops in the room), the original pose grpah, and the optimized pose graph using this cpp(RpgoReadG2oIncremental.cpp). As can be seen in the last picture, the optimized G2o has been completely biased. Could you please help me analyze the reason for this result?

image

image

image

here is my config.

 gtsam::NonlinearFactorGraph nfg = *gv.first;
  gtsam::Values values = *gv.second;
  std::cout << "factor size: " << nfg.size() << std::endl;

  if (useRPGO) {
    std::cout << "SET RPGO PARAMS!!!!" << std::endl;
    rpgoParams.setIncremental();
    rpgoParams.setPcm3DParams(-1, 1.0, Verbosity::VERBOSE);
    rpgoParams.setMultiRobotAlignMethod(MultiRobotAlignMethod::NONE);
    rpgo = KimeraRPGO::make_unique<RobustSolver>(rpgoParams);

    gtsam::Key current_key = nfg[0]->front();
    std::cout << "current key " << current_key << std::endl;
    gtsam::Values init_values;  // add first value with prior factor
    gtsam::NonlinearFactorGraph init_factors;
    init_values.insert(current_key, values.at<Pose3>(current_key));
    gtsam::PriorFactor<Pose3> prior_factor(
        current_key, values.at<Pose3>(current_key), noise_init_pose_prior);
    nfg.add(prior_factor);

    // separate to non loop closures and loop closure factors
    gtsam::NonlinearFactorGraph non_lc_factors, lc_factors;
    int i = 0, j = 0, k = 0;
    for (auto factor : nfg) {
      if (boost::dynamic_pointer_cast<gtsam::BetweenFactor<Pose3>>(factor)) {
        // specifically what outlier rejection handles
        gtsam::Key from_key = factor->front();
        gtsam::Key to_key = factor->back();
        if (from_key + 1 == to_key) {
          non_lc_factors.add(factor);  // odometry
          i++;
        } else {
          lc_factors.add(factor);  // loop closure
          j++;
        }
      } else {
        non_lc_factors.add(factor);  // not between so not lc
        k++;
      }
    }

    std::cout << "factor size: " << non_lc_factors.size() << std::endl;
    std::cout << "odom loop non-loc size: " << i << " " << j << " " << k
              << std::endl;

    // add non lc factors first
    rpgo->update(non_lc_factors, values);

    // Now add loop closure one by one
    for (auto loop_closure : lc_factors) {
      gtsam::NonlinearFactorGraph new_factors;
      new_factors.add(loop_closure);
      rpgo->update(new_factors, gtsam::Values(), false);
    }

    rpgo->saveData(
        dataSaverPtr->save_directory);  // tell pgo to save g2o result

here is my output, i use std::cout insted of the log function for some environmental issues.

 loop closure between keys %1% and %2% 1542 552
 total loop closures registered: %1% 188
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 136s 188 119
 loop closure between keys %1% and %2% 1544 554
 total loop closures registered: %1% 189
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 137s 189 119
 loop closure between keys %1% and %2% 1544 554
 total loop closures registered: %1% 190
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 140s 190 119
.....
....
.....
....
 loop closure between keys %1% and %2% 1610 621
 total loop closures registered: %1% 256
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 183s 256 119
 loop closure between keys %1% and %2% 1610 622
 total loop closures registered: %1% 257
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 194s 257 119
SET RPGO PARAMS SUCCESS!!!!

thanks a lot!!!!!!!!!!!!!!!!!!!!!!!!!!!

yunzc commented 1 year ago

Hi @JokerJohn I'm actually a little confused be the first and last picture, what should the correct (gt) trajectory look like? A few quick questions:

  1. If you set rpgoParams.setPcm3DParams(-1, -1, Verbosity::VERBOSE); do you get as result the odometry only result?
  2. Do you have good covariances for your factors? If not, I would recommend using setPcmSimple3DParams instead, since the threshold would be easier to tune (bound the avg error per node instead of the mahalanobis distance)

Thanks for your interest in our work!

JokerJohn commented 1 year ago

Hi @JokerJohn I'm actually a little confused be the first and last picture, what should the correct (gt) trajectory look like? A few quick questions:

  1. If you set rpgoParams.setPcm3DParams(-1, -1, Verbosity::VERBOSE); do you get as result the odometry only result?
  2. Do you have good covariances for your factors? If not, I would recommend using setPcmSimple3DParams instead, since the threshold would be easier to tune (bound the avg error per node instead of the mahalanobis distance)

Thanks for your interest in our work!

Hi, @yunzc , Thanks for your quick reply!! The folloing pictures show my whole trajectories of SLAM system(almost the same as GT trajectories), since we have a very small and rich-feature scene. The previous 2 pictures you mentioned before are only parts of GT trajectopries.

image

image

1.when i set set rpgoParams.setPcm3DParams(-1, -1, Verbosity::VERBOSE);, i nearly got the same bad results. The trajectory of the odometer is severely pulled by the closed-loop constraints.

2.i do not have good cov, since i just use constant value for odometry and loop noise model. or the cov is simply set as (icp_score, icp_score, icp_score, icp_score*1e2, icp_score*1e2,icp_score*1e2) for (x,y,z,roll, pitch,yaw), the icp_score is equal to the rmse of icp corressondens inliers, the score value is nearly 0.2.

// between facvtor from icp loop detection
  Vector6 precious_loop; 
  precious_loop.head<3>().setConstant(loopNoiseScore * 1e-2); // loopNoiseScore nearly 0.2
  precious_loop.tail<3>().setConstant(loopNoiseScore * 1e-1);
  robustLoopNoise = gtsam::noiseModel::Diagonal::Variances(precious_loop);
    gtsam::Pose3 poseBetween = loopPoseQueue[i];
    gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
    gtSAMgraph.add(BetweenFactor<Pose3>(X(indexFrom), X(indexTo), poseBetween,
                                        noiseBetween));

// between factor from odom
  Vector6 precisions_odom;
  precisions_odom.head<3>().setConstant(1e-6);
  precisions_odom.tail<3>().setConstant(1e-4);
  odomNoise = noiseModel::Diagonal::Variances(precisions_odom);
    gtSAMgraph.add(BetweenFactor<Pose3>(X(prev_node_idx), X(curr_node_idx),
                                        poseFrom.between(poseTo), odomNoise));

// global  pose constraint from other sensors
 double prior_score = keyMeasures.at(curr_node_idx).global_score; // nearly 0.2
      Vector6 precisions;
      precisions.head<3>().setConstant(prior_score * 1e-4);
      precisions.tail<3>().setConstant(prior_score * 1e-2);
      SharedNoiseModel globalNoise =
          noiseModel::Diagonal::Variances(precisions);
      gtSAMgraph.add(
          PriorFactor<Pose3>(X(curr_node_idx), poseGlobal, globalNoise));

when i apply the following config(setPcmSimple3DParams)

 gtsam::GraphAndValues gv = gtsam::load3D(
      std::string(dataSaverPtr->save_directory) + "pose_graph.g2o");
  gtsam::NonlinearFactorGraph nfg = *gv.first;
  gtsam::Values values = *gv.second;

  if (useRPGO) {
    rpgoParams.setIncremental();
    rpgoParams.setPcmSimple3DParams(0.1, 0.01, Verbosity::VERBOSE);

    rpgo = KimeraRPGO::make_unique<RobustSolver>(rpgoParams);

    gtsam::Key current_key = nfg[0]->front();
    std::cout << "current key " << current_key << std::endl;
    gtsam::Values init_values;  // add first value with prior factor
    gtsam::NonlinearFactorGraph init_factors;
    init_values.insert(current_key, values.at<Pose3>(current_key));
    gtsam::PriorFactor<Pose3> prior_factor(
        current_key, values.at<Pose3>(current_key), noise_init_pose_prior);
    nfg.add(prior_factor);

    // separate to non loop closures and loop closure factors
    gtsam::NonlinearFactorGraph non_lc_factors, lc_factors;
    int i = 0, j = 0, k = 0;
    for (auto factor : nfg) {
      if (boost::dynamic_pointer_cast<gtsam::BetweenFactor<Pose3>>(factor)) {
        // specifically what outlier rejection handles
        gtsam::Key from_key = factor->front();
        gtsam::Key to_key = factor->back();
        if (from_key + 1 == to_key) {
          non_lc_factors.add(factor);  // odometry
          i++;
        } else {
          lc_factors.add(factor);  // loop closure
          j++;
        }
      } else {
        non_lc_factors.add(factor);  // not between so not lc
        k++;
      }
    }

    // add non lc factors first
    rpgo->update(non_lc_factors, values);

    // Now add loop closure one by one
    for (auto loop_closure : lc_factors) {
      gtsam::NonlinearFactorGraph new_factors;
      new_factors.add(loop_closure);
      rpgo->update(new_factors, gtsam::Values(), false);
    }

    rpgo->saveData(
        dataSaverPtr->save_directory);  // tell pgo to save g2o result
  }

I got the output log from the terminal. it cost me 3662s for each loop pairs.

....
.....
PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 3689s 455 455
odometry consistency translation distance: %1% 0.0183759
odometry consistency rotation distance: %1% 0.00254755
 loop closure between keys %1% and %2% 1864 1152
 total loop closures registered: %1% 456
number of inliers: %1% 456
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 4255s 456 456
odometry consistency translation distance: %1% 0.0184158
odometry consistency rotation distance: %1% 0.00254964
 loop closure between keys %1% and %2% 1864 1151
 total loop closures registered: %1% 457
.....
....

the result graph show as follows, (the test trajctory is only part of the whole GT trajectory but have encough loop edges.) image

my g2o file is here, download link (password:1) my test.cpp download link(password:1)

yunzc commented 1 year ago

@JokerJohn

  1. Sorry, my mistake, I meant to say rpgoParams.setPcm3DParams(0.0, 0.0, Verbosity::VERBOSE); which should reject all the loop closures and give you the odometry result. Let me know if this works! Setting it to -1 simply disable outlier rejection. The fact that rpgoParams.setPcm3DParams(-1, -1, Verbosity::VERBOSE); gave you the same bad results indicates that PCM is not currently doing much in terms of rejecting outliers.
  2. I would recommend either update after you loaded all the loop closures so that it's faster (performs the max clique search only once) 
    for (auto loop_closure : lc_factors) {
gtsam::NonlinearFactorGraph new_factors;
new_factors.add(loop_closure);
}
rpgo->update(new_factors, gtsam::Values(), false);

    Or you can try the incremental flag which should also speed up operation (though still slower then loading all the loop closures at once). Based on the output you posted above seems like with setPcmSimple3DParams PCM is still not rejecting any loop closures and the number of loop closures and inliers are the same. Maybe you want to try set a tighter threshold? Alternatively, you can try GNC, which I have found tends to work a lot better than PCM. See example here and sample parameters.

  3. Are you using Levenberg-Marquardt? If so, can you show the output from that?