working version 30th

GPU_version/vio/include/vio/ukf.h

@@ -129,7 +129,7 @@ public:
         while(lock)usleep(5);
         lock=true;
         roll=roll+M_PI;
-        if(filter_->predict_up)filter_->correct(-z,-y,roll,match,1e-9*time.toNSec());
+        if(filter_->predict_up)filter_->correct(z,y,roll,match,1e-9*time.toNSec());
         lock=false;
         double roll_f;
         roll_f=filter_->state_(2)+M_PI;

GPU_version/vio/src/reprojector.cpp

@@ -222,6 +222,7 @@ void Reprojector::reprojectMap(
         extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
         list< pair<FramePtr,double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);
+        std::cerr<<close_kfs.size()<<'\n';
         close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                        boost::bind(&std::pair<FramePtr, double>::second, _2));
         size_t n = 0;