From e9404c4ed8a26e03dfd7b65536d17332391d65a9 Mon Sep 17 00:00:00 2001
From: Christian Forster <ch.forster@gmail.com>
Date: Fri, 21 Mar 2014 20:22:56 +0100
Subject: [PATCH] added functionality to set first frame. used for benchmark
 node

---
 svo/include/svo/frame_handler_mono.h | 3 +++
 svo/src/frame_handler_mono.cpp       | 9 +++++++++
 2 files changed, 12 insertions(+)

diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h
index 3dc80ab..f15afaf 100644
--- a/svo/include/svo/frame_handler_mono.h
+++ b/svo/include/svo/frame_handler_mono.h
@@ -34,6 +34,9 @@ public:
   /// Provide an image.
   void addImage(const cv::Mat& img, double timestamp);
 
+  /// Set the first frame (used for synthetic datasets in benchmark node)
+  void setFirstFrame(const FramePtr& first_frame);
+
   /// Get the last frame that has been processed.
   FramePtr lastFrame() { return last_frame_; }
 
diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp
index 95a9787..63672aa 100644
--- a/svo/src/frame_handler_mono.cpp
+++ b/svo/src/frame_handler_mono.cpp
@@ -74,6 +74,15 @@ addImage(const cv::Mat& img, double timestamp)
   finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
 }
 
+void FrameHandlerMono::
+setFirstFrame(const FramePtr& first_frame)
+{
+  resetAll();
+  last_frame_ = first_frame;
+  map_.addKeyframe(last_frame_);
+  stage_ = DEFAULT_FRAME;
+}
+
 FrameHandlerMono::UpdateResult FrameHandlerMono::
 processFirstFrame()
 {