Add a cost function to take intensities into account. (#1760)

Adds (optional) intensities to sensor::PointCloud and
uses it in a cost function. Not yet in use.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-10-14 11:36:47 +02:00 committed by GitHub
parent ee98a92845
commit cad3378929
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 205 additions and 1 deletions

View File

@ -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

View File

@ -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 <typename T>
bool operator()(const T* const translation, const T* const rotation,
T* const residual) const {
const transform::Rigid3<T> transform(
Eigen::Map<const Eigen::Matrix<T, 3, 1>>(translation),
Eigen::Quaternion<T>(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 <typename T>
bool Evaluate(const transform::Rigid3<T>& 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<T, 3, 1> point = point_cloud_[i].position.cast<T>();
const T intensity = T(point_cloud_.intensities()[i]);
const Eigen::Matrix<T, 3, 1> 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_

View File

@ -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 <array>
#include <memory>
#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<ceres::CostFunction> cost_function(
IntensityCostFunction3D::CreateAutoDiffCostFunction(
/*scaling_factor=*/1.0f, /*intensity_threshold=*/100.f, point_cloud,
hybrid_grid));
const std::array<double, 3> translation{{0., 0., 0.}};
const std::array<double, 4> rotation{{1., 0., 0., 0.}};
const std::array<const double*, 2> parameter_blocks{
{translation.data(), rotation.data()}};
std::array<double, 3> 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

View File

@ -25,6 +25,13 @@ namespace sensor {
PointCloud::PointCloud() {} PointCloud::PointCloud() {}
PointCloud::PointCloud(std::vector<PointCloud::PointType> points) PointCloud::PointCloud(std::vector<PointCloud::PointType> points)
: points_(std::move(points)) {} : points_(std::move(points)) {}
PointCloud::PointCloud(std::vector<PointType> points,
std::vector<float> 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(); } size_t PointCloud::size() const { return points_.size(); }
bool PointCloud::empty() const { return points_.empty(); } bool PointCloud::empty() const { return points_.empty(); }
@ -32,6 +39,9 @@ bool PointCloud::empty() const { return points_.empty(); }
const std::vector<PointCloud::PointType>& PointCloud::points() const { const std::vector<PointCloud::PointType>& PointCloud::points() const {
return points_; return points_;
} }
const std::vector<float>& PointCloud::intensities() const {
return intensities_;
}
const PointCloud::PointType& PointCloud::operator[](const size_t index) const { const PointCloud::PointType& PointCloud::operator[](const size_t index) const {
return points_[index]; return points_[index];
} }

View File

@ -30,12 +30,13 @@ namespace sensor {
// Stores 3D positions of points together with some additional data, e.g. // Stores 3D positions of points together with some additional data, e.g.
// intensities. // intensities.
struct PointCloud { class PointCloud {
public: public:
using PointType = RangefinderPoint; using PointType = RangefinderPoint;
PointCloud(); PointCloud();
explicit PointCloud(std::vector<PointType> points); explicit PointCloud(std::vector<PointType> points);
PointCloud(std::vector<PointType> points, std::vector<float> intensities);
// Returns the number of points in the point cloud. // Returns the number of points in the point cloud.
size_t size() const; size_t size() const;
@ -43,6 +44,7 @@ struct PointCloud {
bool empty() const; bool empty() const;
const std::vector<PointType>& points() const; const std::vector<PointType>& points() const;
const std::vector<float>& intensities() const;
const PointType& operator[](const size_t index) const; const PointType& operator[](const size_t index) const;
// Iterator over the points in the point cloud. // Iterator over the points in the point cloud.
@ -55,6 +57,9 @@ struct PointCloud {
private: private:
// For 2D points, the third entry is 0.f. // For 2D points, the third entry is 0.f.
std::vector<PointType> points_; std::vector<PointType> points_;
// Intensities are optional. If non-empty, they must have the same size as
// points.
std::vector<float> intensities_;
}; };
// Stores 3D positions of points with their relative measurement time in the // Stores 3D positions of points with their relative measurement time in the