Skip to content

Commit

Permalink
Relocalization: refactor in several .cpp files. Add new method from B…
Browse files Browse the repository at this point in the history 
…EV point density
  • Loading branch information
@jlblancoc
jlblancoc committed Apr 23, 2024
1 parent 480b8fb commit d7055a9
Show file tree
Hide file tree
Showing 9 changed files with 480 additions and 152 deletions.
12 changes: 8 additions & 4 deletions mola_relocalization/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,22 +19,26 @@ find_package(mola_common REQUIRED)
# find CMake dependencies:
find_package(mrpt-obs REQUIRED)
find_package(mrpt-maps REQUIRED)
find_package(mrpt-slam REQUIRED)
find_package(mp2p_icp REQUIRED)
find_package(mola_pose_list REQUIRED)

find_package(mola_test_datasets) # optional

# -----------------------
# define lib:
file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h)
file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h)

mola_add_library(
TARGET ${PROJECT_NAME}
SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS}
SOURCES
src/find_best_poses_se2.cpp
src/AlignBEV.cpp
src/RelocalizationICP_SE2.cpp
src/RelocalizationLikelihood_SE2.cpp
include/mola_relocalization/relocalization.h
PUBLIC_LINK_LIBRARIES
mrpt::obs
mrpt::maps
mrpt::slam
mola::mp2p_icp
mola::mola_pose_list
# PRIVATE_LINK_LIBRARIES
Expand Down
31 changes: 31 additions & 0 deletions mola_relocalization/include/mola_relocalization/relocalization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,37 @@ struct RelocalizationICP_SE2
static Output run(const Input& in);
};

/** Takes a global and a local point cloud maps, and builds a point cloud
* density "grid map image" from a bird-eye-view (BEV), then applies the
* grid-map align algorithm in mrpt::maps.
*
* \ingroup mola_relocalization_grp
*/
struct AlignBEV
{
struct Input
{
mp2p_icp::metric_map_t reference_map;
std::vector<std::string> ref_map_layers; //!< empty=all
mp2p_icp::metric_map_t local_map;
std::vector<std::string> local_map_layers; //!< empty=all

double resolution_xy = 0.35;

Input() = default;
};

struct Output
{
mola::HashedSetSE3 found_poses;
double time_cost = .0; //!< [s]

Output() = default;
};

static Output run(const Input& in);
};

/** Finds the SE(2) poses with the top given percentile likelihood, and returns
* them sorted by likelihood (higher values are better matches).
*
Expand Down
161 changes: 161 additions & 0 deletions mola_relocalization/src/AlignBEV.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,161 @@
/* -------------------------------------------------------------------------
* A Modular Optimization framework for Localization and mApping (MOLA)
*
* Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
* Licensed under the GNU GPL v3 for non-commercial applications.
*
* This file is part of MOLA.
* MOLA is free software: you can redistribute it and/or modify it under the
* terms of the GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option) any later
* version.
*
* MOLA is distributed in the hope that it will be useful, but WITHOUT ANY
* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
* A PARTICULAR PURPOSE. See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with
* MOLA. If not, see <https://www.gnu.org/licenses/>.
* ------------------------------------------------------------------------- */

#include <mola_relocalization/relocalization.h>
#include <mrpt/core/Clock.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/slam/CGridMapAligner.h>

#include <mrpt/maps/bonxai/bonxai.hpp>

