Program Listing for File SlamSystem.cpp

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

#include <memory>

#include "SlamSystem.h"

#include <boost/thread/shared_lock_guard.hpp>

#include <g3log/g3log.hpp>

#ifdef ANDROID
#include <android/log.h>
#endif

#include <opencv2/opencv.hpp>

#include "DataStructures/Frame.h"
#include "DataStructures/FrameMemory.h"
#include "GlobalMapping/KeyFrameGraph.h"
#include "GlobalMapping/TrackableKeyFrameSearch.h"
#include "util/globalFuncs.h"

#include "SlamSystem/ConstraintSearchThread.h"
#include "SlamSystem/MappingThread.h"
#include "SlamSystem/OptimizationThread.h"

using namespace lsd_slam;

SlamSystem::SlamSystem()
    : _perf(), _outputWrappers(), _finalized(), _initialized(false),
      //    _keyFrames(),
      _keyFrameGraph(new KeyFrameGraph),
      _trackableKeyFrameSearch(new TrackableKeyFrameSearch(_keyFrameGraph)) {

  // Because some of these rely on Conf(), need to explicitly call after
  // static initialization.  Is this true?
  const bool threaded = Conf().runRealTime;
  _optThread.reset(new OptimizationThread(*this, threaded));
  _mapThread.reset(new MappingThread(*this, threaded));
  _constraintThread.reset(new ConstraintSearchThread(*this, threaded));
  _trackingThread.reset(new TrackingThread(*this, threaded));

  timeLastUpdate.start();
}

SlamSystem::~SlamSystem() {
  // make sure no-one is waiting for something.
  LOG(INFO) << "... waiting for all threads to exit";

  _mapThread.reset();
  _constraintThread.reset();
  _optThread.reset();
  _trackingThread.reset();
  LOG(INFO) << "DONE waiting for all threads to exit";

  FrameMemory::getInstance().releaseBuffers();
}

SlamSystem *SlamSystem::fullReset(void) {
  SlamSystem *newSystem = new SlamSystem();
  for (auto &wrapper : _outputWrappers) {
    newSystem->addOutputWrapper(wrapper);
  }
  return newSystem;
}

void SlamSystem::finalize() {
  LOG(INFO) << "Finalizing Graph... adding final constraints!!";

  // Run this in the foreground
  _constraintThread->doFullReConstraintSearch();
  _constraintThread->fullReConstraintSearchComplete.wait();

  LOG(INFO) << "Finalizing Graph... optimizing!!";

  // This happens in the foreground
  // This will kick off a final map publication with the newly optimized offsets
  // (also in foreground)
  _optThread->doFinalOptimization();

  _optThread->finalOptimizationComplete.wait();
  _mapThread->optimizationUpdateMerged.wait();

  LOG(INFO) << "Done Finalizing Graph.!!";
  _finalized.notify();

  LOG(INFO) << "Done Finalizing Graph.!!";
  _finalized.notify();
}

// std::shared_ptr<KeyFrame> &SlamSystem::currentKeyFrame()

// Thin wrapper which turns a bare cv::Mat image into an ImageSe
void SlamSystem::nextImage(unsigned int id, const cv::Mat &img,
                           const libvideoio::Camera &cam) {
  nextImageSet(std::make_shared<ImageSet>(id, img, cam));
}

void SlamSystem::nextImageSet(const std::shared_ptr<ImageSet> &set) {
  if (!_initialized) {
    _mapThread->createFirstKeyFrame(set);
    _initialized = true;
    return;
  }

  _trackingThread->doTrackSet(set);

  logPerformanceData();
}

//=== Keyframe maintenance functions ====

// void SlamSystem::addKeyFrame( const KeyFrame::SharedPtr &keyframe )
// {
//  _keyFrames.push_back( keyframe );
//
//
//  keyFrameGraph()->idToKeyFrame.insert(std::make_pair(keyframe->id(),
// keyframe));
// }

// void SlamSystem::changeKeyframe( const Frame::SharedPtr &candidate, bool
// noCreate, bool force, float maxScore)
// {
//  Frame::SharedPtr newReferenceKF(nullptr);
//
//  if( Conf().doKFReActivation && Conf().SLAMEnabled )
//  {
//      Timer timer;
//      newReferenceKF =
// trackableKeyFrameSearch()->findRePositionCandidate( candidate, maxScore );
//      _perf.findReferences.update( timer );
//  }
//
//  if(newReferenceKF != 0) {
//      LOG(INFO) << "Reloading existing key frame " <<
// newReferenceKF->id();        loadNewCurrentKeyframe(newReferenceKF);
// } else {         if(force)
//      {
//          if(noCreate)
//          {
//              LOG(INFO) << "mapping is disabled & moved outside
// of known map. Starting Relocalizer!";
// trackingThread->setTrackingIsBad();
//              //nextRelocIdx = -1; /// What does this do?
//          }
//          else
//          {
//              createNewCurrentKeyframe( candidate );
//          }
//      }
//  }
//  // createNewKeyFrame = false;
// }
//
// void SlamSystem::loadNewCurrentKeyframe( const Frame::SharedPtr
// &keyframeToLoad)
// {
//  depthMap()->activateExistingKF(keyframeToLoad);
//
//  LOG_IF(DEBUG, Conf().print.regularizeStatistics ) << "re-activate frame
// " << keyframeToLoad->id() << "!";
//
//  // Not entirely sure why they're doing this lookup...
//  //_currentKeyFrame =
// keyFrameGraph()->idToKeyFrame.find(keyframeToLoad->id())->second;
//  //currentKeyFrame()->depthHasBeenUpdatedFlag = false;
// }
//
//
// void SlamSystem::createNewCurrentKeyframe( const Frame::SharedPtr
// &newKeyframeCandidate)
// {
//  LOG_IF(INFO, Conf().print.threadingInfo) << "CREATE NEW KF " <<
// newKeyframeCandidate->id() << ", replacing " << currentKeyFrame()->id();
//
//  if( Conf().SLAMEnabled)
//  {
//      boost::shared_lock_guard< boost::shared_mutex > lock(
// keyFrameGraph()->idToKeyFrameMutex );
//      keyFrameGraph()->idToKeyFrame.insert(std::make_pair(newKeyframeCandidate->id(),
// newKeyframeCandidate));
//  }
//
//  // propagate & make new.
//  depthMap()->createKeyFrame(newKeyframeCandidate);
// }

