pipline completed

GPU_version/vio/CMakeLists.txt

@@ -86,3 +86,4 @@ ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\")
 ADD_DEFINITIONS(-DVIO_DEBUG=true)
 ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON)
 ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF)
+

GPU_version/vio/depth_filter_log.txt

@@ -1,2 +1,6 @@
-[15:02:36.165] New key frame 
+[17:12:03.647] New key frame 
-[15:02:36.165] BA loop 
+[17:12:03.647] BA loop 
+[17:12:44.004] New key frame 
+[17:12:44.004] BA loop 
+[17:12:44.192] New key frame 
+[17:12:44.192] BA loop 

GPU_version/vio/src/frame_handler_mono.cpp

@@ -167,7 +167,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
                        vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
 #endif
   //std::cerr<<"run BA\n";
-  //ba_glob_->new_key_frame();
+  ba_glob_->new_key_frame();
   ROS_INFO("VIO initialized :)");
   ROS_INFO("Running ...");
   return RESULT_IS_KEYFRAME;
@@ -217,10 +217,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
             new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
             fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
 #endif
-/*    fclose(log_);
+    if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.2 ||
-    exit(0);*/
+       fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.02 || sfba_n_edges_final<10){
-    if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
-       fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<10){
         new_frame_=last_frame_;
         return RESULT_FAILURE;
     }
@@ -238,7 +236,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   {
         return RESULT_NO_KEYFRAME;
   }
-    std::cerr<<"241\n";
   double depth_mean=0.0, depth_min=0.0;
   frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
   new_frame_->setKeyframe();
@@ -260,8 +257,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   // add keyframe to map
   map_.addKeyframe(new_frame_);
-  //std::cerr<<"BA run\n";
+  std::cerr<<"BA run\n";
-  //ba_glob_->new_key_frame();
+  ba_glob_->new_key_frame();
   return RESULT_IS_KEYFRAME;
 }
 

GPU_version/vio/src/point.cpp

@@ -184,7 +184,7 @@ void Point::optimize(const size_t n_iter)
       break;
   }
 
-    for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
+/*    for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
             Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * (*it)->point->pos_));
             //e /= (1 << (*it)->level);
 //            std::cerr<<"error - point.h : "<<e.norm()<<std::endl;
@@ -193,7 +193,7 @@ void Point::optimize(const size_t n_iter)
                 (*it)->point->n_failed_reproj_++;
 //            } else
 //                it++;
-    }
+    }*/
 
 }
 

GPU_version/vio/src/reprojector.cpp

@@ -130,31 +130,16 @@ namespace vio {
                 std::vector<std::vector<cv::DMatch>>  matches;
                 keypoints_kfs.push_back(cv::KeyPoint((*it_ref)->px.x(), (*it_ref)->px.y(), 7.f,(*it_ref)->score));
 
-
-//                std::cerr<<"135"<<std::endl;
                 cv::Mat ref_des=cv::Mat(1,64,CV_8UC1,(*it_ref)->descriptor);
-//                std::cerr<<"136"<<std::endl;
+
-                //std::cerr<<maskup.size<<" "<<maskdown.size<<" "<<cur_des.size<<std::endl;
-//                cv::Mat mup, mdown;
-//                cv::cvtColor(maskup, mup, cv::COLOR_GRAY2RGB);
-//                cv::imwrite("/root/Projects/ROS_p_33_dev/src/p_33_vio/GPU_version/vio/maskup.png", mup);
-//                cv::cvtColor(maskdown, mdown, cv::COLOR_GRAY2RGB);
-//                cv::imwrite("/root/Projects/ROS_p_33_dev/src/p_33_vio/GPU_version/vio/maskdown.png", mdown);
-//                std::cerr<<"137"<<std::endl;
                 if ((*it_ref)->px.y() < frame->img().rows/2) {
-                    std::cerr<<"if"<<std::endl;
                     matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskup));
                 }
                 else{
-                    std::cerr<<"else"<<std::endl;
                     matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskdown));
                 }
-//                std::cerr<<"144"<<std::endl;
                 //matcher->knnMatch(ref_des, cur_des, matches, 1);
 
-
-
-
                 for(auto&& match:matches.back()){
                     it_cur=keypoints.begin();
                     std::advance(it_cur,match.trainIdx);
@@ -204,6 +189,7 @@ namespace vio {
                 ++it_ref;
             }
         }
+        //exit(0);
         {
             for (auto&& cell : grid_.cells) {
                 if (reprojectCell(*cell, frame, log_))