Program Listing for File TrackingThread.h

↰ Return to documentation for file (lib/SlamSystem/TrackingThread.h)

#pragma once

#include "active_object/active.h"

#include "DataStructures/ImageSet.h"
#include "DataStructures/KeyFrame.h"
#include "Tracking/Relocalizer.h"
#include "util/MovingAverage.h"

namespace lsd_slam {

class SlamSystem;
class SE3Tracker;

typedef Eigen::Matrix<float, 7, 7> Matrix7x7;

class TrackingThread {
  friend class IntegrationTest;

public:
  // Delete these
  TrackingThread(const TrackingThread &) = delete;
  TrackingThread &operator=(const TrackingThread &) = delete;

  TrackingThread(SlamSystem &system, bool threaded);

  ~TrackingThread();

  //== Calls into the thread ==
  void doTrackSet(const std::shared_ptr<ImageSet> &set) {
    if (_thread) {
      _thread->send(std::bind(&TrackingThread::trackSetImpl, this, set));
    } else {
      trackSetImpl(set);
    }
  }

  void doUseNewKeyFrame(const std::shared_ptr<KeyFrame> &kf) {
    if (_thread) {
      _thread->send(std::bind(&TrackingThread::useNewKeyFrameImpl, this, kf));
    } else {
      useNewKeyFrameImpl(kf);
    }
  }

  KeyFrame::SharedPtr &currentKeyFrame(void) { return _currentKeyFrame; }

  int findConstraintsForNewKeyFrames(Frame *newKeyFrame,
                                     bool forceParent = true,
                                     bool useFABMAP = true,
                                     float closeCandidatesTH = 1.0);

  // void changeKeyframe(std::shared_ptr<Frame> candidate, bool noCreate, bool
  // force, float maxScore);

  void takeRelocalizeResult(const RelocalizerResult &result);

  float lastTrackingClosenessScore;

  bool trackingIsGood(void) const { return _trackingIsGood; }
  bool setTrackingIsBad(void) { return _trackingIsGood = false; }
  bool setTrackingIsGood(void) { return _trackingIsGood = true; }

  struct PerformanceData {
    MsRateAverage track;
  };

  PerformanceData perf() const { return _perf; }

private:
  SlamSystem &_system;
  PerformanceData _perf;

  std::unique_ptr<SE3Tracker> _tracker;

  // Thread Callbacks
  void trackSetImpl(const std::shared_ptr<ImageSet> &set);

  void useNewKeyFrameImpl(const std::shared_ptr<KeyFrame> &kf);

  // ============= EXCLUSIVELY TRACKING THREAD (+ init) ===============

  // std::shared_ptr<TrackingReference> _trackingReference; // tracking
  // reference for current keyframe. only used by tracking. Frame::SharedPtr
  // _trackingReferenceFrameSharedPT;   // only used in odometry-mode, to keep a
  // keyframe alive until it is deleted. ONLY accessed whithin
  // currentKeyFrameMutex lock.

  bool _trackingIsGood;
  bool _newKeyFramePending;

  KeyFrame::SharedPtr _currentKeyFrame;

  Sim3 _latestGoodPoseCamToWorld;

  //
  //
  // // ============= EXCLUSIVELY MAPPING THREAD (+ init) =============
  //
  //
  //
  // // ============= EXCLUSIVELY FIND-CONSTRAINT THREAD (+ init) =============
  //
  //
  //
  //
  // // ============= SHARED ENTITIES =============
  float tracking_lastResidual;
  float tracking_lastUsage;
  float tracking_lastGoodPerBad;
  float tracking_lastGoodPerTotal;

  std::unique_ptr<active_object::Active> _thread;

  //
  // int lastNumConstraintsAddedOnFullRetrack;
  // bool doFinalOptimization;
  //
  // // for sequential operation. Set in Mapping, read in Tracking.
  // // std::condition_variable  newFrameMappedSignal;
  // // std::mutex newFrameMappedMutex;
  //
  //
  //

  //
  //
  //
  //
  //
  // // Tracking: if (!create) set candidate, set create.
  // // Mapping: if (create) use candidate, reset create.
  // // => no locking required.
  // std::shared_ptr<Frame> latestTrackedFrame;
  // bool createNewKeyFrame;
  //
  //
  //
  // // PUSHED in tracking, READ & CLEARED in mapping
  // // std::deque< std::shared_ptr<Frame> > unmappedTrackedFrames;
  // // ThreadSynchronizer unmappedTrackedFramesSynchro;
  // // std::mutex unmappedTrackedFramesMutex;
  // // std::condition_variable  unmappedTrackedFramesSignal;
  //
  //
  // // PUSHED by Mapping, READ & CLEARED by constraintFinder
  // ThreadMutexObject< std::deque< Frame* > > newKeyFrames;
  // // std::deque< Frame* > newKeyFrames;
  // // std::mutex newKeyFrameMutex;
  // // std::condition_variable newKeyFrameCreatedSignal;
  //
  //
  //
  //
  // // threads
  // // std::thread thread_mapping;
  // // std::thread thread_constraint_search;
  // //std::thread thread_optimization;
  //
  // // bool keepRunning; // used only on destruction to signal threads to
  // finish.
  //
  //
  //
  // // optimization thread
  // // bool newConstraintAdded;
  // // std::mutex newConstraintMutex;
  // // std::condition_variable newConstraintCreatedSignal;
  //
  //
  //
  //
  //
  //
  //
  //
  // bool depthMapScreenshotFlag;
  // std::string depthMapScreenshotFilename;
  //

  //

  //
  //
  // void changeKeyframe(bool noCreate, bool force, float maxScore);
  // void createNewCurrentKeyframe(std::shared_ptr<Frame> newKeyframeCandidate);
  // void loadNewCurrentKeyframe(Frame* keyframeToLoad);
  //

  //
  // void constraintSearchThreadLoop();

  // /** Calculates a scale independent error norm for reciprocal tracking
  // results a and b with associated information matrices. */

  // void optimizationThreadLoop();
};

} // namespace lsd_slam