<commit_before>#include <iostream>

#include <osg/Geode>
#include <osg/ShapeDrawable>
#include <osg/Material>

#include <base/TimeMark.hpp>

#include <vizkit3d/ColorConversionHelper.hpp>
#include <maps/grid/OccupancyGridMap.hpp>

#include "MLSMapVisualization.hpp"

#include "PatchesGeode.hpp"

#include "ExtentsRectangle.hpp"

using namespace vizkit3d;
using namespace ::maps::grid;

template <class T>
osg::Vec3 Vec3( const Eigen::Matrix<T,3,1>& v )
{
    return osg::Vec3( v.x(), v.y(), v.z() );
}

namespace vizkit3d {
struct PatchVisualizer
{
    static void visualize(vizkit3d::PatchesGeode& geode, const SurfacePatch<MLSConfig::PRECALCULATED>& p)
    {
        float minZ, maxZ;
        p.getRange(minZ, maxZ);
//        float stdev = p.getStdev() + 1e-4f;
        float zp= (maxZ+minZ)*0.5f;
        float height = (maxZ - minZ) + 1e-3f;
        osg::Vec3 mean = Vec3(p.getCenter());
        mean.z() -= zp;
        osg::Vec3 normal = Vec3(p.getNormal());
        geode.drawPlane(zp, height, mean, normal);
    }
    static void visualize(vizkit3d::PatchesGeode& geode, const SurfacePatch<MLSConfig::KALMAN>& p)
    {
        if(p.isHorizontal())
            geode.drawPlane(p.getMean(), p.getHeight(), osg::Vec3(0.f,0.f,0.f), Vec3(p.getNormal()), p.getStandardDeviation());
        else
            geode.drawBox(p.getMean(), p.getHeight(), Vec3(p.getNormal()));
    }
};
}

struct MLSMapVisualization::Data {
    // Copy of the value given to updateDataIntern.
    //
    // Making a copy is required because of how OSG works
    virtual ~Data() { }
    virtual Eigen::Vector2d getResolution() const = 0;
    virtual void visualize(vizkit3d::PatchesGeode& geode) const = 0;
    virtual void visualizeExtents(osg::Group* group) const = 0;
    virtual void visualizeNegativeInformation(vizkit3d::PatchesGeode& geode) const = 0;
};

template<enum MLSConfig::update_model Type>
struct DataHold : public MLSMapVisualization::Data
{
    MLSMap<Type> mls;

    DataHold(const MLSMap<Type> mls_) : mls(mls_) {}

    Eigen::Vector2d getResolution() const { return mls.getResolution(); }
    void visualize(vizkit3d::PatchesGeode& geode) const
    {
        //const GridMap<SPListST> &mls = *this;
        Vector2ui num_cell = mls.getNumCells();
        for (size_t x = 0; x < num_cell.x(); x++)
        {
            for (size_t y = 0; y < num_cell.y(); y++)
            {
                typedef typename MLSMap<Type>::CellType Cell;
                const Cell &list = mls.at(x, y);

                Vector3d pos(0.00, 0.00, 0.00);
                // FIXME this does not work properly for non-trivial local frames
                mls.fromGrid(Index(x,y), pos);
                geode.setPosition(pos.x(), pos.y());
                for (typename Cell::const_iterator it = list.begin(); it != list.end(); it++)
                {
                    PatchVisualizer::visualize(geode, *it);
                } // for(SPList ...)
            } // for(y ...)
        } // for(x ...)        
    };
    
    void visualizeExtents(osg::Group* group) const
    {
        // get the color as a function of the group
        float scale = ((long)group%1000)/1000.0;
        osg::Vec4 col(0,0,0,1);
        vizkit3d::hslToRgb( scale, 1.0, 0.6, col.x(), col.y(), col.z() );
      
        CellExtents extents = mls.calculateCellExtents();

        Eigen::Vector2d min = extents.min().cast<double>().cwiseProduct(mls.getResolution());
        Eigen::Vector2d max = extents.max().cast<double>().cwiseProduct(mls.getResolution());

        Eigen::Vector3d min_d = mls.getLocalFrame().inverse() * Eigen::Vector3d(min.x(), min.y(), 0.);
        Eigen::Vector3d max_d = mls.getLocalFrame().inverse() * Eigen::Vector3d(max.x(), max.y(), 0.);

        group->addChild( 
            new ExtentsRectangle( Eigen::Vector2d(min_d.x(), min_d.y()),
                Eigen::Vector2d(max_d.x(), max_d.y())));
    };

