| 155 | |
| 156 | namespace map_closures { |
| 157 | Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const double resolution) { |
| 158 | VoxelMap voxel_map(resolution, 100.0); |
| 159 | voxel_map.AddPoints(pointcloud); |
| 160 | const auto [voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal(); |
| 161 | |
| 162 | auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals); |
| 163 | if (ground_samples.empty()) return Eigen::Matrix4d::Identity(); |
| 164 | TransformPoints(T, ground_samples); |
| 165 | for (int iters = 0; iters < max_iterations; iters++) { |
| 166 | const auto [H, b] = BuildLinearSystem(ground_samples); |
| 167 | const Eigen::Vector3d dx = H.ldlt().solve(-b); |
| 168 | Eigen::Matrix<double, 6, 1> se3 = Eigen::Matrix<double, 6, 1>::Zero(); |
| 169 | se3 << 0.0, 0.0, dx.x(), dx.y(), dx.z(), 0.0; |
| 170 | const Sophus::SE3d estimation(Sophus::SE3d::exp(se3)); |
| 171 | TransformPoints(estimation, ground_samples); |
| 172 | T = estimation * T; |
| 173 | if (dx.norm() < convergence_threshold) break; |
| 174 | } |
| 175 | return T.matrix(); |
| 176 | } |
| 177 | } // namespace map_closures |
no test coverage detected