track the trajectory with noise

GPU_version/svo/include/gpu_svo/frame.h

@@ -36,6 +36,7 @@ struct Feature;
 typedef list<Feature*> Features;
 typedef vector<cv::Mat> ImgPyr;
 
+
 /// A frame saves the image, the associated features and the estimated pose.
 class Frame : boost::noncopyable
 {
@@ -46,7 +47,7 @@ public:
   int                           id_;                    //!< Unique id of the frame.
   double                        timestamp_;             //!< Timestamp of when the image was recorded.
   vk::AbstractCamera*           cam_;                   //!< Camera model.
-  Sophus::SE3                   T_f_w_;                 //!< Transform (f)rame from (w)orld.
+  SE2_5                         T_f_w_;                 //!< Transform (f)rame from (w)orld.
   Matrix<double, 6, 6>          Cov_;                   //!< Covariance.
   ImgPyr                        img_pyr_;               //!< Image Pyramid.
   Features                      fts_;                   //!< List of features in the image.
@@ -91,7 +92,11 @@ public:
   inline bool isKeyframe() const { return is_keyframe_; }
 
   /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
-  inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); }
+  inline Vector2d w2c(const Vector3d& xyz_w) const {
+      return cam_->world2cam( Vector3d(xyz_w.x()*T_f_w_.cos_theta_+xyz_w.z()*T_f_w_.sin_theta_+T_f_w_.X_,
+                                       xyz_w.y(),
+                                       -xyz_w.x()*T_f_w_.sin_theta_+xyz_w.z()*T_f_w_.cos_theta_+T_f_w_.Z_) );
+  }
   /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
   inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); }
 
@@ -102,16 +107,22 @@ public:
   inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); }
 
   /// Transforms point coordinates in world-frame (w) to camera-frams (f).
-  inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; }
+  inline Vector3d w2f(const Vector3d& xyz_w) const {
+      return Vector3d(xyz_w.x()*T_f_w_.cos_theta_+xyz_w.z()*T_f_w_.sin_theta_+T_f_w_.X_,
+                      xyz_w.y(),
+                      -xyz_w.x()*T_f_w_.sin_theta_+xyz_w.z()*T_f_w_.cos_theta_+T_f_w_.Z_);
+  }
 
   /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
-  inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; }
+  inline Vector3d f2w(const Vector3d& f) const {
+      return T_f_w_.T->inverse() * f;
+  }
 
   /// Projects Point from unit sphere (f) in camera pixels (c).
   inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
 
   /// Return the pose of the frame in the (w)orld coordinate frame.
-  inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
+  inline Vector3d pos() const { return T_f_w_.T->inverse().translation(); }
 
 
   /// Frame jacobian for projection of 3D point in (f)rame coordinate to

GPU_version/svo/include/gpu_svo/global.h

@@ -27,7 +27,6 @@
 #include <Eigen/Core>
 #include <opencv2/opencv.hpp>
 #include <sophus/se3.h>
-//#include <vikit/performance_monitor.h>
 #include <boost/shared_ptr.hpp>
 #include<Eigen/StdVector>
 #ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
@@ -35,39 +34,13 @@
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
 #endif
+#include <ros/console.h>
+#define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
+#define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
+#define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
+#define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
+#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
 
