left so3 error

GPU_version/vio/include/vio/global.h

@@ -81,20 +81,26 @@ namespace vio
             return *T2_;
         }
         SE2 inverse() const{
-            double roll=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real());
-            return SE2(roll+M_PI,-1.0*T2_->translation());
+            assert(T2_!= nullptr);
+            double pitch=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real());
+            SE2 tem=SE2(pitch+M_PI,-1.0*T2_->translation());
+            return tem;
         }
         // Rotation around y
         double pitch()const{
-            return atan2(T2_->rotation_matrix()(0,1),T2_->rotation_matrix()(0,0));
+            assert(T2_!= nullptr);
+            double pitch=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real());
+            return pitch;
         }
         SE3 se3() const{
+            assert(T2_!= nullptr);
             //Todo add 15 roll orientation
             Quaterniond q;
             q = AngleAxisd(0.261799, Vector3d::UnitX())
                 * AngleAxisd(pitch(), Vector3d::UnitY())
                 * AngleAxisd(0.0, Vector3d::UnitZ());
-            return SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
+            SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
+            return tem;
         }
         ~SE2_5(){
         }

GPU_version/vio/src/frame.cpp

@@ -125,33 +125,34 @@ void Frame::checkKeyPoints(Feature* ftr)
 
 void Frame::removeKeyPoint(Feature* ftr)
 {
-    std::cerr<<key_pts_.size()<<'\t';
+    bool found = false;
     if(key_pts_.size()!=5){
         key_pts_.clear();
         key_pts_.reserve(5);
-        return;
+        found=true;
     }
-  bool found = false;
   for(auto&& key_pt:key_pts_){
-      std::cerr<<"131\t";
       if(key_pt==NULL)continue;
-      std::cerr<<"131\t";
       if(key_pt==ftr){
-          std::cerr<<"131\t";
           key_pt=NULL;
           found=true;
       }
   }
-    std::cerr<<"dd\n";
   if(found)
     setKeyPoints();
 }
 
 bool Frame::isVisible(const Vector3d& xyz_w) const
 {
-  Vector3d xyz_f = Vector3d(this->se3()*xyz_w);
+    if(!id_)return false;
+    std::cerr<<"148is\t";
+    SE3 tem=this->se3();
+    std::cerr<<"150is\t";
+  Vector3d xyz_f = tem*xyz_w;
+    std::cerr<<"152is\t";
   if(xyz_f.z() < 0.0)
     return false; // point is behind the camera
+    std::cerr<<"153is\t";
   Vector2d px = f2c(xyz_f);
   if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height())
     return true;

GPU_version/vio/src/frame_handler_mono.cpp

@@ -106,7 +106,8 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
     res = processFirstFrame();
 
   // set last frame
-  if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_;
+  //if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)
+  last_frame_ = new_frame_;
   // finish processing
   finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
   if(stage_ == STAGE_RELOCALIZING){
@@ -166,10 +167,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->T_f_w_=init_f.second;
   new_frame_->Cov_ = init_f.first;
   // sparse image align
-  //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
-  //assert(new_frame_.get()== nullptr);
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
-  if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
+  img_align.run(last_frame_, new_frame_);
   reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
   //assert (false);
   int n_point=0;

GPU_version/vio/src/map.cpp

@@ -83,10 +83,13 @@ void Map::removePtFrameRef(Frame* frame, Feature* ftr)
 void Map::safeDeletePoint(Point* pt)
 {
   // Delete references to mappoints in all keyframes
-  for(auto&& ftr:pt->obs_){
-      assert(ftr);
-      if(ftr->frame!= nullptr)ftr->frame->removeKeyPoint(ftr);
-      ftr->point=NULL;
+  if(pt->obs_.size()>1){
+      for(auto&& ftr:pt->obs_){
+          ftr->frame->removeKeyPoint(ftr);
+          ftr->point=NULL;
+      }
+  }else{
+      pt->obs_.back()->point=NULL;
   }
   pt->obs_.clear();
 

GPU_version/vio/src/point.cpp

@@ -100,9 +100,9 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) cons
 {
   // TODO: get frame with same point of view AND same pyramid level!
   ftr= nullptr;
+  if(id_<1)return false;
   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->id_==id){
           ftr = ob;

GPU_version/vio/src/reprojector.cpp

@@ -154,10 +154,12 @@ namespace vio {
             for (auto &&f: _for(it_frame.item.first->fts_)) {
                 for (auto &&match:matches) {
                     if (match.queryIdx != f.index)continue;
+                    if(keypoints_cur.size() < match.trainIdx)continue;
                     if (f.item->point == NULL){
                         const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
                                       grid_.grid_n_cols
                                       + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
+                        assert(grid_.cells.at(k) != nullptr);
                         Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
                                     (int) keypoints_cur.at(match.trainIdx).pt.y);
                         frame->fts_.push_back(new Feature(it_frame.item.first.get(),
@@ -170,7 +172,6 @@ namespace vio {
                     }else{
                         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
                                       + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
@@ -209,7 +210,11 @@ namespace vio {
         while(it!=cell.end())
         {
             ++n_trials_;
-            if(it->pt->type_ == Point::TYPE_DELETED || it->pt ==NULL)
+            if( it->pt ==NULL){
+                it = cell.erase(it);
+                continue;
+            }
+            if(it->pt->type_ == Point::TYPE_DELETED)
             {
                 it = cell.erase(it);
                 continue;
@@ -233,21 +238,23 @@ namespace vio {
 
             Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
             frame->addFeature(new_feature);
-
             // Here we add a reference in the feature to the 3D point, the other way
             // round is only done if this frame is selected as keyframe.
             new_feature->point = it->pt;
-
+            if(matcher_.ref_ftr_== nullptr){
+                cell.erase(it);
+                return true;
+            }
+            assert(matcher_.ref_ftr_!= nullptr);
             if(matcher_.ref_ftr_->type == Feature::EDGELET)
             {
                 new_feature->type = Feature::EDGELET;
                 new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;
                 new_feature->grad.normalize();
             }
-
             // If the keyframe is selected and we reproject the rest, we don't have to
             // check this point anymore.
-            it = cell.erase(it);
+            cell.erase(it);
 
             // Maximum one point per cell.
             return true;