first commit

2014-03-12 16:39:58 +01:00
commit 76801f72ac
98 changed files with 14580 additions and 0 deletions
@@ -0,0 +1,170 @@
+// 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 <stdexcept>
+#include <vikit/math_utils.h>
+#include <svo/point.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+ 
+namespace svo {
+
+int Point::point_counter_ = 0;
+
+Point::Point(Vector3d pos, PointOrigin origin) :
+  id_(point_counter_++),
+  pos_(pos),
+  n_obs_(0),
+  v_pt_(NULL),
+  last_published_ts_(0),
+  last_projected_kf_id_(-1),
+  type_(UNKNOWN_QUALITY),
+  origin_(origin),
+  n_failed_reproj_(0),
+  n_succeeded_reproj_(0),
+  last_structure_optim_(0)
+{}
+
+Point::~Point()
+{}
+
+void Point::addFrameRef(Feature* ftr)
+{
+  obs_.push_front(ftr);
+  ++n_obs_;
+}
+
+Feature* Point::findFrameRef(Frame* frame)
+{
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+    if((*it)->frame == frame)
+      return *it;
+  return NULL;    // no keyframe found
+}
+
+bool Point::deleteFrameRef(Frame* frame)
+{
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+  {
+    if((*it)->frame == frame)
+    {
+      obs_.erase(it);
+      return true;
+    }
+  }
+  return false;
+}
+
+bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const
+{
+  // TODO: get frame with same point of view AND same pyramid level!
+  Vector3d obs_dir(framepos - pos_); obs_dir.normalize();
+  auto min_it=obs_.begin();
+  double min_cos_angle = 0;
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+  {
+    Vector3d dir((*it)->frame->pos() - pos_); dir.normalize();
+    double cos_angle = obs_dir.dot(dir);
+    if(cos_angle > min_cos_angle)
+    {
+      min_cos_angle = cos_angle;
+      min_it = it;
+    }
+  }
+  ftr = *min_it;
+  if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless
+    return false;
+  return true;
+}
+
+void Point::pointJacobian(
+    const Vector3d& p_in_f,
+    const Matrix3d& R_f_w,
+    Matrix23d& point_jac) const
+{
+  // we work on the unit plane, therefore focal lenght = 1
+  const double z_inv = 1.0/p_in_f[2];
+  const double z_inv_sq = z_inv*z_inv;
+  point_jac(0, 0) = z_inv;
+  point_jac(0, 1) = 0.0;
+  point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
+  point_jac(1, 0) = 0.0;
+  point_jac(1, 1) = z_inv;
+  point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
+  point_jac = - point_jac * R_f_w;
+}
+
+void Point::optimize(const size_t n_iter)
+{
+  Vector3d old_point = pos_;
+  double chi2 = 0.0;
+  Matrix3d A;
+  Vector3d b;
+
+  for(size_t i=0; i<n_iter; i++)
+  {
+    A.setZero();
+    b.setZero();
+    double new_chi2 = 0.0;
+
+    // compute residuals
+    for(auto it=obs_.begin(); it!=obs_.end(); ++it)
+    {
+      Matrix23d J;
+      const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
+      pointJacobian(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
+      const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
+      new_chi2 += e.squaredNorm();
+      A.noalias() += J.transpose() * J;
+      b.noalias() -= J.transpose() * e;
+    }
+
+    // solve linear system
+    const Vector3d dp(A.ldlt().solve(b));
+
+    // check if error increased
+    if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
+    {
+#ifdef POINT_OPTIMIZER_DEBUG
+      cout << "it " << i
+           << "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
+#endif
+      pos_ = old_point; // roll-back
+      break;
+    }
+
+    // update the model
+    Vector3d new_point = pos_ + dp;
+    old_point = pos_;
+    pos_ = new_point;
+    chi2 = new_chi2;
+#ifdef POINT_OPTIMIZER_DEBUG
+    cout << "it " << i
+         << "\t Success \t new_chi2 = " << new_chi2
+         << "\t norm(b) = " << vk::norm_max(b)
+         << endl;
+#endif
+
+    // stop when converged
+    if(vk::norm_max(dp) <= EPS)
+      break;
+  }
+#ifdef POINT_OPTIMIZER_DEBUG
+  cout << endl;
+#endif
+}
+
+} // namespace svo