API/KeyFramePointRaster_8hpp_source.html

#pragma once

#include "OrbSLAM/Headers/KeyFrame.h"

#include "OrbSLAM/Headers/Converter.h"

#include "PointScanner/Headers/PointContainer.h"

#include "RasterPointScanner/Headers/ViewportPointSmoother.hpp"

#include "RasterPointScanner/Headers/PointSmootherBlockFilter.hpp"

#include "BlockModel/Headers/SegmentedBlockModel.h"

#include "Design/Headers/GeometryRegistration.h"

#include "Design/Headers/Geometry.h"

#include "Design/Headers/Model.h"

#include "Base/Headers/FileResource.h"

#include "Base/Headers/Dictionary.h"

#include <NDEVR/Matrix.h>

namespace NDEVR

{

    struct RasterFrameModel : public Model

    {

        RasterFrameModel(const KeyFrame* frame, const Model& model)

            : Model(model)

            , frame(frame)

        {

            if (!is<NDPN::type>(RasterFrameModel::TypeName()))

                init();

            updateModel();

        }

        template<class t_point_type>

        RasterFrameModel(const PointContainer<t_point_type>& points, const Matrix<fltp08>& transform, const KeyFrame* frame, const Model& model)

            : Model(model)

            , ndv_transform(transform)

            , frame(frame)

        {

            if (!is<NDPN::type>(RasterFrameModel::TypeName()))

                init();

            setPoints(points);

            updateModel();

        }

        void init()

        {

            set<NDPN::type>(RasterFrameModel::TypeName());

        }

        static constexpr StringView TypeName() { return "frame_raster"; }

        template<class t_block_model_type>

        void blockFilter(fltp04 resolution, uint04 weight_filter)

        {

            PointContainer<t_block_model_type> points = getPoints();

            SegmentedBlockModel<t_block_model_type> block_model(resolution, getBounds().expand(1.1f));

            block_model.addPoints(points.locations, points.colors, points.weights);

            block_model.syncRasters();

            BlockModelCache<t_block_model_type, uint04> cache = block_model.getCache(weight_filter);

            Matrix<fltp04> location_transform = block_model.blockTransform().template as<fltp04>();

            points.setSize(cache.geo_add_locations.size());

            for (uint04 i = 0; i < cache.geo_add_locations.size(); i++)

            {

                if constexpr (t_block_model_type::HasOffset())

                    points.locations[i] = location_transform * Vertex<3, fltp04>(cache.geo_add_locations[i].template as<3, fltp04>() + cache.geo_add_info[i].offset());

                else

                    points.locations[i] = location_transform * Vertex<3, fltp04>(cache.geo_add_locations[i].template as<3, fltp04>());

                points.weights[i] = cache.geo_add_weight[i];

                points.data[i] = cache.geo_add_info[i].template getAs<t_block_model_type>();

            }

            setPoints(points);

        }

        template<class t_point_type>

        PointContainer<t_point_type> getPoints() const

        {

            PointContainer<t_point_type> points;

            Geometry geo(getGeometry());

            VertexIterator<Vertex<3, fltp04>> pos_iter(VertexProperty::Position, geo);

            //VertexIterator<Vertex<3, fltp04>> normal_iter(t_point_type::HasNormal() ? VertexProperty::Normal : VertexProperty::Position, geo);

            VertexIterator<RGBColor> color_iter(t_point_type::HasColor() ? VertexProperty::Color : VertexProperty::Position, geo);

            points.setSize(pos_iter.size());

            Matrix<fltp04> transform = geo.get<NDPO::transform>().template as<fltp04>();

            for (uint04 i = 0; i < points.size(); ++i)

            {

                points.locations[i] = transform * pos_iter[i];

                //if constexpr (t_point_type::HasNormal())

                    //points.data[i].setNormal(normal_iter[i].normalized());

                if constexpr (t_point_type::HasColor())

                    points.data[i].setColor(color_iter[i]);

                points.weights[i] = 1U;

            }

            return points;

        }

        template<class t_point_type>

        void setPoints(const PointContainer<t_point_type>& points)