-#ifdef SVO_USE_ROS
-  #include <ros/console.h>
-  #define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
-  #define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
-  #define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
-  #define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
-  #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
-#else
-  #define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "<<x<<"\033[0;0m"<<std::endl;
-  #ifdef SVO_DEBUG_OUTPUT
-    #define SVO_DEBUG_STREAM(x) SVO_INFO_STREAM(x)
-  #else
-    #define SVO_DEBUG_STREAM(x)
-  #endif
-  #define SVO_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<<x<<"\033[0;0m"<<std::endl;
-  #define SVO_ERROR_STREAM(x) std::cerr<<"\033[1;31m[ERROR] "<<x<<"\033[0;0m"<<std::endl;
-  #include <chrono> // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc.
-  #define SVO_WARN_STREAM_THROTTLE(rate, x) \
-    do { \
-      static double __log_stream_throttle__last_hit__ = 0.0; \
-      std::chrono::time_point<std::chrono::system_clock> __log_stream_throttle__now__ = \
-      std::chrono::system_clock::now(); \
-      if (__log_stream_throttle__last_hit__ + rate <= \
-          std::chrono::duration_cast<std::chrono::seconds>( \
-          __log_stream_throttle__now__.time_since_epoch()).count()) { \
-        __log_stream_throttle__last_hit__ = \
-        std::chrono::duration_cast<std::chrono::seconds>( \
-        __log_stream_throttle__now__.time_since_epoch()).count(); \
-        SVO_WARN_STREAM(x); \
-      } \
-    } while(0)
-#endif
 
 namespace svo
 {
@@ -76,25 +49,21 @@ namespace svo
 
   const double EPS = 0.0000000001;
   const double PI = 3.14159265;
-
+  class SE2_5{
-#ifdef SVO_TRACE
+  public:
-  /*
+        double X_,Z_,tetha_,cos_theta_,sin_theta_;
-  extern vk::PerformanceMonitor* g_permon;
+        SE3* T;
-  #define SVO_LOG(value) g_permon->log(std::string((#value)),(value))
+        SE2_5(double x,double z,double tetha):X_(x),Z_(z),tetha_(tetha){
-  #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2)
+            cos_theta_=cos(tetha_);
-  #define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3)
+            sin_theta_=sin(tetha_);
-  #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4)
+            Eigen::Matrix<double, 3,3> R;
-  #define SVO_START_TIMER(name) g_permon->startTimer((name))
+            R<<cos_theta_,0.0,sin_theta_,
-  #define SVO_STOP_TIMER(name) g_permon->stopTimer((name))
+               0.0,1.0,0.0,
-   */
+               -sin_theta_,0.0,cos_theta_;
-#else
+            T=new SE3(R,Vector3d(X_,0.0,Z_));
-  #define SVO_LOG(v)
+        };
-  #define SVO_LOG2(v1, v2)
+        ~SE2_5(){}
-  #define SVO_LOG3(v1, v2, v3)
+  };
-  #define SVO_LOG4(v1, v2, v3, v4)
-  #define SVO_START_TIMER(name)
-  #define SVO_STOP_TIMER(name)
-#endif
 
   class Frame;
   typedef boost::shared_ptr<Frame> FramePtr;

GPU_version/svo/include/gpu_svo/imu_integration.h

@@ -32,6 +32,7 @@ public:
     bool init(boost::shared_ptr<svo::Frame>&);
     Sophus::SE3 preintegrate_predict();
 private:
+    FILE* logtime;
     std::shared_ptr<gtsam::NavState> statePtr;
     std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
     std::shared_ptr<gtsam::Values> valuesPtr;

GPU_version/svo/include/gpu_svo/svoVisionFactor.h

