success loop closer

vio_hw/internal/frame.hpp

@@ -42,6 +42,7 @@ class Frame
   std::vector<Keypoint> GetKeypoints() const;
   void UpdateKeypointStereo( const int lmid, const cv::Point2f &pt );
   cv::Point2f ProjCamToRightImage( const Eigen::Vector3d &pt ) const;
+  Eigen::Vector3d ProjWorldToCam( const Eigen::Vector3d &pt ) const;
   Eigen::Vector3d ProjCamToWorld( const Eigen::Vector3d &pt ) const;
   Sophus::SE3d GetTwc() const;
   Sophus::SE3d GetTcw() const;

vio_hw/internal/map_manager.hpp

@@ -33,13 +33,19 @@ class MapManager
   void RemoveObsFromCurFrameById(const int lmid);
   std::shared_ptr<Frame> GetKeyframe(const int kfid) const;
   std::shared_ptr<MapPoint> GetMapPoint(const int lmid) const;
-  void UpdateMapPoint(const int lmid, const Eigen::Vector3d &wpt, const double kfanch_invdepth);
+  void UpdateMapPoint(const int lmid, const Eigen::Vector3d &wpt, const double kfanch_invdepth=-1);
   void RemoveMapPointObs(const int lmid, const int kfid);
   void DescribeKeypoints(const cv::Mat &im, const std::vector<Keypoint> &vkps, const std::vector<cv::Point2f> &vpts);
+  int GetNumberKF() const;
+  void NumKFPlus();
+  int GetNumberLandmark() const;
+  void NumLandmarkPlus();
+  FramePtr GetCurrentFrame() { return current_frame_; }
 
  public:
   mutable std::mutex kf_mutex_, lm_mutex_;
   mutable std::mutex map_mutex_;
+  mutable std::mutex num_kfs_mutex_, num_lms_mutex_;
 
  private:
   int lm_id_, kf_id_;

vio_hw/internal/optimization.hpp

@@ -6,13 +6,13 @@
 #include <Eigen/Core>
 
 #include "backend/cost_function/se3_left_pgo_factor.hpp"
+#include "backend/example/common/read_g2o_format.hpp"
 #include "backend/parameter_block/se3_parameter_block.hpp"
 #include "backend/parameterization/se3left_parametrization.hpp"
-#include "vio_hw/internal/map_manager.hpp"
-#include "vio_hw/internal/setting.hpp"
 #include "tceres/problem.h"
 #include "tceres/solver.h"
-#include "backend/example/common/read_g2o_format.hpp"
+#include "vio_hw/internal/map_manager.hpp"
+#include "vio_hw/internal/setting.hpp"
 
 namespace viohw {
 
@@ -28,6 +28,7 @@ class Optimization
   Optimization( SettingPtr param, MapManagerPtr map_manager );
 
   bool LocalPoseGraph( Frame &new_frame, int kf_loop_id, const Sophus::SE3d &new_Twc );
+  bool localPoseGraph( Frame &new_frame, int kf_loop_id, const Sophus::SE3d &new_Twc );
 
 private:
   SettingPtr param_;

vio_hw/internal/viz/pangolin_visualization.hpp

@@ -22,6 +22,13 @@ class PangolinVisualization : public VisualizationBase
 
   bool addPoint(const Eigen::Vector3d &t,
                 const Eigen::Vector3d &color) override;
+
+  bool showPoint() override;
+
+  bool showTrajectory() override;
+
+  bool showKFTrajectory() override;
+
 };
 
 }  // namespace viohw

vio_hw/internal/viz/rviz_visualization.hpp

@@ -28,14 +28,22 @@ class RvizVisualization : public VisualizationBase
 
   bool addPoint( const Eigen::Vector3d &t, const Eigen::Vector3d &color ) override;
 