        {

            Geometry geo = getGeometry();

            if (!geo.isValid())

            {

                geo = createChildGeometry();

                geo.setGeometryType(GeometryType::e_points);

                geo.setupVertexTable(points.size()

                    , Geometry::e_cartesian_3S

                    //, t_point_type::HasNormal() ? Geometry::e_cartesian_3S : Geometry::e_no_vertex

                    , Geometry::e_no_vertex

                    , t_point_type::HasColor() ? Geometry::e_color_rgb : Geometry::e_no_vertex);

                uint04 valid_idx = 0;

                VertexIterator<Vertex<3, fltp04>> pos_iter(VertexProperty::Position, geo);

                //VertexIterator<Vertex<3, fltp04>> normal_iter(t_point_type::HasNormal() ? VertexProperty::Normal : VertexProperty::Position, geo);

                VertexIterator<RGBColor> color_iter(t_point_type::HasColor() ? VertexProperty::Color : VertexProperty::Position, geo);

                Bounds<3, fltp04> bounds = Constant<Bounds<3, fltp04>>::Min;

                for (uint04 i = 0; i < points.size(); ++i)

                {

                    bounds.addToBounds(points.locations[i]);

                }

                Vector<3, fltp08> max_scale(2.0 * cast<fltp08>(Constant<sint02>::Max));

                Vector<3, fltp04> pos_scale = max_scale.as<3, fltp04>() / bounds.span();

                //Vector<3, fltp04> normal_scale(cast<fltp04>(Constant<sint02>::Max));

                Vector<3, fltp04> offset = bounds.center();

                for (uint04 i = 0; i < points.size(); ++i)

                {

                    if (points.weights[i] >= 10)

                    {

                        pos_iter.setVertex(valid_idx, pos_scale * (points.locations[i] - offset));

                        //if constexpr (t_point_type::HasNormal())

                            //normal_iter.setVertex(valid_idx, normal_scale * points.data[i].normal());

                        if constexpr (t_point_type::HasColor())

                            color_iter.setVertex(valid_idx, points.data[i].color());

                        ++valid_idx;

                    }

                }

                geo.set<NDPO::transform>(Matrix<fltp08>::OffsetMatrix(offset.as<3, fltp08>()).scale(1.0 / pos_scale.as<3, fltp08>()));

                geo.setVertexCount(valid_idx);

                geo.updateVertexColumns();

                geo.updateModifiedTime();

                geo.setBounds(Bounds<3, fltp08>(Vertex<3,fltp08>(Constant<sint02>::Min), Vertex<3, fltp08>(Constant<sint02>::Max)));

                updateModifiedTime();

            }

        }

        void updateModel()

        {

            if (frame == nullptr)

                return;

            TranslatedString name;

            if (!frame->isBad())

            {

                name = _t("Frame [frame_id]").replace("[frame_id]", String(frame->keyframe_id));

            }

            else

            {

                name = _t("Frame [frame_id] (Bad)").replace("[frame_id]", String(frame->keyframe_id));

                //updateDesignVisible(false);

            }

            update<NDPOC::name>(name);

            update<NDPO::transform>(ndv_transform);

        }

        const KeyFrame* frame = nullptr;

        Matrix<fltp08> ndv_transform;

    };

    class FramePointScan : public Model

    {

    public:

        FramePointScan(const Model& model)

            : Model(model)

        {

            if (!is<NDPN::type>(FramePointScan::TypeName()))

                init();

        }

        void init()

        {

            set<NDPN::type>(TypeName());

            Material material = createChildMaterial();

            material.set<NDPM::shading_model>(Material::ShadingModel::e_flat);

            material.set<NDPOC::name>(_t("Live Material"));

            material.set<NDPM::pixel_thickness>(1.0f);

            material.setUVMode(UVType::e_KD, Material::UVMode::e_color_channel);

        }

        static constexpr StringView TypeName() { return "frame_point_scan"; }


        void setPointStride(uint04 point_stride)

        {

            m_point_stride = point_stride;

        }

        template<class t_point_type>

        void add(const KeyFrame* frame, PointContainer<t_point_type>& points)

