randomness in computing Covariance in dynamic initialization

[CanCanZeng]

Hi, I add some code here https://github.com/rpng/open_vins/blob/db026f3e68eea56b7e3d89daa0b5b4ddb12cd9e0/ov_init/src/dynamic/DynamicInitializer.cpp#L893 , I think if the distance between the first frame and last frame is too small, the motion might be degradated, so the initialization will be considered unusable.

      // Calculate the distance between the head and the tail.
      // If the distance is less than 1.5 meters, the parallax is not enough and initialization is not allowed
      int idx_b = map_states.begin()->second;
      int idx_e = map_states.rbegin()->second;
      double* pos_b = ceres_vars_pos.at(idx_b);
      double* pos_e = ceres_vars_pos.at(idx_e);
      Eigen::Vector3d delt = Eigen::Map<Eigen::Vector3d>(pos_e) - Eigen::Map<Eigen::Vector3d>(pos_b);
      std::cout << std::setprecision(18) << delt.transpose() << std::endl;
      double distance = delt.norm();
      constexpr double distance_threshold = 1.5;
      PRINT_WARNING(YELLOW "[init-d]: the distance is %.3f\n" RESET, distance);
      if (distance < distance_threshold) {
        PRINT_WARNING(YELLOW "[init-d]: the distance is %.3f < %.3f, intialization failed\n" RESET, distance, distance_threshold);
        return false;
      }

But after I add these code, the estimated trajectory becomes different every time. I find that all of the variables are the same between two runs execpt the problem_cov, I cannot understand how this come. The problem_cov is computed from covariance_blocks and problem, I checked that the covariance_blocks is always the same, and the optimized variables (including the states of imus and feature points) by problem are always the same too. Can you give me some advice on how to debug this problem? Thanks in advance!

[goldbattle]

I believe you originally brought up the distance problem in https://github.com/rpng/open_vins/issues/312. 1.5 meters is very very large. The system should be able to initialize with smaller movement then that.

If you make a pull request with a bit more thought out version of the code that you posted, I would be interested in merging it. You would want to read the threshold from the config files. Also you should be able to call the lambda get_pose() with the start and end times directly instead of using iterator logic.

As for the randomness, this can be because of multi-threading in both ceres and the feature tracking frontend. If you disable these and process the bag using serial, I believe the result should be the same. The actual covariance recovery we use relies on ceres, so the only difference one should expect is if different factors are being created (maybe check if the factors being appended are the same?). Hope this helps.

[CanCanZeng]

Thanks for your reply! Yes, I add these code since https://github.com/rpng/open_vins/issues/312. The randomness seems caused by the add code, the original version has no randomness since I set threads of tracking and ceres to be 1. That's why I'm very confused. I will make a pull request as soon as possible.

[CanCanZeng]

Hi, I made a PR just now https://github.com/rpng/open_vins/pull/330

[CanCanZeng]

The weird thing is that, if I set init_dyn_min_movement to 0, the results will always be the same, but if I set it to a larger number, say 1.5, the results will have silight difference. (I set init_dyn_mle_max_threads and num_opencv_threads to 1)

[goldbattle]

Thanks for opening, it will take me awhile to take a look. Not sure why there is randomness, but if you find out why please post.

[CanCanZeng]

I may have found the reason, it is due to a memory leak during dynamic initialization. I will update my RP right now

[goldbattle]

this should be closed in the latest master, feel free to reopen if you can reproduce with the master code.