From 5827af8bcb8b22d76d5fc634aa5fb1f74df4d1a4 Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Thu, 19 Aug 2021 17:40:47 +0200
Subject: [PATCH] ignore craupted image

---
 GPU_version/vio/src/frame_handler_mono.cpp | 2 +-
 GPU_version/vio/src/initialization.cpp     | 7 ++++---
 GPU_version/vio/src/vo_node.cpp            | 7 +++++++
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp
index 7318a1a..e32aff7 100644
--- a/GPU_version/vio/src/frame_handler_mono.cpp
+++ b/GPU_version/vio/src/frame_handler_mono.cpp
@@ -164,7 +164,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
   //assert (false);
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  /*
+/*
   std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
              <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
              <<"\t distance between two frames: "<<
diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp
index 8149b85..2582c0c 100644
--- a/GPU_version/vio/src/initialization.cpp
+++ b/GPU_version/vio/src/initialization.cpp
@@ -43,7 +43,7 @@ InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
 InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
 {
   trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
-  if(disparities_.size() < Config::initMinTracked())
+  if(disparities_.size() < 1)
         return NO_KEYFRAME;
   double disparity = vk::getMedian(disparities_);
   if(disparity < Config::initMinDisparity()){
@@ -134,6 +134,7 @@ void trackKlt(
     vector<Vector3d>& f_cur,
     vector<double>& disparities)
 {
+  disparities.clear();
   const double klt_win_size = 30.0;//30.0
   const int klt_max_iter = 30;//30
   const double klt_eps = 0.001;
@@ -141,17 +142,17 @@ void trackKlt(
   vector<float> error;
   vector<float> min_eig_vec;
   cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);
+  if(frame_ref->img_pyr_[0].empty() || frame_cur->img_pyr_[0].empty())return;
   cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
                            px_ref, px_cur,
                            status, error,
                            cv::Size2i(klt_win_size, klt_win_size),
                            4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);
-
   vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
   vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
   vector<Vector3d>::iterator f_ref_it = f_ref.begin();
   f_cur.clear(); f_cur.reserve(px_cur.size());
-  disparities.clear(); disparities.reserve(px_cur.size());
+  disparities.reserve(px_cur.size());
   for(size_t i=0; px_ref_it != px_ref.end(); ++i)
   {
     if(!status[i])
diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp
index 489d7f2..84bff44 100644
--- a/GPU_version/vio/src/vo_node.cpp
+++ b/GPU_version/vio/src/vo_node.cpp
@@ -90,6 +90,8 @@ private:
     double* imu_;
     double* cmd_;
     ros::Time imu_time_;
+    int width;
+    int height;
     boost::thread* imu_the_= nullptr;
 #if VIO_DEBUG
     vio::Visualizer visualizer_;
@@ -100,6 +102,8 @@ VioNode::VioNode() :
       vo_(NULL),
       cam_(NULL)
 {
+    width=vk::getParam<int>("vio/cam_width", 640);
+    height=vk::getParam<int>("vio/cam_height", 480);
     imu_=(double *)calloc(static_cast<std::size_t>(3), sizeof(double));
     cmd_=(double *)calloc(static_cast<std::size_t>(3), sizeof(double));
     if(!vk::camera_loader::loadFromRosNs("vio", cam_))
@@ -126,6 +130,9 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
       try {
           auto start=std::chrono::steady_clock::now();
           cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
+          if(img.empty())
+              return;
+          if(cv::countNonZero(img) < 0.85*height*width)return;
           vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp);
       } catch (cv_bridge::Exception& e) {
         ROS_ERROR("cv_bridge exception: %s", e.what());