Merge pull request #15 from 2013fangwentao/release/0.0.1-1

Release/0.0.1 1

.gitignore

@@ -48,4 +48,5 @@ result
 sim_data
 kitti
 grc
-grc-vio
+grc-vio
+apaper_config

.vscode/c_cpp_properties.json

@@ -9,7 +9,12 @@
  "/opt/thirdparty/include/eigen3",
  "/usr/local/include",
  "/usr/local/include/eigen3",
- "${workspaceFolder}/submodules/tools/include"
+ "${workspaceFolder}/submodules/tools/include",
+ "/opt/include",
+ "/opt/include/eigen3",
+ "/usr/local/include",
+ "/usr/local/include/eigen3",
+ "/usr/include/suitsparse"
  ],
  "defines": [],
  "compilerPath": "/usr/bin/gcc",

.vscode/settings.json

@@ -56,7 +56,9 @@
  "list": "cpp",
  "cfenv": "cpp",
  "regex": "cpp",
- "csignal": "cpp"
+ "csignal": "cpp",
+ "spqrsupport": "cpp",
+ "random": "cpp"
  },
  "python.pythonPath": "/usr/bin/python3"
 }

CMakeLists.txt

@@ -1,7 +1,8 @@
 cmake_minimum_required( VERSION 2.8 )
 PROJECT(mscnav)
 
-set(Ceres_DIR /opt/thirdparty/lib/cmake/Ceres)
+set(Ceres_DIR /opt/lib/cmake/Ceres)
+# set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake ${CMAKE_MODULE_PATH}")
 
 find_package(glog REQUIRED)
 find_package(gflags REQUIRED)
@@ -11,14 +12,15 @@ find_package(Ceres REQUIRED)
 
 SET(CMAKE_CXX_COMPILE "g++")
 SET(CMAKE_C_COMPILE "gcc")
-SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O0 -pthread" )
-SET( CMAKE_BUILD_TYPE "debug")
+SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O2 -pthread" )
+SET( CMAKE_BUILD_TYPE "release")
 
 SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 
 
-INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/include 
+INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/include
+ /usr/include/suitesparse
  ${PROJECT_SOURCE_DIR}/submodules/tools/include
  ${PROJECT_SOURCE_DIR}/thirdparty
  ${EIGEN3_INCLUDE_DIRS}

exec/mscnav.cc

@@ -31,7 +31,7 @@ int main(int argc, char const *argv[])
  utiltool::NavTime time_start = utiltool::NavTime::NowTime();
  ConfigInfo::Ptr config = ConfigInfo::GetInstance();
  config->open(argv[1]);
- LogInit(argv[0], argv[2], 0);
+ LogInit(argv[0], argv[2], 2);
 
  State::Ptr state = State::GetState();
  state->StartProcessing();

include/camera/feature.hpp

@@ -5,13 +5,15 @@
 ** Login <fangwentao>
 **
 ** Started on Tue Aug 6 下午3:24:38 2019 little fang
-** Last update Fri Nov 7 上午9:28:16 2019 little fang
+** Last update Wed Feb 11 下午2:17:13 2020 little fang
 */
 
 #ifndef FEATURE_H_
 #define FEATURE_H_
 
+#include "msckf.hpp"
 #include <map>
+#include <iomanip>
 #include <Eigen/Dense>
 #include <opencv2/core/core.hpp>
 
@@ -41,6 +43,7 @@ class Feature
  FeatureId feature_id_;
  Eigen::Vector3d position_world_{0, 0, 0}; //特征点的世界坐标
  std::map<StateId, cv::Point2f> observation_uv_;
+ std::map<StateId, cv::Point2f> raw_uv_;
  bool is_initialized_ = false;
 };
 FeatureId Feature::for_next_one(0);
@@ -66,6 +69,41 @@ class CameraState
  static StateId for_next_one;
 };
 StateId CameraState::for_next_one(0);