@@ -16,12 +16,11 @@
 
 namespace svo {
 
-    class VisionFactor: public gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3> {
+    class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
 
     private:
         // measurement information
-        gtsam::Point3 feature;   ///<Unit-bearing vector of the feature.
+        gtsam::Pose3 pose;   ///<Estimated pose in svo after GaussNewton optimization.
-        int level;            ///< Image pyramid level where feature was extracted.
 
     public:
         /// shorthand for a smart pointer to a factor
@@ -29,36 +28,28 @@ namespace svo {
 
         /**
         * Constructor
-        * @param feature Unit-bearing vector of the feature.
+        * @param pose Camera pose.
-        * @param model is the standard deviation
         * @param pointKey is the index of the landmark
-        * @param poseKey is the index of the camera pose
-        * @param landmark 3D position of the landmark
-        * @param body_P_sensor is the transform from body to sensor frame (default identity)
-        * @param level Image pyramid level where feature was extracted.
         */
-        VisionFactor(const gtsam::Point3& feature,const int level,
+        VisionFactor(const gtsam::Pose3& pose,
                      const gtsam::SharedNoiseModel& H,
-                      gtsam::Key poseKey,gtsam::Key landKey ) :
+                      gtsam::Key poseKey) :
-                gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3>(H, poseKey,landKey), feature(feature),level(level){}
+                gtsam::NoiseModelFactor1<gtsam::Pose3>(H, poseKey), pose(pose){}
         virtual ~VisionFactor(){}
 
         /// error function
         /// @param p    the Estimated pose in Pose3
         /// @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
-        gtsam::Vector evaluateError(const gtsam::Pose3& p,const gtsam::Point3& l,
+        gtsam::Vector evaluateError(const gtsam::Pose3& p
-                                    boost::optional<gtsam::Matrix&> H1 = boost::none,boost::optional<gtsam::Matrix&> H2 = boost::none) const {
+                                    ,boost::optional<gtsam::Matrix&> H1 = boost::none) const {
             // note that use boost optional like a pointer
             // only calculate jacobian matrix when non-null pointer exists
-            gtsam::Matrix26 J1;
+            Eigen::Matrix<double,3,3> E_r=0.5*(p.rotation().matrix().transpose()*pose.rotation().matrix()-pose.rotation().matrix().transpose()*p.rotation().matrix());
-            gtsam::Matrix23 J2;
+            Vector3d e_t=p.translation()-pose.translation();
-            gtsam::Point3 point(p*l);
+            if(H1)(*H1) = (gtsam::Matrix(3,6)<< 1,0,0,0,0,0,
-            svo::Frame::jacobian_xyz2uv(point, J1);
+                                                      0,0,1,0,0,0,
-            svo::Frame::jacobian_l2uv(point, J2);
+                                                      0,0,0,0,1,0).finished();
-            Vector2d e=(vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
+            return (gtsam::Matrix(3,1)<<e_t.x(),e_t.z(),-E_r(0,2)).finished();
-            if(H1)(*H1) = (gtsam::Matrix(2,6)<< J1*1.0 / (1<<level)).finished();
-            if(H2)(*H2) = (gtsam::Matrix(2,3)<< J2*1.0 / (1<<level)).finished();
-            return e;
         }
         // The second is a 'clone' function that allows the factor to be copied. Under most
         // circumstances, the following code that employs the default copy constructor should

GPU_version/svo/src/depth_filter.cpp

@@ -219,7 +219,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
     }
 
     // check if point is visible in the current image
-    SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
+    SE3 T_ref_cur = *it->ftr->frame->T_f_w_.T * frame->T_f_w_.T->inverse();
     const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
     if(xyz_f.z() < 0.0)  {
       ++it; // behind the camera
@@ -261,7 +261,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
     if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
     {
       assert(it->ftr->point == NULL); // TODO this should not happen anymore
-      Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
+      Vector3d xyz_world(it->ftr->frame->T_f_w_.T->inverse() * (it->ftr->f * (1.0/it->mu)));
       Point* point = new Point(xyz_world, it->ftr);
       it->ftr->point = point;
       /* FIXME it is not threadsafe to add a feature to the frame here.

GPU_version/svo/src/feature_alignment.cpp

@@ -25,7 +25,7 @@
 namespace svo {
 namespace feature_alignment {
 
-#define SUBPIX_VERBOSE 1
+#define SUBPIX_VERBOSE 0
 
 bool align1D(
     const cv::Mat& cur_img,
@@ -77,11 +77,16 @@ bool align1D(
   {
     int u_r = floor(u);
     int v_r = floor(v);
-    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
+    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_){
-      break;
+        std::cout<<"not in bach size\n";
+        break;
+    }
 
-    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+    if(isnan(u) || isnan(v)){
-      return false;
+        // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+        std::cout<<"H singularity\n";
+        return false;
+    }
 
     // compute interpolation weights
     float subpix_x = u-u_r;
@@ -193,9 +198,12 @@ bool align2D(
   // Compute pixel location in new image:
   float u = cur_px_estimate.x();
   float v = cur_px_estimate.y();
+  double error=1000.0;
+  double scale=1.0;
+  bool sign=true;// +
 
   // termination condition
-  const float min_update_squared = 0.03*0.03;
+  const float min_update_squared = 0.05;
   const int cur_step = cur_img.step.p[0];
 //  float chi2 = 0;
   Vector3f update; update.setZero();
@@ -203,11 +211,14 @@ bool align2D(
   {
     int u_r = floor(u);
     int v_r = floor(v);
-    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
+    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_){
-      break;
+        break;
+    }
 
-    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+    if(isnan(u) || isnan(v)) {
+        // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
       return false;
+    }
 
     // compute interpolation weights
     float subpix_x = u-u_r;
@@ -233,36 +244,39 @@ bool align2D(
         Jres[0] -= res*(*it_ref_dx);
         Jres[1] -= res*(*it_ref_dy);
         Jres[2] -= res;
-//        new_chi2 += res*res;
       }
     }
-
-
-/*
-    if(iter > 0 && new_chi2 > chi2)
-    {
-#if SUBPIX_VERBOSE
-      cout << "error increased." << endl;
-#endif
-      u -= update[0];
-      v -= update[1];
-      break;
-    }
-    chi2 = new_chi2;
-*/
     update = Hinv * Jres;
-    u += update[0];
+    double error_t=update[0]*update[0]+update[1]*update[1];
-    v += update[1];
+    double dt=error_t-error;
-    mean_diff += update[2];
+    if(!signbit(dt))if(sign){
+            sign=false;
+            scale*=0.5;
+        }else{
+            sign=true;
+            scale*=0.5;
+        }
+    if(sign){
+        u += scale*update[0];
+        v += scale*update[1];
+        mean_diff += scale*update[2];
+    }else{
+        u -= scale*update[0];
+        v -= scale*update[1];
+        mean_diff -= scale*update[2];
+    }
+    error=error_t;
+
 
 #if SUBPIX_VERBOSE
     cout << "Iter " << iter << ":"
          << "\t u=" << u << ", v=" << v
-         << "\t update = " << update[0] << ", " << update[1]<< endl;
+         << "\t error = " << error
-//         << "\t new chi2 = " << new_chi2 << endl;
+         << "\t delta error = " << dt
+         << "\t sign = " << sign << endl;
 #endif
 
-    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
+    if(error_t < min_update_squared)
     {
 #if SUBPIX_VERBOSE
       cout << "converged." << endl;
@@ -271,7 +285,9 @@ bool align2D(
       break;
     }
   }
-
+#if SUBPIX_VERBOSE
+    if(!converged)cout << "not converged." << endl;
+#endif
   cur_px_estimate << u, v;
   return converged;
 }

GPU_version/svo/src/frame.cpp

@@ -32,7 +32,8 @@ Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) :
     cam_(cam),
     key_pts_(5),
     is_keyframe_(false),
-    v_kf_(NULL)
+    v_kf_(NULL),
+    T_f_w_(SE2_5(0.0,0.0,0.0))
 {
   initFrame(img);
 }
@@ -137,7 +138,9 @@ void Frame::removeKeyPoint(Feature* ftr)
 
 bool Frame::isVisible(const Vector3d& xyz_w) const
 {
-  Vector3d xyz_f = T_f_w_*xyz_w;
+  Vector3d xyz_f = Vector3d(xyz_w.x()*T_f_w_.cos_theta_+xyz_w.z()*T_f_w_.sin_theta_+T_f_w_.X_,
+                            xyz_w.y(),
+                            -xyz_w.x()*T_f_w_.sin_theta_+xyz_w.z()*T_f_w_.cos_theta_+T_f_w_.Z_);
   if(xyz_f.z() < 0.0)
     return false; // point is behind the camera
   Vector2d px = f2c(xyz_f);

GPU_version/svo/src/frame_handler_base.cpp

@@ -86,8 +86,10 @@ int FrameHandlerBase::finishFrameProcessingCommon(
     stage_ = STAGE_RELOCALIZING;
     tracking_quality_ = TRACKING_INSUFFICIENT;
   }
+  /*
   else if (dropout == RESULT_FAILURE)
     resetAll();
+    */
   if(set_reset_)
     resetAll();
 

GPU_version/svo/src/frame_handler_mono.cpp

@@ -99,7 +99,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
 
 FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 {
-  new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
+  new_frame_->T_f_w_ = SE2_5(0.0,0.0,0.0);
   if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE)
     return RESULT_NO_KEYFRAME;
   new_frame_->setKeyframe();
@@ -137,41 +137,30 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 {
   SE3 tem=imu_integPtr_->preintegrate_predict();
-  Eigen::Quaternion<double> q(tem.rotation_matrix());
+  new_frame_->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0)));
-  q.vec() *= 1/q.norm();
+  //new_frame_->T_f_w_ = last_frame_->T_f_w_;
-  q.w() *= 1/q.norm();
-  q.x()=0.0;
-  q.z()=0.0;
-  tem.translation().y()=0.0;
-  new_frame_->T_f_w_ = SE3(q,tem.translation());
   // sparse image align
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
-                           30, SparseImgAlign::LevenbergMarquardt, false, true);
+                           30, SparseImgAlign::LevenbergMarquardt, false, false);
   img_align.run(last_frame_, new_frame_);
   // map reprojection & feature alignment
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  std::cerr<<"Reprojection Map: "<<"\t nPoint:"<<overlap_kfs_.back().second<<"\t nCell = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<" \t Quality min: "<<Config::qualityMinFts()<<'\n';
   size_t sfba_n_edges_final=0;
-  if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,Config::poseOptimThresh())){
+
-        std::cerr<<"imu failed\n";
+  double sfba_thresh, sfba_error_init, sfba_error_final;
+  if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,reprojector_.n_matches_)){
         return RESULT_FAILURE;
   }
-
-  //usleep(5000);
-  double sfba_thresh, sfba_error_init, sfba_error_final;
   pose_optimizer::optimizeGaussNewton(
-            Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
+            Config::poseOptimThresh(), Config::poseOptimNumIter(), true,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
-  // pose optimization subject to change for adding IMU
-  /*
-  if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,Config::poseOptimThresh())){
-      std::cerr<<"imu failed\n";
-      return RESULT_FAILURE;
-  }*/
 
-  std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
+  std::cerr<<"Reprojection Map: "<<"\t nPoint:"<<overlap_kfs_.back().second
+  <<"\t nCell = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_
+  <<" \t Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
+
   if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
-      return RESULT_FAILURE;
+      return RESULT_NO_KEYFRAME;
   }
 
   // structure optimization
@@ -232,7 +221,7 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
   usleep(2000);
   if(img_align_n_tracked > Config::qualityMinFts())
   {
-    SE3 T_f_w_last = last_frame_->T_f_w_;
+    SE2_5 T_f_w_last = last_frame_->T_f_w_;
     last_frame_ = ref_keyframe;
     FrameHandlerMono::UpdateResult res = processFrame();
     if(res != RESULT_FAILURE)

GPU_version/svo/src/imu_integration.cpp

@@ -7,24 +7,25 @@
 #include <utility>
 #include "gpu_svo/feature.h"
 #include "gpu_svo/point.h"
-#include "gpu_svo/svoVisionFactorSmall.h"
+//#include "gpu_svo/svoVisionFactorSmall.h"
-//#include "gpu_svo/svoVisionFactor.h"
+#include "gpu_svo/svoVisionFactor.h"
 //#include "gpu_svo/svoVisionFactorLandMark.h"
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose3.h>
 #include <vikit/params_helper.h>
 #include <gpu_svo/config.h>
 #include <gpu_svo/for_it.hpp>
-
+#define debug 1
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
     valuesPtr=std::make_shared<gtsam::Values>();
     // Example: pitch and roll of aircraft in an ENU Cartesian frame.
     // If pitch and roll are zero for an aerospace frame,
     // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
-    gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, svo::Config::Gravity(),1.0);
+    //gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, svo::Config::Gravity(),1.0);
-    parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
+    //parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
-    parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
+    parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(0.0,0.0,0.0)));
+    //parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
     parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
     parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
     parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
