correct triangulation

GPU_version/vio/include/vio/global.h

@@ -122,7 +122,7 @@ namespace vio
             //Todo add 15 roll orientation
             //Camera frame z front, x right, y down -> right hands
             Quaterniond q;
-            q = AngleAxisd(-0.122173, Vector3d::UnitX()) // roll
+            q = AngleAxisd(0.122173, Vector3d::UnitX()) // roll
                 * AngleAxisd(pitch(), Vector3d::UnitY())// pitch
                 * AngleAxisd(0.0, Vector3d::UnitZ()); //yaw
             SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));

GPU_version/vio/include/vio/ukf.h

@@ -147,12 +147,12 @@ public:
     //Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
     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*/) {
         boost::unique_lock<boost::mutex> lock(ekf_mut_);
-        filter_->correct(x,z,pitch);
+        filter_->correct(x,z,-1.0*pitch);
         Eigen::Matrix<double,3,3> cov;
         cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2),
                 filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2),
                 filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),(filter_->state_(2))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2))));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
         boost::unique_lock<boost::mutex> lock(ekf_mut_);
@@ -160,7 +160,7 @@ public:
         cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2),
                 filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2),
                 filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),(filter_->state_(2))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2))));
     }
     size_t cmd_syn_count_=0;
     size_t imu_syn_count_=2;

GPU_version/vio/kernel/compute-residual.cl

@@ -26,7 +26,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
     float3 error=ref+cur;
     float yaw=0.0;
     float pitch=error.z;
-    float roll=-0.122173;
+    float roll=0.122173;
     float R00=cos(yaw)*cos(pitch);//cos(pitch)
     float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
     float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)

GPU_version/vio/param/vo_fast.yaml

@@ -5,11 +5,11 @@ svo:
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
   quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
   init_min_tracked: 100
-  init_min_disparity: 40
+  init_min_disparity: 20
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
   poseoptim_thresh: 8.0
-  init_min_inliers: 20
+  init_min_inliers: 25
   ACC_noise: 0.5          #acc white noise in continuous in EKF
   GYO_noise: 0.5         #gyro white noise in continuous in EKF
   svo_ekf: 0.05              #Svo noise that will be devided by number of Reprojected points after opmization in EKF

GPU_version/vio/src/feature_detection.cpp

@@ -117,7 +117,7 @@ void FastDetector::detect(
   }
   if(fts.size()<1){
       ROS_ERROR("GPU Driver crash try again!");
-      exit(0);
+      assert(false);
   }
   if(descriptors){
       cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);

GPU_version/vio/src/frame_handler_mono.cpp

@@ -249,7 +249,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   for(auto&& it:new_frame_->fts_){
       if(it->point != NULL)it->point->addFrameRef(it);
   }
-  //map_.point_candidates_.addCandidatePointToFrame(new_frame_);
 
   // if limited number of keyframes, remove the one furthest apart
   if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())

GPU_version/vio/src/initialization.cpp

@@ -90,7 +90,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
       std::advance(first,id);
       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 */);
+          Vector3d pos = xyz_in_cur_.at(id);//frame_cur->se3() * (xyz_in_cur_.at(id)/* *scale */);
 /*#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(),

GPU_version/vio/src/math_utils.cpp

@@ -22,7 +22,7 @@ triangulateFeatureNonLin(const Matrix3d& R,  const Vector3d& t,
   Vector3d f2 = R * feature2;
   Vector2d b;
   b[0] = t.dot(feature1);
-  b[1] = t.dot(f2);
+  b[1] = t.dot(f2);//cross(t,R*f2)
   Matrix2d A;
   A(0,0) = feature1.dot(feature1);
   A(1,0) = feature1.dot(f2);
@@ -67,13 +67,13 @@ reprojError(const Vector3d& f1,
 }
 
 double
-computeInliers(const vector<Vector3d>& features1, // c2
-               const vector<Vector3d>& features2, // c1
+computeInliers(const vector<Vector3d>& features2, // c2
+               const vector<Vector3d>& features1, // c1
                const Matrix3d& R,                 // R_c1_to_c2
                const Vector3d& t,                 // T_c1_to_c2
                const double reproj_thresh,
                double error_multiplier2,
-               vector<Vector3d>& xyz_vec,         // in frame c1
+               vector<Vector3d>& xyz_vec,         // 3d point: optimal triangulation is to minimally correct the feature rays
                vector<int>& inliers,
                vector<int>& outliers)
 {
@@ -85,10 +85,10 @@ computeInliers(const vector<Vector3d>& features1, // c2
   for(size_t j=0; j<features1.size(); ++j)
   {
     if(t.z()>0.0){
-        xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
+        xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*refrence*/, features2[j]/*current*/ ));
         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 || xyz_vec.back().z()<0.0)
+        if(e1 > reproj_thresh || e2 > reproj_thresh)
             outliers.push_back(j);
         else
         {

GPU_version/vio/src/reprojector.cpp

@@ -120,7 +120,7 @@ namespace vio {
                                                                         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>(frame->se3()*new_point,f.item),
+                                                                        std::make_shared<Point>(it_frame.item.first->se3()*new_point,f.item),
                                                                                 px,f.item->f,(*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));