Program Listing for File HashedSetSE3.h
↰ Return to documentation for file (include/mola_pose_list/HashedSetSE3.h
)
/* -------------------------------------------------------------------------
* 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/>.
* ------------------------------------------------------------------------- */
#pragma once
#include <mola_pose_list/index_se3_t.h>
#include <mrpt/core/round.h>
#include <mrpt/math/TPose3D.h>
#include <unordered_map>
namespace mola
{
class HashedSetSE3
{
public:
using global_index3d_t = index_se3_t<int32_t>;
inline global_index3d_t coordToGlobalIdx(const mrpt::math::TPose3D& p) const
{
return global_index3d_t(
static_cast<int32_t>(p.x * voxel_xyz_size_inv_), //
static_cast<int32_t>(p.y * voxel_xyz_size_inv_), //
static_cast<int32_t>(p.z * voxel_xyz_size_inv_), //
static_cast<int32_t>(p.yaw * voxel_yaw_size_inv_), //
static_cast<int32_t>(p.pitch * voxel_pitch_size_inv_), //
static_cast<int32_t>(p.roll * voxel_roll_size_inv_) //
);
}
inline mrpt::math::TPose3D globalIdxToCoord(
const global_index3d_t idx) const
{
return {
idx.cx * voxel_xyz_size_, //
idx.cy * voxel_xyz_size_, //
idx.cz * voxel_xyz_size_, //
idx.cyaw * voxel_yaw_size_, //
idx.cpitch * voxel_pitch_size_, //
idx.croll * voxel_roll_size_ //
};
}
HashedSetSE3(
double voxel_xyz_size = 1.0,
double voxel_yaw_size = mrpt::DEG2RAD(10.0),
double voxel_pitch_size = mrpt::DEG2RAD(10.0),
double voxel_roll_size = mrpt::DEG2RAD(10.0));
~HashedSetSE3();
void setVoxelProperties(
double voxel_xyz_size, double voxel_yaw_size, double voxel_pitch_size,
double voxel_roll_size);
using pose_vector_t = std::vector<mrpt::math::TPose3D>;
struct VoxelData
{
public:
VoxelData() = default;
auto poses() const { return poses_; }
void insertPose(const mrpt::math::TPose3D& p) { poses_.push_back(p); }
private:
pose_vector_t poses_;
};
using grids_map_t = std::unordered_map<
global_index3d_t, VoxelData, index_se3_t_hash<int32_t>>;
void clear();
bool empty() const;
inline VoxelData* voxelByGlobalIdxs(
const global_index3d_t& idx, bool createIfNew)
{
// 1) Insert into decimation voxel map:
VoxelData* voxel = nullptr;
auto it = voxels_.find(idx);
if (it == voxels_.end())
{
if (!createIfNew)
return nullptr;
else
voxel = &voxels_[idx]; // Create it
}
else
{
// Use the found grid
voxel = const_cast<VoxelData*>(&it->second);
}
return voxel;
}
const VoxelData* voxelByGlobalIdxs(
const global_index3d_t& idx //
/*, bool createIfNew this must be false for const! */) const
{ // reuse the non-const method:
return const_cast<HashedSetSE3*>(this)->voxelByGlobalIdxs(idx, false);
}
VoxelData* voxelByCoords(const mrpt::math::TPose3D& pt, bool createIfNew)
{
return voxelByGlobalIdxs(coordToGlobalIdx(pt), createIfNew);
}
const VoxelData* voxelByCoords(const mrpt::math::TPose3D& pt) const
{
return const_cast<HashedSetSE3*>(this)->voxelByCoords(pt, false);
}
void insertPose(const mrpt::math::TPose3D& pt);
const grids_map_t& voxels() const { return voxels_; }
void visitAllPoses(
const std::function<void(const mrpt::math::TPose3D&)>& f) const;
void visitAllVoxels(
const std::function<void(const global_index3d_t&, const VoxelData&)>& f)
const;
bool saveToTextFile(const std::string& file) const;
private:
double voxel_xyz_size_ = 1.00;
double voxel_yaw_size_ = mrpt::DEG2RAD(10.0);
double voxel_pitch_size_ = mrpt::DEG2RAD(10.0);
double voxel_roll_size_ = mrpt::DEG2RAD(10.0);
// Calculated from the above, in setVoxelProperties()
double voxel_xyz_size_inv_ = 1.0 / voxel_xyz_size_;
double voxel_yaw_size_inv_ = 1.0 / voxel_yaw_size_;
double voxel_pitch_size_inv_ = 1.0 / voxel_pitch_size_;
double voxel_roll_size_inv_ = 1.0 / voxel_roll_size_;
grids_map_t voxels_;
};
} // namespace mola