//===== Debugging output functions =====

void SlamSystem::logPerformanceData() {
  float sPassed = timeLastUpdate.reset();
  if (sPassed > 1.0f) {

    // LOGF(DEBUG, "Mapping: %3.1fms (%.1fHz); Track: %3.1fms (%.1fHz); Create:
    // %3.1fms (%.1fHz); FindRef: %3.1fms (%.1fHz); PermaTrk: %3.1fms (%.1fHz);
    // Opt: %3.1fms (%.1fHz); FindConst: %3.1fms (%.1fHz);\n",
    //          depthMap()->perf().update.ms(),
    // depthMap()->perf().update.rate(),
    // _trackingThread->perf().track.ms(), _trackingThread->perf().track.rate(),
    //          depthMap()->perf().create.ms()+depthMap()->perf().finalize.ms(),
    // depthMap()->perf().create.rate(),
    // _perf.findReferences.ms(), _perf.findReferences.rate(),
    // 0.0, 0.0,
    //          //trackableKeyFrameSearch != 0 ?
    // trackableKeyFrameSearch->trackPermaRef.ms() : 0, trackableKeyFrameSearch
    // != 0 ? trackableKeyFrameSearch->trackPermaRef.rate() : 0,
    //          _optThread->perf.ms(), _optThread->perf.rate(),
    //          _constraintThread->perf().findConstraint.ms(),
    // _constraintThread->perf().findConstraint.rate() );
    //
    // depthMap()->logPerformanceData();
  }
}

void SlamSystem::updateDisplayDepthMap() {
  if (!Conf().displayDepthMap)
    return;

  const double scale = (bool)currentKeyFrame()
                           ? currentKeyFrame()->frame()->getCamToWorld().scale()
                           : 1.0;

  // debug plot depthmap
  char buf1[200] = "";
  char buf2[200] = "";

  if (Conf().onSceenInfoDisplay) {
    // snprintf(buf1,200,"Map: Upd %3.0fms (%2.0fHz); Trk %3.0fms (%2.0fHz); %d
    // / %d",       depthMap()->perf().update.ms(),
    // depthMap()->perf().update.rate(),        _trackingThread->perf().track.ms(),
    // _trackingThread->perf().track.rate(),
    //      currentKeyFrame()->numFramesTrackedOnThis,
    // currentKeyFrame()->numMappedOnThis ); //,
    // (int)unmappedTrackedFrames().size());

    // snprintf(buf2,200,"dens %2.0f%%; good %2.0f%%; scale %2.2f; res %2.1f/;
    // usg %2.0f%%; Map: %d F, %d KF, %d E, %.1fm Pts",
    //      100*currentKeyFrame->numPoints/(float)(Conf().slamImage.area()),
    //      100*tracking_lastGoodPerBad,
    //      scale,
    //      tracking_lastResidual,
    //      100*tracking_lastUsage,
    //      (int)keyFrameGraph()->allFramePoses.size(),
    //      keyFrameGraph()->totalVertices,
    //      (int)keyFrameGraph()->edgesAll.size(),
    //      1e-6 * (float)keyFrameGraph()->totalPoints);
  }

  currentKeyFrame()->depthMap()->plotDepthMap(buf1, buf2);

  CHECK(currentKeyFrame()->depthMap()->debugImages().depthImage().data != NULL);
  publishDepthImage(
      currentKeyFrame()->depthMap()->debugImages().depthImage().data);
}

//=== OutputWrapper functions ==

#define OUTPUT_FOR_EACH(func)                                                  \
  for (auto &wrapper : _outputWrappers) {                                      \
    wrapper->func;                                                             \
  }

void SlamSystem::publishPose(const Sophus::Sim3f &pose) {
  OUTPUT_FOR_EACH(publishPose(pose))
}

void SlamSystem::publishTrackedFrame(const Frame::SharedPtr &frame) {
  OUTPUT_FOR_EACH(publishTrackedFrame(frame))
}

void SlamSystem::publishKeyframeGraph(void) {
  OUTPUT_FOR_EACH(publishKeyframeGraph(keyFrameGraph()))
}

void SlamSystem::publishDepthImage(unsigned char *data) {
  OUTPUT_FOR_EACH(updateDepthImage(data))
}

void SlamSystem::publishKeyframe(const Frame::SharedPtr &frame) {
  OUTPUT_FOR_EACH(publishKeyframe(frame))
}

void SlamSystem::publishCurrentKeyframe() {
  if (currentKeyFrame()) {
    OUTPUT_FOR_EACH(publishKeyframe(currentKeyFrame()->frame()))
    OUTPUT_FOR_EACH(publishPointCloud(currentKeyFrame()->frame()))
  } else {
    LOG(DEBUG) << "No currentKeyframe, unable to publish";
  }
}