    void visualizeNegativeInformation(vizkit3d::PatchesGeode& geode) const
    {
        boost::shared_ptr<maps::grid::OccupancyGridMap> grid;
        if(mls.hasFreeSpaceMap() && (grid = boost::dynamic_pointer_cast<maps::grid::OccupancyGridMap>(mls.getFreeSpaceMap())))
        {
            Eigen::Vector3d res = grid->getVoxelResolution();
            maps::grid::Vector2ui num_cell = grid->getNumCells();
            const maps::grid::OccupancyConfiguration& config = grid->getConfig();
            for (size_t x = 0; x < num_cell.x(); x++)
            {
                for (size_t y = 0; y < num_cell.y(); y++)
                {
                    const maps::grid::OccupancyGridMap::GridMapBase::CellType &tree = grid->at(x, y);
                    maps::grid::Vector3d pos;
                    if(grid->fromGrid(maps::grid::Index(x,y), pos))
                    {
                        geode.setPosition(pos.x(), pos.y());
                        std::vector< std::pair<int,int> > free_cells;
                        for(maps::grid::DiscreteTree<maps::grid::OccupancyGridMap::VoxelCellType>::const_iterator cell_it = tree.begin(); cell_it != tree.end(); cell_it++)
                        {
                            if(cell_it->second.getLogOdds() < config.free_space_logodds)
                            {
                                if(free_cells.empty() || free_cells.back().second + 1 != cell_it->first)
                                    free_cells.push_back(std::make_pair(cell_it->first, cell_it->first));
                                else
                                    free_cells.back().second++;
                            }
                        }

                        for(const std::pair<int,int>& cell : free_cells)
                            geode.drawBox(tree.getCellCenter(cell.second) + res.z() * 0.5, res.z() * (float)((cell.second-cell.first)+1), osg::Vec3(0.f,0.f,1.f));
                    }
                }
            }
        }
    }
};

MLSMapVisualization::MLSMapVisualization()
    : p(0),
    horizontalCellColor(osg::Vec4(0.1,0.5,0.9,1.0)),
    verticalCellColor(osg::Vec4(0.8,0.9,0.5,1.0)), 
    negativeCellColor(osg::Vec4(0.1,0.5,0.9,0.2)), 
    uncertaintyColor(osg::Vec4(0.5,0.1,0.1,0.3)), 
    showMapExtents(false),
    showUncertainty(false),
    showNegative(false),
    estimateNormals(false),
    cycleHeightColor(true),
    cycleColorInterval(1.0),
    showPatchExtents(false),
    uncertaintyScale(1.0)
{
}

MLSMapVisualization::~MLSMapVisualization()
{
}

osg::ref_ptr<osg::Node> MLSMapVisualization::createMainNode()
{
    return vizkit3d::VizPluginBase::createMainNode();
}

