debug close obsview

GPU_version/vio/CMakeLists.txt

@@ -39,7 +39,7 @@ add_service_files(FILES
 
 generate_messages(DEPENDENCIES std_msgs)
 catkin_package(
-        DEPENDS Eigen OpenCV Sophus Boost
+        DEPENDS Eigen OpenCV Boost
         CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime
         INCLUDE_DIRS include
 )

GPU_version/vio/include/sophus/se2.cpp

@@ -62,15 +62,15 @@ SE2 SE2
 {
   SE2 result(*this);
   result.translation_ += so2_*(other.translation_);
-  result.so2_*=other.so2_;
+  result.so2_ *= other.so2_;
   return result;
 }
 
 SE2& SE2
 ::operator *= (const SE2 & other)
 {
-  translation_+= so2_*(other.translation_);
+  translation_ += so2_ * (other.translation_);
-  so2_*=other.so2_;
+  so2_ *= other.so2_;
   return *this;
 }
 
@@ -79,7 +79,7 @@ SE2 SE2
 {
   SE2 ret;
   ret.so2_= so2_.inverse();
-  ret.translation_ = ret.so2_*(translation_*-1.);
+  ret.translation_ = ret.so2_ * (translation_ *-1.);
   return ret;
 }
 
@@ -113,8 +113,8 @@ Matrix<double, 3, 3> SE2
   Matrix3d res;
   res.setIdentity();
   res.topLeftCorner<2,2>() = R;
-  res(0,2) = translation_[1];
+  res(0,2) = translation_.y();
-  res(1,2) = -translation_[0];
+  res(1,2) = -translation_.x();
   return res;
 }
 
@@ -220,7 +220,7 @@ Vector3d SE2
   Matrix2d V_inv;
   V_inv(0,0) = halftheta_by_tan_of_halftheta; V_inv(1,0) = -halftheta;
   V_inv(0,1) = halftheta; V_inv(1,1) = halftheta_by_tan_of_halftheta;
-  upsilon_theta.head<2>() = V_inv*se2.translation_;
+  upsilon_theta.head<2>() = V_inv* se2.translation_;
   return upsilon_theta;
 }
 

GPU_version/vio/include/sophus/se2.h

@@ -44,7 +44,7 @@ public:
                               const Vector2d & translation);
 
   SE2                        (double theta,
-                              const Vector2d & translation_);
+                              const Vector2d & translation);
 
   SE2                        (const SE2 & other);
 
@@ -95,11 +95,11 @@ public:
   {
     return translation_;
   }