+
+/**
+ * @brief override the ostream output map<key,value>
+ * @note 
+ * @param &output: 
+ * @param &data: output data map<key,value>
+ * @retval 
+ */
+template <typename key, typename value>
+std::ostream &
+operator<<(std::ostream &output, const std::map<key, value> &data)
+{
+ for (auto index : data)
+ {
+ output << "{" << index.first << ",[" << index.second << "]} ";
+ }
+ return output;
+};
+
+std::ostream &operator<<(std::ostream &output, const CameraState &camera_state)
+{
+ output << std::setw(9) << camera_state.state_id_ << " ";
+ output << std::fixed << std::setprecision(4) << camera_state.position_.transpose() << " ";
+ output << std::fixed << std::setprecision(8) << camera_state.quat_.coeffs().transpose();
+ return output;
+};
+
+std::ostream &operator<<(std::ostream &output, const Feature &feature)
+{
+ output << std::setw(9) << feature.feature_id_ << " ";
+ output << std::fixed << std::setprecision(8) << feature.observation_uv_ << " ";
+ output << std::fixed << std::setprecision(4) << feature.raw_uv_;
+ return output;
+};
+
 } // namespace camera
 
 } // namespace mscnav

include/camera/imageprocess.h

@@ -5,7 +5,7 @@
 ** Login <fangwentao>
 **
 ** Started on Tue Aug 6 下午5:13:31 2019 little fang
-** Last update Fri Aug 8 下午8:45:35 2019 little fang
+** Last update Tue Feb 10 下午12:34:28 2020 little fang
 */
 
 #ifndef IMAGE_PROCESS_H_
@@ -41,17 +41,46 @@ class ImageProcess
  std::vector<cv::KeyPoint> &keypoints,
  cv::OutputArray descriptors);
 
+ static void GoodFreatureDetect(const cv::Mat &image,
+ std::vector<unsigned long long int> &keypoints_id,
+ std::vector<cv::Point2f> &keypoints);
+
+ static void LKTrack(const cv::Mat &pre_image,
+ const cv::Mat &curr_image,
+ // const Eigen::Matrix3d &rotation,
+ std::vector<unsigned long long int> &keypoints_id,
+ std::vector<cv::Point2f> &pre_keypoints,
+ std::vector<cv::Point2f> &curr_keypoints);
+
  static void FreatureMatch(const std::vector<cv::KeyPoint> &keypoints1,
  const cv::Mat &descriptors1,
  const std::vector<cv::KeyPoint> &keypoints2,
  const cv::Mat &descriptors2,
  std::vector<cv::DMatch> &matches,
  float default_min_distance = 40.0);
+
+ static void TwoPointRansac(const std::vector<cv::Point2f> &pts1,
+ const std::vector<cv::Point2f> &pts2,
+ const cv::Matx33f &R_p_c,
+ const cv::Mat &intrinsics,
+ const cv::Mat &distortion_coeffs,
+ const double &inlier_error,
+ const double &success_probability,
+ std::vector<int> &inlier_markers);
 
 private:
  static void OutlierRemove(const std::vector<cv::KeyPoint> &keypoints1,
  const std::vector<cv::KeyPoint> &keypoints2,
  std::vector<cv::DMatch> &matches);
+
+ static void OutlierRemove(std::vector<cv::Point2f> &keypoints1,
+ std::vector<cv::Point2f> &keypoints2,
+ // const Eigen::Matrix3d &rotation,
+ std::vector<unsigned long long int> &keypoints_id);
+ static void rescalePoints(std::vector<cv::Point2f> &pts1,
+ std::vector<cv::Point2f> &pts2,
+ float &scaling_factor);
+
 };
 
 } // namespace camera

include/camera/msckf.hpp

@@ -7,7 +7,7 @@
 ** Camera State, 每一帧中记录当前的Camera对应的状态，位姿
 **
 ** Started on Tue Aug 6 下午3:19:51 2019 little fang
-** Last update Thu Nov 6 下午7:46:18 2019 little fang
+** Last update Tue Mar 9 下午5:14:26 2020 little fang
 */
 
 #ifndef MSCKFPROCESS_H_
@@ -16,13 +16,15 @@
 #include "navtime.h"
 #include "navconfig.hpp"
 #include "filter/navfilter.h"
+#include "navattitude.hpp"
 #include "feature.hpp"
 #include "imageprocess.h"
 #include "Eigen/Dense"
 #include <opencv2/core/core.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <ceres/ceres.h>
