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

Release/0.0.1 1

CMakeLists.txt

@@ -22,6 +22,6 @@ include_directories(${PROJECT_SOURCE_DIR}/include)
 aux_source_directory(${PROJECT_SOURCE_DIR}/src cpp_files)
 
 add_library(${PROJECT_NAME} SHARED ${cpp_files})
-target_link_libraries(${PROJECT_NAME}  glog::glog)
+target_link_libraries(${PROJECT_NAME}  ${GLOG_LIBRARY})
 
-add_subdirectory(${PROJECT_SOURCE_DIR}/test)
+# add_subdirectory(${PROJECT_SOURCE_DIR}/test)

include/navbase.hpp

@@ -5,7 +5,7 @@
 ** Login   <fangwentao>
 **
 ** Started on  Thu Jul 11 上午9:11:49 2019 little fang
-** Last update Thu Jul 17 下午5:57:33 2019 little fang
+** Last update Fri Feb 6 下午2:40:57 2020 little fang
 */
 
 #ifndef UTIL_BASE_H_
@@ -23,19 +23,70 @@
 
 namespace utiltool
 {
+
+// // 0.9
+// const double chi_squared_test_table[100] =
+//     {0.015790774, 0.210721031, 0.584374374, 1.063623217,
+//      1.610307987, 2.204130656, 2.833106918, 3.489539126,
+//      4.168159008, 4.865182052, 5.57778479, 6.30379606,
+//      7.04150458, 7.78953361, 8.546756242, 9.312236354,
+//      10.08518633, 10.86493612, 11.65091003, 12.44260921,
+//      13.23959798, 14.04149319, 14.8479558, 15.65868405,
+//      16.473408, 17.29188499, 18.11389597, 18.93924237,
+//      19.76774356, 20.59923461, 21.4335645, 22.27059448,
+//      23.11019674, 23.95225327, 24.79665478, 25.64329988,
+//      26.49209426, 27.34295004, 28.19578518, 29.05052293,
+//      29.90709137, 30.76542301, 31.6254544, 32.48712579,
+//      33.35038089, 34.21516651, 35.08143242, 35.94913102,
+//      36.81821727, 37.68864839, 38.56038379, 39.43338485,
+//      40.30761485, 41.18303878, 42.05962329, 42.93733653,
+//      43.8161481, 44.69602892, 45.5769512, 46.4588883,
+//      47.34181472, 48.225706, 49.11053868, 49.99629021,
+//      50.88293897, 51.77046414, 52.65884569, 53.54806438,
+//      54.43810163, 55.32893957, 56.22056097, 57.11294919,
+//      58.00608819, 58.89996246, 59.79455705, 60.68985747,
+//      61.58584975, 62.48252034, 63.37985614, 64.27784447,
+//      65.17647304, 66.07572996, 66.97560368, 67.87608301,
+//      68.77715708, 69.67881538, 70.58104766, 71.48384399,
+//      72.38719472, 73.29109048, 74.19552215, 75.10048085,
+//      76.00595797, 76.91194512, 77.81843412, 78.72541703,
+//      79.6328861, 80.54083379, 81.44925275, 82.35813581};
+//0.95
+const double chi_squared_test_table[100] =
+    {0.00393214, 0.102586589, 0.351846318, 0.710723021, 1.145476226, 1.635382894,
+     2.167349909, 2.732636793, 3.325112843, 3.940299136, 4.574813079, 5.226029488,
+     5.891864338, 6.570631384, 7.260943928, 7.961645572, 8.671760205, 9.390455081,
+     10.11701306, 10.85081139, 11.59130521, 12.33801458, 13.09051419, 13.84842503,
+     14.61140764, 15.37915658, 16.15139585, 16.92787504, 17.70836618, 18.49266098,
+     19.28056856, 20.07191346, 20.86653399, 21.66428071, 22.46501522, 23.26860902,
+     24.07494256, 24.88390438, 25.6953904, 26.5093032, 27.32555147, 28.1440495,
+     28.96471667, 29.78747708, 30.61225915, 31.43899527, 32.26762153, 33.09807743,
+     33.93030562, 34.76425168, 35.59986394, 36.43709324, 37.2758928, 38.11621806,
+     38.95802653, 39.80127763, 40.64593263, 41.49195448, 42.33930773, 43.18795845,
+     44.03787413, 44.88902356, 45.74137684, 46.59490522, 47.4495811, 48.30537793,
+     49.16227018, 50.02023325, 50.87924348, 51.73927805, 52.60031495, 53.46233296,
+     54.3253116, 55.18923108, 56.05407229, 56.91981675, 57.78644661, 58.65394456,
+     59.52229389, 60.39147839, 61.26148237, 62.13229063, 63.00388842, 63.87626144,
+     64.74939583, 65.62327812, 66.49789524, 67.37323449, 68.24928355, 69.12603043,
+     70.00346347, 70.88157134, 71.76034302, 72.63976779, 73.51983519, 74.40053508,
+     75.28185754, 76.16379294, 77.04633186, 77.92946517};
 /**
  * @brief  spilt string 
  * @note   
  * @param  &in: string in which need split
  * @param  &delim: spliter mark
  * @retval 
  */
-std::vector<std::string> TextSplit(const std::string &in, const std::string &delim);
+std::vector<std::string>
+TextSplit(const std::string &in, const std::string &delim);
 
 std::string &trim(std::string &s);
 
 std::ostream &operator<<(std::ostream &output, const NavTime &time);
 
+NavInfo InterpolateNavInfo(const NavInfo &nav_info1, const NavInfo &nav_info2, const NavTime &time_);
+
+NavInfo &NormalizeAttitude(NavInfo &nav_info);
 /**
  * @brief  override the ostream output NavResultInfo
  * @note   
@@ -103,6 +154,14 @@ operator<<(std::ostream &output, const std::vector<T> &data)
   }
   return output;
 }
+
+template <typename T>
+T interpolate(const T &t1, const T &t2, double coeff)
+{
+  T t_res = t1 + (t2 - t1) * coeff;
+  return t_res;
+}
+
 } // namespace utiltool
 
 #endif /* !UTIL_BASE_H_ */

include/navearth.hpp

@@ -5,13 +5,15 @@
 ** Login   <fangwentao>
 **
 ** Started on  Thu Jul 11 下午7:17:43 2019 little fang
-** Last update Sun Jul 13 上午6:26:32 2019 little fang
+** Last update Thu Jul 31 下午3:29:57 2019 little fang
 */
 
 #ifndef UTIL_EARTH_H_
 #define UTIL_EARTH_H_
 
 #include "constant.hpp"
+#include "navstruct.hpp"
+#include "navbase.hpp"
 #include <Eigen/Dense>
 
 namespace utiltool
@@ -29,7 +31,7 @@ using Eigen::Vector3d;
 template <typename T>
 inline T POW3(const T &t)
 {
-    return t * t * t;
+  return t * t * t;
 }
 
 Vector3d CalculateGravity(const Vector3d &pos, bool IsECEF = true);
@@ -70,6 +72,32 @@ Eigen::Matrix3d CalCe2n(double B, double L);
 
 inline Eigen::Vector3d wiee() { return Eigen::Vector3d(0, 0, WGS84_AngleRate); }
 
+/**
+ * @brief  Correct the leverarm for Position
+ * @note   
+ * @param  &navinfo: 
+ * @retval 
+ */
+inline Eigen::Vector3d CorrectLeverarmPos(const NavInfo &navinfo)
+{
+  return navinfo.pos_ + navinfo.rotation_ * navinfo.leverarm_;
+}
+
+/**
+ * @brief  Correct the leverarm for Velocity
+ * @note   
+ * @param  &navinfo: 
+ * @retval 
+ */
+inline Eigen::Vector3d CorrectLeverarmVel(const NavInfo &navinfo)
+{
+  Eigen::Matrix3d wie_x = skew(wiee());
+  Eigen::Matrix3d lb_x = skew(navinfo.leverarm_);
+  auto &lb = navinfo.leverarm_;
+  auto &Rbe = navinfo.rotation_;
+  return navinfo.vel_ - wie_x * Rbe * lb - Rbe * lb_x * navinfo.wibb_;
+}
+
 } // namespace earth
 } // namespace utiltool
 

