Optimizer.h

#pragma once
#include "OrbSLAM/Headers/Map.h"
#include "OrbSLAM/Headers/MapPoint.h"
#include "OrbSLAM/Headers/Frame.h"
#include "Base/Headers/Dictionary.h"
#include <math.h>
 
#include "GraphOptimization/Headers/types_seven_dof_expmap.h"
#include "GraphOptimization/Headers/types_six_dof_expmap.h"
#include "GraphOptimization/Headers/sparse_block_matrix.h"
#include "GraphOptimization/Headers/block_solver.h"
#include "GraphOptimization/Headers/optimization_algorithm_levenberg.h"
#include "GraphOptimization/Headers/optimization_algorithm_gauss_newton.h"
#include "GraphOptimization/Headers/linear_solver_eigen.h"
#include "GraphOptimization/Headers/robust_kernel_impl.h"
#include "GraphOptimization/Headers/linear_solver_dense.h"
#include "GraphOptimization/Headers/OptimizableTypes.h"
#include "GraphOptimization/Headers/sparse_optimizer.h"
#include "GraphOptimization/Headers/edge_se3_expmap.h"
#include "OrbSLAM/Headers/G2oTypes.h"
#include "OrbSLAM/Headers/Converter.h"
namespace NDEVR
{
 
class LoopClosing;

class Optimizer
{
public:
    typedef std::pair<Set<KeyFrame*>, int> ConsistentGroup;         
    typedef std::map<KeyFrame*, Sim3, std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<KeyFrame* const, Sim3>>> KeyFrameAndPose; 
    void bundleAdjustment(const Buffer<KeyFrame*> &vpKF, const Buffer<MapPoint*> &vpMP, int nIterations = 5, bool *pbStopFlag = nullptr, const uint04 nLoopKF=0, const bool bRobust = true);
    void globalBundleAdjustment(Map* pMap, int nIterations=5, bool *pbStopFlag=nullptr, const uint04 nLoopKF = 0, const bool bRobust = true);
    void fullInertialBA(Map *pMap, int its, const bool bFixLocal=false, const uint04 nLoopKF = 0, bool* pbStopFlag = nullptr, bool bInit=false, float priorG = 1e2, float priorA=1e6);
 
    void localBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap);
 