+#include <ceres/rotation.h>
 #include <map>
 
 namespace mscnav
@@ -41,14 +43,16 @@ class MsckfProcess
  MsckfProcess(const KalmanFilter::Ptr &filter);
  ~MsckfProcess() {}
 
- bool ProcessImage(const cv::Mat &img1, const utiltool::NavTime &time, utiltool::NavInfo &navinfo);
+ bool ProcessImage(const cv::Mat &img_raw, const utiltool::NavTime &time, utiltool::NavInfo &navinfo);
+ void ReviseCameraState(const Eigen::VectorXd &dx_camera);
 
 private:
  void FirstImageProcess(const cv::Mat &img1, const utiltool::NavInfo &navifo);
  void AguementedState(const utiltool::NavInfo &navinfo);
  void AddObservation();
  void DetermineMeasureFeature();
  bool CheckEnableTriangleate(const Feature &feature);
+ bool CheckMotionStatus(const Feature &feature);
  bool LMOptimizatePosition(Feature &feature);
  bool TriangulatePoint(const Feature &feature, const Eigen::Isometry3d &cam0_trans, double position_cam0[3]);
 
@@ -59,25 +63,36 @@ class MsckfProcess
  Eigen::VectorXd MeasurementUpdate(const Eigen::MatrixXd &H_state,
  const Eigen::VectorXd &z_measure);
 
- void ReviseCameraState(const Eigen::VectorXd &dx_camera);
-
  void FeatureMeasureUpdate(utiltool::NavInfo &navinfo);
 
  void RemoveCameraState();
  void RemoveRedundantCamStates(utiltool::NavInfo &navinfo);
+ bool GatingTest(const Eigen::MatrixXd &H, const Eigen::VectorXd &r, const int &dof);
+
+ void NormKeyPoints(const std::vector<cv::Point2f> &keypoint_distorted,
+ std::vector<cv::Point2f> &keypoint_undistorted,
+ const cv::Mat &camera_mat_);
+ void Test();
+ bool CheckStaticMotion();
 
 private:
  std::vector<cv::DMatch> matches_;
  cv::Mat pre_frame_descriptors_;
  cv::Mat curr_frame_descriptors_;
- Eigen::Isometry3d cam_imu_tranformation_; //* coordinate from imu to camera
- std::vector<cv::KeyPoint> pre_frame_keypoints_;
- std::vector<cv::KeyPoint> curr_frame_keypoints_;
+ Eigen::Isometry3d cam_imu_tranformation_;
+ // std::vector<cv::KeyPoint> pre_frame_keypoints_;
+ // std::vector<cv::KeyPoint> curr_frame_keypoints_;
+ std::vector<cv::Point2f> pre_frame_keypoints_;
+ std::vector<cv::Point2f> curr_frame_keypoints_;
+ std::vector<unsigned long long int> keypoints_id_;
+ std::map<unsigned long long int, FeatureId> keypoints_featureid_;
 
  std::map<FeatureId, Feature> map_feature_set_;
  std::map<FeatureId, Feature> map_observation_set_;
  std::map<StateId, CameraState> map_state_set_;
 
+ std::vector<int> inlier_markers;
+
 private:
  bool is_first_ = true;
  double tracking_rate_ = 1.0;
@@ -90,25 +105,69 @@ class MsckfProcess
  utiltool::NavInfo nav_info_;
 
  cv::Mat pre_img;