-  bool showLoopResultImage(const cv::Mat &img) override;
+  bool showLoopResultImage( const cv::Mat &img ) override;
+
+  bool clearKFTraj() override;
+
+  bool showPoint() override;
+
+  bool showTrajectory() override;
+
+  bool showKFTrajectory() override;
 
 private:
   CameraPoseVisualization camera_pose_visual_;
   ros::Publisher camera_pose_visual_pub_;
   ros::Publisher pub_kfs_traj_, pub_vo_traj_;
   ros::Publisher pub_img_tracker_result_, pub_loop_img_result_;
-  nav_msgs::Path vo_traj_path_;
+  nav_msgs::Path vo_traj_path_, kf_traj_path_;
   const std::string world_frame_ = "map";
 };
 

vio_hw/internal/viz/visualization_base.hpp

@@ -33,6 +33,17 @@ class VisualizationBase
 
   virtual bool addPoint( const Eigen::Vector3d &t, const Eigen::Vector3d &color ) = 0;
 
+  virtual bool showPoint() = 0;
+
+  virtual bool showTrajectory() = 0;
+
+  virtual bool showKFTrajectory() = 0;
+
+  virtual bool clearKFTraj() {
+    LOG( WARNING ) << "clearKFTraj tobe impl";
+    return false;
+  };
+
   virtual bool showLoopResultImage( const cv::Mat &img ) {
     LOG( WARNING ) << "showLoopResultImage tobe impl";
     return false;

vio_hw/internal/world_manager.hpp

@@ -24,6 +24,7 @@ class WorldManager
   bool getNewImage(cv::Mat &iml, cv::Mat &imr, double &time);
   void setupCalibration();
   bool VisualizationImage();
+  void VisualizationKFTraj();
 
  private:
   std::queue<cv::Mat> img_left_queen_, img_right_queen_;
@@ -32,6 +33,7 @@ class WorldManager
   std::mutex img_mutex_;
 
   bool is_new_img_available_ = false;
+  bool kf_viz_is_on_ = false;
   int frame_id_ = -1;
 
   FeatureBasePtr feature_extractor_;

vio_hw/params/rviz/kitti_setting.rviz

@@ -104,6 +104,30 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Show Trail: false
       Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 242; 21; 21
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Billboards
+      Line Width: 0.4000000059604645
+      Name: KeyFrameTraj
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /hello_world_vio/kf_traj
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -132,25 +156,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 625.1298828125
+      Distance: 869.33203125
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -15.903653144836426
-        Y: 44.36784362792969
-        Z: 225.76937866210938
+        X: -17.284948348999023
+        Y: 48.1305046081543
+        Z: 101.27821350097656
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.035202331840991974
+      Pitch: -0.08479773253202438
       Target Frame: <Fixed Frame>
-      Yaw: 1.6804203987121582
+      Yaw: 1.5354207754135132
     Saved: ~
 Window Geometry:
   Displays:

vio_hw/src/frame.cpp

@@ -200,10 +200,9 @@ void Frame::UpdateKeypointStereo( const int lmid, const cv::Point2f &pt ) {
   ComputeStereoKeypoint( pt, it->second );
 }
 
-bool Frame::isObservingKp(const int lmid) const
-{
-  std::lock_guard<std::mutex> lock(kps_mutex_);
-  return mapkps_.count(lmid);
+bool Frame::isObservingKp( const int lmid ) const {
+  std::lock_guard<std::mutex> lock( kps_mutex_ );
+  return mapkps_.count( lmid );
 }
 
 // Compute stereo keypoint from raw pixel position
@@ -249,28 +248,26 @@ std::vector<Keypoint> Frame::GetKeypoints2d() const {
   return v;
 }
 
-void Frame::UpdateKeypointDesc(const int lmid, const cv::Mat &desc)
-{
-  std::lock_guard<std::mutex> lock(kps_mutex_);
+void Frame::UpdateKeypointDesc( const int lmid, const cv::Mat &desc ) {
+  std::lock_guard<std::mutex> lock( kps_mutex_ );
 
-  auto it = mapkps_.find(lmid);
-  if( it == mapkps_.end() ) {
+  auto it = mapkps_.find( lmid );
+  if ( it == mapkps_.end() ) {
     return;
   }
 
   it->second.desc_ = desc;
 }
 
 // Return vector of 3D keypoint objects
-std::vector<Keypoint> Frame::GetKeypoints3d() const
-{
-  std::lock_guard<std::mutex> lock(kps_mutex_);
+std::vector<Keypoint> Frame::GetKeypoints3d() const {
+  std::lock_guard<std::mutex> lock( kps_mutex_ );
 
   std::vector<Keypoint> v;
-  v.reserve(nb3dkps_);
-  for( const auto &kp : mapkps_ ) {
-    if( kp.second.is3d_ ) {
-      v.push_back(kp.second);
+  v.reserve( nb3dkps_ );
+  for ( const auto &kp : mapkps_ ) {
+    if ( kp.second.is3d_ ) {
+      v.push_back( kp.second );
     }
   }
   return v;
@@ -325,6 +322,13 @@ cv::Point2f Frame::ProjCamToRightImage( const Eigen::Vector3d &pt ) const {
   return pcalib_rightcam_->projectCamToImage( pcalib_rightcam_->Tcic0_ * pt );
 }
 
+Eigen::Vector3d Frame::ProjWorldToCam( const Eigen::Vector3d &pt ) const {
+  std::lock_guard<std::mutex> lock( pose_mutex_ );
+
+  Eigen::Vector3d campt = Tcw_ * pt;
+
+  return campt;
+}
 Eigen::Vector3d Frame::ProjCamToWorld( const Eigen::Vector3d &pt ) const {
   std::lock_guard<std::mutex> lock( pose_mutex_ );
 

vio_hw/src/loop_closer.cpp

@@ -158,7 +158,7 @@ bool LoopCloser::ComputePnP( const Frame& frame, const std::vector<std::pair<int
   }
 
   // If at least 3 correspondances, go
-  if ( vkps.size() >= 3 ) {
+  if ( vkps.size() >= 6 ) {
     bool buse_robust = true;
     bool bapply_l2_after_robust = false;
 

vio_hw/src/map_manager.cpp

@@ -47,7 +47,8 @@ void MapManager::AddKeyframe() {  // Create a copy of Cur. Frame shared_ptr for
 
   // Add KF to the unordered map and update id/nb
   map_kfs_.emplace( kf_id_, pkf );
-  num_kfs_++;
+  // num_kfs_++;
+  NumKFPlus();
   kf_id_++;
 }
 
@@ -146,8 +147,8 @@ void MapManager::AddMapPoint() {
   // Add new MP to the map and update id/nb
   map_lms_.emplace( lm_id_, plm );
   lm_id_++;
-  num_lms_++;
-
+  // num_lms_++;
+  NumLandmarkPlus();
   // Visualization related part for pointcloud obs
   // TODO
 }
@@ -160,8 +161,8 @@ void MapManager::AddMapPoint( const cv::Mat& desc ) {
   // Add new MP to the map and update id/nb
   map_lms_.emplace( lm_id_, plm );
   lm_id_++;
-  num_lms_++;
-
+  // num_lms_++;
+  NumLandmarkPlus();
   // Visualization related part for pointcloud obs
   // TODO
 }
@@ -305,4 +306,20 @@ void MapManager::RemoveMapPointObs( const int lmid, const int kfid ) {
   // }
 }
 
+int MapManager::GetNumberKF() const {
+  std::lock_guard<std::mutex> lck( num_kfs_mutex_ );
+  return num_kfs_;
+}
+void MapManager::NumKFPlus() {
+  std::lock_guard<std::mutex> lck( num_kfs_mutex_ );
+  num_kfs_++;
+}
+int MapManager::GetNumberLandmark() const {
+  std::lock_guard<std::mutex> lck( num_lms_mutex_ );
+  return num_lms_;
+}
+void MapManager::NumLandmarkPlus() {
+  std::lock_guard<std::mutex> lck( num_lms_mutex_ );
+  num_lms_++;
+}
 }  // namespace viohw