diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc new file mode 100644 index 0000000..fe16ff5 --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc @@ -0,0 +1,25 @@ +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// This method is defined here instead of the header file as it was observed +// that defining it in the header file has a negative impact on the runtime +// performance. +ceres::CostFunction* IntensityCostFunction3D::CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) { + CHECK(!point_cloud.intensities().empty()); + return new ceres::AutoDiffCostFunction< + IntensityCostFunction3D, ceres::DYNAMIC /* residuals */, + 3 /* translation variables */, 4 /* rotation variables */>( + new IntensityCostFunction3D(scaling_factor, intensity_threshold, + point_cloud, hybrid_grid), + point_cloud.size()); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h new file mode 100644 index 0000000..59f1c29 --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h @@ -0,0 +1,98 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a +// 'translation' and 'rotation'. The cost increases when points fall into space +// for which different intensity has been observed, i.e. at voxels with different +// values. Only points up to a certain threshold are evaluated which is intended +// to ignore data from retroreflections. +class IntensityCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid); + + template + bool operator()(const T* const translation, const T* const rotation, + T* const residual) const { + const transform::Rigid3 transform( + Eigen::Map>(translation), + Eigen::Quaternion(rotation[0], rotation[1], rotation[2], + rotation[3])); + return Evaluate(transform, residual); + } + + private: + IntensityCostFunction3D(const double scaling_factor, + const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) + : scaling_factor_(scaling_factor), + intensity_threshold_(intensity_threshold), + point_cloud_(point_cloud), + interpolated_grid_(hybrid_grid) {} + + IntensityCostFunction3D(const IntensityCostFunction3D&) = delete; + IntensityCostFunction3D& operator=(const IntensityCostFunction3D&) = delete; + + template + bool Evaluate(const transform::Rigid3& transform, + T* const residual) const { + for (size_t i = 0; i < point_cloud_.size(); ++i) { + if (point_cloud_.intensities()[i] > intensity_threshold_) { + residual[i] = T(0.f); + } else { + const Eigen::Matrix point = point_cloud_[i].position.cast(); + const T intensity = T(point_cloud_.intensities()[i]); + + const Eigen::Matrix world = transform * point; + const T interpolated_intensity = + interpolated_grid_.GetInterpolatedValue(world[0], world[1], + world[2]); + residual[i] = scaling_factor_ * (interpolated_intensity - intensity); + } + } + return true; + } + + const double scaling_factor_; + // We will ignore returns with intensity above this threshold. + const float intensity_threshold_; + const sensor::PointCloud& point_cloud_; + const InterpolatedIntensityGrid interpolated_grid_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc new file mode 100644 index 0000000..90fc8c4 --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc @@ -0,0 +1,66 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" + +#include +#include + +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +using ::testing::DoubleNear; +using ::testing::ElementsAre; + +TEST(IntensityCostFunction3DTest, SmokeTest) { + const sensor::PointCloud point_cloud( + {{{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}, + {50.f, 100.f, 150.f}); + IntensityHybridGrid hybrid_grid(0.3f); + hybrid_grid.AddIntensity(hybrid_grid.GetCellIndex({0.f, 0.f, 0.f}), 50.f); + + std::unique_ptr cost_function( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + /*scaling_factor=*/1.0f, /*intensity_threshold=*/100.f, point_cloud, + hybrid_grid)); + + const std::array translation{{0., 0., 0.}}; + const std::array rotation{{1., 0., 0., 0.}}; + + const std::array parameter_blocks{ + {translation.data(), rotation.data()}}; + std::array residuals; + + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + /*jacobians=*/nullptr); + EXPECT_THAT(residuals, + ElementsAre(DoubleNear(0., 1e-9), DoubleNear(-100., 1e-9), + DoubleNear(0., 1e-9))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 01a1416..5845f23 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -25,6 +25,13 @@ namespace sensor { PointCloud::PointCloud() {} PointCloud::PointCloud(std::vector points) : points_(std::move(points)) {} +PointCloud::PointCloud(std::vector points, + std::vector intensities) + : points_(std::move(points)), intensities_(std::move(intensities)) { + if (!intensities_.empty()) { + CHECK_EQ(points_.size(), intensities_.size()); + } +} size_t PointCloud::size() const { return points_.size(); } bool PointCloud::empty() const { return points_.empty(); } @@ -32,6 +39,9 @@ bool PointCloud::empty() const { return points_.empty(); } const std::vector& PointCloud::points() const { return points_; } +const std::vector& PointCloud::intensities() const { + return intensities_; +} const PointCloud::PointType& PointCloud::operator[](const size_t index) const { return points_[index]; } diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index f20307f..19a40aa 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -30,12 +30,13 @@ namespace sensor { // Stores 3D positions of points together with some additional data, e.g. // intensities. -struct PointCloud { +class PointCloud { public: using PointType = RangefinderPoint; PointCloud(); explicit PointCloud(std::vector points); + PointCloud(std::vector points, std::vector intensities); // Returns the number of points in the point cloud. size_t size() const; @@ -43,6 +44,7 @@ struct PointCloud { bool empty() const; const std::vector& points() const; + const std::vector& intensities() const; const PointType& operator[](const size_t index) const; // Iterator over the points in the point cloud. @@ -55,6 +57,9 @@ struct PointCloud { private: // For 2D points, the third entry is 0.f. std::vector points_; + // Intensities are optional. If non-empty, they must have the same size as + // points. + std::vector intensities_; }; // Stores 3D positions of points with their relative measurement time in the