@@ -39,6 +40,7 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     optimizerParamPtr->relinearizeSkip = 1;
     optimizerParamPtr->evaluateNonlinearError=true;
     optimizerParamPtr->enableDetailedResults=true;
+    if(debug)logtime = fopen((std::string(getenv("HOME"))+"/imu_preIntegrate.txt").c_str(), "w+");
     assert(preintegratedPtr);
 }
 Imu_Integration::~Imu_Integration(){
@@ -50,9 +52,9 @@ bool Imu_Integration::update(double* imu){
     auto in=std::chrono::steady_clock::now();
     if(std::chrono::duration<double>(in-t_1).count()>20.0){
         t_1=in;
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),imu[1]-svo::Config::Y_off(),imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]-svo::Config::Teta_off()),0.005);
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),0.0,-1.0*imu[1]-svo::Config::Y_off()),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),0.005);
     }else{
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),imu[1]-svo::Config::Y_off(),imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]-svo::Config::Teta_off()),std::chrono::duration<double>(in-t_1).count());
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),0.0,-1.0*imu[1]-svo::Config::Y_off()),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),std::chrono::duration<double>(in-t_1).count());
         t_1=in;
         ++imu_n;
     }
@@ -67,7 +69,7 @@ bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame
     preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
     ++imu_factor_id;
     syn=false;
-    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+    new_frame->T_f_w_=svo::SE2_5(statePtr->t().x(),statePtr->t().z(),atan(statePtr->R()(0,2)/statePtr->R()(0,0)));
     new_frame->Cov_=optimizer.marginalCovariance(P(imu_factor_id-1));
     graphPtr->resize(0);
     valuesPtr->clear();
@@ -88,7 +90,7 @@ bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame
 }
 bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
     gtsam::Values result = optimizer.optimize();
-    optimizer.print();
+    if(debug)optimizer.print();
     // Overwrite the beginning of the preintegration for the next step.
     statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
     imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
@@ -96,8 +98,14 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost:
     ++imu_factor_id;
     syn=false;
     gtsam::Marginals marginals(*graphPtr,result);
-    marginals.print();
+    if(debug)fprintf(logtime, "%f,%f,%f,%f,%f,%f,\n",
-    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+                     new_frame->T_f_w_.T->translation().x(),
+                     new_frame->T_f_w_.T->translation().y(),
+                     new_frame->T_f_w_.T->translation().z(),
+                     statePtr->pose().x(),
+                     statePtr->pose().y(),
+                     statePtr->pose().z());
+    new_frame->T_f_w_=svo::SE2_5(statePtr->t().x(),statePtr->t().z(),atan(statePtr->R()(0,2)/statePtr->R()(0,0)));
     new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
     graphPtr->resize(0);
     valuesPtr->clear();
@@ -107,16 +115,6 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost:
     graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1)));
     graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1)));
     graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1)));
-    /*
-    for(auto&& f_n:_for(new_frame->fts_)){
-        if(f_n.item->point == NULL)continue;
-        if(!result.exists(L(f_n.index)))continue;
-        f_n.item->point->pos_=result.at<gtsam::Point3>(L(f_n.index));
-        //valuesPtr->insert(L(f_n.index),f_n.item->point->pos_);
-        //graphPtr->emplace_shared<svo::VisionFactor>(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index));
-    }
-    //graphPtr->addPrior(L(0),new_frame->fts_.front()->point->pos_,gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
-     */
     return true;
 }
 Sophus::SE3 Imu_Integration::preintegrate_predict() {
@@ -133,54 +131,39 @@ Sophus::SE3 Imu_Integration::preintegrate_predict() {
 }
 bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,
                               boost::shared_ptr<svo::Frame>& last_frame,
-                              std::size_t& num_obs,const double reproj_thresh){
+                              std::size_t& num_obs,const double cell_match){
-
+    if(debug)std::cout<<"----------------------------IMU infuse---------------------------------------\n";
+    //if(debug)preintegratedPtr->print();
     auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
-    valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
+    valuesPtr->insert(P(imu_factor_id), Integ_predict.pose());
     valuesPtr->insert(V(imu_factor_id), Integ_predict.v());
     valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
     gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
                                             preint_imu_combined);
     graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
     imu_n=0;
