Question about the querying efficiency.

Dear Professor, Thank you for your wonderful work on LiDAR-Odometry. One of the main contributions of this paper is its focus on improving efficiency. From my point of view, the time costs in LiDAR-Odometry primarily involve two parts: building the KD-Tree and conducting searches that rely on the pre-built KD-Tree. The purposed recycling datasture can efficiently resolve the aforementioned question, while the latter one can be solved by using the OpenMP for parallel acceleration. However, when I split out the covariance calculation into a standalone project, its performance(5-25ms) was much slower compared to the same procedure in the direct-lidar-odometry(2-8ms). I don't know what went wrong. Could you please help me check if there is any mistake in my code or if there are any tricks that I might have missed? The code is listed below, thank you very much.

  auto cov_start = omp_get_wtime();
#pragma omp parallel for num_threads(16) schedule(guided, 8)
  for (int i = 0; i < current_features->points.size(); i++) {
    std::vector<int> k_indices;
    std::vector<float> k_sq_distances;
    frame_kdtree_->nearestKSearch(current_features->at(i), k_correspondences_, k_indices, k_sq_distances);
    Eigen::Matrix<double, 4, -1> neighbors(4, k_correspondences_);
    for(int j = 0; j < k_indices.size(); ++j){
      neighbors.col(j) = current_features->at(k_indices[j]).getVector4fMap().template cast<double>();
    }
    neighbors.colwise() -= neighbors.rowwise().mean().eval();
    Eigen::Matrix4d cov = neighbors * neighbors.transpose() / k_correspondences_;
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector3d values = Eigen::Vector3d(1, 1, 1e-3);
    covariances[i].setZero();
    covariances[i].template block<3, 3>(0, 0) = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();
  }
  auto cov_end = omp_get_wtime();
  double cov_duration = (cov_end - cov_start) * 1000;
  LOG(INFO) << "The time of covariance calculation is: " << cov_duration << " ms.";

vectr-ucla / direct_lidar_odometry

Question about the querying efficiency. #30