Program Listing for File KeyFrameGraph.h

↰ Return to documentation for file (lib/GlobalMapping/KeyFrameGraph.h)

#pragma once
#include <vector>
#include <unordered_map>
#include <mutex>
#include <deque>

#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>

#include "util/EigenCoreInclude.h"
#include <g2o/core/sparse_optimizer.h>

#include "util/SophusUtil.h"

#include "DataStructures/KeyFrame.h"

namespace lsd_slam
{


class KeyFrameGraph;
class VertexSim3;
class EdgeSim3;
class FramePoseStruct;

struct KFConstraintStruct
{
    inline KFConstraintStruct()
    {
        firstFrame = secondFrame = 0;
        information.setZero();
        robustKernel = 0;
        edge = 0;

        usage = meanResidual = meanResidualD = meanResidualP = 0;
        reciprocalConsistency = 0;


        idxInAllEdges = -1;
    }

    ~KFConstraintStruct();


    KeyFrame::SharedPtr firstFrame;
    KeyFrame::SharedPtr secondFrame;
    Sophus::Sim3d secondToFirst;
    Eigen::Matrix<double, 7, 7> information;
    g2o::RobustKernel* robustKernel;
    EdgeSim3* edge;

    float usage;
    float meanResidualD;
    float meanResidualP;
    float meanResidual;

    float reciprocalConsistency;

    int idxInAllEdges;
};






class KeyFrameGraph
{
friend class IntegrationTest;
public:
    KeyFrameGraph();

    ~KeyFrameGraph();

    void addKeyFrame( const KeyFrame::SharedPtr &keyframe);
    void dropKeyFrame( const KeyFrame::SharedPtr &keyframe );

//  void addFrame(const Frame::SharedPtr &frame);

    void dumpMap(std::string folder);

    void insertConstraint(KFConstraintStruct* constraint);

    int size() const { return keyframesAll.size(); }

    int optimize(int num_iterations);
    bool addElementsFromBuffer();


    void calculateGraphDistancesToFrame(const KeyFrame::SharedPtr &frame, std::unordered_map<KeyFrame::SharedPtr, int> &distanceMap);


    int totalPoints;
    int totalEdges;
    int totalVertices;


    //=========================== Keyframe & Pose Lists & Maps ====================================
    // Always lock the list with the corresponding mutex!
    // central point to administer keyframes, iterate over keyframes, do lookups etc.

    std::vector< KeyFrame::SharedPtr > _keyFrames;

    // contains ALL keyframes, as soon as they are "finished".
    // does NOT yet contain the keyframe that is currently being created.
    boost::shared_mutex keyframesAllMutex;
    std::vector< KeyFrame::SharedPtr > keyframesAll;


    /* this is where the shared pointers of Keyframe Frames are kept, so they are not deleted ever */
    boost::shared_mutex idToKeyFrameMutex;
    std::unordered_map< int, KeyFrame::SharedPtr > idToKeyFrame;


    // contains ALL edges, as soon as they are created
    boost::shared_mutex edgesListsMutex;
    std::vector< KFConstraintStruct* > edgesAll;



    // contains ALL frame poses, chronologically, as soon as they are tracked.
    // the corresponding frame may have been removed / deleted in the meantime.
    // these are the ones that are also referenced by the corresponding Frame / Keyframe object
    // boost::shared_mutex allFramePosesMutex;
    // std::vector< FramePoseStruct::SharedPtr  > allFramePoses;


    // contains all keyframes in graph, in some arbitrary (random) order. if a frame is re-tracked,
    // it is put to the end of this list; frames for re-tracking are always chosen from the first third of
    // this list.
    std::mutex keyframesForRetrackMutex;
    std::deque< KeyFrame::SharedPtr > keyframesForRetrack;

    // Used to live with the optimizer, but not needed as signal anymore,
    // now just used as a simple data mutex.
    std::mutex newKeyFrameMutex;


private:

    g2o::SparseOptimizer graph;

    std::vector< KeyFrame::SharedPtr > newKeyframesBuffer;
    std::vector< KFConstraintStruct* > newEdgeBuffer;


    int nextEdgeId;
};

}