diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 729bd92..8ff28ff 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -55,7 +55,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D( Eigen::JacobiSVD 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 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 RansacAlignment2D( optimal_inlier_indices = inlier_indices; } } - + optimal_inlier_indices.shrink_to_fit(); const std::size_t num_inliers = optimal_inlier_indices.size(); std::vector 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 diff --git a/cpp/map_closures/DensityMap.cpp b/cpp/map_closures/DensityMap.cpp index 6477ac8..aa95271 100644 --- a/cpp/map_closures/DensityMap.cpp +++ b/cpp/map_closures/DensityMap.cpp @@ -65,7 +65,7 @@ DensityMap GenerateDensityMap(const std::vector &pcd, }; std::vector 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; diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 9fde229..be65d78 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -86,20 +86,19 @@ std::vector ComputeLowestPoints(const std::vector 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 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 &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 se3 = Eigen::Matrix::Zero(); se3.block<3, 1>(2, 0) = dx; Sophus::SE3d estimation(Sophus::SE3d::exp(se3)); diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 7d098c5..01062eb 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/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 &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 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> bf_matches; - matcher.knnMatch(orb_descriptors, orb_descriptors, bf_matches, 2); + const auto self_matcher = cv::BFMatcher(cv::NORM_HAMMING); + std::vector> 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(density_map.lower_bound.y()); @@ -90,10 +93,10 @@ void MapClosures::MatchAndAddToDatabase(const int id, std::vector 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 MapClosures::GetTopKClosures( [&](const auto &descriptor_match) { const auto ref_id = static_cast(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 top_k_closures; - top_k_closures.reserve(std::min(k, static_cast(closures.size()))); - std::sort(closures.begin(), closures.end(), compare_closure_candidates); - std::copy_n(closures.cbegin(), std::min(k, static_cast(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(closures.size()))); + } + return closures; } } // namespace map_closures