    uint04 poseOptimization(Frame* pFrame);
    uint04 poseInertialOptimizationLastFrame(Frame& pFrame, Frame* reference_frame, bool locked_frame, bool bRecInit = false);
    template<class t_solver_type>
    uint04 fillOutMPData(const Buffer<MapPoint*>& map_points, uint04 id_offset, t_solver_type& solver
        , std::function<bool(KeyFrame* pKFi, MapPoint* pMP)> filter, bool use_kernels);
    template<class t_solver_type>
    uint04 fillOutInertialMPData(const Buffer<MapPoint*>& map_points, uint04 id_offset, t_solver_type& solver
        , std::function<bool(KeyFrame* pKFi, MapPoint* pMP)> filter, bool use_kernels);
    // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono)
    void optimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
                                       const KeyFrameAndPose &NonCorrectedSim3,
                                       const KeyFrameAndPose &CorrectedSim3,
                                       const Dictionary<KeyFrame *, Set<KeyFrame *> > &LoopConnections,
                                       const bool &bFixScale);
    void optimizeEssentialGraph(KeyFrame* pCurKF, Buffer<KeyFrame*> &vpFixedKFs, Buffer<KeyFrame*> &vpFixedCorrectedKFs,
                                       Buffer<KeyFrame*> &vpNonFixedKFs, Buffer<MapPoint*> &vpNonCorrectedMPs);
 
    // For inertial loopclosing
    void optimizeEssentialGraph4DoF(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
                                       const KeyFrameAndPose &NonCorrectedSim3,
                                       const KeyFrameAndPose &CorrectedSim3,
                                       const Dictionary<KeyFrame*, Set<KeyFrame*>>& LoopConnections);
 
 
    // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) (NEW)
    uint04 optimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, Buffer<MapPoint *> &vpMatches1,
                            Sim3 &ParkeS12, const float th2, const bool bFixScale, const bool bAllPoints=false);
 
    // For inertial systems
 
    void localInertialBA(KeyFrame* pKF, bool *pbStopFlag, bool bLarge = false, bool bRecInit = false);
    void mergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbStopFlag, Map *pMap, KeyFrameAndPose &corrPoses);
 
    // Local BA in welding area when two maps are merged
    void localBundleAdjustment(KeyFrame* pMainKF, Buffer<KeyFrame*> vpAdjustKF, Buffer<KeyFrame*> vpFixedKF, bool *pbStopFlag);
 
    // Marginalize block element (start:end,start:end). Perform Schur complement.
    // Marginalized elements are filled with zeros.
    static Eigen::MatrixX<g_type> Marginalize(const Eigen::MatrixX<g_type>& H, int start, int end);
 
    // Inertial pose-graph
    void inertialOptimization(Map *pMap, Eigen::Matrix3<g_type>& Rwg, fltp04& scale, Eigen::Vector3<g_type>& bg, Eigen::Vector3<g_type>& ba, bool bMono, bool bFixedVel=false, float priorG = 1e2, float priorA = 1e6);
    void inertialOptimization(Map *pMap, Eigen::Vector3<g_type>& bg, Eigen::Vector3<g_type>& ba, float priorG = 1e2, float priorA = 1e6);
    void inertialOptimization(Map *pMap, Eigen::Matrix3<g_type>& Rwg, fltp04& scale);
    uint04 sensorPoseOptimization(Frame* pFrame, Frame* pLastFrame);
    //static int Opti_MapFitModel(KeyFrame* pKF, Dictionary<int, Dictionary<int, Buffer<cv::Point3f>>>& map_2d_building, Config::PointSet all_seenbuilding)
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    template<class t_edge>
    struct MP : public t_edge
    {
        MP(t_edge&& e = t_edge())
            : t_edge(std::forward<t_edge>(e))
        {}
        void eraseIfNeeded(g_type chi)
        {
            if (point->isBad())
                return;
            if (t_edge::chi2() <= chi && t_edge::isDepthPositive())
                return;
            {
                WLock lock(frame->mapPointsLockPtr());
                frame->eraseMapPoint(point);
            }
            {
                WLock lock(point->featureLockPtr());
                point->eraseObservation(frame);
            }
        }
        KeyFrame* frame;
        MapPoint* point;
    };
    template<class t_edge>
    struct MI : public t_edge
    {
        MI(t_edge&& e = t_edge())
            : t_edge(std::forward<t_edge>(e))
        {}
        uint04 index;
    };
 
    IndexScratch m_scratch_pad;
    Buffer<MapPoint*> m_local_map_points;
    Buffer<VertexSE3Expmap> m_se3_exp_map;
    Buffer<MP<EdgeSE3ProjectXYZ>> m_edges_mono;
    Buffer<MP<EdgeSE3ProjectXYZToBody>> m_edges_body;
    Buffer<MP<EdgeStereoSE3ProjectXYZ>> m_edges_stereo;
    Buffer<VertexSim3Expmap> m_exp_map;
    Buffer<EdgeSim3> m_sim_3_edge;
    Buffer<VertexSBAPointXYZ> m_sba_points;
    Buffer<RobustKernelHuber> m_robust_kernels;
 
    Buffer<MI<EdgeMonoOnlyPose>> m_inertial_mono;
    Buffer<MI<EdgeStereoOnlyPose>> m_inertial_stereo;
    Buffer<MI<EdgeSE3ProjectXYZOnlyPose>> m_edges_pose_mono;
    Buffer<MI<EdgeSE3ProjectXYZOnlyPose>> m_edges_pose_mono_last;
    Buffer<MI<EdgeSE3ProjectXYZOnlyPoseToBody>> m_edges_pose_body;
    Buffer<MI<EdgeStereoSE3ProjectXYZOnlyPose>> m_edges_pose_stereo;
    //inertial
    Buffer<VertexPose> m_vertex_pose;
    Buffer<VertexVelocity> m_vertex_velocity;
    Buffer<VertexGyroBias> m_gyro_biases;
    Buffer<VertexAccBias> m_acc_biases;
    Buffer<EdgeInertialGS> m_edge_inertial_gs;
    Buffer<EdgeInertial> m_vei;
    Buffer<EdgeGyroRW> m_vegr;
    Buffer<EdgeAccRW> m_vear;
    Buffer<MP<EdgeMono>> m_inertial_edges_mono;
    Buffer<MP<EdgeStereo>> m_inertial_edges_stereo;
};
 
 
}