Merge branch 'dev2' into development

# Conflicts:
#	GPU_version/vio/frame_handler_log.txt

GPU_version/vio/include/vio/point.h

@@ -100,6 +100,45 @@ public:
     point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
     point_jac = - point_jac * R_f_w;
   }
+
+    void jacobian_xyz2uv_(
+            const Vector3d& p_in_f,
+            const Matrix3d& R_f_w,
+            Matrix23d& point_jac,
+            double * cam_params,
+            SE2_5 fram_t_f_w)
+    {
+        double fx = cam_params[0];
+        double fy = cam_params[1];
+        double s = cam_params[4];
+        double r = cam_params[5];
+        double x_n = p_in_f.x();
+        double y_n = p_in_f.y();
+        double z_n = p_in_f.z();
+        double x_c =  fram_t_f_w.se3().translation().x();
+        double z_c = fram_t_f_w.se3().translation().y();
+        double theta = fram_t_f_w.pitch();
+
+        double alpha = (fx*(theta/r))-(fx*((x_n*x_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fx*x_n*x_n)/(r*r));
+        double beta  =               -(fx*((x_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fx*x_n*y_n)/(r*r));
+        double gamma =               -(fy*((x_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fy*x_n*y_n)/(r*r));
+        double lamda = (fy*(theta/r))-(fy*((y_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fy*y_n*y_n)/(r*r));
+
+        double Xf_Xc = x_n - x_c;
+        double Zf_Zc = z_n - z_c;
+        double n1 = -1*sin(theta)*Xf_Xc + cos(theta)*Zf_Zc;
+        double n2 = -1*cos(theta)*Xf_Xc - sin(theta)*Zf_Zc;
+
+        point_jac(0, 0) = ((cos(theta)/z_n)*alpha)+(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*sin(theta));
+        point_jac(0, 1) = ((sin(theta)/z_n)*alpha)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*cos(theta));
+        point_jac(0, 2) = ((1/z_n)*alpha*n1)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n2);
+
+        point_jac(1, 0) = ((cos(theta)/z_n)*gamma)+(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*sin(theta));
+        point_jac(1, 1) = ((sin(theta)/z_n)*gamma)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*cos(theta));
+        point_jac(1, 2) = ((1/z_n)*gamma*n1)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n2);
+        //point_jac = - point_jac * R_f_w;
+    }
+
 };
 
 } // namespace vio

GPU_version/vio/include/vio/reprojector.h

@@ -66,7 +66,8 @@ public:
       FramePtr frame,
       FramePtr last_frame,
       std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
-      opencl* gpu_fast_);
+      opencl* gpu_fast_,
+      FILE* log_);
 
 
 private:
@@ -99,7 +100,7 @@ private:
   static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
   void initializeGrid(vk::AbstractCamera* cam);
   void resetGrid();
-  bool reprojectCell(Cell& cell, FramePtr frame);
+  bool reprojectCell(Cell& cell, FramePtr frame, FILE* log_);
 };
 
 } // namespace vio

GPU_version/vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

GPU_version/vio/src/feature_alignment.cpp

@@ -216,6 +216,7 @@ bool align2D(
 
     if(isnan(u) || isnan(v)) {
         // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+        std::cerr<<"if is nan"<<std::endl;
       return false;
     }
 

GPU_version/vio/src/frame_handler_mono.cpp

@@ -194,7 +194,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
       new_frame_=last_frame_;
       return RESULT_FAILURE;
   }
-  reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_);
+  reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_);
   int n_point=0;
   for(auto i:overlap_kfs_)n_point+=i.second;
 #if VIO_DEBUG

GPU_version/vio/src/matcher.cpp