namespace
{

class DensityGridMap
{
public:
DensityGridMap(
double resolution, uint8_t inner_bits = 2, uint8_t leaf_bits = 3)
: grid_(resolution, inner_bits, leaf_bits)
{
}

void insert_point_cloud(const mrpt::maps::CPointsMap& p)
{
const auto& xs = p.getPointsBufferRef_x();
const auto& ys = p.getPointsBufferRef_y();
const size_t N = xs.size();

for (size_t i = 0; i < N; i++)
{
const auto pt = mrpt::math::TPoint3Df(xs[i], ys[i], .0f);

auto* cell = accessor_.value(
Bonxai::PosToCoord({pt.x, pt.y, pt.z}, grid_.inv_resolution),
true /*create*/);
cell->pointCount++;

mrpt::keep_max<uint32_t>(maxDensity_, cell->pointCount);

if (!bbox_)
{
bbox_.emplace();
bbox_ = mrpt::math::TBoundingBoxf(pt, pt);
}
else
bbox_->updateWithPoint(pt);
}
}

mrpt::maps::COccupancyGridMap2D::Ptr get_as_occupancy_grid()
{
ASSERT_(bbox_.has_value());
ASSERT_(maxDensity_ > 0);

auto og = mrpt::maps::COccupancyGridMap2D::Create();
og->setSize(
bbox_->min.x, bbox_->max.x, bbox_->min.y, bbox_->max.y,
grid_.resolution, .01f);

const float nPts_inv = 1.0f / maxDensity_;

grid_.forEachCell(
[&](Node& v, const Bonxai::CoordT& c)
{
const auto pt = Bonxai::CoordToPos(c, grid_.resolution);

const float density = v.pointCount * nPts_inv;
og->setPos(pt.x, pt.y, density);
});

return og;
}

private:
struct Node
{
uint32_t pointCount = 0;
};

Bonxai::VoxelGrid<Node> grid_;
Bonxai::VoxelGrid<Node>::Accessor accessor_{grid_};
uint32_t maxDensity_ = 1;
std::optional<mrpt::math::TBoundingBoxf> bbox_;
};

} // namespace

mola::AlignBEV::Output mola::AlignBEV::run(const Input& in)
{
mola::AlignBEV::Output result;

const double t0 = mrpt::Clock::nowDouble();

ASSERT_(!in.reference_map.layers.empty());
ASSERT_(!in.local_map.layers.empty());

auto lambdaProcessMap = [](DensityGridMap& g,
const mp2p_icp::metric_map_t& m,
const std::vector<std::string>& inLayers)
{
std::vector<std::string> layers = inLayers;
if (layers.empty())
for (const auto& [l, ml] : m.layers) layers.push_back(l);

for (const auto& layer : layers)
{
auto pts = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
m.layers.at(layer));
if (!pts) continue;
g.insert_point_cloud(*pts);
}
};

// Build density maps:
DensityGridMap gGrid(in.resolution_xy);
DensityGridMap lGrid(in.resolution_xy);

lambdaProcessMap(gGrid, in.reference_map, in.ref_map_layers);
lambdaProcessMap(lGrid, in.local_map, in.local_map_layers);

const auto gOccGrid = gGrid.get_as_occupancy_grid();
const auto lOccGrid = lGrid.get_as_occupancy_grid();

#if 1
gOccGrid->saveAsBitmapFile("og_reference.png");
lOccGrid->saveAsBitmapFile("og_local.png");
#endif

mrpt::slam::CGridMapAligner ga;
ga.options.dumpToTextStream(std::cout);

ga.options.feature_descriptor = mrpt::vision::descORB;
ga.options.debug_save_map_pairs = true;

auto resPose = ga.Align(gOccGrid.get(), lOccGrid.get(), {});

std::cout << "res: " << resPose->getMeanVal() << "\n";

// end
result.time_cost = mrpt::Clock::nowDouble() - t0;

return result;
}
147 changes: 0 additions & 147 deletions mola_relocalization/src/relocalization.cpp → ...ocalization/src/RelocalizationICP_SE2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,153 +29,6 @@
#include <mrpt/core/WorkerThreadsPool.h>
#include <mrpt/version.h>

#include <optional>

/** \defgroup mola_relocalization_grp mola-relocalization
* Algorithms for localization starting with large uncertainty.
*/

