-
Notifications
You must be signed in to change notification settings - Fork 2.2k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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>
- Loading branch information
Showing
5 changed files
with
205 additions
and
1 deletion.
There are no files selected for viewing
25 changes: 25 additions & 0 deletions
25
cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
98 changes: 98 additions & 0 deletions
98
cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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_ |
66 changes: 66 additions & 0 deletions
66
cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters