best config till 20.09.21 the drift on x has been removed

GPU_version/vio/include/vio/homography.h

@@ -45,6 +45,8 @@ public:
 
   Homography            (const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
                          const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
+                         const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts1,
+                         const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts2,
                          double _error_multiplier2,
                          double _thresh_in_px);
 
@@ -69,8 +71,10 @@ public:
 
   double thresh;
   double error_multiplier2;
-  const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c1; //!< Features on first image on unit plane
+  const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c1; //!< Features on first image on px
-  const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c2; //!< Features on second image on unit plane
+  const vector<Vector3d, aligned_allocator<Vector3d> >& pfts_c1; //!< Features on first image in world
+  const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c2; //!< Features on second image on px
+  const vector<Vector3d, aligned_allocator<Vector3d> >& pfts_c2; //!< Features on second image on unit plane
   vector<bool> inliers;
   SE3 T_c2_from_c1;             //!< Relative translation and rotation of two images
   Matrix3d H_c2_from_c1;                   //!< Homography

GPU_version/vio/include/vio/initialization.h

@@ -79,33 +79,31 @@ protected:
             f_cur.at(ftr.index) = frame->c2f(px_cur_.at(ftr.index).x,px_cur_.at(ftr.index).y);
             f_ref.at(ftr.index) = ftr.item->f;
         }
-        vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
+        vk::Homography Homography(uv_ref, uv_cur,f_ref,f_cur, focal_length, reprojection_threshold);
         Homography.computeSE3fromMatches();
-        vector<int> outliers;
+        vector<int> outliers_hom, inlier_hom;
+        vector<int> outliers_ekf,inlier_ekf;
         assert(ukf_!= nullptr);
-        auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2);
+        vk::computeInliers(f_cur, f_ref,
+                           Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
+                           reprojection_threshold, focal_length,
+                           xyz_in_cur, inlier_hom, outliers_hom);
         auto res=ukf_->get_location();
-        size_t in=0;
-        for(auto&& i:Homography.inliers)if(i)++in;
-#if VIO_DEBUG
-      fprintf(log_,"[%s] homography: x=%f y=%f z=%f roll=%f pitch=%f yaw=%f \t ekf: x=%f z=%f pitch=%f\n",
-              vio::time_in_HH_MM_SS_MMM().c_str(),
-              Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().y(),Homography.T_c2_from_c1.translation().z(),
-              euler(0),euler(1),euler(2)
-              ,res.second.se2().translation().x(),res.second.se2().translation().y(),res.second.pitch());
-#endif
-        if(in>20)
-        res=ukf_->UpdateSvo(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
         vk::computeInliers(f_cur, f_ref,
                            res.second.se3().rotation_matrix(), res.second.se3().translation(),
                                reprojection_threshold, focal_length,
-                               xyz_in_cur, inliers, outliers);
+                               xyz_in_cur, inlier_ekf, outliers_ekf);
-#if VIO_DEBUG
+
-      fprintf(log_,"[%s] homography after fuse: x=%f y=%f z=%f pitch=%f\t number of outlier: %d\n",
+        if(inlier_ekf.size()>inlier_hom.size()){
-              vio::time_in_HH_MM_SS_MMM().c_str(),
+            T_cur_from_ref=res.second;
-              res.second.se3().translation().x(),res.second.se3().translation().y(),res.second.se3().translation().z(),res.second.pitch(),outliers.size());
+            inliers=inlier_ekf;
-#endif
+        }else if((Homography.T_c2_from_c1.translation()-res.second.se3().translation()).norm()<0.1 && inlier_ekf.size()<inlier_hom.size()){
-        T_cur_from_ref=res.second;
+            auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2);
+            res=ukf_->UpdateSvo(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
+            T_cur_from_ref=res.second;
+            inliers=inlier_hom;
+        }
+
     }
 };
 

GPU_version/vio/include/vio/ukf.h

@@ -129,12 +129,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(10.0*x,-10.0*z,-1.0*pitch);
+        filter_->correct(x,-1.0*z,-1.0*pitch);
-        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))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),-1.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_);
-        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))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
     }
     bool start_=false;
 private:

GPU_version/vio/src/homography.cpp

