Optimize use of STL library

cpp/map_closures/AlignRansac2D.cpp

@@ -55,7 +55,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D(
     Eigen::JacobiSVD<Eigen::Matrix2d> svd(covariance_matrix,
                                           Eigen::ComputeFullU | Eigen::ComputeFullV);
     Eigen::Isometry2d T = Eigen::Isometry2d::Identity();
-    const Eigen::Matrix2d &&R = svd.matrixV() * svd.matrixU().transpose();
+    const Eigen::Matrix2d &R = svd.matrixV() * svd.matrixU().transpose();
     T.linear() = R.determinant() > 0 ? R : -R;
     T.translation() = mean.query - R * mean.ref;
 
@@ -93,7 +93,7 @@ std::pair<Eigen::Isometry2d, std::size_t> RansacAlignment2D(
 
         std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), 2,
                     std::mt19937{std::random_device{}()});
-        auto T = KabschUmeyamaAlignment2D(sample_keypoint_pairs);
+        const auto &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs);
 
         int index = 0;
         std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(),
@@ -108,13 +108,13 @@ std::pair<Eigen::Isometry2d, std::size_t> RansacAlignment2D(
             optimal_inlier_indices = inlier_indices;
         }
     }
-
+    optimal_inlier_indices.shrink_to_fit();
     const std::size_t num_inliers = optimal_inlier_indices.size();
     std::vector<PointPair> inlier_keypoint_pairs(num_inliers);
     std::transform(optimal_inlier_indices.cbegin(), optimal_inlier_indices.cend(),
                    inlier_keypoint_pairs.begin(),
                    [&](const auto index) { return keypoint_pairs[index]; });
-    Eigen::Isometry2d T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs);
+    const Eigen::Isometry2d &T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs);
     return {T, num_inliers};
 }
 }  // namespace map_closures

cpp/map_closures/DensityMap.cpp

@@ -65,7 +65,7 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &pcd,
     };
     std::vector<Eigen::Array2i> pixels(pcd.size());
     std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) {
-        const auto pixel = Discretize2D(point);
+        const auto &pixel = Discretize2D(point);
         lower_bound_coordinates = lower_bound_coordinates.min(pixel);
         upper_bound_coordinates = upper_bound_coordinates.max(pixel);
         return pixel;

cpp/map_closures/GroundAlign.cpp

@@ -86,20 +86,19 @@ std::vector<Eigen::Vector3d> ComputeLowestPoints(const std::vector<Eigen::Vector
     };
 
     std::for_each(pointcloud.cbegin(), pointcloud.cend(), [&](const Eigen::Vector3d &point) {
-        auto pixel = PointToPixel(point);
+        const auto &pixel = PointToPixel(point);
         if (lowest_point_hash_map.find(pixel) == lowest_point_hash_map.cend()) {
             if (point.z() < 0) {
-                lowest_point_hash_map.insert({pixel, point});
+                lowest_point_hash_map.emplace(pixel, point);
             }
         } else if (point.z() < lowest_point_hash_map[pixel].z()) {
             lowest_point_hash_map[pixel] = point;
         }
     });
 
-    std::vector<Eigen::Vector3d> low_lying_points;
-    low_lying_points.reserve(lowest_point_hash_map.size());
-    std::for_each(lowest_point_hash_map.cbegin(), lowest_point_hash_map.cend(),
-                  [&](const auto &entry) { low_lying_points.emplace_back(entry.second); });
+    std::vector<Eigen::Vector3d> low_lying_points(lowest_point_hash_map.size());
+    std::transform(lowest_point_hash_map.cbegin(), lowest_point_hash_map.cend(),
+                   low_lying_points.begin(), [](const auto &entry) { return entry.second; });
     return low_lying_points;
 }
 }  // namespace