include/navlog.hpp

@@ -26,7 +26,7 @@ inline void LogInit(const char *argv, const std::string &path, int loglevel = 0)
 #ifdef GLOGOUTPUT
   FLAGS_stderrthreshold = loglevel;
   google::InitGoogleLogging((const char *)argv);
-  google::SetLogDestination(loglevel, path.c_str());
+  google::SetLogDestination(0, path.c_str());
 #else
   FLAGS_stderrthreshold = google::FATAL;
   google::InitGoogleLogging((const char *)argv);

include/navstruct.hpp

@@ -5,7 +5,7 @@
 ** Login   <fangwentao>
 **
 ** Started on  Wed Jul 3 14:38:27 2019 little fang
-** Last update Sat Jul 19 下午5:24:56 2019 little fang
+** Last update Thu Aug 21 下午9:29:43 2019 little fang
 */
 
 #ifndef NAVSTRUCT_H_
@@ -20,7 +20,7 @@
 #include <sstream>
 #include <deque>
 #include <Eigen/Dense>
-
+#include <map>
 #include "navtime.h"
 
 namespace utiltool
@@ -45,7 +45,7 @@ enum DataType
   IMUDATA,
   GNSSDATA,
   ODODATA,
-  FEATUREDATA,
+  CAMERADATA,
   RESULTDATA
 };
 