+
+private:
+ std::ofstream ofs_camera_state_log_file_;
+ std::ofstream ofs_msckf_info_log_file_;
+ std::ofstream ofs_debug_info_log_file_;
+ std::ofstream ofs_feature_log_file_;
+ // std::ofstream ofs_feature_log_file_;
+ bool camera_state_log_enable_ = false;
+ bool msckf_info_log_enable_ = false;
+ bool debug_info_log_enable_ = false;
+ bool feature_log_enable_ = false;
 };
 
 struct ReprojectionError
 {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+ // // ReprojectionError(const cv::Point2f &measure_point_uv,
+ // // const Eigen::Isometry3d &cam0_cami_transform) : image_uv(measure_point_uv),
+ // // cam0_cami(cam0_cami_transform) {}
+ // // template <typename T>
+ // // bool operator()(const T *const feature_point_world, T *residuals) const
+ // // {
+ // // Eigen::Matrix<T, 3, 1> point;
+ // // point << feature_point_world[0], feature_point_world[1], feature_point_world[2];//
+
+ // // point = cam0_cami.inverse() * point;
+ // // T point_reproject_image[2] = {point(0) / point(2), point(1) / point(2)};
+ // // residuals[0] = point_reproject_image[0] - T(image_uv.x);
+ // // residuals[1] = point_reproject_image[1] - T(image_uv.y);
+ // // return true;
+ // // }
+
  ReprojectionError(const cv::Point2f &measure_point_uv,
- const Eigen::Isometry3d &cam0_cami_transform) : image_uv(measure_point_uv),
- cam0_cami(cam0_cami_transform) {}
+ const Eigen::Isometry3d &cam0_cami_transform) : image_uv(measure_point_uv)
+ {
+ Eigen::Isometry3d cami_cam0 = cam0_cami_transform.inverse();
+ quat = utiltool::attitude::RotationMartix2Quaternion(cami_cam0.rotation());
+ cam0_cami_translation[0] = cami_cam0.translation()(0);
+ cam0_cami_translation[1] = cami_cam0.translation()(1);
+ cam0_cami_translation[2] = cami_cam0.translation()(2);
+ }
  template <typename T>
  bool operator()(const T *const feature_point_world, T *residuals) const
  {
- Eigen::Matrix<T, 3, 1> point;
- point << feature_point_world[0], feature_point_world[1], feature_point_world[2];
-
- point = cam0_cami * point;
- T point_reproject_image[2] = {point(0) / point(2), point(1) / point(2)};
- residuals[0] = point_reproject_image[0] - T(image_uv.x);
- residuals[1] = point_reproject_image[1] - T(image_uv.y);
+ T point[3] = {T(feature_point_world[0]), T(feature_point_world[1]), T(1.0)};
+ T point_result[3];
+ T cam0_cami_quat[4] = {T(quat.w()),
+ T(quat.x()),
+ T(quat.y()),
+ T(quat.z())};
+ T point_translation[3] = {T(feature_point_world[2] * cam0_cami_translation[0]),
+ T(feature_point_world[2] * cam0_cami_translation[1]),
+ T(feature_point_world[2] * cam0_cami_translation[2])};
+ ceres::QuaternionRotatePoint(cam0_cami_quat, point, point_result);
+ point_result[0] += point_translation[0];
+ point_result[1] += point_translation[1];
+ point_result[2] += point_translation[2];
+
+ T point_reproject_image[2] = {T(point_result[0] / point_result[2]),
+ T(point_result[1] / point_result[2])};
+ residuals[0] = -point_reproject_image[0] + T(image_uv.x);
+ residuals[1] = -point_reproject_image[1] + T(image_uv.y);
  return true;
  }
 
@@ -119,7 +178,9 @@ struct ReprojectionError
  }
 
  const cv::Point2f image_uv;
- const Eigen::Isometry3d cam0_cami;
+ Eigen::Quaterniond quat;
+ double cam0_cami_translation[3];
+ // const Eigen::Isometry3d cam0_cami;
 };
 } // namespace camera
 } // namespace mscnav

include/data/navgnss.h

@@ -5,7 +5,7 @@
 ** Login <fangwentao>
 **
 ** Started on undefined Jul 21 上午10:02:18 2019 little fang
-** Last update Thu Nov 6 下午8:04:40 2019 little fang
+** Last update Fri Jan 16 下午2:39:02 2020 little fang
 */
 
 #ifndef GNSSDATA_H_
@@ -70,9 +70,15 @@ class FileGnssData : public GnssDataCollect
 
 private:
  void ReadingData();
+ void DetermineBreakTime(const std::vector<double> &break_time);
+ void DetermineBreakTimeArray(const std::vector<double> &break_time);
+ bool IsBreak(double sow);
 
 private:
  std::ifstream ifs_file_;
+ double up_bound_[20];
+ double low_bound_[20];
+ int bound_count_ = 0;
 };
 } // namespace mscnav
 #endif