@@ -21,12 +21,16 @@ namespace vk {
     Homography::
     Homography(const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
                const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
+               const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts1,
+               const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts2,
                double _error_multiplier2,
                double _thresh_in_px) :
             thresh(_thresh_in_px),
             error_multiplier2(_error_multiplier2),
             fts_c1(_fts1),
-            fts_c2(_fts2)
+            fts_c2(_fts2),
+            pfts_c1(_pfts1),
+            pfts_c2(_pfts2)
     {
     }
 
@@ -49,7 +53,7 @@ namespace vk {
 
         // TODO: replace this function to remove dependency from opencv!
 //        cv::Mat cvH = cv::findHomography(src_pts, dst_pts, CV_RANSAC, 2./error_multiplier2);
-        cv::Mat cvH = cv::findHomography(src_pts, dst_pts, cv::RANSAC, 2./error_multiplier2);//2./error_multiplier2
+        cv::Mat cvH = cv::findHomography(src_pts, dst_pts, cv::RANSAC,2./error_multiplier2 );//2./error_multiplier2
         H_c2_from_c1(0,0) = cvH.at<double>(0,0);
         H_c2_from_c1(0,1) = cvH.at<double>(0,1);
         H_c2_from_c1(0,2) = cvH.at<double>(0,2);
@@ -68,10 +72,8 @@ namespace vk {
         size_t n_inliers = 0;
         for(size_t i=0; i<fts_c1.size(); i++)
         {
-            Vector2d projected = project2d(H_c2_from_c1 * unproject2d(fts_c1[i]));
+            Vector3d e = pfts_c2[i] - (H_c2_from_c1 * pfts_c1[i]);
-            Vector2d e = fts_c2[i] - projected;
+            inliers[i] = (e.norm() < 15.0);
-            double e_px = error_multiplier2 * e.norm();
-            inliers[i] = (e_px < thresh);
             n_inliers += inliers[i];
         }
         return n_inliers;
@@ -85,7 +87,7 @@ namespace vk {
         bool res = decompose();
         if(!res)
             return false;
-        computeMatchesInliers();
+        //computeMatchesInliers();
         findBestDecomposition();
         Eigen::Quaternion<double> q(decompositions.front().T.rotation_matrix());
         q.vec() *= 1/q.norm();
@@ -155,7 +157,7 @@ namespace vk {
             decomp.t[1] = 0.0;
             decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
 
-            np[0] = x1_PM * e1[signs];
+            np[0] = 0.0;//x1_PM * e1[signs];
             np[1] = 0.0;
             np[2] = x3_PM * e3[signs];
             decomp.n = V * np;
@@ -171,9 +173,9 @@ namespace vk {
             decomp.R = -1 * Matrix3d::Identity();
             double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
             double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
-            /*
+
-            * correct the rotation matrix by Majid
+            // correct the rotation matrix by Majid
-           */
+
             decomp.R(0,0) = dCosPhi;
             decomp.R(0,2) = dSinPhi;
             decomp.R(2,0) = dSinPhi;
@@ -184,7 +186,7 @@ namespace vk {
             decomp.t[1] = 0.0;
             decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
 
-            np[0] = x1_PM * e1[signs];
+            np[0] = 0.0;//x1_PM * e1[signs];
             np[1] = 0.0;
             np[2] = x3_PM * e3[signs];
             decomp.n = V * np;
@@ -217,8 +219,8 @@ namespace vk {
             size_t nPositive = 0;
             for(size_t m=0; m<fts_c1.size(); m++)
             {
-                if(!inliers[m])
+/*                if(!inliers[m])
-                    continue;
+                    continue;*/
                 const Vector2d& v2 = fts_c1[m];
                 double dVisibilityTest = (H_c2_from_c1(2,0) * v2[0] + H_c2_from_c1(2,1) * v2[1] + H_c2_from_c1(2,2)) / decom.d;
                 if(dVisibilityTest > 0.0)
@@ -236,9 +238,9 @@ namespace vk {
             int nPositive = 0;
             for(size_t m=0; m<fts_c1.size(); m++)
             {
-                if(!inliers[m])
+/*                if(!inliers[m])
-                    continue;
+                    continue;*/
-                Vector3d v3 = unproject2d(fts_c1[m]);
+                Vector3d v3 = pfts_c1[m];
                 double dVisibilityTest = v3.dot(decom.n) / decom.d;
                 if(dVisibilityTest > 0.0)
                     nPositive++;

GPU_version/vio/src/vo_node.cpp

@@ -159,7 +159,7 @@ void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
 #if VIO_DEBUG
     auto odom=vo_->ukfPtr_.get_location();
     fprintf(vo_->log_,"[%s] Odometry x=%f, y=%f, theta=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
-            10.0*odom.second.se2().translation()(0),10.0*odom.second.se2().translation()(1),
+            odom.second.se2().translation()(0),odom.second.se2().translation()(1),
             odom.second.pitch());
 #endif
 }