inlier into point error

GPU_version/vio/depth_filter_log.txt

@@ -0,0 +1,22 @@
+[16:31:45.913] init depth filter
+[16:31:47.423] init depth filter
+[16:31:50.898] DepthFilter: RESET.
+[16:32:05.679] add key frame
+[16:32:05.679] init seeds
+[16:32:05.679] update 0 seeds
+[16:32:05.679] init seeds
+[16:32:05.682] DepthFilter: Initialized 82 new seeds
+[16:32:05.682] add key frame
+[16:32:05.682] update 82 seeds
+[16:32:05.682] init seeds
+[16:32:05.687] DepthFilter: Initialized 112 new seeds
+[16:32:05.692] DepthFilter: Initialized 126 new seeds
+[16:32:08.165] add new frame
+[16:32:08.165] add key frame
+[16:32:08.165] update 320 seeds
+[16:32:08.165] init seeds
+[16:32:08.168] DepthFilter: Initialized 149 new seeds
+[16:32:10.624] add new frame
+[16:32:10.624] update 469 seeds
+[16:32:47.056] DepthFilter stop thread invoked.
+[16:32:47.056] DepthFilter interrupt and join thread 

GPU_version/vio/src/initialization.cpp

@@ -99,7 +99,13 @@ 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->getSE3Inv() * (xyz_in_cur_.at(id)/* *scale */);
+          Vector3d pos = 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(),
+                  xyz_in_cur_.at(id).x(),xyz_in_cur_.at(id).y(),xyz_in_cur_.at(id).z(),
+                  pos.x(),pos.y(),pos.z());
+#endif
           std::shared_ptr<Point> new_point = std::make_shared<Point>(pos);
           std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, Vector2d(px_cur_.at(id).x,px_cur_.at(id).y),
                                                                      frame_cur->c2f(px_cur_.at(id).x,px_cur_.at(id).y), (*first)->level);