示例#1
0
size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{
  reset();

  if(ref_frame->fts_.empty())
  {
    SVO_WARN_STREAM("SparseImgAlign: no features to track!");
    return 0;
  }

  ref_frame_ = ref_frame;
  cur_frame_ = cur_frame;
  ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
  jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
  visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?

  SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());

  for(level_=max_level_; level_>=min_level_; --level_)
  {
    mu_ = 0.1;
    jacobian_cache_.setZero();
    have_ref_patch_cache_ = false;
    if(verbose_)
      printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
    optimize(T_cur_from_ref);
  }
  cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;



  return n_meas_/patch_area_;
}
示例#2
0
InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
{
  trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
  SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");

  if(disparities_.size() < Config::initMinTracked())
    return FAILURE;

  double disparity = vk::getMedian(disparities_);
  SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
  if(disparity < Config::initMinDisparity())
    return NO_KEYFRAME;

  computeHomography(
      f_ref_, f_cur_,
      frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
      inliers_, xyz_in_cur_, T_cur_from_ref_);
  SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");

  if(inliers_.size() < Config::initMinInliers())
  {
    SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
    return FAILURE;
  }

  // Rescale the map such that the mean scene depth is equal to the specified scale
  vector<double> depth_vec;
  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
    depth_vec.push_back((xyz_in_cur_[i]).z());
  double scene_depth_median = vk::getMedian(depth_vec);
  double scale = Config::mapScale()/scene_depth_median;
  frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
  frame_cur->T_f_w_.translation() =
      -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));

  // For each inlier create 3D point and add feature in both frames
  SE3 T_world_cur = frame_cur->T_f_w_.inverse();
  for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
  {
    Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
    Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
    if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
    {
      Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
      Point* new_point = new Point(pos);

      Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
      frame_cur->addFeature(ftr_cur);
      new_point->addFrameRef(ftr_cur);

      Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
      frame_ref_->addFeature(ftr_ref);
      new_point->addFrameRef(ftr_ref);
    }
  }
  return SUCCESS;
}
示例#3
0
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;
}
示例#4
0
void DepthFilter::updateSeeds(FramePtr frame)
{
  // update only a limited number of seeds, because we don't have time to do it
  // for all the seeds in every frame!
  size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
  lock_t lock(seeds_mut_);
  list<Seed>::iterator it=seeds_.begin();

  const double focal_length = frame->cam_->errorMultiplier2();
  double px_noise = 1.0;
  double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)

  while( it!=seeds_.end())
  {
    // set this value true when seeds updating should be interrupted
    if(seeds_updating_halt_)
      return;

    // check if seed is not already too old
    if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
      it = seeds_.erase(it);
      continue;
    }

    // check if point is visible in the current image
    SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
    const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
    if(xyz_f.z() < 0.0)  {
      ++it; // behind the camera
      continue;
    }
    if(!it->ftr->frame->cam_->isInFrame(it->ftr->frame->f2c(xyz_f).cast<int>())) {
      ++it; // point does not project in image
      continue;
    }

    // we are using inverse depth coordinates
    float z_inv_min = it->mu + sqrt(it->sigma2);
    float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
    double z;
    if(!matcher_.findEpipolarMatchDirect(
        *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
    {
      it->b++; // increase outlier probability when no match was found
      ++it;
      ++n_failed_matches;
      continue;
    }

    // compute tau
    double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
    double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));

    // update the estimate
    updateSeed(1./z, tau_inverse*tau_inverse, &*it);
    ++n_updates;

    if(frame->isKeyframe())
    {
      // The feature detector should not initialize new seeds close to this location
      feature_detector_->setGridOccpuancy(matcher_.px_cur_);
    }

    // if the seed has converged, we initialize a new candidate point and remove the seed
    if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
    {
      assert(it->ftr->point == NULL); // TODO this should not happen anymore
      Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
      Point* point = new Point(xyz_world, it->ftr);
      it->ftr->point = point;
      /* FIXME it is not threadsafe to add a feature to the frame here.
      if(frame->isKeyframe())
      {
        Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_);
        ftr->point = point;
        point->addFrameRef(ftr);
        frame->addFeature(ftr);
        it->ftr->frame->addFeature(it->ftr);
      }
      else
      */
      {
        seed_converged_cb_(point, it->sigma2); // put in candidate list
      }
      it = seeds_.erase(it);
    }
    else if(isnan(z_inv_min))
    {
      SVO_WARN_STREAM("z_min is NaN");
      it = seeds_.erase(it);
    }
    else
      ++it;
  }
}
示例#5
0
void DepthFilter::updateLineSeeds(FramePtr frame)
{
  // update only a limited number of seeds, because we don't have time to do it
  // for all the seeds in every frame!
  size_t n_updates=0, n_failed_matches=0, n_seeds = seg_seeds_.size();
  lock_t lock(seeds_mut_);
  list<LineSeed>::iterator it=seg_seeds_.begin();

  const double focal_length = frame->cam_->errorMultiplier2();
  double px_noise = 1.0;
  double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)

  while( it!=seg_seeds_.end())
  {
    // set this value true when seeds updating should be interrupted
    if(seeds_updating_halt_)
      return;

    // check if seed is not already too old
    if((LineSeed::batch_counter - it->batch_id) > options_.max_n_kfs) {
      it = seg_seeds_.erase(it);
      continue;
    }

    // check if segment is visible in the current image
    SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
    const Vector3d xyz_f_s(T_ref_cur.inverse()*(1.0/it->mu_s * static_cast<LineFeat*>(it->ftr)->sf) );
    const Vector3d xyz_f_e(T_ref_cur.inverse()*(1.0/it->mu_e * static_cast<LineFeat*>(it->ftr)->ef) );
    if( xyz_f_s.z() < 0.0 || xyz_f_e.z() < 0.0 )  {
      ++it; // behind the camera
      continue;
    }
    if( !frame->cam_->isInFrame(frame->f2c(xyz_f_s).cast<int>()) ||
        !frame->cam_->isInFrame(frame->f2c(xyz_f_e).cast<int>()) ) {
      ++it; // segment does not project in image
      continue;
    }

    // we are using inverse depth coordinates
    float z_inv_min_s = it->mu_s + sqrt(it->sigma2_s);
    float z_inv_max_s = max(it->mu_s - sqrt(it->sigma2_s), 0.00000001f);
    float z_inv_min_e = it->mu_e + sqrt(it->sigma2_e);
    float z_inv_max_e = max(it->mu_e - sqrt(it->sigma2_e), 0.00000001f);
    double z_s, z_e;
    if(!matcherls_.findEpipolarMatchDirectSegmentEndpoint(
        *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu_s, 1.0/z_inv_min_s, 1.0/z_inv_max_s, z_s) ||
       !matcherls_.findEpipolarMatchDirectSegmentEndpoint(
        *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu_e, 1.0/z_inv_min_e, 1.0/z_inv_max_e, z_e)  )
    {
      it->b++; // increase outlier probability when no match was found
      ++it;
      ++n_failed_matches;
      continue;
    }

    // compute tau
    double tau_s = computeTau(T_ref_cur, static_cast<LineFeat*>(it->ftr)->sf, z_s, px_error_angle);
    double tau_inverse_s = 0.5 * (1.0/max(0.0000001, z_s-tau_s) - 1.0/(z_s+tau_s));
    double tau_e = computeTau(T_ref_cur, static_cast<LineFeat*>(it->ftr)->ef, z_e, px_error_angle);
    double tau_inverse_e = 0.5 * (1.0/max(0.0000001, z_e-tau_e) - 1.0/(z_e+tau_e));

    // update the estimate
    updateLineSeed(1./z_s, tau_inverse_s*tau_inverse_s, 1./z_e, tau_inverse_e*tau_inverse_e, &*it);
    ++n_updates;

    if(frame->isKeyframe())
    {
      // The feature detector should not initialize new seeds close to this location
      seg_feature_detector_->setGridOccpuancy(LineFeat(matcher_.px_cur_,matcherls_.px_cur_));
    }

    // if the seed has converged, we initialize a new candidate point and remove the seed
    if(sqrt(it->sigma2_s) < it->z_range_s/options_.seed_convergence_sigma2_thresh &&
       sqrt(it->sigma2_e) < it->z_range_e/options_.seed_convergence_sigma2_thresh  )
    {
      assert(static_cast<LineFeat*>(it->ftr)->feat3D == NULL); // TODO this should not happen anymore
      Vector3d xyz_world_s(it->ftr->frame->T_f_w_.inverse() * (static_cast<LineFeat*>(it->ftr)->sf * (1.0/it->mu_s)));
      Vector3d xyz_world_e(it->ftr->frame->T_f_w_.inverse() * (static_cast<LineFeat*>(it->ftr)->ef * (1.0/it->mu_e)));
      LineSeg* line = new LineSeg(xyz_world_s, xyz_world_e, it->ftr);
      static_cast<LineFeat*>(it->ftr)->feat3D = line;
      /* FIXME it is not threadsafe to add a feature to the frame here.
      if(frame->isKeyframe())
      {
        Feature* ftr = new PointFeat(frame.get(), matcher_.px_cur_, matcher_.search_level_);
        ftr->point = point;
        point->addFrameRef(ftr);
        frame->addFeature(ftr);
        it->ftr->frame->addFeature(it->ftr);
      }
      else
      */
      {
        seed_converged_cb_ls_(line, it->sigma2_s, it->sigma2_e); // put in candidate list
      }
      it = seg_seeds_.erase(it);
    }
    else if( isnan(z_inv_min_s) || isnan(z_inv_min_e) )
    {
      SVO_WARN_STREAM("z_min_s or z_min_e is NaN");
      it = seg_seeds_.erase(it);
    }
    else
      ++it;
  }
}
示例#6
0
InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur, Matrix3d orient, Vector3d pos)
{
  trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
  SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");

  if(disparities_.size() < Config::initMinTracked())
    return FAILURE;

  double disparity = vk::getMedian(disparities_);
  SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
  if(disparity < Config::initMinDisparity())
    return NO_KEYFRAME;

  computeHomography(
      f_ref_, f_cur_,
      frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
      inliers_, xyz_in_cur_, T_cur_from_ref_);
  SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");

  if(inliers_.size() < Config::initMinInliers())
  {
    SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
    return FAILURE;
  }

  // Transformation in real world
  T_cur_frame_real_scale = SE3(orient, pos);
  Vector3d trans = T_cur_frame_real_scale.translation() - T_first_frame_real_scale.translation();
  double length_real = sqrt(pow(trans[0],2) + pow(trans[1],2) + pow(trans[2],2));

  SVO_INFO_STREAM("Real world transform x: " << trans[0] << " y:" << trans[1] << " z:" << trans[2] << " length:" << length_real);

  double x = T_cur_from_ref_.translation()[0];
  double y = T_cur_from_ref_.translation()[1];
  double z = T_cur_from_ref_.translation()[2];
  double length_svo = sqrt(pow(x,2) + pow(y,2) + pow(z,2));

  SVO_INFO_STREAM("SVO transform x: " << x << " y:" << y << " z:" << z << " length:" << length_svo);

#ifdef USE_ASE_IMU
  // Rescale the map such that the real length of the movement matches with the svo movement length
  double scale =length_real / length_svo;
#else
  // Rescale the map such that the mean scene depth is equal to the specified scale
  vector<double> depth_vec;
  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
    depth_vec.push_back((xyz_in_cur_[i]).z());
  double scene_depth_median = vk::getMedian(depth_vec);
  double scale = Config::mapScale()/scene_depth_median;
#endif
  frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
  frame_cur->T_f_w_.translation() =
     -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));

  //frame_cur->T_f_w_ = T_cur_frame_real_scale;
  //frame_ref_->T_f_w_ = T_first_frame_real_scale;

//  // Rescale the map such that the mean scene depth is equal to the specified scale
//  vector<double> depth_vec;
//  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
//    depth_vec.push_back((xyz_in_cur_[i]).z());
//  double scene_depth_median = vk::getMedian(depth_vec);
//  double scale = Config::mapScale()/scene_depth_median;
//  frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
//  frame_cur->T_f_w_.translation() =
//      -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));

  // For each inlier create 3D point and add feature in both frames
  SE3 T_world_cur = frame_cur->T_f_w_.inverse();
  for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
  {
    Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
    Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
    if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
    {
      Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
      Point* new_point = new Point(pos);

      Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
      frame_cur->addFeature(ftr_cur);
      new_point->addFrameRef(ftr_cur);

      Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
      frame_ref_->addFeature(ftr_ref);
      new_point->addFrameRef(ftr_ref);
    }
  }
  return SUCCESS;
}