| 77 | max_distance_(max_distance) {} |
| 78 | |
| 79 | void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose) { |
| 80 | const Eigen::Matrix3d R = pose.block<3, 3>(0, 0); |
| 81 | const Eigen::Vector3d t = pose.block<3, 1>(0, 3); |
| 82 | std::vector<Eigen::Vector3d> points_transformed(points.size()); |
| 83 | std::transform(points.cbegin(), points.cend(), points_transformed.begin(), |
| 84 | [&](const auto &point) { return R * point + t; }); |
| 85 | AddPoints(points_transformed); |
| 86 | } |
| 87 | |
| 88 | void VoxelMap::AddPoints(const Vector3dVector &points) { |
| 89 | std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &point) { |