@@ -103,18 +103,18 @@ class GnssData : public BaseData
   }
   virtual ~GnssData() {}
 
-// public:
-//   virtual std::string to_string() override
-//   {
-//     std::ostringstream osstream;
-//     osstream << t0_.Time2String() << "\t";
-//     osstream << std::fixed << std::setprecision(9) << std::setw(13) << pos_.transpose() << "\t";
-//     osstream << std::fixed << std::setprecision(4) << std::setw(7) << vel_.transpose() << "\t";
-//     osstream << std::fixed << std::setprecision(4) << std::setw(7) << pos_std_.transpose() << "\t";
-//     osstream << std::fixed << std::setprecision(4) << std::setw(7) << vel_std_.transpose() << "\t";
-//     osstream << std::fixed << gnss_type_;
-//     return osstream.str();
-//   }
+  // public:
+  //   virtual std::string to_string() override
+  //   {
+  //     std::ostringstream osstream;
+  //     osstream << t0_.Time2String() << "\t";
+  //     osstream << std::fixed << std::setprecision(9) << std::setw(13) << pos_.transpose() << "\t";
+  //     osstream << std::fixed << std::setprecision(4) << std::setw(7) << vel_.transpose() << "\t";
+  //     osstream << std::fixed << std::setprecision(4) << std::setw(7) << pos_std_.transpose() << "\t";
+  //     osstream << std::fixed << std::setprecision(4) << std::setw(7) << vel_std_.transpose() << "\t";
+  //     osstream << std::fixed << gnss_type_;
+  //     return osstream.str();
+  //   }
 
 public:
   Eigen::Vector3d pos_{0, 0, 0};
@@ -183,14 +183,14 @@ class OdoData : public BaseData
   }
   virtual ~OdoData() {}
 