@@ -41,7 +41,7 @@ void getWarpMatrixAffine(
     Matrix2d& A_cur_ref)
 {
   // Compute affine warp matrix A_ref_cur
-  const int halfpatch_size = 5;
+  const int halfpatch_size = 4;
   const Vector3d xyz_ref(f_ref*depth_ref);
   Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
   Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
@@ -160,7 +160,7 @@ bool Matcher::findMatchDirect(
   // warp affine
   warp::getWarpMatrixAffine(
       *ref_ftr_->frame->cam_, *(cur_frame.cam_), ref_ftr_->px, ref_ftr_->f,
-      (Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(),
+      (ref_ftr_->frame->se3().inverse()*pt.pos_).norm(),/*(Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(),*/
       cur_frame.se3().inverse() * ref_ftr_->frame->se3(), ref_ftr_->level, A_cur_ref_);
   search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
   if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,

GPU_version/vio/src/point.cpp

@@ -155,7 +155,8 @@ void Point::optimize(const size_t n_iter)
     {
       Matrix23d J;
       const Vector3d p_in_f((*it)->frame->se3()*pos_);
-      Point::jacobian_xyz2uv(p_in_f, (*it)->frame->se3().rotation_matrix(), J);
+      //Point::jacobian_xyz2uv(p_in_f, (*it)->frame->se3().rotation_matrix(), J);
+      Point::jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
       const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
       new_chi2 += e.squaredNorm();
       A.noalias() += J.transpose() * J;

GPU_version/vio/src/reprojector.cpp

@@ -69,7 +69,8 @@ namespace vio {
             FramePtr frame,
             FramePtr last_frame,
             std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs,
-            opencl* gpu_fast_) {
+            opencl* gpu_fast_,
+            FILE* log_) {
         if(frame->id_<1)return;
         resetGrid();
         cv::Mat descriptors_cur;
@@ -142,13 +143,22 @@ namespace vio {
             }
         }
         {
+            int cl = 0;
             for (auto&& cell : grid_.cells) {
-                if (reprojectCell(*cell, frame))
+#if VIO_DEBUG
+                if(cell->size()>=1) {
+                    fprintf(log_, "[%s] %d-%d/%d : \n", vio::time_in_HH_MM_SS_MMM().c_str(), frame->id_, ++cl,
+                            grid_.cells.size());
+                }
+#endif
+                //std::cerr<<frame->id_<<"-"<<++cl<<"/"<<grid_.cells.size()<<" : ";
+                if (reprojectCell(*cell, frame, log_))
                     ++n_matches_;
                 if (n_matches_ > (size_t) Config::maxFts())
                     break;
             }
         }
+        //std::exit(154);
     }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -158,8 +168,11 @@ namespace vio {
         return false;
     }
 
-    bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) {
+    bool Reprojector::reprojectCell(Cell &cell, FramePtr frame, FILE* log_) {
-        if(cell.size()<1)return false;
+        //cv::Mat image = frame->img();
+        if(cell.size()<1){
+            return false;
+        }
         cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
         Cell::iterator it=cell.begin();
         while(it!=cell.end())
@@ -175,8 +188,22 @@ namespace vio {
                 continue;
             }
 ///TODO check point candidate handling
-
+            //cv::Point pointb(it->px.x(), it->px.y());
-            if(!matcher_.findMatchDirect(*it->pt, *frame, it->px))
+            //cv::circle(image, pointb,1,(255, 0, 0), 1);
+            Vector2d uvb(it->px.x(), it->px.y());
+#if VIO_DEBUG
+            fprintf(log_,"befor : %f  %f  after : ", it->px.x(), it->px.y());
+#endif
+            //std::cerr<<"before : "<<it->px.x()<<" "<<it->px.y()<<" after : ";
+            bool res = matcher_.findMatchDirect(*it->pt, *frame, it->px);
+            Vector2d uva(it->px.x(), it->px.y());
+#if VIO_DEBUG
+            fprintf(log_,"%f  %f  %s %f\n", it->px.x(), it->px.y(), (res?" TRUE":" FALSE"), (uvb - uva).squaredNorm());
+#endif
+            //std::cerr<<it->px.x()<<" "<<it->px.y()<<(res?" TRUE":" FALSE")<<std::endl;
+            //cv::Point pointa(it->px.x(), it->px.y());
+            //cv::circle(image, pointa,1,(0, 255, 0), 1);
+            if(!res)
             {
                 it->pt->n_failed_reproj_++;
 /*                if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
@@ -214,6 +241,8 @@ namespace vio {
             // Maximum one point per cell.
             return true;
         }
+        //cv::imwrite("/root/Projects/ROS_p_33_dev/src/p_33_vio/GPU_version/vio/image.png", image);
+        //std::exit(223);
         return false;
     }