-
+    graphPtr->emplace_shared<svo::VisionFactor>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
-    for(auto&& f:new_frame->fts_){
+                                                gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(3) << 1.0, 1.0, 1.0).finished()/(cell_match+1)),
-        if(f->point==NULL)continue;
+                                                P(imu_factor_id));
-        graphPtr->emplace_shared<svo::VisionFactor>(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id));
-        ++num_obs;
-    }
-
-    std::cout<<"------------------------------------------------------------------------------\n";
-    std::cout<<"------------------------------------------------------------------------------\n";
-    std::cout<<"------------------------------------------------------------------------------\n";
-    graphPtr->print();
     gtsam::LevenbergMarquardtParams params0;
     //params0.verbosity=gtsam::LevenbergMarquardtParams::LINEAR;
     params0.verbosityLM = gtsam::LevenbergMarquardtParams::SUMMARY;
     params0.setlambdaFactor(10.0);
+    params0.setlambdaInitial(100);
+    params0.setlambdaUpperBound(1e10);
     gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr,params0);
     reset(optimizer,new_frame);
-    std::cout<<"------------------------------------------------------------------------------\n";
+
-    // Remove Measurements with too large reprojection error
+    if(debug)std::cout<<"------------------------------------------------------------------------------\n";
-    /*
-    double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2();
-    for(auto&& f:new_frame->fts_)
-    {
-        if(f->point == NULL)continue;
-        Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<<f->level);
-        if(e.norm() > reproj_thresh_scaled)
-        {
-            // we don't need to delete a reference in the point since it was not created yet
-            f->point = NULL;
-            --num_obs;
-        }
-    }
-     */
     return true;
 }
 bool Imu_Integration::init(boost::shared_ptr<svo::Frame>& new_frame){
-    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.matrix()),
+    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
                                                gtsam::Vector3(1e-9,1e-9,1e-9));
+    /*
+    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0,0.0,0.0),gtsam::Point3(0.0,0.0,0.0)),
+                                               gtsam::Vector3(1e-9,1e-9,1e-9));
+                                               */
     valuesPtr->insert(P(0),statePtr->pose());
     valuesPtr->insert(V(0),statePtr->v());
     valuesPtr->insert(B(0),*imu_biasPtr);

GPU_version/svo/src/initialization.cpp

@@ -69,11 +69,14 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
     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_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
+  SE3 tem=T_cur_from_ref_ * (*frame_ref_->T_f_w_.T);
-  frame_cur->T_f_w_.translation() = -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
+  frame_cur->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0)));
+  tem.translation() = -frame_cur->T_f_w_.T->rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
+  frame_cur->T_f_w_.X_ = tem.translation().x();
+  frame_cur->T_f_w_.Z_ = tem.translation().z();
 
   // For each inlier create 3D point and add feature in both frames
-  SE3 T_world_cur = frame_cur->T_f_w_.inverse();
+  SE3 T_world_cur = frame_cur->T_f_w_.T->inverse();
     for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
     {
         Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);

GPU_version/svo/src/map.cpp

@@ -114,12 +114,11 @@ void Map::getCloseKeyframes(
     {
       if(keypoint == nullptr)
         continue;
-
       if(frame->isVisible(keypoint->point->pos_))
       {
         close_kfs.push_back(
             std::make_pair(
-                kf, (frame->T_f_w_.translation()-kf->T_f_w_.translation()).norm()));
+                kf, (frame->T_f_w_.T->translation()-kf->T_f_w_.T->translation()).norm()));
         break; // this keyframe has an overlapping field of view -> add to close_kfs
       }
     }