-
+/*
   Vector2d& translation()
   {
     return translation_;
-  }
+  }*/
 
   Matrix2d rotation_matrix() const
   {

GPU_version/vio/include/vio/frame.h

@@ -119,8 +119,8 @@ namespace vio {
 
         /// Return the pose of the frame in the (w)orld coordinate frame.
         inline Vector2d pos() const {
-            Vector2d tem=T_f_w_.se2().translation();
+            SE2 tem=T_f_w_.se2();
-            return tem;
+            return tem.translation();
         }
 
         inline SE3 se3() const{

GPU_version/vio/include/vio/global.h

@@ -52,31 +52,29 @@ namespace vio
     const double PI = 3.14159265;
     class SE2_5: public SE2{
     public:
-        SE2_5(SE2&& se2):T2_(new SE2(se2)){
+        SE2_5(SE2&& se2){
+            T2_=new SE2(se2);
+            assert(T2_!= nullptr);
         }
-        SE2_5(SE2& se2):T2_(new SE2(se2)){
+        SE2_5(SE2& se2){
+            T2_=new SE2(se2);
+            assert(T2_!= nullptr);
         }
         SE2_5(SE3&& se3){
             Quaterniond q=se3.unit_quaternion().normalized();
             auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
-            Eigen::Matrix<double, 2,2> R;
+            T2_ = new SE2(SO2(euler(1)),Vector2d(se3.translation().x(),se3.translation().z()));
-            R<<cos(euler(0)),sin(euler(0)),
+            assert(T2_!= nullptr);
-                    -sin(euler(0)),cos(euler(0));
-            T2_ = new SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
         }
         SE2_5(SE3& se3){
             Quaterniond q=se3.unit_quaternion().normalized();
             auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
-            Eigen::Matrix<double, 2,2> R;
+            T2_=new SE2(SO2(euler(1)),Vector2d(se3.translation().x(),se3.translation().z()));
-            R<<cos(euler(0)),sin(euler(0)),
+            assert(T2_!= nullptr);
-                    -sin(euler(0)),cos(euler(0));
-            T2_=new SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
         }
-        SE2_5(double y,double z,double roll){
+        SE2_5(double y,double z,double pitch){
-            Eigen::Matrix<double, 2,2> R;
+            T2_=new SE2(SO2(pitch),Vector2d(y,z));
-            R<<cos(roll),sin(roll),
+            assert(T2_!= nullptr);
-                    -sin(roll),cos(roll);
-            T2_=new SE2(R,Vector2d(y,z));
         };
         SE2 se2() const{
             assert(T2_!= nullptr);
@@ -100,7 +98,7 @@ namespace vio
         }
         ~SE2_5(){
         }
-        bool empty(){
+        bool empty() const{
             return T2_ == nullptr ? true : false;
         }
 

GPU_version/vio/include/vio/point.h

@@ -59,6 +59,7 @@ public:
   int                         n_failed_reproj_;         //!< Number of failed reprojections. Used to assess the quality of the point.
   int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
   int                         last_structure_optim_;    //!< Timestamp of last point optimization
+  bool                        syn_on=false;
 
   Point(const Vector3d& pos);
   Point(const Vector3d& pos, Feature* ftr);

GPU_version/vio/src/depth_filter.cpp

@@ -141,7 +141,7 @@ namespace vio {
         size_t n_removed = 0;
         while(it!=seeds_.end())
         {
-            if(it->ftr->frame == frame.get())
+            if(it->ftr->frame->id_ == frame->id_)
             {
                 it = seeds_.erase(it);
                 ++n_removed;

GPU_version/vio/src/matcher.cpp

@@ -141,10 +141,9 @@ bool Matcher::findMatchDirect(
     Vector2d& px_cur)
 {
   if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;
-  //if(ref_ftr_->frame->cam_== nullptr)return false;
+  if(ref_ftr_== nullptr)return false;
-  assert(ref_ftr_->frame!= nullptr);
+  if(ref_ftr_->frame== nullptr)return false;
   if(ref_ftr_->frame->cam_== nullptr)return false;
-  assert(ref_ftr_->frame->cam_!= nullptr);
   Vector2i pxi=ref_ftr_->px.cast<int>();
   if(ref_ftr_->level==NULL){
       if(!ref_ftr_->frame->cam_->isInFrame(pxi, 6))return false;

GPU_version/vio/src/point.cpp

@@ -75,7 +75,7 @@ bool Point::deleteFrameRef(Frame* frame)
 {
   for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
   {
-    if((*it)->frame == frame)
+    if((*it)->frame->id_ == frame->id_)
     {
       obs_.erase(it);
       return true;
@@ -98,11 +98,12 @@ void Point::initNormal()
 bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
 {
   // TODO: get frame with same point of view AND same pyramid level!
+  ftr= nullptr;
   Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize();
   double max_cos_angle = 1.0;
   if(obs_.empty())return false;
   for(auto&& ob:obs_){
-      if(ob->frame->T_f_w_.empty())continue;
+      if(ob == nullptr)continue;
       Vector2d t=ob->frame->pos();
       Vector3d dir(Vector3d(t.x(),1e-9,t.y()) + pos_);
       double cos_angle = obs_dir.dot(dir.normalized());
@@ -112,6 +113,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
           ftr = ob;
       }
   }
+  if(ftr== nullptr)return false;
   return true;
 }
 

GPU_version/vio/src/reprojector.cpp

@@ -139,6 +139,7 @@ namespace vio {
         close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                        boost::bind(&std::pair<FramePtr, double>::second, _2));
         overlap_kfs.reserve(options_.max_n_kfs);
+        boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
         for (auto &&it_frame:_for(close_kfs)) {
             if (it_frame.index > options_.max_n_kfs)continue;
             std::vector<cv::KeyPoint> keypoints_kfs;
@@ -151,11 +152,14 @@ namespace vio {
             extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref);
             matcher->match(descriptors_ref, descriptors_cur, matches);
             for (auto &&f: _for(it_frame.item.first->fts_)) {
-                if (f.item->point == NULL)f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item);
-                if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
-                f.item->point->last_frame_overlap_id_ = frame->id_;
                 for (auto &&match:matches) {
                     if (match.queryIdx != f.index)continue;
+                    if (f.item->point == NULL){
+                        f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()));
+                       // assert(!f.item->point->obs_.empty());
+                    }
+                    if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
+                    f.item->point->last_frame_overlap_id_ = frame->id_;
                     assert(keypoints_cur.size() > match.trainIdx);
                     const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
                                   grid_.grid_n_cols
@@ -163,6 +167,8 @@ namespace vio {
                     assert(grid_.cells.at(k) != nullptr);
                     Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
                                 (int) keypoints_cur.at(match.trainIdx).pt.y);
+                    f.item->point->addFrameRef(new Feature(it_frame.item.first.get(),f.item->px,0));
+                    //f.item->point->addFrameRef(f.item);
                     grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
                     overlap_kfs.back().second++;
                     break;
@@ -170,7 +176,6 @@ namespace vio {
             }
         }
         {
-            boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
             for (auto&& cell : grid_.cells) {
                 if (reprojectCell(*cell, frame))
                     ++n_matches_;
@@ -193,16 +198,14 @@ namespace vio {
         while(it!=cell.end())
         {
             ++n_trials_;
-
             if(it->pt->type_ == Point::TYPE_DELETED)
             {
                 it = cell.erase(it);
                 continue;
             }
-            bool found_match = true;
+
-            if(options_.find_match_direct)
+            syn_=true;
-                found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
+            if(!matcher_.findMatchDirect(*it->pt, *frame, it->px))
-            if(!found_match)
             {
                 it->pt->n_failed_reproj_++;
                 if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
@@ -212,6 +215,7 @@ namespace vio {
                 it = cell.erase(it);
                 continue;
             }
+            syn_=false;
             it->pt->n_succeeded_reproj_++;
             if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
                 it->pt->type_ = Point::TYPE_GOOD;