        {

            if (points.locations.size() == 0 || m_last_keyframe == nullptr)

                return;

            frame->setDoNotReduce(true);

            if (m_last_keyframe != nullptr)

            {

                if (m_last_keyframe->isBad())

                {

                    //find the closest valid keyframe to this

                    uint04 original_id = m_last_keyframe->id;

                    m_last_keyframe = frame;

                    while (m_last_keyframe->mPrevKF && m_last_keyframe->mPrevKF->id > original_id)

                        m_last_keyframe = m_last_keyframe->mPrevKF;

                }

                Matrix<fltp08> mat = Converter::ToNDVMat(m_last_keyframe->poseInverse().matrix()).as<fltp08>();

                Matrix<fltp04> inverse_mat = mat.invert().as<fltp04>();

                for (Vertex<3, fltp04>& p0 : points.locations)

                    p0 = inverse_mat * p0;

                RasterFrameModel frame_model(points, mat, m_last_keyframe, createChild());

                m_rasters.add(frame_model);

                updateModifiedTime();

            }

            m_last_keyframe = frame;

        }

        bool shouldAddFramePoints(const KeyFrame* frame)

        {

            if (frame == nullptr)

                return false;

            if (m_last_keyframe == nullptr)

            {

                m_last_keyframe = frame;

                return false;

            }

            if (frame == m_last_keyframe)

                return false;

            if (!m_last_keyframe->isSet())

                return false;

            if (frame->isFirstConnection())

                return true;

            if (frame->timestamp() - m_last_keyframe->timestamp() < 0.1)

                return false;

            Eigen::Vector3f r1 = frame->GetRotation() * Eigen::Vector3f(1.0, 0.0, 0.0);

            Eigen::Vector3f r2 = m_last_keyframe->GetRotation() * Eigen::Vector3f(1.0, 0.0, 0.0);

            Eigen::Vector3f d1 = frame->cameraCenter();

            Eigen::Vector3f d2 = m_last_keyframe->cameraCenter();

            if ((d1 - d2).norm() < 0.1f && (r1 - r2).norm() < 0.2f)

                return false;

            return true;

        }

        template<class t_block_filter_type>

        void blockFilter(fltp04 resolution, uint04 weight_filter)

        {

            for (uint04 i = 0; i < m_rasters.size(); ++i)

            {

                m_rasters[i].template blockFilter<t_block_filter_type>(resolution, weight_filter);

            }

        }

        void updateModels()

        {

            for (uint04 i = 0; i < m_rasters.size(); i++)

                m_rasters[i].updateModel();

        }

        bool updateRaster()

        {

            bool updated = false;

            for(uint04 i = m_rasters.size() - 1; IsValid(i); --i)

            {

                bool needs_update_model = false;

                const KeyFrame* frame = m_rasters[i].frame;

                Matrix<fltp08> new_ndv_mat = Converter::ToNDVMat(frame->poseInverse().matrix()).as<fltp08>();

                if (!equals(m_rasters[i].ndv_transform, new_ndv_mat, 0.000001))

                    needs_update_model = true;

                m_rasters[i].ndv_transform = new_ndv_mat;

                if (!needs_update_model)

                    needs_update_model = frame->isBad() && m_rasters[i].isVisible();

                if (needs_update_model)

                    m_rasters[i].updateModel();

                updated |= needs_update_model;

            }

            return updated;

        }

        template<class t_point_type, class t_block_model_type>

        void addAllToBlockModel(ViewportPointSmoother<t_point_type>& smoother

            , SegmentedBlockModel<t_block_model_type>& block_model

            , InfoPipe* log)

        {

            if (m_rasters.size() == 0)

                return;

            ProgressInfo progress(_t("Optimizing Graph"), log, 0.0f);

            block_model.update<NDPO::transform>(block_model.getParentTransform().invert() * getCompleteTransform());

            smoother.setFrameReduction(0U);

            smoother.setBoundsFilter(false);

            smoother.setLockWeights(true);

            for (uint04 i = 0; i < m_rasters.size(); i++)

            {

                auto& iter = m_rasters[i];

                if (iter.frame->isBad())

                    continue;

                Matrix<fltp04> ref_mat = m_rasters[i].ndv_transform.template as<fltp04>();

                //filter.transferActiveLocations();

                auto points = iter.getPoints<t_point_type>();

                for (auto& point : points.locations)

                    point = ref_mat * point;

                //smoother.update(ref_mat, points, iter.points.colors, Buffer<Vector<2, uint04>>(), iter.points.weights, 12.0f, 10U);

                //auto inactive = smoother.inactive();

                block_model.addPoints(points.locations, points.data, points.weights);

                //smoother.clearInactiveLocations();

                //block_model.filterGeometries(filter);

                //block_model.add

                progress.setProgress(i, m_rasters.size());

            }

            progress.setProgress(1.0f);

            const auto& active = smoother.active();

            block_model.addPoints(active.locations, active.data, active.weights);

            const auto& inactive = smoother.inactive();

            block_model.addPoints(inactive.locations, inactive.data, inactive.weights);

            smoother.postClearAll();

            smoother.setBoundsFilter(false);

        }