@@ -178,8 +177,9 @@ void Map::transform(const Matrix3d& R, const Vector3d& t, const double& s)
   for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
   {
     Vector3d pos = s*R*(*it)->pos() + t;
-    Matrix3d rot = R*(*it)->T_f_w_.rotation_matrix().inverse();
+    Matrix3d rot = R*(*it)->T_f_w_.T->rotation_matrix().inverse();
-    (*it)->T_f_w_ = SE3(rot, pos).inverse();
+    SE3 inv=SE3(rot, pos).inverse();
+    (*it)->T_f_w_ = SE2_5(inv.translation().x(),inv.translation().z(),atan(inv.rotation_matrix()(0,2)/inv.rotation_matrix()(0,0)));
     for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
     {
       if((*ftr)->point == NULL)

GPU_version/svo/src/matcher.cpp

@@ -147,7 +147,7 @@ bool Matcher::findMatchDirect(
   warp::getWarpMatrixAffine(
       *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
       (ref_ftr_->frame->pos() - pt.pos_).norm(),
-      cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
+      (*cur_frame.T_f_w_.T) * ref_ftr_->frame->T_f_w_.T->inverse(), ref_ftr_->level, A_cur_ref_);
   search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
   warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
                    ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
@@ -184,7 +184,7 @@ bool Matcher::findEpipolarMatchDirect(
     const double d_max,
     double& depth)
 {
-  SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
+  SE3 T_cur_ref = (*cur_frame.T_f_w_.T) * ref_frame.T_f_w_.T->inverse();
   int zmssd_best = PatchScore::threshold();
   Vector2d uv_best;
 

GPU_version/svo/src/point.cpp

@@ -89,7 +89,7 @@ void Point::initNormal()
   assert(!obs_.empty());
   const Feature* ftr = obs_.back();
   assert(ftr->frame != NULL);
-  normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f);
+  normal_ = ftr->frame->T_f_w_.T->rotation_matrix().transpose()*(-ftr->f);
   normal_information_ = DiagonalMatrix<double,3,3>(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0);
   normal_set_ = true;
 }
@@ -133,8 +133,10 @@ void Point::optimize(const size_t n_iter)
     for(auto it=obs_.begin(); it!=obs_.end(); ++it)
     {
       Matrix23d J;
-      const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
+      const Vector3d p_in_f(pos_.x()*(*it)->frame->T_f_w_.cos_theta_+pos_.z()*(*it)->frame->T_f_w_.sin_theta_+(*it)->frame->T_f_w_.X_,
-      Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
+                 pos_.y(),
+                 -pos_.x()*(*it)->frame->T_f_w_.sin_theta_+pos_.z()*(*it)->frame->T_f_w_.cos_theta_+(*it)->frame->T_f_w_.Z_);
+      Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.T->rotation_matrix(), J);
       const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
       new_chi2 += e.squaredNorm();
       A.noalias() += J.transpose() * J;

GPU_version/svo/src/pose_optimizer.cpp

@@ -39,7 +39,7 @@ void optimizeGaussNewton(
   double chi2(0.0);
   vector<double> chi2_vec_init, chi2_vec_final;
   vk::robust_cost::TukeyWeightFunction weight_function;
-  SE3 T_old(frame->T_f_w_);
+  SE2_5 T_old(frame->T_f_w_.X_,frame->T_f_w_.Z_,frame->T_f_w_.tetha_);
   Matrix6d A;
   Vector6d b;
 
@@ -51,7 +51,10 @@ void optimizeGaussNewton(
       continue;
     //Reprojection error
     Vector2d e = vk::project2d((*it)->f)
-               - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
+               - vk::project2d(Vector3d((*it)->point->pos_.x()*frame->T_f_w_.cos_theta_+(*it)->point->pos_.z()*frame->T_f_w_.sin_theta_+frame->T_f_w_.X_,
+                                        (*it)->point->pos_.y(),
+                                        -(*it)->point->pos_.x()*frame->T_f_w_.sin_theta_+(*it)->point->pos_.z()*frame->T_f_w_.cos_theta_+frame->T_f_w_.Z_));
+
     e *= 1.0 / (1<<(*it)->level);
     errors.push_back(e.norm());
   }
@@ -80,7 +83,9 @@ void optimizeGaussNewton(
       if((*it)->point == NULL)
         continue;
       Matrix26d J;
-      Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_);
+      Vector3d xyz_f(Vector3d((*it)->point->pos_.x()*frame->T_f_w_.cos_theta_+(*it)->point->pos_.z()*frame->T_f_w_.sin_theta_+frame->T_f_w_.X_,
+                (*it)->point->pos_.y(),
+                -(*it)->point->pos_.x()*frame->T_f_w_.sin_theta_+(*it)->point->pos_.z()*frame->T_f_w_.cos_theta_+frame->T_f_w_.Z_));
       Frame::jacobian_xyz2uv(xyz_f, J);
       Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
       double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
@@ -108,9 +113,9 @@ void optimizeGaussNewton(
     }
 
     // update the model
-    SE3 T_new = SE3::exp(dT)*frame->T_f_w_;
+    SE3 T_new = SE3::exp(dT)*(*frame->T_f_w_.T);
     T_old = frame->T_f_w_;
-    frame->T_f_w_ = T_new;
+    frame->T_f_w_ = SE2_5(T_new.translation().x(),T_new.translation().z(),atan(T_new.rotation_matrix()(0,2)/T_new.rotation_matrix()(0,0)));
     chi2 = new_chi2;
     if(verbose)
       std::cout << "it " << iter
@@ -133,7 +138,9 @@ void optimizeGaussNewton(
   {
     if((*it)->point == NULL)
       continue;
-    Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
+    Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->point->pos_.x()*frame->T_f_w_.cos_theta_+(*it)->point->pos_.z()*frame->T_f_w_.sin_theta_+frame->T_f_w_.X_,
+                                                                  (*it)->point->pos_.y(),
+                                                                  -(*it)->point->pos_.x()*frame->T_f_w_.sin_theta_+(*it)->point->pos_.z()*frame->T_f_w_.cos_theta_+frame->T_f_w_.Z_));
     double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
     e *= sqrt_inv_cov;
     chi2_vec_final.push_back(e.squaredNorm());

GPU_version/svo/src/sparse_img_align.cpp

@@ -55,8 +55,7 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
   ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
   jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
   //visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
-  SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());
+  SE3 T_cur_from_ref(*cur_frame_->T_f_w_.T * ref_frame_->T_f_w_.T->inverse());
-
   for(level_=max_level_; level_>=min_level_; --level_)
   {
     visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
@@ -67,7 +66,8 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
       printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
     optimize(T_cur_from_ref);
   }
-  cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;
+  SE3 tem=T_cur_from_ref * (*ref_frame_->T_f_w_.T);
+  cur_frame_->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0)));
 
   return n_meas_/patch_area_;
 }

