RealSensePositionSensor.h

#pragma once
#include <NDEVR/PositionSensor.h>
#include "Design/Headers/DesignObjectLookup.h"
#include "Design/Headers/StationModel.h"
#include "Base/Headers/QueueBuffer.hpp"
#include <librealsense2/rs.hpp>
namespace NDEVR
{
    struct PositionReading
    {
        PositionReading()
        {}

        PositionReading(
              Vertex<3, fltp08> position
            , Ray<3, fltp08> velocity
            , Ray<3, fltp08> acceleration, uint01 tracker_confidence, uint01 mapper_confidence, fltp08 time)
            : position(position)
            , velocity(velocity)
            , acceleration(acceleration)
            , time(time)
            , tracker_confidence(tracker_confidence)
            , mapper_confidence(mapper_confidence)
        {}

        Vertex<3, fltp08> positionAt(fltp08 t)
        {
            fltp08 dt_s = (t - time) / 1000.0;;
            return position + dt_s * ((dt_s / 2 * acceleration) + velocity);
        }

        fltp08 confidence() const
        {
            return cast<fltp08>(tracker_confidence) / (2 * acceleration.magnitudeSquared() + velocity.magnitudeSquared() + 0.1);
        }
        Vertex<3, fltp08> position;     
        Ray<3, fltp08> velocity;        
        Ray<3, fltp08> acceleration;    
        fltp08 time;                    
        uint01 tracker_confidence;   
        uint01 mapper_confidence;    
    };

    class RealSensePositionSensor : public PositionSensor
    {
    public:
        typedef QueueBuffer<PositionReading, BufferAllocator<PositionReading, AlignmentDeterminator<PositionReading>::Size(), true>> PositionReadingBuffer;
        RealSensePositionSensor(DesignObjectLookup* lookup, const Model& model, ProgressInfo* log, QObject* parent = nullptr)
            : PositionSensor(model, log, parent)
            , m_historic_positions(200)
            , m_transform(1.0)
            , m_lookup(lookup)
        {}

        void setLocation(rs2::pose_frame frame)
        {
            auto pose_data = frame.get_pose_data();
            Vertex<3, fltp08> offset(-pose_data.translation.z, -pose_data.translation.x, pose_data.translation.y);
 
            Ray<3, fltp08> acceleration(-pose_data.acceleration.z, -pose_data.acceleration.x, pose_data.acceleration.y);
 
            Ray<3, fltp08> velocity(-pose_data.velocity.z, -pose_data.velocity.x, pose_data.velocity.y);
 
            offset = m_transform * offset;
            acceleration = m_transform * acceleration;
            velocity = m_transform * velocity;
 
            PositionReading reading(offset, velocity, acceleration,
                pose_data.tracker_confidence,
                pose_data.mapper_confidence, frame.get_timestamp());
            setPosition(reading.position);
            WLock lock(&m_historic_positions);
            if (m_historic_positions.size() > 100)
                m_historic_positions.pop();
            m_historic_positions.push(reading);
        }

        void setLocationFromSLAM(const Matrix<fltp08>& slam, fltp08 time)
        {
            Vertex<3, fltp08> offset = m_transform * Vertex<3, fltp08>(slam.decomposeOffset());
            Vector<3, fltp08> velocity(0.0);
            Vector<3, fltp08> acceleration(0.0);
            if (m_historic_positions.size() > 0)
            {
                auto& reading = m_historic_positions[m_historic_positions.size() - 1];
                if (time > reading.time)
                {
                    velocity = (offset - reading.position) / (time - reading.time);
                    acceleration = (velocity - reading.velocity) / (time - reading.time);
                }
            }
            PositionReading reading(offset, velocity, acceleration, 1.0, 1.0, time);
            setPosition(reading.position);
            /*if (m_historic_positions.size() > 100)
                m_historic_positions.pop();
            m_historic_positions.push(reading);*/
        }

        void addDeploymentSegment(fltp08) override
        {
            WLock wlock(&m_station_model);
            lib_assert(m_station_model.isValid(), "No station for segment");
            if (m_station_model.isValid())
            {
                WLock lock = m_lookup->writeLock();
                if (!m_deployment.isValid())
                {
                    m_deployment = Deployment(m_station_model.createChild());
                    m_deployment.set<NDPO::deletion_allowed>(true);
                    m_deployment.setUseTube(Geometry::ThicknessMode::e_pixel, 1.0);
                    m_lookup->addModel(m_deployment);
                    emit deploymentChangedSignal(m_deployment.get<NDPO::guid>());
                }
                else
                {
                }
                setPosition(m_deployment.currentLocation());
                emit deploymentSegmentAddedSignal();
                m_lookup->modified_time.set(Time::SystemTime());
                m_lookup->requestAutoSave();
            }
        }

        Vertex<3, fltp08> locationAt(fltp08 time)
        {
            PositionReading reading;
            {
                RLock lock(&m_historic_positions);
 
                uint04 size = m_historic_positions.size();
                if (size == 0)
                {
                    Vertex<3, fltp08> location = lastPosition();
                    if (IsInvalid(location))
                        location = 0.0;
                    return location;
                }
                reading = m_historic_positions[size - 1];
                for (uint04 i = size - 2; !IsInvalid(i); i--)
                {
                    if (m_historic_positions[i].time < time)
                    {
                        reading = m_historic_positions[i];
                        break;
                    }
                }
            }
            if (reading.time > time || (time - reading.time) > 200.0)
            {
                return Constant<Vertex<3, fltp08>>::Invalid;
            }
            if (reading.time < time)
            {
                return reading.positionAt(time);
            }
            return reading.position;
        }

        void requestReset(bool reset_requested)
        {
            m_request_reset = reset_requested;
        }

        bool resetRequested() const
        {
            return m_request_reset;
        }
    protected:
        PositionReadingBuffer m_historic_positions; 
        Matrix<fltp08> m_transform;                 
        bool m_request_reset = false;               
        DesignObjectLookup* m_lookup;               
    };
}