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