// METHOD: likelihood
mola::RelocalizationLikelihood_SE2::Output
mola::RelocalizationLikelihood_SE2::run(const Input& in)
{
mola::RelocalizationLikelihood_SE2::Output result;

const double t0 = mrpt::Clock::nowDouble();

ASSERT_(!in.reference_map.layers.empty());

result.likelihood_grid = mrpt::poses::CPosePDFGrid(
in.corner_min.x, in.corner_max.x, in.corner_min.y, in.corner_max.y,
in.resolution_xy, in.resolution_phi, in.corner_min.phi,
in.corner_max.phi);

auto& grid = result.likelihood_grid;

const size_t nX = grid.getSizeX();
const size_t nY = grid.getSizeY();
const size_t nPhi = grid.getSizePhi();

const size_t nCells = nX * nY * nPhi;
ASSERT_(nCells > 0);

// evaluate over the grid:
std::optional<double> minW, maxW;

for (size_t iX = 0, iGlobal = 0; iX < nX; iX++)
{
const double x = grid.idx2x(iX);
for (size_t iY = 0; iY < nY; iY++)
{
const double y = grid.idx2y(iY);
for (size_t iPhi = 0; iPhi < nPhi; iPhi++, iGlobal++)
{
const double phi = grid.idx2phi(iPhi);

const auto pose = mrpt::poses::CPose3D::FromXYZYawPitchRoll(
x, y, 0, phi, 0, 0);

for (const auto& [layerName, map] : in.reference_map.layers)
{
ASSERT_(map);
const double logLik = map->computeObservationsLikelihood(
in.observations, pose);

double* cell = grid.getByIndex(iX, iY, iPhi);
ASSERT_(cell);
*cell = logLik;

if (!minW || logLik < *minW) minW = logLik;
if (!maxW || logLik > *maxW) maxW = logLik;
}
}
}
}

// normalizeWeights and convert log-lik ==> likelihood
for (size_t iX = 0; iX < nX; iX++)
for (size_t iY = 0; iY < nY; iY++)
for (size_t iPhi = 0; iPhi < nPhi; iPhi++)
{
double& cell = *grid.getByIndex(iX, iY, iPhi);
cell -= *maxW;
cell = std::exp(cell);
}
*minW -= *maxW;

// Normalize PDF:
grid.normalize();

result.time_cost = mrpt::Clock::nowDouble() - t0;
result.max_log_likelihood = 0; // by definition of the normalization above
result.min_log_likelihood = *minW;

return result;
}

std::map<double, mrpt::math::TPose2D> mola::find_best_poses_se2(
const mrpt::poses::CPosePDFGrid& grid, const double percentile)
{
ASSERT_GT_(percentile, 0.0);
ASSERT_LT_(percentile, 1.0);

#if MRPT_VERSION < 0x020c01
const size_t nX = grid.getSizeX();
const size_t nY = grid.getSizeY();
const size_t nPhi = grid.getSizePhi();

double maxLik = .0;
for (size_t iX = 0; iX < nX; iX++)
for (size_t iY = 0; iY < nY; iY++)
for (size_t iPhi = 0; iPhi < nPhi; iPhi++)
{
const double& cell = *grid.getByIndex(iX, iY, iPhi);
mrpt::keep_max(maxLik, cell);
}
#else
const double maxLik =
*std::max_element(grid.data().begin(), grid.data().end());
#endif

const double threshold = percentile * maxLik;

std::map<double, mrpt::math::TPose2D> best;

#if MRPT_VERSION < 0x020c01
for (size_t iX = 0; iX < nX; iX++)
{
const double x = grid.idx2x(iX);
for (size_t iY = 0; iY < nY; iY++)
{
for (size_t iPhi = 0; iPhi < nPhi; iPhi++)
{
const double& cell = *grid.getByIndex(iX, iY, iPhi);
if (cell < threshold) continue;

const double y = grid.idx2y(iY);
const double phi = grid.idx2phi(iPhi);

best[cell] = {x, y, phi};
}
}
}
#else
const size_t N = grid.data().size();
for (size_t i = 0; i < N; i++)
{
const double cell = grid.data()[i];
if (cell < threshold) continue;

const auto [ix, iy, iphi] = grid.absidx2idx(i);
const double x = grid.idx2x(ix);
const double y = grid.idx2y(iy);
const double phi = grid.idx2phi(iphi);
best[cell] = {x, y, phi};
}
#endif
return best;
}

// METHOD: ICP
mola::RelocalizationICP_SE2::Output mola::RelocalizationICP_SE2::run(
const Input& in)
Expand Down
Loading

0 comments on commit d7055a9

Please sign in to comment.