        template<class t_block_model_type>

        void registerPoints(SegmentedBlockModel<t_block_model_type>& block_model

            , GeometryRegistration& reg

            , InfoPipe* log

            , const void* lock_ptr)

        {

            {

                WLock lock(lock_ptr);

                block_model.update<NDPO::transform>(block_model.getParentTransform().invert()

                    * getCompleteTransform());

                updateModels();

            }

            ProgressInfo progress(_t("Register"), log);

            progress.setProgress(0.0f);

            if (m_rasters.size() < 2)

                return;

            {

                WLock lock(lock_ptr);

                set<NDPO::spacial_visible>(false);

            }

            GeometryRegistrationParameters params;

            {

                Matrix<fltp04> initial_transform = m_rasters[0].ndv_transform.template as<fltp04>();

                PointContainer<t_block_model_type> points = m_rasters[0].getPoints<t_block_model_type>();

                for (uint04 i = 0; i < points.locations.size(); i++)

                    points.locations[i] = initial_transform * points.locations[i];

                block_model.addPoints(points.locations, points.data, points.weights);

            }

            uint04 finished_count = 0;

            params.update_data_callback = [&](uint04 i, uint04 iteration_b, Matrix<fltp08>& mat, bool finished)

            {

                //if (m_root && IsValid(mat))

                {

                    //WLock lock(lock_ptr);

                    //m_root->getChild(i + 1).update<NDPO::transform>(mat);

                }

                if (!finished)

                    return;

                progress.setProgress(++finished_count, m_rasters.size() - 1);

                if (IsInvalid(mat))

                    return;//Bad registration

                auto& raster = m_rasters[i + 1];

                Matrix<fltp04> adjustment = mat.template as<fltp04>();

                if (IsInvalid(adjustment))

                    return;//Bad registration

                PointContainer<t_block_model_type> points = raster.getPoints<t_block_model_type>();

                for (uint04 i = 0; i < points.locations.size(); i++)

                    points.locations[i] = adjustment * points.locations[i];

                block_model.addPoints(points.locations, points.data, points.weights);

                block_model.syncRasters();

                block_model.updateCloudGeometries(10, 0, lock_ptr);

            };

            params.registration_points.setSize(m_rasters.size() - 1);

            for (uint04 i = 0; i < m_rasters.size(); i++)

            {

                auto& iter = m_rasters[i];

                if (!iter.get<NDPO::spacial_visible>())

                    continue;

                if (params.reference.positions.size() == 0)

                {

                    PointContainer<t_block_model_type> points = m_rasters[0].getPoints<t_block_model_type>();

                    params.reference_transform = m_rasters[0].ndv_transform;

                    params.reference.positions = points.locations;

                }

                else

                {

                    PointContainer<t_block_model_type> points = iter.getPoints<t_block_model_type>();

                    params.registration_points[i - 1].positions = points.locations;

                    params.original_transforms.add(m_rasters[i].ndv_transform);

                }

            }

            reg.runRegistration(params);

            progress.setProgress(1.0f);

            {

                WLock lock(lock_ptr);

                set<NDPO::spacial_visible>(false);

            }

        }

        void setMinResolution(fltp04 min_resolution) { m_min_resolution = min_resolution; }

        const KeyFrame* lastKeyFrame() const { return m_last_keyframe; };

    protected:

        Buffer<RasterFrameModel> m_rasters;

        const KeyFrame* m_last_keyframe = nullptr;

        fltp04 m_min_resolution = 0.0f;

        uint04 m_point_stride = 1U;

    };

}

Converter::ToNDVMat
static Matrix< fltp04 > ToNDVMat(const Eigen::Matrix< float, 4, 4 > &m)
Converts a 4x4 float Eigen matrix to an NDEVR Matrix.
Definition Converter.h:105

DesignObject::set
void set(t_property_type property, const t_type &value)
Sets a property value in the database.
Definition DesignObject.h:200

