12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add TSDF interpolator. (#1274) 

Based on [interpolated_grid.h](https://github.com/googlecartographer/cartographer/blob/master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h).
master
Kevin Daun 2018-07-13 13:47:09 +02:00 committed by Wally B. Feed
parent 8c9104568a
commit a08037dc47
2 changed files with 263 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h Normal file
View File

@ -0,0 +1,136 @@
/*
* Copyright 2018 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_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_
#include <cmath>
#include "cartographer/mapping/2d/tsdf_2d.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
// Interpolates between TSDF2D pixels with bilinear interpolation.
//
// This class is templated to work with Ceres autodiff.
class InterpolatedTSDF2D {
public:
explicit InterpolatedTSDF2D(const TSDF2D& grid) : tsdf_(grid) {}
InterpolatedTSDF2D(const InterpolatedTSDF2D&) = delete;
InterpolatedTSDF2D& operator=(const InterpolatedTSDF2D&) = delete;
// Returns the interpolated correspondence cost at (x,y). Cells with at least
// one 'unknown' interpolation point result in "MaxCorrespondenceCost()" with
// zero gradient.
template <typename T>
T GetCorrespondenceCost(const T& x, const T& y) const {
float x1, y1, x2, y2;
ComputeInterpolationDataPoints(x, y, &x1, &y1, &x2, &y2);
const Eigen::Array2i index1 =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1));
const float w11 = tsdf_.GetWeight(index1);
const float w12 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, 0));
const float w21 = tsdf_.GetWeight(index1 + Eigen::Array2i(0, -1));
const float w22 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, -1));
if (w11 == 0.0 || w12 == 0.0 || w21 == 0.0 || w22 == 0.0) {
return T(tsdf_.GetMaxCorrespondenceCost());
}
const float q11 = tsdf_.GetCorrespondenceCost(index1);
const float q12 =
tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(-1, 0));
const float q21 =
tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(0, -1));
const float q22 =
tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(-1, -1));
return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22);
}
// Returns the interpolated weight at (x,y).
template <typename T>
T GetWeight(const T& x, const T& y) const {
float x1, y1, x2, y2;
ComputeInterpolationDataPoints(x, y, &x1, &y1, &x2, &y2);
const Eigen::Array2i index1 =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1));
const float q11 = tsdf_.GetWeight(index1);
const float q12 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, 0));
const float q21 = tsdf_.GetWeight(index1 + Eigen::Array2i(0, -1));
const float q22 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, -1));
return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22);
}
private:
template <typename T>
void ComputeInterpolationDataPoints(const T& x, const T& y, float* x1,
float* y1, float* x2, float* y2) const {
const Eigen::Vector2f lower = CenterOfLowerPixel(x, y);
*x1 = lower.x();
*y1 = lower.y();
*x2 = lower.x() + static_cast<float>(tsdf_.limits().resolution());
*y2 = lower.y() + static_cast<float>(tsdf_.limits().resolution());
}
template <typename T>
T InterpolateBilinear(const T& x, const T& y, float x1, float y1, float x2,
float y2, float q11, float q12, float q21,
float q22) const {
const T normalized_x = (x - x1) / (x2 - x1);
const T normalized_y = (y - y1) / (y2 - y1);
const T q1 = (q12 - q11) * normalized_y + q11;
const T q2 = (q22 - q21) * normalized_y + q21;
return (q2 - q1) * normalized_x + q1;
}
// Center of the next lower pixel, i.e., not necessarily the pixel containing
// (x, y). For each dimension, the largest pixel index so that the
// corresponding center is at most the given coordinate.
Eigen::Vector2f CenterOfLowerPixel(double x, double y) const {
// Center of the cell containing (x, y).
Eigen::Vector2f center = tsdf_.limits().GetCellCenter(
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)));
// Move to the next lower pixel center.
if (center.x() > x) {
center.x() -= tsdf_.limits().resolution();
}
if (center.y() > y) {
center.y() -= tsdf_.limits().resolution();
}
return center;
}
// Uses the scalar part of a Ceres Jet.
template <typename T>
Eigen::Vector2f CenterOfLowerPixel(const T& jet_x, const T& jet_y) const {
return CenterOfLowerPixel(jet_x.a, jet_y.a);
}
const TSDF2D& tsdf_;
};
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_

127
cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc Normal file
View File

@ -0,0 +1,127 @@
/*
* Copyright 2018 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/2d/scan_matching/interpolated_tsdf_2d.h"
#include "Eigen/Core"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
namespace {
class InterpolatedTSDF2DTest : public ::testing::Test {
protected:
InterpolatedTSDF2DTest()
: tsdf_(MapLimits(1., Eigen::Vector2d(5.5, 5.5), CellLimits(10, 10)),
1.0f, 10.0f, &conversion_tables_),
interpolated_tsdf_(tsdf_) {}
float GetUninterpolatedCorrespondenceCost(float x, float y) const {
return tsdf_.GetCorrespondenceCost(
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)));
}
float GetUninterpolatedWeight(float x, float y) const {
return tsdf_.GetWeight(tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)));
}
ValueConversionTables conversion_tables_;
TSDF2D tsdf_;
InterpolatedTSDF2D interpolated_tsdf_;
};
TEST_F(InterpolatedTSDF2DTest, InterpolatesGridPoints) {
std::vector<Eigen::Vector2f> inner_points = {
Eigen::Vector2f(1.f, 1.f), Eigen::Vector2f(2.f, 1.f),
Eigen::Vector2f(1.f, 2.f), Eigen::Vector2f(2.f, 2.f)};
for (const Eigen::Vector2f& point : inner_points) {
tsdf_.SetCell(tsdf_.limits().GetCellIndex(point), 0.1f, 1.0f);
}
// Insert surrounding points.
for (size_t x = 0; x < 4; ++x) {
tsdf_.SetCell(tsdf_.limits().GetCellIndex(
Eigen::Vector2f(static_cast<float>(x), 0.f)),
0.1f, 1.0f);
tsdf_.SetCell(tsdf_.limits().GetCellIndex(
Eigen::Vector2f(static_cast<float>(x), 3.f)),
0.1f, 1.0f);
}
for (size_t y = 1; y < 3; ++y) {
tsdf_.SetCell(tsdf_.limits().GetCellIndex(
Eigen::Vector2f(0.f, static_cast<float>(y))),
0.1f, 1.0f);
tsdf_.SetCell(tsdf_.limits().GetCellIndex(
Eigen::Vector2f(3.f, static_cast<float>(y))),
0.1f, 1.0f);
}
for (const Eigen::Vector2f& point : inner_points) {
EXPECT_NEAR(
GetUninterpolatedCorrespondenceCost(point[0], point[1]),
interpolated_tsdf_.GetCorrespondenceCost(static_cast<double>(point[0]),
static_cast<double>(point[1])),
1e-4);
EXPECT_NEAR(GetUninterpolatedWeight(point[0], point[1]),
interpolated_tsdf_.GetWeight(static_cast<double>(point[0]),
static_cast<double>(point[1])),
1e-4);
}
// Check unknown cell.
EXPECT_NEAR(tsdf_.GetMaxCorrespondenceCost(),
interpolated_tsdf_.GetCorrespondenceCost(3.0, 2.0), 1e-4);
EXPECT_NEAR(GetUninterpolatedWeight(3.f, 2.f),
interpolated_tsdf_.GetWeight(3.0, 2.0), 1e-4);
}
TEST_F(InterpolatedTSDF2DTest, InterpolatesWithinCell) {
const float tsd_00 = 0.1f;
const float tsd_01 = 0.2f;
const float tsd_10 = 0.3f;
const float tsd_11 = 0.4f;
const float w_00 = 1.f;
const float w_01 = 2.f;
const float w_10 = 3.f;
const float w_11 = 4.f;
tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.f, 0.f)), tsd_00,
w_00);
tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.f, 1.f)), tsd_01,
w_01);
tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(1.f, 0.f)), tsd_10,
w_10);
tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(1.f, 1.f)), tsd_11,
w_11);
const double kSampleStep = tsdf_.limits().resolution() / 100.;
for (double x = 0. + kSampleStep; x < 1.; x += tsdf_.limits().resolution()) {
for (double y = 0. + kSampleStep; y < 1.;
y += tsdf_.limits().resolution()) {
const float tsd_expected = (x * tsd_10 + (1.f - x) * tsd_00) * (1.f - y) +
(x * tsd_11 + (1.f - x) * tsd_01) * y;
EXPECT_NEAR(interpolated_tsdf_.GetCorrespondenceCost(x, y), tsd_expected,
1e-3);
const float w_expected = (x * w_10 + (1.f - x) * w_00) * (1.f - y) +
(x * w_11 + (1.f - x) * w_01) * y;
EXPECT_NEAR(interpolated_tsdf_.GetWeight(x, y), w_expected, 1e-3);
}
}
}
} // namespace
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer