TraversabilityGenerator3d.hpp

Go to the documentation of this file.

#pragma once
 
#include <maps/grid/MLSMap.hpp>
#include <memory>
#include <cstdint>
#include <unordered_map>
#include "TraversabilityConfig.hpp"
#include "TravGenNode.hpp"
#include "SoilNode.hpp"
 
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/convex_hull_3.h>
#include <CGAL/Polygon_mesh_processing/intersection.h>
#include <cmath> // for trigonometric functions
 
typedef CGAL::Exact_predicates_inexact_constructions_kernel  K;
typedef CGAL::Polyhedron_3<K>                                Polyhedron_3;
typedef K::Point_3                                           Point_3;
 
namespace traversability_generator3d
{
 
class TraversabilityGenerator3d
{
public:
    // TODO use MLSMapPrecalculated and actually use slope information?
//    typedef maps::grid::MultiLevelGridMap< maps::grid::SurfacePatchBase > MLGrid;
    typedef maps::grid::MLSMapSloped MLGrid;
    
protected:
    
    typedef MLGrid::CellType Cell;
    typedef MLGrid::View View;
    typedef View::CellType ViewCell;
    typedef MLGrid::PatchType Patch;
 
    Polyhedron_3 generatePolyhedron(const std::vector<Eigen::Vector3d>& points);
    Polyhedron_3 createPolyhedronFromSurfacePatch(const maps::grid::SurfacePatch<maps::grid::MLSConfig::SLOPE> *p, const Eigen::Vector3d& position);
    std::shared_ptr<MLGrid > mlsGrid;
    bool addInitialPatch;
    Eigen::Affine3d initialPatch2Mls;
    double patchRadius;
 
    std::vector<TravGenNode*> obstacleNodesGrowList;
    
    maps::grid::TraversabilityMap3d<TravGenNode*> trMap;
    maps::grid::TraversabilityMap3d<SoilNode*> soilMap;
 
    int currentNodeId = 0; //used while expanding
    int currentSoilNodeId = 0; //used while expanding
 
    std::vector<TravGenNode *> frontierNodesGrowList;
    
    bool computePlaneRansac(TravGenNode &node);
    double computeSlope(const Eigen::Hyperplane< double, int(3) >& plane) const;
    Eigen::Vector3d computeSlopeDirection(const Eigen::Hyperplane< double, int(3) >& plane) const;
    
    bool checkStepHeightAABB(TravGenNode* node);
    bool checkStepHeightOBB(TravGenNode* node);
 
    struct BodyHull
    {
        Polyhedron_3 poly;
        Eigen::Vector3d planeNormal;
        std::vector<Eigen::Vector3d> bottomCorners;
        Eigen::Vector3d heightOffset;
    };
    BodyHull buildBodyPolyhedron(const Eigen::Vector3d& nodePos,
                                 const Eigen::Hyperplane<double, 3>& plane, double yaw);
 
    Eigen::AlignedBox3d rotationSafeSearchBox(const Eigen::Vector3d& nodePos) const;
 
#ifdef ENABLE_DEBUG_DRAWINGS
    void drawCollisionDebug(const std::string& tag, int counter, const BodyHull& body,
                            const maps::grid::SurfacePatch<maps::grid::MLSConfig::SLOPE>* p,
                            const Eigen::Vector3d& pos);
#endif
    
    struct YawCheckPatch
    {
        Polyhedron_3 poly;
        Eigen::Vector2d cellXY;
        double zMax;
        const maps::grid::SurfacePatch<maps::grid::MLSConfig::SLOPE>* patch;
        Eigen::Vector3d pos;
    };
 
    std::vector<YawCheckPatch> collectYawCheckPatches(TravGenNode* node);
 
    bool checkCollisionForYaw(TravGenNode* node, double yaw,
                              const std::vector<YawCheckPatch>& patches);
 
    bool computeSafeOrientations(TravGenNode* node);
    
    bool computeAllowedOrientations(TravGenNode* node);
    
    bool checkForFrontier(const TravGenNode* node);
 
    enum class NodeClassification : uint8_t
    {
        Unknown,              
        PreexistingObstacle,  
        Obstacle,             
        Traversable           
    };
 
    NodeClassification classifyNode(TravGenNode* node);
 
 
    struct PrefitPatch
    {
        TravGenNode* node = nullptr;
    };
 
    PrefitPatch buildPatchNodeAt(const maps::grid::Index& idx, const double curHeight);
 
    TravGenNode* finishPatchNode(const PrefitPatch& prefit, const maps::grid::Index& idx);
 
    struct PrefitEntry
    {
        maps::grid::Index idx;
        double requestHeight;
        PrefitPatch patch;
    };
    typedef std::unordered_map<uint64_t, PrefitEntry> PrefitCache;
 
    bool finalizeNode(TravGenNode* node, NodeClassification classification,
                      PrefitCache* prefits);
 
    enum class NeighborRequest : uint8_t { Ok, Skip, Abort };
 
    NeighborRequest computeNeighborRequest(const TravGenNode* node, const maps::grid::Index& idxS,
                                           maps::grid::Index& outIdx, double& outHeight) const;
 
    void addConnectedPatches(TravGenNode* node);
 
    void addConnectedPatches(TravGenNode* node, PrefitCache* prefits);
 
    bool getConnectedPatch(const maps::grid::Index& idx, double height, const Patch*& patch);
    
    static double interpolate(double x, double x0, double y0, double x1, double y1);
    
    TravGenNode *createTraversabilityPatchAt(maps::grid::Index idx, const double curHeight);
    SoilNode *createSoilPatchAt(maps::grid::Index idx, const double curHeight);
 
    void inflateFrontiers();
 
    void inflateObstacles();
    
    TraversabilityConfig config;
    
    void addInitialPatchToMLS();
 
    SoilNode* generateStartSoilNode(const Eigen::Vector3d& startPos);
 
public:
    TraversabilityGenerator3d(const TraversabilityConfig &config);
 
    virtual ~TraversabilityGenerator3d();
 
    void clearTrMap();
    void clearSoilMap();
 
    void setInitialPatch(const Eigen::Affine3d &ground2Mls, double patchRadius);
 
    void setSoilType(SoilNode * node, SoilType soilType);
    bool addSoilNode(const SoilSample& sample);
 
    double gaussian2D(double x, double y, double meanX, double meanY, double sigmaX, double sigmaY);
 
    virtual TravGenNode *generateStartNode(const Eigen::Vector3d &startPos);
    TravGenNode *findMatchingTraversabilityPatchAt(maps::grid::Index idx, const double curHeight) const;
    SoilNode* findMatchingSoilPatchAt(maps::grid::Index idx, const double curHeight) const;
    void updateSoilInformation();
 
    void expandAll(const std::vector<Eigen::Vector3d>& positions);
    
    void expandAll(const Eigen::Vector3d &startPos);
    
    
    void expandAll(const Eigen::Vector3d &startPos, const double expandDist);
    
    void expandAll(TravGenNode *startNode);
    
    void expandAll(TravGenNode *startNode, const double expandDist);
 
    virtual bool expandNode(TravGenNode *node);
    
    void setMLSGrid(std::shared_ptr<MLGrid> &grid);
    
    int getNumNodes() const;
    
    const maps::grid::TraversabilityMap3d<TravGenNode *> &getTraversabilityMap() const;
    const maps::grid::TraversabilityMap3d<SoilNode *> &getSoilMap() const;
    void addConnectedPatches(SoilNode *  node);
        
    void setConfig(const TraversabilityConfig &config);
 
 
};
 
}