DesignObject::is
bool is(t_property_type property, const StringView &value) const
Checks whether a string property matches the given StringView value.
Definition DesignObject.h:284

DesignObject::update
bool update(t_property_type property, const t_type &value, const void *lock=nullptr)
Updates a property only if the new value differs from the current value.
Definition DesignObject.h:229

Geometry::e_cartesian_3S
@ e_cartesian_3S
3D Cartesian coordinates, short integer.
Definition Geometry.h:177

Geometry::e_color_rgb
@ e_color_rgb
RGB color data.
Definition Geometry.h:171

Geometry::e_no_vertex
@ e_no_vertex
No vertex data present.
Definition Geometry.h:151

Material::e_flat
@ e_flat
Flat shading. Shading is done on per-face base, diffuse only. Also known as 'faceted shading'.
Definition Material.h:163

Material::e_color_channel
@ e_color_channel
Color is taken from the per-vertex color channel.
Definition Material.h:188

Model
A core class that represents a node on model hierarchy.
Definition Model.h:292

Model::updateModifiedTime
void updateModifiedTime(Time time=Time::SystemTime())
Updates the modified timestamp for this model.

Model::getGeometry
Geometry getGeometry() const
Returns the geometry attached to this model.

Model::createChild
Model createChild()
Creates a new child model and appends it to this model's child list.

Model::createChildGeometry
Geometry createChildGeometry()
Creates a new child geometry for this model.

Model::getCompleteTransform
Matrix< fltp08 > getCompleteTransform() const
Returns the fully composed local-to-global transform matrix for this model.

Model::getBounds
Bounds< 3, fltp08 > getBounds() const
Returns the local bounding box of this model.

Model::createChildMaterial
Material createChildMaterial(bool copy_child=false)
Creates a new child material for this model.

NDEVR
The primary namespace for the NDEVR SDK.
Definition ArialTileFetcherModule.h:35

NDPM::pixel_thickness
@ pixel_thickness
Thickness in pixels for line and point rendering.
Definition Material.h:62

NDPM::shading_model
@ shading_model
The shading model index used for rendering (see Material::ShadingModel).
Definition Material.h:55

VertexProperty::Position
@ Position
XYZ position of the vertex.
Definition DesignObjectBase.h:62

VertexProperty::Color
@ Color
Per-vertex RGBA color.
Definition DesignObjectBase.h:66

fltp04
float fltp04
Defines an alias representing a 4 byte floating-point number Bit layout is as follows: -Sign: 1 bit a...
Definition BaseValues.hpp:128

IsValid
static constexpr bool IsValid(const Angle< t_type > &value)
Checks whether the given Angle holds a valid value.
Definition Angle.h:398

uint04
uint32_t uint04
-Defines an alias representing a 4 byte, unsigned integer -Can represent exact integer values 0 throu...
Definition BaseValues.hpp:97

GeometryType::e_points
@ e_points
Point cloud or discrete points.
Definition DesignObjectBase.h:107

fltp08
double fltp08
Defines an alias representing an 8 byte floating-point number.
Definition BaseValues.hpp:150

NDPOC::name
@ name
The calculated/translated display name.
Definition DesignProperties.h:524

IsInvalid
static constexpr bool IsInvalid(const Angle< t_type > &value)
Checks whether the given Angle holds an invalid value.
Definition Angle.h:388

NDPO::name
@ name
The display name of the object.
Definition DesignProperties.h:241

NDPO::transform
@ transform
A 4x4 transform matrix that maps local coordinates into global space.
Definition DesignProperties.h:239

NDPO::spacial_visible
@ spacial_visible
Whether the object is visible in the 3D spatial view.
Definition DesignProperties.h:251

Min
constexpr HSLColor Constant< HSLColor >::Min
The minimum HSLColor constant with saturation, brightness, and alpha at zero.
Definition HSLColor.h:266

equals
constexpr bool equals(const LineSegment< t_dims, t_type, t_vertex > &left, const LineSegment< t_dims, t_type, t_vertex > &right, const t_type &epsilon=cast< t_type >(0))
Tests if objects are considered equal.
Definition Line.hpp:791

UVType::e_KD
@ e_KD
Diffuse texture/color channel.
Definition DesignObjectBase.h:160

cast
constexpr t_to cast(const Angle< t_from > &value)
Casts an Angle from one backing type to another.
Definition Angle.h:408