first commit

2014-03-12 16:39:58 +01:00
commit 76801f72ac
98 changed files with 14580 additions and 0 deletions
@@ -0,0 +1,197 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include <boost/bind.hpp>
+#include <vikit/math_utils.h>
+#include <vikit/vision.h>
+#include <vikit/performance_monitor.h>
+#include <fast/fast.h>
+
+namespace svo {
+
+int Frame::frame_counter_ = 0;
+
+Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) :
+    id_(frame_counter_++),
+    timestamp_(timestamp),
+    cam_(cam),
+    key_pts_(5),
+    is_keyframe_(false),
+    v_kf_(NULL)
+{
+  initFrame(img);
+}
+
+Frame::~Frame()
+{
+  std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;});
+}
+
+void Frame::initFrame(const cv::Mat& img)
+{
+  // check image
+  if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height())
+    throw std::runtime_error("Frame: provided image has not the same size as the camera model");
+
+  // Set keypoints to NULL
+  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; });
+
+  // Build Image Pyramid
+  frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
+}
+
+void Frame::setKeyframe()
+{
+  is_keyframe_ = true;
+  setKeyPoints();
+}
+
+void Frame::addFeature(Feature* ftr)
+{
+  fts_.push_back(ftr);
+}
+
+void Frame::setKeyPoints()
+{
+  for(size_t i = 0; i < 5; ++i)
+    if(key_pts_[i] != NULL)
+      if(key_pts_[i]->point == NULL)
+        key_pts_[i] = NULL;
+
+  std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); });
+}
+
+void Frame::checkKeyPoints(Feature* ftr)
+{
+  const int cu = cam_->width()/2;
+  const int cv = cam_->height()/2;
+
+  // center pixel
+  if(key_pts_[0] == NULL)
+    key_pts_[0] = ftr;
+  else if(max(abs(ftr->px[0]-cu), abs(ftr->px[1]-cv))
+        < max(abs(key_pts_[0]->px[0]-cu), abs(key_pts_[0]->px[1]-cv)))
+    key_pts_[0] = ftr;
+
+  if(ftr->px[0] >= cu && ftr->px[1] >= cv)
+  {
+    if(key_pts_[1] == NULL)
+      key_pts_[1] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[1]->px[0]-cu) * (key_pts_[1]->px[0]-cv))
+      key_pts_[1] = ftr;
+  }
+  if(ftr->px[0] >= cu && ftr->px[1] < cv)
+  {
+    if(key_pts_[2] == NULL)
+      key_pts_[2] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          < (key_pts_[2]->px[0]-cu) * (key_pts_[2]->px[0]-cv))
+      key_pts_[2] = ftr;
+  }
+  if(ftr->px[0] < cv && ftr->px[1] < cv)
+  {
+    if(key_pts_[3] == NULL)
+      key_pts_[3] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[3]->px[0]-cu) * (key_pts_[3]->px[0]-cv))
+      key_pts_[3] = ftr;
+  }
+  if(ftr->px[0] < cv && ftr->px[1] >= cv)
+  {
+    if(key_pts_[4] == NULL)
+      key_pts_[4] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          < (key_pts_[4]->px[0]-cu) * (key_pts_[4]->px[0]-cv))
+      key_pts_[4] = ftr;
+  }
+}
+
+void Frame::removeKeyPoint(Feature* ftr)
+{
+  bool found = false;
+  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){
+    if(i == ftr) {
+      i = NULL;
+      found = true;
+    }
+  });
+  if(found)
+    setKeyPoints();
+}
+
+size_t Frame::nObs() const
+{
+  size_t n=0;
+  std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){if(i->point != NULL) ++n; });
+  return n;
+}
+
+bool Frame::isVisible(const Vector3d& xyz_w) const
+{
+  Vector3d xyz_f = T_f_w_*xyz_w;
+  if(xyz_f.z() < 0.0)
+    return false; // point is behind the camera
+  Vector2d px = f2c(xyz_f);
+  if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height())
+    return true;
+  return false;
+}
+
+
+/// Utility functions for the Frame class
+namespace frame_utils {
+
+void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
+{
+  pyr.resize(n_levels);
+  pyr[0] = img_level_0;
+  for(int i=1; i<n_levels; ++i)
+  {
+    pyr[i] = cv::Mat(pyr[i-1].rows/2, pyr[i-1].cols/2, CV_8U);
+    vk::halfSample(pyr[i-1], pyr[i]);
+  }
+}
+
+bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
+{
+  vector<double> depth_vec;
+  depth_vec.reserve(frame.fts_.size());
+  depth_min = std::numeric_limits<double>::max();
+  for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it)
+  {
+    if((*it)->point != NULL)
+    {
+      const double z = frame.w2f((*it)->point->pos_).z();
+      depth_vec.push_back(z);
+      depth_min = fmin(z, depth_min);
+    }
+  }
+  if(depth_vec.empty())
+  {
+    SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
+    return false;
+  }
+  depth_mean = vk::getMedian(depth_vec);
+  return true;
+}
+
+} // namespace frame_utils
+} // namespace svo