debug feature score in fast

GPU_version/vio/include/vio/ukf.h

@@ -132,12 +132,12 @@ public:
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
         while(lock)usleep(5);
         lock=true;
-        filter_->correct(10.0*x,-1.0*z,-1.0*pitch);
+        filter_->correct(10.0*x,-10.0*z,-1.0*pitch);
         lock=false;
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
     }
     bool start_=false;
 private:

GPU_version/vio/src/feature_detection.cpp

@@ -109,7 +109,7 @@ void FastDetector::detect(
       }
       const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
       if(score > corners.at(k).score){
-          corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
+          corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f);
           fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y)*scale, corners.at(k).level));
           if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 1));
       }

GPU_version/vio/src/initialization.cpp

@@ -84,14 +84,14 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
   }
     frame_cur->T_f_w_ = T_cur_from_ref_;
     // Rescale the map such that the mean scene depth is equal to the specified scale
-    vector<double> depth_vec;
+/*    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_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
     frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
-                                  -frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()))));
+                                  -frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()))));*/
   // For each inlier create 3D point and add feature in both frames
   for(auto&& id:inliers_){
       Features::iterator first=features_ref_.begin();

GPU_version/vio/src/vo_node.cpp

@@ -127,9 +127,9 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
           cv::Laplacian(frame,imgbul,CV_64F);
           cv::Scalar mean, stddev;
           meanStdDev(imgbul, mean, stddev, cv::Mat());
-          cv::cvtColor(frame,show,CV_GRAY2RGB);
+/*          cv::cvtColor(frame,show,CV_GRAY2RGB);
           imshow("New Image", show);
-          cv::waitKey(10);
+          cv::waitKey(10);*/
           if(stddev.val[0] * stddev.val[0]< 30.0){
               ROS_WARN("Frame is blur or too dark");
               return;