add triangulation in reprojection

GPU_version/vio/include/vio/initialization.h

@@ -84,6 +84,12 @@ protected:
         vector<int> outliers_hom, inlier_hom;
         vector<int> outliers_ekf,inlier_ekf;
         assert(ukf_!= nullptr);
+/*#if VIO_DEBUG
+        for(int i=0;i<f_cur.size();++i)fprintf(log_,"[%s] cur: x=%f, y=%f, z=%f ref: x=%f y=%f z=%f\n",
+              vio::time_in_HH_MM_SS_MMM().c_str(),
+              f_cur.at(i).x(),f_cur.at(i).y(),f_cur.at(i).z(),
+              f_ref.at(i).x(),f_ref.at(i).y(),f_ref.at(i).z());
+#endif*/
         vk::computeInliers(f_cur, f_ref,
                            Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
                            reprojection_threshold, focal_length,

GPU_version/vio/src/depth_filter.cpp

@@ -67,29 +67,18 @@ namespace vio {
     DepthFilter::~DepthFilter()
     {
         stopThread();
-#if VIO_DEBUG
-        fprintf(log_,"[%s] DepthFilter destructed.\n",vio::time_in_HH_MM_SS_MMM().c_str());
-#endif
     }
 
     void DepthFilter::startThread()
     {
         thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this);
-#if VIO_DEBUG
-        fprintf(log_,"[%s] init depth filter\n",vio::time_in_HH_MM_SS_MMM().c_str());
-#endif
     }
 
     void DepthFilter::stopThread()
     {
-#if VIO_DEBUG
+
-        fprintf(log_,"[%s] DepthFilter stop thread invoked.\n",vio::time_in_HH_MM_SS_MMM().c_str());
-#endif
         if(thread_ != NULL)
         {
-#if VIO_DEBUG
-            fprintf(log_,"[%s] DepthFilter interrupt and join thread \n",vio::time_in_HH_MM_SS_MMM().c_str());
-#endif
             seeds_updating_halt_ = true;
             thread_->interrupt();
             usleep(5000);
@@ -322,7 +311,7 @@ namespace vio {
             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->getSE3Inv() * ((*it)->ftr->f/(*it)->mu));
+                Vector3d xyz_world((*it)->ftr->frame->se3() * ((*it)->ftr->f/(*it)->mu));
                 std::shared_ptr<Point> point = std::make_shared<Point>(xyz_world, (*it)->ftr);
                 (*it)->ftr->point = point;
                 seed_converged_cb_(point, (*it)->sigma2); // put in candidate list

GPU_version/vio/src/feature_detection.cpp

@@ -98,7 +98,7 @@ void FastDetector::detect(
     }
     for(uint i=0;i<count[0];++i)
     {
-      if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0){
+      if(fast_corners[i].x<1 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){
           continue;
       }
       const int k = static_cast<int>((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_
@@ -115,6 +115,10 @@ void FastDetector::detect(
       }
     }
   }
+  if(fts.size()<1){
+      ROS_ERROR("GPU Driver crash try again!");
+      exit(0);
+  }
   if(descriptors){
       cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
       extractor->compute(frame->img(), keypoints, *descriptors);

GPU_version/vio/src/frame_handler_mono.cpp

@@ -173,6 +173,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
     fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n",
                        vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
 #endif
+  ROS_INFO("VIO initialized :)");
+  ROS_INFO("Running ...");
   return RESULT_IS_KEYFRAME;
 }
 

GPU_version/vio/src/initialization.cpp

@@ -100,12 +100,12 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
       if(frame_cur->cam_->isInFrame(Vector2d(px_cur_.at(id).x,px_cur_.at(id).y).cast<int>(), 10) &&
       frame_ref_->cam_->isInFrame((*first)->px.cast<int>(), 10)){
           Vector3d pos = frame_cur->se3() * (xyz_in_cur_.at(id)/* *scale */);
-#if VIO_DEBUG
+/*#if VIO_DEBUG
           fprintf(log_,"[%s] inlier: x=%f, y=%f, z=%f pos: x=%f y=%f z=%f\n",
                   vio::time_in_HH_MM_SS_MMM().c_str(),
                   xyz_in_cur_.at(id).x(),xyz_in_cur_.at(id).y(),xyz_in_cur_.at(id).z(),
                   pos.x(),pos.y(),pos.z());
-#endif
+#endif*/
           std::shared_ptr<Point> new_point = std::make_shared<Point>(pos);
           std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, Vector2d(px_cur_.at(id).x,px_cur_.at(id).y),
                                                                      frame_cur->c2f(px_cur_.at(id).x,px_cur_.at(id).y), (*first)->level);

GPU_version/vio/src/math_utils.cpp

@@ -64,10 +64,10 @@ reprojError(const Vector3d& f1,
 }
 
 double
-computeInliers(const vector<Vector3d>& features1, // c1
+computeInliers(const vector<Vector3d>& features1, // c2
-               const vector<Vector3d>& features2, // c2
+               const vector<Vector3d>& features2, // c1
-               const Matrix3d& R,                 // R_c1_c2
+               const Matrix3d& R,                 // R_c1_to_c2
-               const Vector3d& t,                 // c1_t
+               const Vector3d& t,                 // T_c1_to_c2
                const double reproj_thresh,
                double error_multiplier2,
                vector<Vector3d>& xyz_vec,         // in frame c1
@@ -81,21 +81,17 @@ computeInliers(const vector<Vector3d>& features1, // c1
   //triangulate all features and compute reprojection errors and inliers
   for(size_t j=0; j<features1.size(); ++j)
   {
-    if(t.z()>0.0){
+    if(t.z()>0.0)
         xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
         double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
         double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
-        if(e1 > reproj_thresh || e2 > reproj_thresh)
+        if(e1 > reproj_thresh || e2 > reproj_thresh || xyz_vec.back().z()<0.0)
             outliers.push_back(j);
         else
         {
             inliers.push_back(j);
             tot_error += e1+e2;
         }
-    }else{
-        ROS_ERROR("Backward motion is not supported for initialization, the robot needs to start its motion with a forward maneuver");
-    }
-
   }
   return tot_error;
 }

GPU_version/vio/src/reprojector.cpp

@@ -116,10 +116,13 @@ namespace vio {
                         assert(grid_.cells.at(k) != nullptr);
                         Vector2d px((int) (*point)->px.x(),
                                     (int) (*point)->px.y());
+                        SE3 T_ref_cur=it_frame.item.first->getSE3Inv()*frame->se3();
+                        Vector3d new_point=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
+                                                                        frame->c2f(px),f.item->f);
+                        if(new_point.z()<0.0)break;
                         frame->fts_.push_back(std::make_shared<Feature>(it_frame.item.first,
-                                                                        std::make_shared<Point>(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
+                                                                        std::make_shared<Point>(frame->se3()*new_point,f.item),
-                                px,
+                                                                                px,f.item->f,(*point)->level));
-                                it_frame.item.first->cam_->cam2world(f.item->px),(*point)->level));
                         frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
                         grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
                         overlap_kfs.back().second++;