void MLSMapVisualization::updateMainNode ( osg::Node* node )
{
    if(!p) return;
    osg::Group* group = static_cast<osg::Group*>(node);
    group->removeChildren(0, group->getNumChildren());

    Eigen::Vector2d res = p->getResolution();

    osg::ref_ptr<PatchesGeode> geode = new PatchesGeode(res.x(), res.y());
    group->addChild( geode );

    // draw the extents of the mls
    if( showMapExtents )
    {
        p->visualizeExtents(group);
    }

    if(cycleHeightColor)
    {
        geode->showCycleColor(true);
        geode->setCycleColorInterval(cycleColorInterval);
        geode->setColorHSVA(0, 1.0, 0.6, 1.0);<commit_msg>viz: fixed handling of the local map frame in MLS visualization<commit_after>#include <iostream>

#include <osg/Geode>
#include <osg/ShapeDrawable>
#include <osg/Material>

#include <base/TimeMark.hpp>

#include <vizkit3d/ColorConversionHelper.hpp>
#include <maps/grid/OccupancyGridMap.hpp>

#include "MLSMapVisualization.hpp"

#include "PatchesGeode.hpp"

#include "ExtentsRectangle.hpp"

using namespace vizkit3d;
using namespace ::maps::grid;

template <class T, int options>
osg::Vec3 Vec3( const Eigen::Matrix<T,3,1, options>& v )
{
    return osg::Vec3( v.x(), v.y(), v.z() );
}

template <class T, int options>
osg::Quat Quat( const Eigen::Quaternion<T, options>& q )
{
    return osg::Quat( q.x(), q.y(), q.z(), q.w() );
}

namespace vizkit3d {
struct PatchVisualizer
{
    static void visualize(vizkit3d::PatchesGeode& geode, const SurfacePatch<MLSConfig::PRECALCULATED>& p)
    {
        float minZ, maxZ;
        p.getRange(minZ, maxZ);
//        float stdev = p.getStdev() + 1e-4f;
        float zp= (maxZ+minZ)*0.5f;
        float height = (maxZ - minZ) + 1e-3f;
        osg::Vec3 mean = Vec3(p.getCenter());
        mean.z() -= zp;
        osg::Vec3 normal = Vec3(p.getNormal());
        geode.drawPlane(zp, height, mean, normal);
    }
    static void visualize(vizkit3d::PatchesGeode& geode, const SurfacePatch<MLSConfig::KALMAN>& p)
    {
        if(p.isHorizontal())
            geode.drawPlane(p.getMean(), p.getHeight(), osg::Vec3(0.f,0.f,0.f), Vec3(p.getNormal()), p.getStandardDeviation());
        else
            geode.drawBox(p.getMean(), p.getHeight(), Vec3(p.getNormal()));
    }
};
}

struct MLSMapVisualization::Data {
    // Copy of the value given to updateDataIntern.
    //
    // Making a copy is required because of how OSG works
    virtual ~Data() { }
    virtual Eigen::Vector2d getResolution() const = 0;
    virtual void visualize(vizkit3d::PatchesGeode& geode) const = 0;
    virtual void visualizeExtents(osg::Group* group) const = 0;
    virtual void visualizeNegativeInformation(vizkit3d::PatchesGeode& geode) const = 0;
    virtual void setLocalFrame(osg::Node* node) = 0;
    virtual void setLocalFrame(osg::PositionAttitudeTransform* local_frame) = 0;
};

template<enum MLSConfig::update_model Type>
struct DataHold : public MLSMapVisualization::Data
{
private:
    MLSMap<Type> mls;
    base::Transform3d local_transform;
public:
    DataHold(const MLSMap<Type> mls_) : mls(mls_), local_transform(mls.getLocalFrame().inverse())
    {
        // reset local frames since they are modelled separately in the OSG tree
        mls.getLocalFrame() = base::Transform3d::Identity();
        boost::shared_ptr<maps::grid::OccupancyGridMap> grid;
        if(mls.hasFreeSpaceMap() && (grid = boost::dynamic_pointer_cast<maps::grid::OccupancyGridMap>(mls.getFreeSpaceMap())))
            grid->getLocalFrame() = base::Transform3d::Identity();
    }

    Eigen::Vector2d getResolution() const { return mls.getResolution(); }
    void visualize(vizkit3d::PatchesGeode& geode) const
    {
        //const GridMap<SPListST> &mls = *this;
        Vector2ui num_cell = mls.getNumCells();
        for (size_t x = 0; x < num_cell.x(); x++)
        {
            for (size_t y = 0; y < num_cell.y(); y++)
            {
                typedef typename MLSMap<Type>::CellType Cell;
                const Cell &list = mls.at(x, y);

                Vector3d pos(0.00, 0.00, 0.00);
                mls.fromGrid(Index(x,y), pos);
                geode.setPosition(pos.x(), pos.y());
                for (typename Cell::const_iterator it = list.begin(); it != list.end(); it++)
                {
                    PatchVisualizer::visualize(geode, *it);
                } // for(SPList ...)
            } // for(y ...)
        } // for(x ...)        
    };
    
    void visualizeExtents(osg::Group* group) const
    {
        // get the color as a function of the group
        float scale = ((long)group%1000)/1000.0;
        osg::Vec4 col(0,0,0,1);
        vizkit3d::hslToRgb( scale, 1.0, 0.6, col.x(), col.y(), col.z() );
      
        CellExtents extents = mls.calculateCellExtents();

        Eigen::Vector2d min = extents.min().cast<double>().cwiseProduct(mls.getResolution());
        Eigen::Vector2d max = extents.max().cast<double>().cwiseProduct(mls.getResolution());

        Eigen::Vector3d min_d = Eigen::Vector3d(min.x(), min.y(), 0.);
        Eigen::Vector3d max_d = Eigen::Vector3d(max.x(), max.y(), 0.);

        group->addChild( 
            new ExtentsRectangle( Eigen::Vector2d(min_d.x(), min_d.y()),
                Eigen::Vector2d(max_d.x(), max_d.y())));
    };

    void visualizeNegativeInformation(vizkit3d::PatchesGeode& geode) const
    {
        boost::shared_ptr<maps::grid::OccupancyGridMap> grid;
        if(mls.hasFreeSpaceMap() && (grid = boost::dynamic_pointer_cast<maps::grid::OccupancyGridMap>(mls.getFreeSpaceMap())))
        {
            Eigen::Vector3d res = grid->getVoxelResolution();
            maps::grid::Vector2ui num_cell = grid->getNumCells();
            const maps::grid::OccupancyConfiguration& config = grid->getConfig();
            for (size_t x = 0; x < num_cell.x(); x++)
            {
                for (size_t y = 0; y < num_cell.y(); y++)
                {
                    const maps::grid::OccupancyGridMap::GridMapBase::CellType &tree = grid->at(x, y);
                    maps::grid::Vector3d pos;
                    if(grid->fromGrid(maps::grid::Index(x,y), pos))
                    {
                        geode.setPosition(pos.x(), pos.y());
                        std::vector< std::pair<int,int> > free_cells;
                        for(maps::grid::DiscreteTree<maps::grid::OccupancyGridMap::VoxelCellType>::const_iterator cell_it = tree.begin(); cell_it != tree.end(); cell_it++)
                        {
                            if(cell_it->second.getLogOdds() < config.free_space_logodds)
                            {
                                if(free_cells.empty() || free_cells.back().second + 1 != cell_it->first)
                                    free_cells.push_back(std::make_pair(cell_it->first, cell_it->first));
                                else
                                    free_cells.back().second++;
                            }
                        }

                        for(const std::pair<int,int>& cell : free_cells)
                            geode.drawBox(tree.getCellCenter(cell.second) + res.z() * 0.5, res.z() * (float)((cell.second-cell.first)+1), osg::Vec3(0.f,0.f,1.f));
                    }
                }
            }
        }
    }

    void setLocalFrame(osg::Node* node)
    {
        osg::Transform* transform = node->asTransform();
        if(transform)
        {
            osg::PositionAttitudeTransform* local_frame = transform->asPositionAttitudeTransform();
            if(local_frame)
            {
                setLocalFrame(local_frame);
                return;
            }
        }
        std::cerr << "Couldn't set local transformation. The given osg::Node is not a osg::PositionAttitudeTransform" << std::endl;
    }

    void setLocalFrame(osg::PositionAttitudeTransform* local_frame)
    {
        local_frame->setPosition(Vec3(Eigen::Vector3d(local_transform.translation())));
        local_frame->setAttitude(Quat(Eigen::Quaterniond(local_transform.linear())));
    }
};

MLSMapVisualization::MLSMapVisualization()
    : p(0),
    horizontalCellColor(osg::Vec4(0.1,0.5,0.9,1.0)),
    verticalCellColor(osg::Vec4(0.8,0.9,0.5,1.0)), 
    negativeCellColor(osg::Vec4(0.1,0.5,0.9,0.2)), 
    uncertaintyColor(osg::Vec4(0.5,0.1,0.1,0.3)), 
    showMapExtents(false),
    showUncertainty(false),
    showNegative(false),
    showNormals(false),
    estimateNormals(false),
    cycleHeightColor(true),
    cycleColorInterval(1.0),
    showPatchExtents(false),
    uncertaintyScale(1.0)
{
}

MLSMapVisualization::~MLSMapVisualization()
{
}

osg::ref_ptr<osg::Node> MLSMapVisualization::createMainNode()
{
    osg::ref_ptr<osg::PositionAttitudeTransform> local_frame(new osg::PositionAttitudeTransform);
    return local_frame;
}

void MLSMapVisualization::updateMainNode ( osg::Node* node )
{
    if(!p) return;
    p->setLocalFrame(node);

    osg::Group* group = node->asGroup();
    group->removeChildren(0, group->getNumChildren());

    Eigen::Vector2d res = p->getResolution();

    osg::ref_ptr<PatchesGeode> geode = new PatchesGeode(res.x(), res.y());
    group->addChild( geode );

    // draw the extents of the mls
    if( showMapExtents )
    {
        p->visualizeExtents(group);
    }

    if(cycleHeightColor)
    {
        geode->showCycleColor(true);
        geode->setCycleColorInterval(cycleColorInterval);
        geode->setColorHSVA(0, 1.0, 0.6, 1.0);
{'fraction_non_alphanumeric': 0.15088071209176956, 'fraction_numerical': 0.018178399047201154, 'mean_word_length': 2.4765744170843322, 'pattern_counts': {'":': 0, '<': 75, '<?xml version=': 0, '>': 104, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6931463', 'n_tokens_mistral': 5517, 'n_tokens_neox': 5264, 'n_words': 899}