MapPointProjection.h

#pragma once
#include "OrbSLAM/Headers/MapPoint.h"
#include "OrbSLAM/Headers/Frame.h"
#include "GraphOptimization/Headers/GeometricCamera.h"
namespace NDEVR
{
    struct MapPointProjection
    {
        MapPoint* reference;        
        float mTrackProjX;          
        float mTrackProjY;          
        float mTrackDepth;          
        float mTrackDepthR;         
        float mTrackProjXR;         
        float mTrackProjYR;         
 
        uint04 track_scale_level, track_scale_levelR;   
        float mTrackViewCos, mTrackViewCosR;            
        bool mbTrackInView = false;     
        bool mbTrackInViewR = false;    
        bool is_in_frustum = false;     
        MapPointProjection(const Frame& frame, MapPoint* pMP, fltp04 view_cos_limit)
            : reference(pMP)
        {
            is_in_frustum = checkInFrustum(frame, pMP, view_cos_limit);
        }

        bool checkInFrustum(const Frame& frame, MapPoint* pMP, fltp04 view_cos_limit)
        {
            if (frame.m_view[0]->keypoint_count == 0 || IsInvalid(frame.m_view[0]->keypoint_count))
            {
                mbTrackInView = false;
                mTrackProjX = -1.0f;
                mTrackProjY = -1.0f;
 
                // 3D in absolute coordinates
                Eigen::Matrix<fltp04, 3, 1> P = pMP->worldPos();
 
                // 3D in camera coordinates
                const Eigen::Vector3f pc = frame.inRefCoordinates(P);
                mTrackDepth = pc.norm();
 
                // Check positive depth
                const fltp04 pcz = pc(2);
                const fltp04 invz = 1.0f / pcz;
                if (pcz < 0.0f)
                    return false;
 
                const Eigen::Vector2f uv = frame.m_view[0]->camera->project(pc);
 
                if (uv(X) < frame.info().image_bounds[MIN][X] || uv(X) > frame.info().image_bounds[MAX][X])
                    return false;
                if (uv(Y) < frame.info().image_bounds[MIN][Y] || uv(Y) > frame.info().image_bounds[MAX][Y])
                    return false;
 
                mTrackProjX = uv(0);
                mTrackProjY = uv(1);
 
                // Check distance is in the scale invariance region of the MapPoint
                const fltp04 max_distance = pMP->GetMaxDistanceInvariance();
                const fltp04 min_distance = pMP->GetMinDistanceInvariance();
                const Eigen::Vector3<g_type> PO  = P - frame.cameraCenter();
                const g_type dist = PO.norm();
 
                if (dist < min_distance || dist > max_distance)
                    return false;
 
                // Check viewing angle
                Eigen::Vector3<g_type> Pn = pMP->GetNormal();
 
                mTrackViewCos = PO.dot(Pn) / dist;
 
                if (mTrackViewCos < view_cos_limit)
                    return false;
 
                // Predict scale in the image
                track_scale_level = pMP->predictScale(dist, frame);
 
                // Data used by the tracking
                mbTrackInView = true;
                mTrackProjX  = uv(0);
                mTrackProjXR = uv(0) - frame.info().mbf * invz;
                mTrackProjY  = uv(1);
                return true;
            }
            else
            {
                track_scale_level = Constant<uint04>::Invalid;
                track_scale_levelR = Constant<uint04>::Invalid;
                mbTrackInView  = isInFrustumChecks(frame, pMP, view_cos_limit, false);
                mbTrackInViewR = isInFrustumChecks(frame, pMP, view_cos_limit, true);
                return mbTrackInView || mbTrackInViewR;
            }
        }

        bool isInFrustumChecks(const Frame& frame, MapPoint* pMP, float viewingCosLimit, bool bRight = false)
        {
            // 3D in absolute coordinates
            Eigen::Vector3f P = pMP->worldPos();
            Eigen::Matrix3f mR;
            Eigen::Vector3f mt, twc;
            if (bRight)
            {
                
                auto mTrl = frame.GetRelativePoseTrl();
                //Eigen::Matrix3f Rrl = mTrl.rotationMatrix();
                Eigen::Matrix3f Rrl = mTrl.rotationMatrix();
                //Eigen::Vector3f trl = mTrl.translation();
                Eigen::Vector3f trl = mTrl.translation();
                mR = Rrl * frame.GetRotation();
                mt = Rrl * frame.tcw() + trl;
                twc = frame.GetRotation() * frame.GetRelativePoseTlr_translation() + frame.cameraCenter();
            }
            else
            {
                mR = frame.GetRotation();
                mt = frame.tcw();
                twc = frame.cameraCenter();
            }
 
            // 3D in camera coordinates
            Eigen::Vector3f Pc = mR * P + mt;
            const float Pc_dist = Pc.norm();
            const float& PcZ = Pc(2);
 
            // Check positive depth
            if (PcZ < 0.0f)
                return false;
 
            // Project in image and check it is not outside
            Eigen::Vector2f uv = frame.m_view[bRight ? 1 : 0]->camera->project(Pc);
 
            if (uv(0) < frame.info().image_bounds[MIN][X] || uv(0) > frame.info().image_bounds[MAX][X])
                return false;
            if (uv(1) < frame.info().image_bounds[MIN][Y] || uv(1) > frame.info().image_bounds[MAX][Y])
                return false;
 
            // Check distance is in the scale invariance region of the MapPoint
            const Eigen::Vector3f PO = P - twc;
            const float dist = PO.norm();
 
            if (dist < pMP->GetMinDistanceInvariance())
                return false;
            if (dist > pMP->GetMaxDistanceInvariance())
                return false;
 
            // Check viewing angle
            Eigen::Vector3f Pn = pMP->GetNormal();
 
            const float viewCos = PO.dot(Pn) / dist;
 
            if (viewCos < viewingCosLimit)
                return false;
 
            // Predict scale in the image
            const uint04 nPredictedLevel = pMP->predictScale(dist, frame);
 
            if (bRight)
            {
                mTrackProjXR = uv(0);
                mTrackProjYR = uv(1);
                track_scale_levelR = nPredictedLevel;
                mTrackViewCosR = viewCos;
                mTrackDepthR = Pc_dist;
            }
            else
            {
                mTrackProjX = uv(0);
                mTrackProjY = uv(1);
                track_scale_level = nPredictedLevel;
                mTrackViewCos = viewCos;
                mTrackDepth = Pc_dist;
            }
 
            return true;
        }
    };
}