GPU_version/vio/launch/Px30.launch

@@ -10,7 +10,7 @@
     <rosparam file="$(find vio_gpu)/param/px30.yaml" />
 
     <!-- IMU 2 Camera Frame -->
-    <param name="init_rx" value="1.88496" /><!--  90 + 18 -->
+    <param name="init_rx" value="-1.88496" /><!--  90 + 18 -->
     <param name="init_ry" value="0.0" /> <!--  00 -->
     <param name="init_rz" value="0.0" /> <!-- 0  -->
 

GPU_version/vio/param/vo_fast.yaml

@@ -15,8 +15,8 @@ svo:
   ABC: 0.1                #acc bias in continuous
   GBC: 0.1                #gyro bias in continuous
   EBP: 0.1                #error in the bias used for preintegration
-  NRP: 0.0000001                #noise used for projection in preintegration
+  NRP: 0.001                #noise used for projection in preintegration
-  gravity: 9.65
+  gravity: -9.65
   Acceleration_offset_x: -0.05
   Acceleration_offset_y: -0.04
   Gyroscope_offset_z: 0.001

GPU_version/vio/src/visualizer.cpp

@@ -34,10 +34,9 @@ Visualizer() :
     T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
 {
   // Init ROS Marker Publishers
-  pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
+  //pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
-  pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
+ // pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
   pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
-  pub_info_ = pnh_.advertise<vio_gpu::Info>("info", 10);
 }
 
 void Visualizer::publishMinimal(
@@ -48,38 +47,18 @@ void Visualizer::publishMinimal(
 {
   ++trace_id_;
   std_msgs::Header header_msg;
-  header_msg.frame_id = "/cam";
+  header_msg.frame_id = "/world";
   header_msg.seq = trace_id_;
   header_msg.stamp = ros::Time(timestamp);
-
-  // publish info msg.
-  if(pub_info_.getNumSubscribers() > 0)
-  {
-      vio_gpu::Info msg_info;
-    msg_info.header = header_msg;
-    //msg_info.processing_time = slam.lastProcessingTime();
-    msg_info.keyframes.reserve(slam.map().keyframes_.size());
-    for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
-      msg_info.keyframes.push_back((*it)->id_);
-    msg_info.stage = static_cast<int>(slam.stage());
-    msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
-    /*
-    if(frame != NULL)
-      msg_info.num_matches = slam.lastNumObservations();
-    else
-      msg_info.num_matches = 0;
-      */
-    pub_info_.publish(msg_info);
-  }
   if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
   {
     Quaterniond q;
     Vector3d p;
     Matrix<double,6,6> Cov;
     // publish cam in world frame (Estimated odometry in the worls frame)
-    SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
+    SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.T->inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
-    q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+    //q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
-    p = T_world_from_cam.translation();
+    //p = T_world_from_cam.translation();
     Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
     geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
     msg_pose->header = header_msg;
@@ -105,7 +84,7 @@ void Visualizer::visualizeMarkers(
     return;
 
   vk::output_helper::publishTfTransform(
-      frame->T_f_w_*T_world_from_vision_.inverse(),
+      *frame->T_f_w_.T*T_world_from_vision_.inverse(),
       ros::Time(frame->timestamp_), "cam_pos", "world", br_);
 
   if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)

GPU_version/vio/src/vo_node.cpp

@@ -95,8 +95,6 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
           cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
           vo_->addImage(img, msg->header.stamp.toSec());
           visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
-          if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
-              visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
           if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
               usleep(100000);
           fprintf(logtime, "%s\n",std::to_string(std::chrono::duration<double>(std::chrono::steady_clock::now()-start).count()).c_str());