-// public:
-//   virtual std::string to_string() override
-//   {
-//     std::ostringstream osstream;
-//     osstream << t0_.Time2String() << "\t";
-//     osstream << std::fixed << std::setprecision(5) << std::setw(10) << odo_vel.transpose();
-//     return osstream.str();
-//   }
+  // public:
+  //   virtual std::string to_string() override
+  //   {
+  //     std::ostringstream osstream;
+  //     osstream << t0_.Time2String() << "\t";
+  //     osstream << std::fixed << std::setprecision(5) << std::setw(10) << odo_vel.transpose();
+  //     return osstream.str();
+  //   }
 
 public:
   Eigen::Vector4d odo_vel{0, 0, 0, 0};
@@ -202,7 +202,8 @@ using IMUDATAPOOL = std::deque<ImuData::Ptr>;
 struct NavInfo
 {
   NavTime time_;
-  Eigen::Quaterniond quat_{0, 0, 0, 0};
+  Eigen::Quaterniond quat_{1, 0, 0, 0};
+  Eigen::Matrix3d rotation_ = Eigen::Matrix3d::Identity();
   Eigen::Vector3d pos_{0, 0, 0};
   Eigen::Vector3d vel_{0, 0, 0};
   Eigen::Vector3d att_{0, 0, 0};
@@ -216,7 +217,11 @@ struct NavInfo
   Eigen::Vector3d gyro_scale_{0, 0, 0};
   Eigen::Vector3d acce_scale_{0, 0, 0};
   Eigen::Vector3d leverarm_{0, 0, 0};
+  Eigen::Vector3d cam_imu_{0, 0, 0};
   long long int result_type_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 struct StateIndex
@@ -233,6 +238,9 @@ struct StateIndex
   int camera_rotation_index_ = 0;
   int camera_translation_index_ = 0;
   int imu_vehicle_rotation_index_ = 0;
+  int total_state = 0;  //exclude camera state
+  std::map<long long int, int, std::less<long long int>>
+      camera_state_index; //fisrt CameraState ID，second CameraState index
   bool is_initialized = false;
 };
 

include/navtime.h

@@ -60,7 +60,7 @@ class NavTime
   NavTime operator-(double second);
   NavTime operator+(int second);
   NavTime operator-(int second);
-  double operator-(const NavTime &time);
+  double operator-(const NavTime &time) const;
   void operator+=(double second);
   void operator-=(double second);
   bool operator<(const NavTime &time) const;
@@ -72,7 +72,7 @@ class NavTime
 
 public:
   static NavTime NowTime();
-  std::string Time2String(const std::string &format = "%04d-%02d-%02d %02d-%02d-%04.1f", TimeType time_type = COMMONTIME) const;
+  std::string Time2String(const std::string &format = "%04d %02d %02d %02d %02d %04.1f", TimeType time_type = COMMONTIME) const;
   int Year() const;
   int Month() const;
   int Day() const;

src/navattitude.cc

@@ -39,7 +39,6 @@ Eigen::Quaterniond RotationMartix2Quaternion(const Eigen::Matrix3d &mat)
  */
 Eigen::Quaterniond Euler2Quaternion(const Euler &euler)
 {
-    //TODO need check the order
     //DONE check by little fang in 20190712
     return (Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *
@@ -65,7 +64,6 @@ Eigen::Matrix3d Euler2RotationMatrix(const Euler &euler)
   */
 Euler RotationMartix2Euler(const Eigen::Matrix3d &mat)
 {
-    // TODO need to check the order of the axis coincide with roll>pitch>heading/yaw
     // DONE check by little fang. writed by ourselies, do not use Eigen.
     return eulerAngles(mat);
 }
@@ -93,7 +91,7 @@ Eigen::Quaterniond RotationVector2Quaternion(const RotationVector &rv)
     RotationVector rv_2 = rv * 0.5;
     double norm = rv_2.norm();
     qfromrv.w() = cos(norm);
-    qfromrv.vec() = norm < 1e-5 ? rv_2 : (sin(norm) / norm) * rv_2;
+    qfromrv.vec() = norm < 1e-8 ? rv_2 : (sin(norm) / norm) * rv_2;
     return qfromrv;
 }