@@ -112,7 +111,7 @@ Eigen::Matrix4d AlignToLocalGround(const std::vector<Eigen::Vector3d> &pointclou
 
     for (int iters = 0; iters < max_iterations; iters++) {
         const auto &[H, b] = BuildLinearSystem(low_lying_points, resolution);
-        const Eigen::Vector3d dx = H.ldlt().solve(-b);
+        const Eigen::Vector3d &dx = H.ldlt().solve(-b);
         Eigen::Matrix<double, 6, 1> se3 = Eigen::Matrix<double, 6, 1>::Zero();
         se3.block<3, 1>(2, 0) = dx;
         Sophus::SE3d estimation(Sophus::SE3d::exp(se3));

cpp/map_closures/MapClosures.cpp

@@ -69,17 +69,20 @@ MapClosures::MapClosures(const Config &config) : config_(config) {
 
 void MapClosures::MatchAndAddToDatabase(const int id,
                                         const std::vector<Eigen::Vector3d> &local_map) {
-    Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, ground_alignment_resolution);
+    const Eigen::Matrix4d &T_ground = AlignToLocalGround(local_map, ground_alignment_resolution);
     DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution,
                                                 config_.density_threshold);
     cv::Mat orb_descriptors;
     std::vector<cv::KeyPoint> orb_keypoints;
+    orb_keypoints.reserve(nfeatures);
     orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints,
                                      orb_descriptors);
+    orb_keypoints.shrink_to_fit();
 
-    auto matcher = cv::BFMatcher(cv::NORM_HAMMING);
-    std::vector<std::vector<cv::DMatch>> bf_matches;
-    matcher.knnMatch(orb_descriptors, orb_descriptors, bf_matches, 2);
+    const auto self_matcher = cv::BFMatcher(cv::NORM_HAMMING);
+    std::vector<std::vector<cv::DMatch>> self_matches;
+    self_matches.reserve(orb_keypoints.size());
+    self_matcher.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2);
 
     std::for_each(orb_keypoints.begin(), orb_keypoints.end(), [&](cv::KeyPoint &keypoint) {
         keypoint.pt.x = keypoint.pt.x + static_cast<float>(density_map.lower_bound.y());
@@ -90,10 +93,10 @@ void MapClosures::MatchAndAddToDatabase(const int id,
 
     std::vector<Matchable *> hbst_matchable;
     hbst_matchable.reserve(orb_descriptors.rows);
-    std::for_each(bf_matches.cbegin(), bf_matches.cend(), [&](const auto &bf_match) {
-        if (bf_match[1].distance > self_similarity_threshold) {
-            auto index_descriptor = bf_match[0].queryIdx;
-            auto keypoint = orb_keypoints[index_descriptor];
+    std::for_each(self_matches.cbegin(), self_matches.cend(), [&](const auto &self_match) {
+        if (self_match[1].distance > self_similarity_threshold) {
+            const auto index_descriptor = self_match[0].queryIdx;
+            const auto &keypoint = orb_keypoints[index_descriptor];
             hbst_matchable.emplace_back(
                 new Matchable(keypoint, orb_descriptors.row(index_descriptor), id));
         }
@@ -169,20 +172,18 @@ std::vector<ClosureCandidate> MapClosures::GetTopKClosures(
                   [&](const auto &descriptor_match) {
                       const auto ref_id = static_cast<int>(descriptor_match.first);
                       if (is_far_enough(ref_id, query_id)) {
-                          ClosureCandidate closure =
-                              ValidateClosure(descriptor_match.first, query_id);
+                          const ClosureCandidate &closure = ValidateClosure(ref_id, query_id);
                           if (closure.number_of_inliers > min_no_of_matches) {
                               closures.emplace_back(closure);
                           }
                       }
                   });
-    if (k == -1) return closures;
-
-    std::vector<ClosureCandidate> top_k_closures;
-    top_k_closures.reserve(std::min(k, static_cast<int>(closures.size())));
-    std::sort(closures.begin(), closures.end(), compare_closure_candidates);
-    std::copy_n(closures.cbegin(), std::min(k, static_cast<int>(closures.size())),
-                std::back_inserter(top_k_closures));
-    return top_k_closures;
+    closures.shrink_to_fit();
+
+    if (k != -1) {
+        std::sort(closures.begin(), closures.end(), compare_closure_candidates);
+        closures.resize(std::min(k, static_cast<int>(closures.size())));
+    }
+    return closures;
 }
 }  // namespace map_closures