├── App ├── CMakeLists.txt ├── EuRoC_test.cpp ├── InputSequence.h └── run_backend.cpp ├── Backend ├── BundleAdjustment │ ├── GlobalBundleAdjustor.cpp │ ├── GlobalBundleAdjustor.h │ ├── GlobalBundleAdjustorDL.cpp │ ├── GlobalBundleAdjustorDebug.cpp │ ├── GlobalBundleAdjustorGN.cpp │ ├── GlobalBundleAdjustorIO.cpp │ ├── LocalBundleAdjustor.cpp │ ├── LocalBundleAdjustor.h │ ├── LocalBundleAdjustorDL.cpp │ ├── LocalBundleAdjustorDebug.cpp │ ├── LocalBundleAdjustorGN.cpp │ └── LocalBundleAdjustorIO.cpp ├── CMakeLists.txt ├── Geometry │ ├── BoundingBox.h │ ├── Camera.cpp │ ├── Camera.h │ ├── CameraPrior.cpp │ ├── CameraPrior.h │ ├── CameraTrajectory.h │ ├── Depth.cpp │ ├── Depth.h │ ├── IMU.cpp │ ├── IMU.h │ ├── Intrinsic.cpp │ ├── Intrinsic.h │ ├── M-Estimator.h │ ├── Point.h │ ├── Rigid.h │ ├── Rotation.h │ └── Similarity.h ├── IBA │ ├── IBA.cpp │ ├── IBA.h │ ├── IBA_config.h │ ├── IBA_datatype.h │ ├── IBA_internal.h │ ├── Parameter.cpp │ ├── Parameter.h │ ├── stdafx.cpp │ ├── stdafx.h │ └── targetver.h ├── LinearAlgebra │ ├── LinearSystem.h │ ├── Matrix2x2.h │ ├── Matrix2x3.h │ ├── Matrix2x4.h │ ├── Matrix2x6.h │ ├── Matrix2x7.h │ ├── Matrix2x8.h │ ├── Matrix3x2.h │ ├── Matrix3x3.h │ ├── Matrix3x4.h │ ├── Matrix3x6.h │ ├── Matrix3x7.h │ ├── Matrix3x8.h │ ├── Matrix4x4.h │ ├── Matrix6x6.h │ ├── Matrix6x7.h │ ├── Matrix8x8.h │ ├── Matrix9x9.h │ ├── MatrixMxN.h │ ├── Vector12.h │ ├── Vector2.h │ ├── Vector3.h │ ├── Vector4.h │ ├── Vector6.h │ ├── Vector7.h │ ├── Vector8.h │ ├── Vector9.h │ └── VectorN.h ├── Map │ ├── Feature.cpp │ ├── Feature.h │ ├── Frame.h │ ├── GlobalMap.cpp │ ├── GlobalMap.h │ ├── LocalMap.cpp │ └── LocalMap.h ├── Utility │ ├── AlignedMatrix.h │ ├── AlignedVector.h │ ├── Candidate.h │ ├── Configurator.h │ ├── FillMap.h │ ├── MultiThread.cpp │ ├── MultiThread.h │ ├── SIMD.h │ ├── Table.h │ ├── Timer.h │ ├── Utility.cpp │ ├── Utility.h │ └── simd_sse_neon.h └── Visualization │ ├── Arcball.h │ ├── EventHandler.h │ ├── Frustrum.h │ ├── Keyboard.h │ ├── TextureGL.h │ ├── TextureGL.hpp │ ├── UtilityWM.h │ ├── Viewer.cpp │ ├── Viewer.h │ ├── ViewerIBA.cpp │ ├── ViewerIBA.h │ ├── ViewerIBADraw.cpp │ ├── ViewerIBAIO.cpp │ ├── ViewerIBAKey.hpp │ ├── stdafx.cpp │ ├── stdafx.h │ └── targetver.h ├── CMakeLists.txt ├── Frontend ├── CMakeLists.txt ├── CameraBase.cpp ├── ORBextractor.cc ├── ORBextractor.h ├── basic_datatype.h ├── cameras │ ├── CameraBase.hpp │ ├── DistortionBase.hpp │ ├── PinholeCamera.hpp │ ├── RadialTangentialDistortion.hpp │ ├── RadialTangentialDistortion8.hpp │ └── implementation │ │ ├── CameraBase.hpp │ │ ├── PinholeCamera.hpp │ │ ├── RadialTangentialDistortion.hpp │ │ └── RadialTangentialDistortion8.hpp ├── feature_utils.cc ├── feature_utils.h ├── feature_utils_align.cc ├── feature_utils_direct_matcher.cc ├── feature_utils_feature_track.cc ├── feature_utils_uniformity.cc ├── feature_utils_warp.cc ├── iba_helper.cc ├── iba_helper.h ├── image_utils.cc ├── image_utils.h ├── param.cc ├── param.h ├── patch_score.h ├── plotting_utils.cpp ├── plotting_utils.h ├── pose_viewer.cc ├── pose_viewer.h ├── rotation.cc ├── rotation.h ├── shared_queue.h ├── timer.cc ├── timer.h ├── xp_quaternion.cc ├── xp_quaternion.h ├── xppyramid.cpp └── xppyramid.hpp ├── LICENSE ├── README.md ├── build.sh ├── cmake ├── FindCVD.cmake ├── FindEigen.cmake ├── FindGFlags.cmake ├── FindGTest.cmake ├── FindGlog.cmake └── FindYaml.cmake ├── config ├── config_of_mono.txt └── config_of_stereo.txt ├── metadata └── scripts ├── run_backend_only.sh ├── run_ice_ba_mono.sh └── run_ice_ba_stereo.sh /App/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(EuRoC_test) 3 | 4 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/../bin) 5 | 6 | # ========================= 7 | # run front-end + back-end with EuRoC dataset 8 | # ========================= 9 | add_executable(ice_ba 10 | ../App/EuRoC_test.cpp 11 | ) 12 | target_link_libraries(ice_ba 13 | IBA 14 | OF 15 | ) 16 | if (CVD_FOUND) 17 | target_link_libraries(ice_ba 18 | IBAVis 19 | ) 20 | endif(CVD_FOUND) 21 | 22 | 23 | 24 | # ========================= 25 | # run back-end only with .dat file 26 | # ========================= 27 | add_executable(back_end 28 | ../App/run_backend.cpp 29 | ) 30 | target_include_directories(back_end PUBLIC 31 | ${CMAKE_CURRENT_SOURCE_DIR} 32 | ${GLOBAL_IBA_INCLUDE_DIRS} 33 | ${EIGEN_INCLUDE_DIR} 34 | ) 35 | target_link_libraries(back_end 36 | IBA 37 | OF 38 | ) 39 | if (CVD_FOUND) 40 | target_include_directories(back_end PUBLIC 41 | ${GLOBAL_VISUALIZATION_INCLUDE_DIR} 42 | ) 43 | target_link_libraries(back_end 44 | IBAVis 45 | ) 46 | endif (CVD_FOUND) 47 | -------------------------------------------------------------------------------- /Backend/Geometry/BoundingBox.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _BOUNDING_BOX_H_ 17 | #define _BOUNDING_BOX_H_ 18 | 19 | #include "Point.h" 20 | 21 | class BoundingBox2D { 22 | 23 | public: 24 | 25 | inline BoundingBox2D() {} 26 | inline BoundingBox2D(const float xMin, const float yMin, const float xMax, 27 | const float yMax) : m_xMin(xMin, yMin), m_xMax(xMax, yMax) {} 28 | inline BoundingBox2D(const Point2D &xMin, const Point2D &xMax) : m_xMin(xMin), m_xMax(xMax) {} 29 | inline void Initialize() { m_xMin.Set(FLT_MAX, FLT_MAX); m_xMax.Set(-FLT_MAX, -FLT_MAX); } 30 | inline void Include(const Point2D &x) { 31 | if (x.x() < m_xMin.x()) { 32 | m_xMin.x() = x.x(); 33 | } 34 | if (x.y() < m_xMin.y()) { 35 | m_xMin.y() = x.y(); 36 | } 37 | if (x.x() > m_xMax.x()) { 38 | m_xMax.x() = x.x(); 39 | } 40 | if (x.y() > m_xMax.y()) { 41 | m_xMax.y() = x.y(); 42 | } 43 | } 44 | inline void Set(const Point2D &xMin, const Point2D &xMax) { m_xMin = xMin; m_xMax = xMax; } 45 | inline void Get(Point2D &xMin, Point2D &xMax) const { xMin = m_xMin; xMax = m_xMax; } 46 | inline bool Inside(const Point2D &x) const { return x.x() >= m_xMin.x() && x.y() >= m_xMin.y() && x.x() <= m_xMax.x() && x.y() <= m_xMax.y(); } 47 | inline bool Outside(const Point2D &x) const { return x.x() < m_xMin.x() || x.y() < m_xMin.y() || x.x() > m_xMax.x() || x.y() > m_xMax.y(); } 48 | //inline float ComputeDiagonalLength() const { return sqrtf(m_xMin.SquaredDistance(m_xMax)); } 49 | //inline float ComputeDiagonalSquaredLength() const { return m_xMin.SquaredDistance(m_xMax); } 50 | 51 | public: 52 | 53 | Point2D m_xMin, m_xMax; 54 | 55 | }; 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /Backend/Geometry/Similarity.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _SIMILARITY_H_ 17 | #define _SIMILARITY_H_ 18 | 19 | #include "Rotation.h" 20 | 21 | class Similarity3D : public Rotation3D { 22 | 23 | public: 24 | 25 | inline const xp128f& sr00_sr01_sr02_tx() const { return r_00_01_02_x(); } 26 | inline xp128f& sr00_sr01_sr02_tx() { return r_00_01_02_x(); } 27 | inline const xp128f& sr10_sr11_sr12_ty() const { return r_10_11_12_x(); } 28 | inline xp128f& sr10_sr11_sr12_ty() { return r_10_11_12_x(); } 29 | inline const xp128f& sr20_sr21_sr22_tz() const { return r_20_21_22_x(); } 30 | inline xp128f& sr20_sr21_sr22_tz() { return r_20_21_22_x(); } 31 | 32 | inline const float& sr00() const { return r00(); } inline float& sr00() { return r00(); } 33 | inline const float& sr01() const { return r01(); } inline float& sr01() { return r01(); } 34 | inline const float& sr02() const { return r02(); } inline float& sr02() { return r02(); } 35 | inline const float& tx () const { return r0 (); } inline float& tx () { return r0 (); } 36 | inline const float& sr10() const { return r10(); } inline float& sr10() { return r10(); } 37 | inline const float& sr11() const { return r11(); } inline float& sr11() { return r11(); } 38 | inline const float& sr12() const { return r12(); } inline float& sr12() { return r12(); } 39 | inline const float& ty () const { return r1 (); } inline float& ty () { return r1 (); } 40 | inline const float& sr20() const { return r20(); } inline float& sr20() { return r20(); } 41 | inline const float& sr21() const { return r21(); } inline float& sr21() { return r21(); } 42 | inline const float& sr22() const { return r22(); } inline float& sr22() { return r22(); } 43 | inline const float& tz () const { return r2 (); } inline float& tz () { return r2 (); } 44 | 45 | inline Point3D operator * (const Point3D &X) const { 46 | #ifdef CFG_DEBUG 47 | UT_ASSERT(X.w() == 1.0f); 48 | #endif 49 | Point3D SX; 50 | Apply(X, SX); 51 | return SX; 52 | } 53 | 54 | inline void Set(const float s12, const Quaternion &q1, const Quaternion &q2, const Point3D &p1, 55 | const Point3D &p2) { 56 | Quaternion q12; 57 | Quaternion::AIB(q2, q1, q12); 58 | m_s = _mm_set1_ps(s12); 59 | m_R.SetQuaternion(q12); 60 | m_t = p2 - Rotation3D::GetApplied(p1); 61 | Update(); 62 | } 63 | inline void Update() { 64 | m_R.GetScaled(m_s, *this); 65 | tx() = m_t.v0(); 66 | ty() = m_t.v1(); 67 | tz() = m_t.v2(); 68 | } 69 | 70 | inline void Apply(const Point3D &X, LA::AlignedVector3f &SX) const { 71 | #ifdef CFG_DEBUG 72 | UT_ASSERT(X.w() == 1.0f); 73 | #endif 74 | SX.x() = SIMD::Sum(_mm_mul_ps(sr00_sr01_sr02_tx(), X.xyzw())); 75 | SX.y() = SIMD::Sum(_mm_mul_ps(sr10_sr11_sr12_ty(), X.xyzw())); 76 | SX.z() = SIMD::Sum(_mm_mul_ps(sr20_sr21_sr22_tz(), X.xyzw())); 77 | } 78 | inline void ApplyRotation(const Point3D &X, LA::AlignedVector3f &RX) const { m_R.Apply(X, RX); } 79 | inline void ApplyRotationScale(const Point3D &X, LA::AlignedVector3f &sRX) const { Rotation3D::Apply(X, sRX); } 80 | inline LA::AlignedVector3f GetApplied(const Point3D &X) const { LA::AlignedVector3f SX; Apply(X, SX); return SX; } 81 | 82 | inline void Print(const bool e = false) const { 83 | UT::PrintSeparator(); 84 | if (e) { 85 | UT::Print(" T = %e %e %e %e\n", sr00(), sr01(), sr02(), tx()); 86 | UT::Print(" %e %e %e %e\n", sr10(), sr11(), sr12(), ty()); 87 | UT::Print(" %e %e %e %e\n", sr20(), sr21(), sr22(), tz()); 88 | UT::Print(" s = %e\n", m_s[0]); 89 | UT::Print(" R = %e %e %e\n", r00(), r01(), r02()); 90 | UT::Print(" %e %e %e\n", r10(), r11(), r12()); 91 | UT::Print(" %e %e %e\n", r20(), r21(), r22()); 92 | UT::Print(" t = %e %e %e\n", tx(), ty(), tz()); 93 | } else { 94 | UT::Print(" T = %f %f %f %f\n", sr00(), sr01(), sr02(), tx()); 95 | UT::Print(" %f %f %f %f\n", sr10(), sr11(), sr12(), ty()); 96 | UT::Print(" %f %f %f %f\n", sr20(), sr21(), sr22(), tz()); 97 | UT::Print(" s = %e\n", m_s[0]); 98 | UT::Print(" R = %f %f %f\n", r00(), r01(), r02()); 99 | UT::Print(" %f %f %f\n", r10(), r11(), r12()); 100 | UT::Print(" %f %f %f\n", r20(), r21(), r22()); 101 | UT::Print(" t = %f %f %f\n", tx(), ty(), tz()); 102 | } 103 | } 104 | 105 | public: 106 | 107 | xp128f m_s; 108 | Rotation3D m_R; 109 | LA::AlignedVector3f m_t; 110 | }; 111 | 112 | #ifdef CFG_DEBUG_EIGEN 113 | class EigenSimilarity3D { 114 | public: 115 | inline EigenSimilarity3D() {} 116 | #ifdef _MSC_VER 117 | inline EigenSimilarity3D(const Similarity3D &S) : m_s(S.m_s[0]), m_R(S.m_R), m_t(S.m_t) {} 118 | #else 119 | inline EigenSimilarity3D(const Similarity3D &S) : m_s(S.m_s[0]), m_R(S.m_R), m_t(S.m_t) {} 120 | #endif // _MSC_VER 121 | inline EigenSimilarity3D(const float e_s, const EigenRotation3D &e_R, 122 | const EigenVector3f &e_t) : m_s(e_s), m_R(e_R), m_t(e_t) {} 123 | inline EigenVector3f operator * (const EigenVector3f &e_X) const { 124 | return EigenVector3f(m_s * m_R * e_X + m_t); 125 | } 126 | inline Similarity3D GetSimilarity() const { 127 | Similarity3D S; 128 | S.m_s = _mm_set1_ps(m_s); 129 | S.m_R = m_R.GetAlignedMatrix3x3f(); 130 | S.m_t = m_t.GetAlignedVector3f(); 131 | S.Update(); 132 | return S; 133 | } 134 | public: 135 | float m_s; 136 | EigenRotation3D m_R; 137 | EigenVector3f m_t; 138 | }; 139 | #endif 140 | 141 | #endif 142 | -------------------------------------------------------------------------------- /Backend/IBA/IBA_config.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef IBA_IBA_CONFIG_H_ 17 | #define IBA_IBA_CONFIG_H_ 18 | 19 | #define CFG_IMU_FULL_COVARIANCE 20 | #define CFG_INCREMENTAL_PCG 21 | //#define CFG_PCG_DOUBLE 22 | //#define CFG_PCG_FULL_BLOCK 23 | #ifdef CFG_PCG_DOUBLE 24 | typedef double PCG_TYPE; 25 | #else 26 | typedef float PCG_TYPE; 27 | #endif 28 | //#define CFG_CAMERA_PRIOR_SQUARE_FORM 29 | #define CFG_CAMERA_PRIOR_DOUBLE 30 | //#define CFG_CAMERA_PRIOR_REORDER 31 | //#define CFG_HANDLE_SCALE_JUMP 32 | #define CFG_CHECK_REPROJECTION 33 | 34 | #define CFG_STEREO 35 | #define CFG_SERIAL 36 | #define CFG_VERBOSE 37 | #define CFG_HISTORY 38 | #define CFG_GROUND_TRUTH 39 | #ifdef WIN32 40 | //#define CFG_DEBUG 41 | #else 42 | //#define CFG_DEBUG 43 | #endif 44 | #ifdef CFG_DEBUG 45 | #define CFG_DEBUG_EIGEN 46 | #endif 47 | //#define CFG_DEBUG_MT 48 | 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #ifdef WIN32 55 | #include 56 | #include 57 | #include 58 | #endif 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | typedef unsigned char ubyte; 68 | typedef unsigned short ushort; // NOLINT 69 | typedef unsigned int uint; 70 | typedef unsigned long long ullong; // NOLINT 71 | 72 | #ifdef max 73 | #undef max 74 | #endif 75 | #ifdef min 76 | #undef min 77 | #endif 78 | #ifdef small 79 | #undef small 80 | #endif 81 | #ifdef _C2 82 | #undef _C2 83 | #endif 84 | #ifdef _T 85 | #undef _T 86 | #endif 87 | #ifdef ERROR 88 | #undef ERROR 89 | #endif 90 | 91 | #endif // IBA_IBA_CONFIG_H_ 92 | -------------------------------------------------------------------------------- /Backend/IBA/IBA_datatype.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _IBA_DATATYPE_H_ 17 | #define _IBA_DATATYPE_H_ 18 | 19 | #include 20 | 21 | #define IBA_SERIAL_NONE 0x00000000 22 | #define IBA_SERIAL_LBA 0x00000001 23 | #define IBA_SERIAL_GBA 0x00000100 24 | 25 | #define IBA_VERBOSE_NONE 0x00000000 26 | #define IBA_VERBOSE_LBA 0x00000001 27 | #define IBA_VERBOSE_GBA 0x00000100 28 | 29 | #define IBA_DEBUG_NONE 0x00000000 30 | #define IBA_DEBUG_LBA 0x00000001 31 | #define IBA_DEBUG_GBA 0x00000100 32 | 33 | #define IBA_HISTORY_NONE 0x00000000 34 | #define IBA_HISTORY_LBA 0x00000001 35 | #define IBA_HISTORY_GBA 0x00000100 36 | 37 | namespace IBA { 38 | 39 | struct Intrinsic { 40 | float fx, fy; // focal length 41 | float cx, cy; // optic center 42 | float ds[8]; // distortion parameters 43 | }; 44 | 45 | struct Calibration { 46 | int w, h; // image resolution 47 | bool fishEye; // fish eye distortion model 48 | float Tu[3][4]; // X_cam = Tu * X_imu 49 | float ba[3]; // initial acceleration bias 50 | float bw[3]; // initial gyroscope bias 51 | //float sa[3]; 52 | Intrinsic K; // intrinsic parameters 53 | #ifdef CFG_STEREO 54 | float Tr[3][4]; // X_left = Tr * X_right 55 | Intrinsic Kr; // intrinsic parameters for right camera 56 | #endif 57 | }; 58 | 59 | struct CameraPose { 60 | float R[3][3]; // rotation matrix, R[0][0] = FLT_MAX for unknown camera pose 61 | float p[3]; // position 62 | }; // for a 3D point in world frame X, its coordinate in camera frame is obtained by R * (X - p) 63 | 64 | struct CameraPoseCovariance { 65 | float S[6][6]; // position + rotation 66 | // p = \hat p + \tilde p 67 | // R = \hat R * exp(\tilde\theta) 68 | }; 69 | 70 | struct CameraIMUState { 71 | CameraPose C; // camera pose 72 | float v[3]; // velocity, v[0] = FLT_MAX for unknown velocity 73 | float ba[3]; // acceleration bias, ba[0] = FLT_MAX for unknown acceleration bias 74 | float bw[3]; // gyroscope bias, bw[0] = FLT_MAX for unknown gyroscope bias 75 | }; 76 | 77 | struct Depth { 78 | float d; // inverse depth, d = 0 for unknown depth 79 | float s2; // variance 80 | }; 81 | 82 | struct Point2D { 83 | float x[2]; // feature location in the original image 84 | float S[2][2]; // covariance matrix in the original image 85 | }; 86 | 87 | struct Point3D { 88 | int idx; // global point index 89 | float X[3]; // 3D position, X[0] = FLT_MAX for unknown 3D position 90 | }; 91 | 92 | struct MapPointMeasurement { 93 | union { 94 | int iFrm; // frame ID 95 | int idx; // global point ID 96 | }; 97 | inline bool operator < (const MapPointMeasurement &X) const { 98 | return iFrm < X.iFrm 99 | #ifdef CFG_STEREO 100 | //#if 1 101 | || iFrm <= X.iFrm && !right && X.right 102 | #endif 103 | ; 104 | } 105 | Point2D x; 106 | #ifdef CFG_STEREO 107 | //#if 1 108 | ubyte right; 109 | #endif 110 | }; 111 | 112 | struct MapPoint { 113 | Point3D X; 114 | std::vector zs; 115 | }; 116 | 117 | struct FeatureTrack { 118 | int idx; 119 | Point2D x; 120 | }; 121 | 122 | struct IMUMeasurement { 123 | float a[3]; // acceleration 124 | float w[3]; // gyroscope 125 | float t; // timestamp 126 | }; 127 | 128 | struct CurrentFrame { 129 | int iFrm; // frame index 130 | CameraIMUState C; // initial camera/IMU state of current frame 131 | std::vector zs; // feature measurements of current frame 132 | std::vector us; // IMU measurements between last frame and current frame; 133 | // the timestamp of first IMU must be the same as last frame 134 | float t; // timestamp of current frame, should be greater than the timestamp of last IMU 135 | Depth d; // a rough depth estimate for current frame 136 | // (e.g. average over all visible points in current frame) 137 | std::string fileName; // image file name, just for visualization 138 | #ifdef CFG_STEREO 139 | //#if 1 140 | std::string fileNameRight; 141 | #endif 142 | }; 143 | 144 | struct KeyFrame { 145 | int iFrm; // frame index, -1 for invalid keyframe 146 | CameraPose C; // initial camera pose of keyframe 147 | std::vector zs; // feature measurements of keyframe 148 | std::vector Xs; // new map points 149 | Depth d; // a rough depth estimate 150 | }; 151 | 152 | struct SlidingWindow { 153 | std::vector iFrms; // frame indexes of those sliding window frames whose 154 | // camera/IMU state has been updated since last call 155 | std::vector CsLF; // camera/IMU states corresponding to iFrms 156 | std::vector iFrmsKF; // frame indexes of those keyframes whose camera pose 157 | // has been updated since last call 158 | std::vector CsKF; // camera poses corresponding to iFrmsKF 159 | std::vector Xs; // updated 3D points since last call 160 | #ifdef CFG_CHECK_REPROJECTION 161 | std::vector > esLF, esKF; 162 | #endif 163 | }; 164 | 165 | struct RelativeConstraint { 166 | int iFrm1, iFrm2; 167 | CameraPose T; // X2 = T * X1 = R * (X1 - p) 168 | CameraPoseCovariance S; 169 | }; 170 | 171 | struct Error { 172 | float ex; // feature reprojection error 173 | float eur, eup, euv, euba, eubw; // IMU delta error 174 | float edr, edp; // drift error compared to ground truth 175 | }; 176 | 177 | struct Time { 178 | float t; 179 | int n; 180 | }; 181 | 182 | } // namespace IBA 183 | 184 | #endif // _IBA_DATATYPE_H_ 185 | -------------------------------------------------------------------------------- /Backend/IBA/IBA_internal.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _IBA_INTERNAL_H_ 17 | #define _IBA_INTERNAL_H_ 18 | 19 | #include "IBA.h" 20 | #include "LocalBundleAdjustor.h" 21 | #include "GlobalBundleAdjustor.h" 22 | 23 | class ViewerIBA; 24 | 25 | namespace IBA { 26 | 27 | class Internal { 28 | 29 | public: 30 | 31 | class FeatureMeasurement { 32 | public: 33 | inline bool operator < (const FeatureMeasurement &z) const { 34 | return m_id < z.m_id 35 | #ifdef CFG_STEREO 36 | || (m_id == z.m_id && !m_right && z.m_right) 37 | #endif 38 | ; 39 | } 40 | inline bool operator < (const int id) const { 41 | return m_id < id; 42 | } 43 | public: 44 | int m_id; 45 | ::Point2D m_z; 46 | LA::SymmetricMatrix2x2f m_W; 47 | ubyte m_right; 48 | }; 49 | 50 | class MapPointIndex { 51 | public: 52 | inline MapPointIndex() {} 53 | inline MapPointIndex(const int iFrm, const int idx, const int ix) : 54 | m_iFrm(iFrm), m_idx(idx), m_ix(ix) {} 55 | inline bool operator < (const MapPointIndex &idx) const { 56 | return m_iFrm < idx.m_iFrm || (m_iFrm == idx.m_iFrm && m_idx < idx.m_idx); 57 | } 58 | inline void Set(const int iFrm, const int idx, const int ix) { 59 | m_iFrm = iFrm; 60 | m_idx = idx; 61 | m_ix = ix; 62 | } 63 | public: 64 | int m_iFrm, m_idx, m_ix; 65 | }; 66 | 67 | public: 68 | 69 | void* operator new(std::size_t count) { 70 | // [NOTE] : we don't use count here, count is equal to sizeof(Internal) 71 | return SIMD::Malloc(); 72 | } 73 | 74 | void operator delete(void* p) { 75 | SIMD::Free(static_cast(p)); 76 | } 77 | 78 | protected: 79 | 80 | const LocalBundleAdjustor::InputLocalFrame& PushCurrentFrame(const CurrentFrame &CF); 81 | const GlobalMap::InputKeyFrame& PushKeyFrame(const KeyFrame &KF, const Camera *C = NULL); 82 | void ConvertFeatureMeasurements(const std::vector &zs, FRM::Frame *F); 83 | #ifdef CFG_GROUND_TRUTH 84 | void PushDepthMeasurementsGT(const FRM::Frame &F); 85 | #endif 86 | 87 | bool SavePoints(const AlignedVector &CsKF, 88 | const std::vector<::Depth::InverseGaussian> &ds, 89 | const std::string fileName, const bool append = true); 90 | 91 | void AssertConsistency(); 92 | 93 | protected: 94 | 95 | friend Solver; 96 | friend LocalBundleAdjustor; 97 | friend GlobalBundleAdjustor; 98 | friend ViewerIBA; 99 | 100 | LocalMap m_LM; 101 | GlobalMap m_GM; 102 | LocalBundleAdjustor m_LBA; 103 | GlobalBundleAdjustor m_GBA; 104 | int m_debug; 105 | int m_nFrms; 106 | std::vector m_Ts; 107 | std::vector m_iKF2d, m_id2X, m_iX2d, m_id2idx, m_idx2iX; 108 | std::vector<::Point2D> m_xs; 109 | std::list m_CsLF; 110 | std::vector m_CsKF; 111 | std::vector<::Depth::InverseGaussian> m_ds; 112 | std::vector m_uds; 113 | #ifdef CFG_GROUND_TRUTH 114 | AlignedVector m_usGT; 115 | std::vector m_iusGT; 116 | std::vector m_tsGT; 117 | std::vector<::Depth::InverseGaussian> m_dsGT; 118 | AlignedVector m_RsGT; 119 | AlignedVector m_TsGT; 120 | std::vector > m_zsGT; 121 | #endif 122 | 123 | Camera::Calibration m_K; 124 | ::Intrinsic::UndistortionMap m_UM; 125 | #ifdef CFG_STEREO 126 | ::Intrinsic::UndistortionMap m_UMr; 127 | #endif 128 | AlignedVector m_CsGT; 129 | std::vector<::Depth::InverseGaussian> m_DsGT; 130 | std::string m_dir; 131 | 132 | std::vector m_zsSortTmp; 133 | std::vector m_zsTmp; 134 | std::vector m_idxsSortTmp; 135 | std::vector m_idxsTmp; 136 | 137 | LocalBundleAdjustor::InputLocalFrame m_ILF; 138 | GlobalMap::InputKeyFrame m_IKF; 139 | 140 | std::vector m_ICs; 141 | 142 | IMU::Delta m_D; 143 | AlignedVector m_us; 144 | AlignedVector m_work; 145 | }; 146 | 147 | } 148 | 149 | #endif 150 | -------------------------------------------------------------------------------- /Backend/IBA/stdafx.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | // stdafx.cpp : source file that includes just the standard includes 17 | // IBA.pch will be the pre-compiled header 18 | // stdafx.obj will contain the pre-compiled type information 19 | 20 | #include "stdafx.h" 21 | 22 | // TODO: reference any additional headers you need in STDAFX.H 23 | // and not in this file -------------------------------------------------------------------------------- /Backend/IBA/stdafx.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | // stdafx.h : include file for standard system include files, 17 | // or project specific include files that are used frequently, but 18 | // are changed infrequently 19 | // 20 | 21 | #pragma once 22 | 23 | #include "targetver.h" 24 | 25 | #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers 26 | 27 | 28 | 29 | // TODO: reference additional headers your program requires here 30 | #include "IBA_config.h" 31 | -------------------------------------------------------------------------------- /Backend/IBA/targetver.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #pragma once 17 | 18 | // Including SDKDDKVer.h defines the highest available Windows platform. 19 | 20 | // If you wish to build your application for a previous Windows platform, include WinSDKVer.h and 21 | // set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h. 22 | 23 | #ifdef WIN32 24 | #include 25 | #endif 26 | -------------------------------------------------------------------------------- /Backend/LinearAlgebra/Matrix2x4.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _MATRIX_2x4_H_ 17 | #define _MATRIX_2x4_H_ 18 | 19 | #include "Utility.h" 20 | 21 | namespace LA { 22 | 23 | class AlignedMatrix2x4f { 24 | 25 | public: 26 | 27 | inline const xp128f& m_00_01_02_03() const { return m_data[0]; } inline xp128f& m_00_01_02_03() { return m_data[0]; } 28 | inline const xp128f& m_10_11_12_13() const { return m_data[1]; } inline xp128f& m_10_11_12_13() { return m_data[1]; } 29 | inline const float& m00() const { return m_data[0][0]; } inline float& m00() { return m_data[0][0]; } 30 | inline const float& m01() const { return m_data[0][1]; } inline float& m01() { return m_data[0][1]; } 31 | inline const float& m02() const { return m_data[0][2]; } inline float& m02() { return m_data[0][2]; } 32 | inline const float& m03() const { return m_data[0][3]; } inline float& m03() { return m_data[0][3]; } 33 | inline const float& m10() const { return m_data[1][0]; } inline float& m10() { return m_data[1][0]; } 34 | inline const float& m11() const { return m_data[1][1]; } inline float& m11() { return m_data[1][1]; } 35 | inline const float& m12() const { return m_data[1][2]; } inline float& m12() { return m_data[1][2]; } 36 | inline const float& m13() const { return m_data[1][3]; } inline float& m13() { return m_data[1][3]; } 37 | 38 | inline operator const float* () const { return (const float *) this; } 39 | inline operator float* () { return ( float *) this; } 40 | 41 | inline const float& operator() (int row, int col) const { 42 | return data[row * 4 + col]; 43 | } 44 | 45 | inline float& operator() (int row, int col) { 46 | return data[row * 4 + col]; 47 | } 48 | inline void operator += (const AlignedMatrix2x4f &M) { 49 | m_data[0] += M.m_data[0]; 50 | m_data[1] += M.m_data[1]; 51 | } 52 | inline void operator -= (const AlignedMatrix2x4f &M) { 53 | m_data[0] -= M.m_data[0]; 54 | m_data[1] -= M.m_data[1]; 55 | } 56 | inline void operator *= (const xp128f &s) { 57 | m_data[0] *= s; 58 | m_data[1] *= s; 59 | } 60 | inline AlignedMatrix2x4f operator - (const AlignedMatrix2x4f &B) const { 61 | AlignedMatrix2x4f AmB; 62 | AmB.m_00_01_02_03() = m_00_01_02_03() - B.m_00_01_02_03(); 63 | AmB.m_10_11_12_13() = m_10_11_12_13() - B.m_10_11_12_13(); 64 | return AmB; 65 | } 66 | 67 | inline void Set(const float m00, const float m01, const float m02, const float m03, 68 | const float m10, const float m11, const float m12, const float m13) { 69 | m_data[0].vset_all_lane(m00, m01, m02, m03); 70 | m_data[1].vset_all_lane(m10, m11, m12, m13); 71 | } 72 | inline void Set(const float *M) { memcpy(&m00(), M, sizeof(AlignedMatrix2x4f)); } 73 | inline void Get(float *M) const { memcpy(M, &m00(), sizeof(AlignedMatrix2x4f)); } 74 | 75 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix2x4f)); } 76 | 77 | inline void Print(const bool e = false) const { 78 | if (e) { 79 | UT::Print("%e %e %e %e\n", m00(), m01(), m02(), m03()); 80 | UT::Print("%e %e %e %e\n", m10(), m11(), m12(), m13()); 81 | } else { 82 | UT::Print("%f %f %f %f\n", m00(), m01(), m02(), m03()); 83 | UT::Print("%f %f %f %f\n", m10(), m11(), m12(), m13()); 84 | } 85 | } 86 | inline bool AssertEqual(const AlignedMatrix2x4f &M, 87 | const int verbose = 1, const std::string str = "", 88 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 89 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 8, verbose, str, epsAbs, epsRel)) { 90 | return true; 91 | } else if (verbose) { 92 | UT::PrintSeparator(); 93 | Print(verbose > 1); 94 | UT::PrintSeparator(); 95 | M.Print(verbose > 1); 96 | const AlignedMatrix2x4f E = *this - M; 97 | UT::PrintSeparator(); 98 | E.Print(verbose > 1); 99 | } 100 | return false; 101 | } 102 | inline bool AssertZero(const int verbose = 1, const std::string str = "", 103 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 104 | if (UT::VectorAssertZero(&m00(), 8, verbose, str, epsAbs, epsRel)) { 105 | return true; 106 | } else if (verbose) { 107 | UT::PrintSeparator(); 108 | Print(verbose > 1); 109 | } 110 | return false; 111 | } 112 | 113 | protected: 114 | union { 115 | xp128f m_data[2]; 116 | float data[8]; 117 | }; 118 | }; 119 | } 120 | 121 | #ifdef CFG_DEBUG_EIGEN 122 | #include 123 | class EigenMatrix2x4f : public Eigen::Matrix { 124 | public: 125 | inline EigenMatrix2x4f() : Eigen::Matrix() {} 126 | inline EigenMatrix2x4f(const Eigen::Matrix &e_M) : Eigen::Matrix(e_M) {} 127 | inline EigenMatrix2x4f(const LA::AlignedMatrix2x4f &M) : Eigen::Matrix() { 128 | Eigen::Matrix &e_M = *this; 129 | e_M(0, 0) = M.m00(); e_M(0, 1) = M.m01(); e_M(0, 2) = M.m02(); e_M(0, 3) = M.m03(); 130 | e_M(1, 0) = M.m10(); e_M(1, 1) = M.m11(); e_M(0, 2) = M.m02(); e_M(0, 3) = M.m03(); 131 | } 132 | inline void operator = (const Eigen::Matrix &e_M) { *((Eigen::Matrix *) this) = e_M; } 133 | inline LA::AlignedMatrix2x4f GetAlignedMatrix2x4f() const { 134 | LA::AlignedMatrix2x4f M; 135 | const Eigen::Matrix &e_M = *this; 136 | M.m00() = e_M(0, 0); M.m01() = e_M(0, 1); M.m02() = e_M(0, 2); M.m03() = e_M(0, 3); 137 | M.m10() = e_M(1, 0); M.m11() = e_M(1, 1); M.m12() = e_M(1, 2); M.m13() = e_M(1, 3); 138 | return M; 139 | } 140 | inline void Print(const bool e = false) const { GetAlignedMatrix2x4f().Print(e); } 141 | inline bool AssertEqual(const LA::AlignedMatrix2x4f &M, 142 | const int verbose = 1, const std::string str = "", 143 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 144 | return GetAlignedMatrix2x4f().AssertEqual(M, verbose, str, epsAbs, epsRel); 145 | } 146 | inline bool AssertEqual(const EigenMatrix2x4f &e_M, 147 | const int verbose = 1, const std::string str = "", 148 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 149 | return AssertEqual(e_M.GetAlignedMatrix2x4f(), verbose, str, epsAbs, epsRel); 150 | } 151 | }; 152 | #endif 153 | #endif 154 | -------------------------------------------------------------------------------- /Backend/LinearAlgebra/Matrix2x8.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _MATRIX_2x8_H_ 17 | #define _MATRIX_2x8_H_ 18 | 19 | #include "Vector8.h" 20 | #include "Matrix2x7.h" 21 | 22 | namespace LA { 23 | 24 | class AlignedMatrix2x8f { 25 | 26 | public: 27 | 28 | inline const xp128f& m_00_01_02_03() const { return m_data[0]; } inline xp128f& m_00_01_02_03() { return m_data[0]; } 29 | inline const xp128f& m_04_05_06_07() const { return m_data[1]; } inline xp128f& m_04_05_06_07() { return m_data[1]; } 30 | inline const xp128f& m_10_11_12_13() const { return m_data[2]; } inline xp128f& m_10_11_12_13() { return m_data[2]; } 31 | inline const xp128f& m_14_15_16_17() const { return m_data[3]; } inline xp128f& m_14_15_16_17() { return m_data[3]; } 32 | 33 | inline const float& m00() const { return m_data[0][0]; } inline float& m00() { return m_data[0][0]; } 34 | inline const float& m01() const { return m_data[0][1]; } inline float& m01() { return m_data[0][1]; } 35 | inline const float& m02() const { return m_data[0][2]; } inline float& m02() { return m_data[0][2]; } 36 | inline const float& m03() const { return m_data[0][3]; } inline float& m03() { return m_data[0][3]; } 37 | inline const float& m04() const { return m_data[1][0]; } inline float& m04() { return m_data[1][0]; } 38 | inline const float& m05() const { return m_data[1][1]; } inline float& m05() { return m_data[1][1]; } 39 | inline const float& m06() const { return m_data[1][2]; } inline float& m06() { return m_data[1][2]; } 40 | inline const float& m07() const { return m_data[1][3]; } inline float& m07() { return m_data[1][3]; } 41 | inline const float& m10() const { return m_data[2][0]; } inline float& m10() { return m_data[2][0]; } 42 | inline const float& m11() const { return m_data[2][1]; } inline float& m11() { return m_data[2][1]; } 43 | inline const float& m12() const { return m_data[2][2]; } inline float& m12() { return m_data[2][2]; } 44 | inline const float& m13() const { return m_data[2][3]; } inline float& m13() { return m_data[2][3]; } 45 | inline const float& m14() const { return m_data[3][0]; } inline float& m14() { return m_data[3][0]; } 46 | inline const float& m15() const { return m_data[3][1]; } inline float& m15() { return m_data[3][1]; } 47 | inline const float& m16() const { return m_data[3][2]; } inline float& m16() { return m_data[3][2]; } 48 | inline const float& m17() const { return m_data[3][3]; } inline float& m17() { return m_data[3][3]; } 49 | 50 | inline operator const float* () const { return (const float *) this; } 51 | inline operator float* () { return ( float *) this; } 52 | inline void operator += (const AlignedMatrix2x8f &M) { 53 | m_00_01_02_03() += M.m_00_01_02_03(); 54 | m_04_05_06_07() += M.m_04_05_06_07(); 55 | m_10_11_12_13() += M.m_10_11_12_13(); 56 | m_14_15_16_17() += M.m_14_15_16_17(); 57 | } 58 | inline void operator -= (const AlignedMatrix2x8f &M) { 59 | m_00_01_02_03() -= M.m_00_01_02_03(); 60 | m_04_05_06_07() -= M.m_04_05_06_07(); 61 | m_10_11_12_13() -= M.m_10_11_12_13(); 62 | m_14_15_16_17() -= M.m_14_15_16_17(); 63 | } 64 | inline void operator *= (const xp128f &s) { 65 | m_00_01_02_03() *= s; 66 | m_04_05_06_07() *= s; 67 | m_10_11_12_13() *= s; 68 | m_14_15_16_17() *= s; 69 | } 70 | inline AlignedMatrix2x8f operator - (const AlignedMatrix2x8f &B) const { 71 | AlignedMatrix2x8f AmB; 72 | AmB.m_00_01_02_03() = m_00_01_02_03() - B.m_00_01_02_03(); 73 | AmB.m_04_05_06_07() = m_04_05_06_07() - B.m_04_05_06_07(); 74 | AmB.m_10_11_12_13() = m_10_11_12_13() - B.m_10_11_12_13(); 75 | AmB.m_14_15_16_17() = m_14_15_16_17() - B.m_14_15_16_17(); 76 | return AmB; 77 | } 78 | 79 | inline void Set(const AlignedMatrix2x7f &M0, const Vector2f &M1) { 80 | memcpy(this, &M0, sizeof(AlignedMatrix2x7f)); 81 | m07() = M1.v0(); 82 | m17() = M1.v1(); 83 | } 84 | 85 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix2x8f)); } 86 | 87 | inline void Print(const bool e = false) const { 88 | if (e) { 89 | UT::Print("%e %e %e %e %e %e %e %e\n", m00(), m01(), m02(), m03(), m04(), m05(), m06(), m07()); 90 | UT::Print("%e %e %e %e %e %e %e %e\n", m10(), m11(), m12(), m13(), m14(), m15(), m16(), m17()); 91 | } else { 92 | UT::Print("%f %f %f %f %f %f %f %f\n", m00(), m01(), m02(), m03(), m04(), m05(), m06(), m07()); 93 | UT::Print("%f %f %f %f %f %f %f %f\n", m10(), m11(), m12(), m13(), m14(), m15(), m16(), m17()); 94 | } 95 | } 96 | inline bool AssertEqual(const AlignedMatrix2x8f &M, 97 | const int verbose = 1, const std::string str = "", 98 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 99 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 16, verbose, str, epsAbs, epsRel)) { 100 | return true; 101 | } else if (verbose) { 102 | UT::PrintSeparator(); 103 | Print(verbose > 1); 104 | UT::PrintSeparator(); 105 | M.Print(verbose > 1); 106 | const AlignedMatrix2x8f E = *this - M; 107 | UT::PrintSeparator(); 108 | E.Print(verbose > 1); 109 | } 110 | return false; 111 | } 112 | inline bool AssertZero(const int verbose = 1, const std::string str = "", 113 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 114 | if (UT::VectorAssertZero(&m00(), 16, verbose, str, epsAbs, epsRel)) { 115 | return true; 116 | } else if (verbose) { 117 | UT::PrintSeparator(); 118 | Print(verbose > 1); 119 | } 120 | return false; 121 | } 122 | 123 | protected: 124 | 125 | xp128f m_data[4]; 126 | }; 127 | } 128 | 129 | #ifdef CFG_DEBUG_EIGEN 130 | class EigenMatrix2x8f : public Eigen::Matrix { 131 | public: 132 | inline EigenMatrix2x8f() : Eigen::Matrix() {} 133 | inline EigenMatrix2x8f(const Eigen::Matrix &e_M) : Eigen::Matrix(e_M) {} 134 | inline EigenMatrix2x8f(const LA::AlignedMatrix2x8f &M) : Eigen::Matrix() { 135 | const float* _M[2] = {&M.m00(), &M.m10()}; 136 | Eigen::Matrix &e_M = *this; 137 | for (int i = 0; i < 2; ++i) 138 | for (int j = 0; j < 8; ++j) 139 | e_M(i, j) = _M[i][j]; 140 | } 141 | inline void operator = (const Eigen::Matrix &e_M) { *((Eigen::Matrix *) this) = e_M; } 142 | inline LA::AlignedMatrix2x8f GetAlignedMatrix2x8f() const { 143 | LA::AlignedMatrix2x8f M; 144 | float* _M[2] = {&M.m00(), &M.m10()}; 145 | const Eigen::Matrix &e_M = *this; 146 | for (int i = 0; i < 2; ++i) 147 | for (int j = 0; j < 8; ++j) 148 | _M[i][j] = e_M(i, j); 149 | return M; 150 | } 151 | inline void Print(const bool e = false) const { GetAlignedMatrix2x8f().Print(e); } 152 | inline bool AssertEqual(const LA::AlignedMatrix2x8f &M, 153 | const int verbose = 1, const std::string str = "", 154 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 155 | return GetAlignedMatrix2x8f().AssertEqual(M, verbose, str, epsAbs, epsRel); 156 | } 157 | inline bool AssertEqual(const EigenMatrix2x8f &e_M, 158 | const int verbose = 1, const std::string str = "", 159 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 160 | return AssertEqual(e_M.GetAlignedMatrix2x8f(), verbose, str, epsAbs, epsRel); 161 | } 162 | }; 163 | #endif 164 | #endif 165 | -------------------------------------------------------------------------------- /Backend/LinearAlgebra/Matrix3x2.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _MATRIX_3x2_H_ 3 | #define _MATRIX_3x2_H_ 4 | 5 | namespace LA { 6 | 7 | template class Matrix3x2 { 8 | public: 9 | inline const TYPE* operator [] (const int i) const { return m_data[i]; } 10 | inline TYPE* operator [] (const int i) { return m_data[i]; } 11 | public: 12 | TYPE m_data[3][2]; 13 | }; 14 | 15 | typedef Matrix3x2 Matrix3x2f; 16 | typedef Matrix3x2 Matrix3x2d; 17 | 18 | } 19 | 20 | #endif -------------------------------------------------------------------------------- /Backend/LinearAlgebra/Vector12.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _VECTOR_12_H_ 17 | #define _VECTOR_12_H_ 18 | 19 | #include "Vector6.h" 20 | #include "Utility.h" 21 | 22 | namespace LA { 23 | 24 | class AlignedVector12f { 25 | 26 | public: 27 | 28 | inline const xp128f &v0123() const { return m_data4[0]; } inline xp128f &v0123() { return m_data4[0]; } 29 | inline const xp128f &v4567() const { return m_data4[1]; } inline xp128f &v4567() { return m_data4[1]; } 30 | inline const xp128f &v_8_9_10_11() const { return m_data4[2]; } 31 | inline xp128f &v_8_9_10_11() { return m_data4[2]; } 32 | inline const float& v0() const { return m_data[0]; } inline float& v0() { return m_data[0]; } 33 | inline const float& v1() const { return m_data[1]; } inline float& v1() { return m_data[1]; } 34 | inline const float& v2() const { return m_data[2]; } inline float& v2() { return m_data[2]; } 35 | inline const float& v3() const { return m_data[3]; } inline float& v3() { return m_data[3]; } 36 | inline const float& v4() const { return m_data[4]; } inline float& v4() { return m_data[4]; } 37 | inline const float& v5() const { return m_data[5]; } inline float& v5() { return m_data[5]; } 38 | inline const float& v6() const { return m_data[6]; } inline float& v6() { return m_data[6]; } 39 | inline const float& v7() const { return m_data[7]; } inline float& v7() { return m_data[7]; } 40 | inline const float& v8() const { return m_data[8]; } inline float& v8() { return m_data[8]; } 41 | inline const float& v9() const { return m_data[9]; } inline float& v9() { return m_data[9]; } 42 | inline const float& v10() const { return m_data[10]; } inline float& v10() { return m_data[10]; } 43 | inline const float& v11() const { return m_data[11]; } inline float& v11() { return m_data[11]; } 44 | 45 | inline operator const float* () const { return (const float *) this; } 46 | inline operator float* () { return ( float *) this; } 47 | inline const float& operator() (const int row, const int col = 0) const { 48 | return m_data[row]; 49 | } 50 | inline float& operator() (const int row, const int col = 0) { 51 | return m_data[row]; 52 | } 53 | 54 | inline void operator += (const AlignedVector12f &v) { 55 | m_data4[0] += v.m_data4[0]; 56 | m_data4[1] += v.m_data4[1]; 57 | m_data4[2] += v.m_data4[2]; 58 | } 59 | 60 | inline void Set(const float *v) { memcpy(this, v, 48); } 61 | inline void Set(const AlignedVector3f &v0, const AlignedVector3f &v1, 62 | const AlignedVector3f &v2, const AlignedVector3f &v3) { 63 | memcpy(&this->v0(), v0, 12); 64 | memcpy(&this->v3(), v1, 12); 65 | memcpy(&this->v6(), v2, 12); 66 | memcpy(&this->v9(), v3, 12); 67 | } 68 | inline void Set(const Vector6f &v0, const Vector6f &v1) { 69 | memcpy(&this->v0(), v0, 24); 70 | memcpy(&this->v6(), v1, 24); 71 | } 72 | inline void Get(float *v) const { memcpy(v, this, 48); } 73 | inline void Get(ProductVector6f &v0, ProductVector6f &v1) const { 74 | v0.Set(&this->v0()); 75 | v1.Set(&this->v6()); 76 | } 77 | inline void GetMinus(AlignedVector12f &v) const { 78 | const xp128f zero = xp128f::get(0.0f); 79 | v.m_data4[0] = zero - m_data4[0]; 80 | v.m_data4[1] = zero - m_data4[1]; 81 | v.m_data4[2] = zero - m_data4[2]; 82 | v.m_data4[3] = zero - m_data4[3]; 83 | } 84 | 85 | inline void MakeZero() { memset(this, 0, sizeof(AlignedVector12f)); } 86 | 87 | inline float SquaredLength() const { 88 | return (m_data4[0] * m_data4[0] + m_data4[1] * m_data4[1] + 89 | m_data4[2] * m_data4[2]).vsum_all(); 90 | } 91 | 92 | protected: 93 | union { 94 | xp128f m_data4[3]; 95 | float m_data[12]; 96 | }; 97 | }; 98 | 99 | } // namespace LA 100 | #endif 101 | -------------------------------------------------------------------------------- /Backend/LinearAlgebra/Vector7.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _VECTOR_7_H_ 17 | #define _VECTOR_7_H_ 18 | 19 | #include "Vector6.h" 20 | #include "Utility.h" 21 | 22 | namespace LA { 23 | 24 | class AlignedVector7f { 25 | public: 26 | inline const xp128f &v0123() const { return m_data4[0]; } inline xp128f &v0123() { return m_data4[0]; } 27 | inline const xp128f &v456x() const { return m_data4[1]; } inline xp128f &v456x() { return m_data4[1]; } 28 | inline const float& v0() const { return m_data[0]; } inline float& v0() { return m_data[0]; } 29 | inline const float& v1() const { return m_data[1]; } inline float& v1() { return m_data[1]; } 30 | inline const float& v2() const { return m_data[2]; } inline float& v2() { return m_data[2]; } 31 | inline const float& v3() const { return m_data[3]; } inline float& v3() { return m_data[3]; } 32 | inline const float& v4() const { return m_data[4]; } inline float& v4() { return m_data[4]; } 33 | inline const float& v5() const { return m_data[5]; } inline float& v5() { return m_data[5]; } 34 | inline const float& v6() const { return m_data[6]; } inline float& v6() { return m_data[6]; } 35 | 36 | 37 | inline operator const float* () const { return (const float *) this; } 38 | inline operator float* () { return ( float *) this; } 39 | 40 | inline const float& operator() (const int row, const int col = 0) const { 41 | return m_data[row]; 42 | } 43 | 44 | inline float& operator() (const int row, const int col = 0) { 45 | return m_data[row]; 46 | } 47 | 48 | inline void operator += (const AlignedVector7f &v) { 49 | v0123() += v.v0123(); 50 | v456x() += v.v456x(); 51 | } 52 | inline void operator -= (const AlignedVector7f &v) { 53 | v0123() -= v.v0123(); 54 | v456x() -= v.v456x(); 55 | } 56 | inline void operator *= (const float s) { Scale(s); } 57 | inline void operator *= (const xp128f &s) { Scale(s); } 58 | inline AlignedVector7f operator - (const AlignedVector7f &b) const { 59 | AlignedVector7f amb; 60 | amb.v0123() = v0123() - b.v0123(); 61 | amb.v456x() = v456x() - b.v456x(); 62 | return amb; 63 | } 64 | inline AlignedVector7f operator * (const xp128f &s) const { 65 | AlignedVector7f v; 66 | GetScaled(s, v); 67 | return v; 68 | } 69 | 70 | inline void Set(const AlignedVector6f &v) { memcpy(this, &v, 24); } 71 | inline void Set(const float *v) { memcpy(this, v, sizeof(AlignedVector7f)); } 72 | inline void Get(float *v) const { memcpy(v, this, sizeof(AlignedVector7f)); } 73 | 74 | inline void MakeZero() { memset(this, 0, sizeof(AlignedVector7f)); } 75 | inline void MakeMinus() { 76 | v0123().vmake_minus(); 77 | v456x().vmake_minus(); 78 | } 79 | 80 | inline void Scale(const float s) { 81 | xp128f _s; _s.vdup_all_lane(s); 82 | Scale(_s); 83 | } 84 | inline void Scale(const xp128f &s) { 85 | v0123() *= s; 86 | v456x() *= s; 87 | } 88 | inline void GetScaled(const xp128f &s, AlignedVector7f &v) const { 89 | v.v0123() = s * v0123(); 90 | v.v456x() = s * v456x(); 91 | } 92 | 93 | inline float SquaredLength() const { 94 | #ifdef CFG_DEBUG 95 | UT_ASSERT(m_data4[1][3] == 0.0f); 96 | #endif 97 | // Make sure m_data4[1][3] is always 0 98 | return (v0123() * v0123() + v456x() * v456x()).vsum_all(); 99 | } 100 | inline float Dot(const AlignedVector7f &v) const { 101 | #ifdef CFG_DEBUG 102 | UT_ASSERT(m_data4[1][3] == 0.0f || v.m_data4[1][3] == 0.0f); 103 | #endif 104 | // Make sure m_data4[1][3] is always 0 105 | return (v0123() * v.v0123() + v456x() * v.v456x()).vsum_all(); 106 | } 107 | 108 | inline void Print(const bool e = false) const { 109 | if (e) { 110 | UT::Print("%e %e %e %e %e %e %e\n", v0(), v1(), v2(), v3(), v4(), v5(), v6()); 111 | } else { 112 | UT::Print("%f %f %f %f %f %f %f\n", v0(), v1(), v2(), v3(), v4(), v5(), v6()); 113 | } 114 | } 115 | 116 | static inline void apb(const AlignedVector7f &a, const AlignedVector7f &b, AlignedVector7f &apb) { 117 | apb.v0123() = a.v0123() + b.v0123(); 118 | apb.v456x() = a.v456x() + b.v456x(); 119 | } 120 | 121 | inline void Random(const float vMax) { UT::Random(&v0(), 7, -vMax, vMax); } 122 | static inline AlignedVector7f GetRandom(const float vMax) { AlignedVector7f v; v.Random(vMax); return v; } 123 | 124 | inline bool AssertEqual(const AlignedVector7f &v, 125 | const int verbose = 1, const std::string str = "", 126 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 127 | if (UT::VectorAssertEqual(&v0(), &v.v0(), 7, verbose, str, epsAbs, epsRel)) { 128 | return true; 129 | } else if (verbose) { 130 | UT::PrintSeparator(); 131 | Print(verbose > 1); 132 | v.Print(verbose > 1); 133 | const AlignedVector7f e = *this - v; 134 | e.Print(verbose > 1); 135 | } 136 | return false; 137 | } 138 | inline bool AssertZero(const int verbose = 1, const std::string str = "", 139 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 140 | if (UT::VectorAssertZero(&v0(), 7, verbose, str, epsAbs, epsRel)) { 141 | return true; 142 | } else if (verbose) { 143 | UT::PrintSeparator(); 144 | Print(verbose > 1); 145 | } 146 | return false; 147 | } 148 | 149 | protected: 150 | union { 151 | float m_data[7]; 152 | xp128f m_data4[2]; 153 | }; 154 | }; 155 | } 156 | 157 | #ifdef CFG_DEBUG_EIGEN 158 | #include 159 | class EigenVector7f : public Eigen::Matrix { 160 | public: 161 | inline EigenVector7f() : Eigen::Matrix() {} 162 | inline EigenVector7f(const Eigen::Matrix &e_v) : Eigen::Matrix(e_v) {} 163 | inline EigenVector7f(const LA::AlignedVector7f &v) : Eigen::Matrix() { 164 | const float* _v = v; 165 | Eigen::Matrix &e_v = *this; 166 | for (int i = 0; i < 7; ++i) 167 | e_v(i, 0) = _v[i]; 168 | } 169 | inline void operator = (const Eigen::Matrix &e_v) { 170 | *((Eigen::Matrix *) this) = e_v; 171 | } 172 | inline LA::AlignedVector7f GetAlignedVector7f() const { 173 | LA::AlignedVector7f v; 174 | float* _v = v; 175 | const Eigen::Matrix &e_v = *this; 176 | for (int i = 0; i < 7; ++i) 177 | _v[i] = e_v(i, 0); 178 | return v; 179 | } 180 | inline float SquaredLength() const { return GetAlignedVector7f().SquaredLength(); } 181 | inline void Print(const bool e = false) const { GetAlignedVector7f().Print(e); } 182 | static inline EigenVector7f GetRandom(const float vMax) { 183 | return EigenVector7f(LA::AlignedVector7f::GetRandom(vMax)); 184 | } 185 | inline bool AssertEqual(const LA::AlignedVector7f &v, 186 | const int verbose = 1, const std::string str = "", 187 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 188 | return GetAlignedVector7f().AssertEqual(v, verbose, str, epsAbs, epsRel); 189 | } 190 | inline bool AssertEqual(const EigenVector7f &e_v, 191 | const int verbose = 1, const std::string str = "", 192 | const float epsAbs = 0.0f, const float epsRel = 0.0f) const { 193 | return AssertEqual(e_v.GetAlignedVector7f(), verbose, str, epsAbs, epsRel); 194 | } 195 | }; 196 | #endif 197 | #endif 198 | -------------------------------------------------------------------------------- /Backend/Map/GlobalMap.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #include "stdafx.h" 17 | //#ifndef CFG_DEBUG 18 | //#define CFG_DEBUG 19 | //#endif 20 | #include "GlobalMap.h" 21 | 22 | void GlobalMap::LBA_Reset() { 23 | MT_WRITE_LOCK_BEGIN(m_MT, MT_TASK_NONE, MT_TASK_GM_LBA_Reset); 24 | m_Cs.resize(0); 25 | m_Uc = GM_FLAG_FRAME_DEFAULT; 26 | MT_WRITE_LOCK_END(m_MT, MT_TASK_NONE, MT_TASK_GM_LBA_Reset); 27 | } 28 | 29 | void GlobalMap::LBA_PushKeyFrame(const Camera &C) { 30 | MT_WRITE_LOCK_BEGIN(m_MT, C.m_iFrm, MT_TASK_GM_LBA_PushKeyFrame); 31 | m_Cs.push_back(C); 32 | MT_WRITE_LOCK_END(m_MT, C.m_iFrm, MT_TASK_GM_LBA_PushKeyFrame); 33 | } 34 | 35 | void GlobalMap::LBA_DeleteKeyFrame(const int iFrm, const int iKF) { 36 | MT_WRITE_LOCK_BEGIN(m_MT, iFrm, MT_TASK_GM_LBA_DeleteKeyFrame); 37 | const ubyte uc = m_Cs[iKF].m_uc; 38 | m_Cs.erase(m_Cs.begin() + iKF); 39 | if (uc & GM_FLAG_FRAME_UPDATE_CAMERA) { 40 | bool found = false; 41 | const int N = static_cast(m_Cs.size()); 42 | for (int i = 0; i < N && !found; ++i) { 43 | found = (m_Cs[i].m_uc & GM_FLAG_FRAME_UPDATE_CAMERA) != 0; 44 | } 45 | if (!found) { 46 | m_Uc &= ~GM_FLAG_FRAME_UPDATE_CAMERA; 47 | } 48 | } 49 | if (uc & GM_FLAG_FRAME_UPDATE_DEPTH) { 50 | bool found = false; 51 | const int N = static_cast(m_Cs.size()); 52 | for (int i = 0; i < N && !found; ++i) { 53 | found = (m_Cs[i].m_uc & GM_FLAG_FRAME_UPDATE_DEPTH) != 0; 54 | } 55 | if (!found) { 56 | m_Uc &= ~GM_FLAG_FRAME_UPDATE_DEPTH; 57 | } 58 | } 59 | MT_WRITE_LOCK_END(m_MT, iFrm, MT_TASK_GM_LBA_DeleteKeyFrame); 60 | } 61 | 62 | ubyte GlobalMap::LBA_Synchronize(const int iFrm, AlignedVector &Cs, 63 | AlignedVector &CsBkp, std::vector &ucs 64 | #ifdef CFG_HANDLE_SCALE_JUMP 65 | , std::vector &ds, std::vector &dsBkp 66 | #endif 67 | ) { 68 | ubyte ret; 69 | MT_WRITE_LOCK_BEGIN(m_MT, iFrm, MT_TASK_GM_LBA_Synchronize); 70 | if (m_Uc == GM_FLAG_FRAME_DEFAULT) { 71 | ret = GM_FLAG_FRAME_DEFAULT; 72 | } else { 73 | m_Uc = GM_FLAG_FRAME_DEFAULT; 74 | const int N = static_cast(m_Cs.size()); 75 | #ifdef CFG_DEBUG 76 | UT_ASSERT(Cs.Size() == N); 77 | #endif 78 | CsBkp.Resize(N); 79 | ucs.assign(N, GM_FLAG_FRAME_DEFAULT); 80 | #ifdef CFG_HANDLE_SCALE_JUMP 81 | dsBkp.resize(N); 82 | #endif 83 | for (int i = 0; i < N; ++i) { 84 | Camera &C = m_Cs[i]; 85 | if (C.m_uc == GM_FLAG_FRAME_DEFAULT) { 86 | continue; 87 | } 88 | if (C.m_uc & GM_FLAG_FRAME_UPDATE_CAMERA) { 89 | CsBkp[i] = Cs[i]; 90 | Cs[i] = C.m_C; 91 | } 92 | ucs[i] = C.m_uc; 93 | #ifdef CFG_HANDLE_SCALE_JUMP 94 | if (C.m_uc & GM_FLAG_FRAME_UPDATE_DEPTH) { 95 | dsBkp[i] = ds[i]; 96 | ds[i] = C.m_d; 97 | } 98 | #endif 99 | C.m_uc = GM_FLAG_FRAME_DEFAULT; 100 | } 101 | ret = GM_FLAG_FRAME_UPDATE_CAMERA; 102 | } 103 | MT_WRITE_LOCK_END(m_MT, iFrm, MT_TASK_GM_LBA_Synchronize); 104 | return ret; 105 | } 106 | 107 | void GlobalMap::GBA_Update(const std::vector &iFrms, const AlignedVector &Cs, 108 | const std::vector &ucs 109 | #ifdef CFG_HANDLE_SCALE_JUMP 110 | , const std::vector &ds 111 | #endif 112 | ) { 113 | MT_WRITE_LOCK_BEGIN(m_MT, iFrms.back(), MT_TASK_GM_GBA_Update); 114 | std::vector::iterator i = m_Cs.begin(); 115 | const int N = static_cast(iFrms.size()); 116 | for (int j = 0; j < N; ++j) { 117 | const ubyte uc = ucs[j]; 118 | if (uc == GM_FLAG_FRAME_DEFAULT) { 119 | continue; 120 | } 121 | const int iFrm = iFrms[j]; 122 | i = std::lower_bound(i, m_Cs.end(), iFrm); 123 | if (i == m_Cs.end()) { 124 | break; 125 | } else if (i->m_iFrm != iFrm) { 126 | continue; 127 | } 128 | if (uc & GM_FLAG_FRAME_UPDATE_CAMERA) { 129 | i->m_C = Cs[j]; 130 | i->m_uc |= GM_FLAG_FRAME_UPDATE_CAMERA; 131 | m_Uc |= GM_FLAG_FRAME_UPDATE_CAMERA; 132 | } 133 | #ifdef CFG_HANDLE_SCALE_JUMP 134 | if (uc & GM_FLAG_FRAME_UPDATE_DEPTH) { 135 | i->m_d = ds[j]; 136 | i->m_uc |= GM_FLAG_FRAME_UPDATE_DEPTH; 137 | m_Uc |= GM_FLAG_FRAME_UPDATE_DEPTH; 138 | } 139 | #endif 140 | } 141 | MT_WRITE_LOCK_END(m_MT, iFrms.back(), MT_TASK_GM_GBA_Update); 142 | } 143 | 144 | void GlobalMap::SaveB(FILE *fp) { 145 | MT_READ_LOCK_BEGIN(m_MT, MT_TASK_NONE, MT_TASK_NONE); 146 | UT::VectorSaveB(m_Cs, fp); 147 | UT::SaveB(m_Uc, fp); 148 | MT_READ_LOCK_END(m_MT, MT_TASK_NONE, MT_TASK_NONE); 149 | } 150 | 151 | void GlobalMap::LoadB(FILE *fp) { 152 | MT_WRITE_LOCK_BEGIN(m_MT, MT_TASK_NONE, MT_TASK_NONE); 153 | UT::VectorLoadB(m_Cs, fp); 154 | UT::LoadB(m_Uc, fp); 155 | MT_WRITE_LOCK_END(m_MT, MT_TASK_NONE, MT_TASK_NONE); 156 | } 157 | 158 | void GlobalMap::AssertConsistency() { 159 | MT_READ_LOCK_BEGIN(m_MT, MT_TASK_NONE, MT_TASK_NONE); 160 | ubyte Uc = GM_FLAG_FRAME_DEFAULT; 161 | const int N = static_cast(m_Cs.size()); 162 | for (int i = 0; i < N; ++i) { 163 | const Camera &C = m_Cs[i]; 164 | C.m_C.AssertOrthogonal(); 165 | Uc |= C.m_uc; 166 | } 167 | UT_ASSERT(m_Uc == Uc); 168 | MT_READ_LOCK_END(m_MT, MT_TASK_NONE, MT_TASK_NONE); 169 | } 170 | -------------------------------------------------------------------------------- /Backend/Map/LocalMap.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _LOCAL_MAP_H_ 17 | #define _LOCAL_MAP_H_ 18 | 19 | #include "GlobalMap.h" 20 | #include "MultiThread.h" 21 | 22 | #define LM_FLAG_FRAME_DEFAULT 0 23 | #define LM_FLAG_FRAME_UPDATE_CAMERA_LF 1 24 | #define LM_FLAG_FRAME_UPDATE_CAMERA_KF 2 25 | #define LM_FLAG_FRAME_UPDATE_DEPTH 4 26 | 27 | #define LM_FLAG_TRACK_DEFAULT 0 28 | #define LM_FLAG_TRACK_UPDATE_DEPTH 1 29 | 30 | class LocalMap { 31 | 32 | public: 33 | 34 | class CameraLF { 35 | public: 36 | inline CameraLF() {} 37 | inline CameraLF(const Camera &C, const int iFrm, const ubyte uc = LM_FLAG_FRAME_DEFAULT) : 38 | m_C(C), m_iFrm(iFrm), m_uc(uc) { 39 | #ifdef CFG_CHECK_REPROJECTION 40 | m_e.first = m_e.second = FLT_MAX; 41 | #endif 42 | } 43 | inline bool operator < (const int iFrm) const { return m_iFrm < iFrm; } 44 | public: 45 | Camera m_C; 46 | int m_iFrm; 47 | ubyte m_uc; 48 | #ifdef CFG_CHECK_REPROJECTION 49 | std::pair m_e; 50 | #endif 51 | }; 52 | 53 | class CameraKF : public GlobalMap::Camera { 54 | public: 55 | inline CameraKF() : GlobalMap::Camera() {} 56 | inline CameraKF(const Rigid3D &C, const int iFrm, const ubyte uc = GM_FLAG_FRAME_DEFAULT) : 57 | GlobalMap::Camera(C, iFrm, uc) { 58 | #ifdef CFG_CHECK_REPROJECTION 59 | m_e.first = m_e.second = FLT_MAX; 60 | #endif 61 | } 62 | public: 63 | #ifdef CFG_CHECK_REPROJECTION 64 | std::pair m_e; 65 | #endif 66 | }; 67 | 68 | public: 69 | 70 | void IBA_Reset(); 71 | void IBA_PushLocalFrame(const CameraLF &C); 72 | void IBA_PushKeyFrame(const GlobalMap::InputKeyFrame &KF); 73 | void IBA_DeleteKeyFrame(const int iFrm, const int iKF); 74 | ubyte IBA_Synchronize(const int iFrm, std::list &CsLF, std::vector &CsKF, 75 | std::vector &ds, std::vector &uds); 76 | void LBA_Update(const int iFrm1, const int iFrm2, const std::vector &ic2LF, 77 | const AlignedVector &CsLF, const std::vector &ucsLF, 78 | const std::vector &iFrmsKF, const AlignedVector &CsKF, 79 | const std::vector &ucsKF, const std::vector &iKF2d, 80 | const std::vector &ds, const std::vector &uds 81 | #ifdef CFG_CHECK_REPROJECTION 82 | , const std::vector > &esLF, 83 | const std::vector > &esKF 84 | #endif 85 | ); 86 | 87 | void SaveB(FILE *fp); 88 | void LoadB(FILE *fp); 89 | void AssertConsistency(); 90 | 91 | protected: 92 | 93 | std::list m_CsLF; 94 | std::vector m_CsKF; 95 | std::vector m_ds; 96 | std::vector m_iKF2d; 97 | ubyte m_Uc; 98 | std::vector m_uds; 99 | boost::shared_mutex m_MT; 100 | 101 | }; 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /Backend/Utility/Candidate.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _CANDIDATE_H_ 17 | #define _CANDIDATE_H_ 18 | 19 | template 20 | class Candidate { 21 | public: 22 | inline Candidate() {} 23 | inline Candidate(const int idx, const TYPE score) { Set(idx, score); } 24 | inline void Set(const int idx, const TYPE score) { m_idx = idx; m_score = score; } 25 | inline void Get(int &idx, TYPE &score) const { idx = m_idx; score = m_score; } 26 | inline bool operator < (const Candidate &c) const { return m_score < c.m_score; } 27 | inline bool operator > (const Candidate &c) const { return m_score > c.m_score; } 28 | public: 29 | int m_idx; 30 | TYPE m_score; 31 | }; 32 | 33 | template 34 | class CandidateVector { 35 | public: 36 | inline int Size() const { return int(m_data.size()); } 37 | inline void Resize(const int N) { m_data.resize(N); } 38 | inline void Push(const int idx, const TYPE score) { m_data.push_back(Candidate(idx, score)); } 39 | inline void MakeZero() { 40 | const int N = int(m_data.size()); 41 | for (int i = 0; i < N; ++i) 42 | m_data[i].Set(i, 0); 43 | } 44 | inline void RemoveZero() { 45 | int i, j; 46 | const int N = int(m_data.size()); 47 | for (i = j = 0; i < N; ++i) { 48 | if (m_data[i].m_score != 0) 49 | m_data[j++] = m_data[i]; 50 | } 51 | m_data.resize(j); 52 | } 53 | inline void SortAscending() { 54 | std::sort(m_data.begin(), m_data.end()); 55 | } 56 | inline void SortDescending() { 57 | std::sort(m_data.begin(), m_data.end(), std::greater >()); 58 | } 59 | inline void MarkFirstN(const int N, std::vector &marks, const ubyte mark) const { 60 | const int kN = std::min(Size(), N); 61 | for (int i = 0; i < kN; ++i) 62 | marks[m_data[i].m_idx] = mark; 63 | } 64 | inline void GetFirstN(const int N, std::vector &idxs) const { 65 | const int kN = std::min(Size(), N); 66 | idxs.resize(kN); 67 | for (int i = 0; i < kN; ++i) 68 | idxs[i] = m_data[i].m_idx; 69 | } 70 | public: 71 | std::vector > m_data; 72 | }; 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /Backend/Utility/Configurator.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _CONFIGURATOR_H_ 17 | #define _CONFIGURATOR_H_ 18 | 19 | #include "Utility.h" 20 | #include 21 | 22 | class Configurator { 23 | 24 | public: 25 | 26 | inline Configurator() {} 27 | inline Configurator(const char *fileName) { Load(fileName); } 28 | inline Configurator(const std::string& fileName) { Load(fileName.c_str()); } 29 | 30 | inline std::string GetArgument(const std::string directive, 31 | const std::string default_val = "") const { 32 | const DirectiveArgumentMap::const_iterator i = m_directiveArgumentMap.find(directive); 33 | if (i == m_directiveArgumentMap.end()) { 34 | return default_val; 35 | } else { 36 | return i->second; 37 | } 38 | } 39 | inline int GetArgument(const std::string directive, const int default_val) const { 40 | const DirectiveArgumentMap::const_iterator i = m_directiveArgumentMap.find(directive); 41 | if (i == m_directiveArgumentMap.end()) { 42 | return default_val; 43 | } else { 44 | return atoi(i->second.c_str()); 45 | } 46 | } 47 | inline float GetArgument(const std::string directive, const float default_val) const { 48 | const DirectiveArgumentMap::const_iterator i = m_directiveArgumentMap.find(directive); 49 | if (i == m_directiveArgumentMap.end()) { 50 | return default_val; 51 | } else { 52 | return float(atof(i->second.c_str())); 53 | } 54 | } 55 | inline double GetArgument(const std::string directive, const double default_val) const { 56 | const DirectiveArgumentMap::const_iterator i = m_directiveArgumentMap.find(directive); 57 | if (i == m_directiveArgumentMap.end()) { 58 | return default_val; 59 | } else { 60 | return double(atof(i->second.c_str())); 61 | } 62 | } 63 | inline float GetArgumentInverse(const std::string directive, const float default_val) const { 64 | const float argument = GetArgument(directive, UT::Inverse(default_val)); 65 | return UT::Inverse(argument); 66 | } 67 | inline float GetArgumentScaled(const std::string directive, const float default_val, 68 | const float scalar) const { 69 | const float argument = GetArgument(directive, default_val / scalar); 70 | return argument * scalar; 71 | } 72 | inline int GetArgumentScaled(const std::string directive, const int default_val, 73 | const float scalar) const { 74 | const float argument = GetArgument(directive, default_val / scalar); 75 | return int(argument * scalar + 0.5f); 76 | } 77 | inline float GetArgumentSquared(const std::string directive, const float default_val) const { 78 | const float argument = GetArgument(directive, sqrtf(default_val)); 79 | return argument * argument; 80 | } 81 | inline float GetArgumentScaledSquared(const std::string directive, const float default_val, 82 | const float scalar) const { 83 | const float argument = GetArgumentScaled(directive, sqrtf(default_val), scalar); 84 | return argument * argument; 85 | } 86 | inline float GetArgumentRadian(const std::string directive, const float default_val) const { 87 | return GetArgumentScaled(directive, default_val, UT_FACTOR_DEG_TO_RAD); 88 | } 89 | inline float GetArgumentRadianSquared(const std::string directive, const float default_val) const { 90 | return GetArgumentScaledSquared(directive, default_val, UT_FACTOR_DEG_TO_RAD); 91 | } 92 | 93 | inline void SetArgument(const std::string directive, const std::string argument) { 94 | DirectiveArgumentMap::iterator i = m_directiveArgumentMap.find(directive); 95 | if (i == m_directiveArgumentMap.end()) { 96 | m_directiveArgumentMap.insert(DirectiveArgumentMap::value_type(directive, argument)); 97 | } else { 98 | i->second = argument; 99 | } 100 | } 101 | inline void SetArgument(const std::string directive, const int argument) { 102 | char buf[UT_STRING_WIDTH_MAX]; 103 | sprintf(buf, "%d", argument); 104 | DirectiveArgumentMap::iterator i = m_directiveArgumentMap.find(directive); 105 | if (i == m_directiveArgumentMap.end()) { 106 | m_directiveArgumentMap.insert(DirectiveArgumentMap::value_type(directive, buf)); 107 | } else { 108 | i->second = buf; 109 | } 110 | } 111 | inline void SetArgument(const std::string directive, const float argument) { 112 | char buf[UT_STRING_WIDTH_MAX]; 113 | sprintf(buf, "%f", argument); 114 | DirectiveArgumentMap::iterator i = m_directiveArgumentMap.find(directive); 115 | if (i == m_directiveArgumentMap.end()) { 116 | m_directiveArgumentMap.insert(DirectiveArgumentMap::value_type(directive, buf)); 117 | } else { 118 | i->second = buf; 119 | } 120 | } 121 | inline void SetArgument(const std::string directive, const double argument) { 122 | char buf[UT_STRING_WIDTH_MAX]; 123 | sprintf(buf, "%lf", argument); 124 | DirectiveArgumentMap::iterator i = m_directiveArgumentMap.find(directive); 125 | if (i == m_directiveArgumentMap.end()) { 126 | m_directiveArgumentMap.insert(DirectiveArgumentMap::value_type(directive, buf)); 127 | } else { 128 | i->second = buf; 129 | } 130 | } 131 | 132 | inline bool Load(const char *fileName) { 133 | m_directiveArgumentMap.clear(); 134 | FILE *fp = fopen(fileName, "r"); 135 | if (!fp) { 136 | return false; 137 | } 138 | char line[UT_STRING_WIDTH_MAX], *end; 139 | while (fgets(line, UT_STRING_WIDTH_MAX, fp)) { 140 | end = std::remove(line, line + strlen(line), 10); 141 | end = std::remove(line, end, ' '); 142 | end = std::remove(line, end, 13); 143 | const int len = static_cast(end - line); 144 | if (len < 2 || (line[0] == '/' && line[1] == '/')) { 145 | continue; 146 | } 147 | line[len] = 0; 148 | const std::string directive = strtok(line, "="); 149 | const char *argument = strtok(NULL, "="); 150 | if (argument) { 151 | m_directiveArgumentMap.insert(DirectiveArgumentMap::value_type(directive, argument)); 152 | } 153 | } 154 | fclose(fp); 155 | UT::PrintLoaded(fileName); 156 | return true; 157 | } 158 | 159 | void Print() const { 160 | UT::Print("[Configurator]\n"); 161 | for (DirectiveArgumentMap::const_iterator i = m_directiveArgumentMap.begin(); 162 | i != m_directiveArgumentMap.end(); i++) { 163 | UT::Print(" %s = %s\n", i->first.c_str(), i->second.c_str()); 164 | } 165 | } 166 | 167 | private: 168 | 169 | typedef std::map DirectiveArgumentMap; 170 | DirectiveArgumentMap m_directiveArgumentMap; 171 | 172 | }; 173 | 174 | #endif 175 | -------------------------------------------------------------------------------- /Backend/Utility/FillMap.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _FILL_MAP_H_ 17 | #define _FILL_MAP_H_ 18 | 19 | #include "Table.h" 20 | 21 | class FillMap : public Table { 22 | 23 | public: 24 | 25 | inline void Initialize(const int w, const int h) { Resize(w, h); MakeZero(); } 26 | 27 | inline void Fill(const int x, const int y, const int dist = 0) { 28 | if (dist == 0) { 29 | if (x >= 0 && x < m_w && y >= 0 && y < m_h) 30 | m_rows[y][x] = UCHAR_MAX; 31 | return; 32 | } 33 | const int x1 = std::min(std::max(x - dist, 0), m_w), x2 = std::min(x + dist, m_w); 34 | const int y1 = std::min(std::max(y - dist, 0), m_h), y2 = std::min(y + dist, m_h); 35 | const int size = sizeof(ubyte) * (x2 - x1); 36 | for (int _y = y1; _y < y2; ++_y) 37 | memset(m_rows[_y] + x1, UINT_MAX, size); 38 | } 39 | inline bool Filled(const int x, const int y) const { return m_rows[y][x] != 0; } 40 | inline int CountFilled() const { 41 | int SN = 0; 42 | for (int y = 0; y < m_h; ++y) 43 | for (int x = 0; x < m_w; ++x) { 44 | if (m_rows[y][x]) 45 | ++SN; 46 | } 47 | return SN; 48 | } 49 | 50 | inline void GetDilated(FillMap &FM, const int dist = 1) const { 51 | FM.Resize(m_w, m_h); 52 | for (int y = 0; y < m_h; ++y) { 53 | const int y1 = std::max(y - dist, 0), y2 = std::min(y + dist, m_h); 54 | for (int x = 0; x < m_w; ++x) { 55 | const int x1 = std::max(x - dist, 0), x2 = std::min(x + dist, m_w); 56 | ubyte v = 0; 57 | for (int _y = y1; _y < y2; ++_y) 58 | for (int _x = x1; _x < x2; ++_x) 59 | v = v | m_rows[_y][_x]; 60 | FM.m_rows[y][x] = v; 61 | } 62 | } 63 | } 64 | }; 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /Backend/Utility/Table.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _TABLE_H_ 17 | #define _TABLE_H_ 18 | 19 | #include "Utility.h" 20 | 21 | template 22 | class Table { 23 | 24 | public: 25 | 26 | inline Table() : m_w(0), m_h(0) {} 27 | 28 | inline const TYPE* operator[] (const int i) const { return m_rows[i]; } 29 | inline TYPE* operator[] (const int i) { return m_rows[i]; } 30 | 31 | inline const TYPE* Data() const { return m_data.empty() ? NULL : m_data.data(); } 32 | inline TYPE* Data() { return m_data.empty() ? NULL : m_data.data(); } 33 | 34 | inline const int& w() const { return m_w; } 35 | inline const int& h() const { return m_h; } 36 | inline bool Empty() const { return m_data.empty(); } 37 | inline int Size() const { return int(m_data.size()); } 38 | inline void Resize(const int w, const int h) { 39 | if (m_w == w && m_h == h) 40 | return; 41 | m_w = w; 42 | m_h = h; 43 | const int N = m_w * m_h; 44 | m_data.resize(N); 45 | m_rows.resize(m_h); 46 | m_rows[0] = m_data.data(); 47 | for (int y = 1; y < m_h; ++y) 48 | m_rows[y] = m_rows[y - 1] + m_w; 49 | } 50 | 51 | inline void operator = (const Table &T) { 52 | Resize(T.m_w, T.m_h); 53 | memcpy(m_data.data(), T.m_data.data(), sizeof(TYPE) * m_data.size()); 54 | } 55 | 56 | inline void MakeZero() { memset(m_data.data(), 0, sizeof(TYPE) * m_data.size()); } 57 | 58 | inline void Swap(Table &T) { 59 | UT_SWAP(m_w, T.m_w); 60 | UT_SWAP(m_h, T.m_h); 61 | m_data.swap(T.m_data); 62 | m_rows.swap(T.m_rows); 63 | } 64 | 65 | inline void SaveB(FILE *fp) const { 66 | UT::SaveB(m_w, fp); 67 | UT::SaveB(m_h, fp); 68 | UT::VectorSaveB(m_data, fp); 69 | } 70 | inline void LoadB(FILE *fp) { 71 | const int w = UT::LoadB(fp); 72 | const int h = UT::LoadB(fp); 73 | Resize(w, h); 74 | UT::VectorLoadB(m_data, fp); 75 | } 76 | inline float MemoryMB() const { 77 | return UT::MemoryMB(2) 78 | + UT::VectorMemoryMB(m_data) 79 | + UT::VectorMemoryMB(m_rows); 80 | } 81 | 82 | protected: 83 | 84 | int m_w, m_h; 85 | std::vector m_data; 86 | std::vector m_rows; 87 | }; 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /Backend/Utility/Timer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef UTILITY_TIMER_H_ 17 | #define UTILITY_TIMER_H_ 18 | 19 | #include "Utility.h" 20 | #ifndef WIN32 21 | #include 22 | #endif 23 | #include 24 | 25 | class Timer { 26 | public: 27 | inline Timer(const int N = 1) { 28 | this->Reset(N); 29 | } 30 | inline ~Timer() { } 31 | inline void Reset(const int N = 1) { 32 | m_first = true; 33 | m_idx = 0; 34 | m_start = m_avg = 0.0; 35 | m_times.assign(N, 0.0); 36 | } 37 | inline void Start() { m_start = GetTime(); } 38 | inline void Stop(const bool finish = false) { 39 | m_times[m_idx] = GetTime() - m_start + m_times[m_idx]; 40 | if (finish) { 41 | Finish(); 42 | } 43 | } 44 | inline void Finish() { 45 | ++m_idx; 46 | m_avg = 0.0; 47 | if (m_first) { 48 | for (int i = 0; i < m_idx; ++i) { 49 | m_avg = m_times[i] + m_avg; 50 | } 51 | m_avg /= m_idx; 52 | } else { 53 | const int N = static_cast(m_times.size()); 54 | for (int i = 0; i < N; ++i) { 55 | m_avg = m_times[i] + m_avg; 56 | } 57 | m_avg /= N; 58 | } 59 | if (m_idx == m_times.size()) { 60 | m_first = false; 61 | m_idx = 0; 62 | } 63 | m_times[m_idx] = 0.0; 64 | } 65 | inline double GetAverageSeconds() const { return m_avg; } 66 | inline double GetAverageMilliseconds() const { return m_avg * 1000.0; } 67 | inline double GetFPS() const { return m_avg == 0.0 ? 0.0 : 1 / GetAverageSeconds(); } 68 | 69 | inline void SaveB(FILE *fp) const { 70 | UT::SaveB(m_first, fp); 71 | UT::SaveB(m_idx, fp); 72 | UT::SaveB(m_start, fp); 73 | UT::SaveB(m_avg, fp); 74 | UT::VectorSaveB(m_times, fp); 75 | } 76 | inline void LoadB(FILE *fp) { 77 | UT::LoadB(m_first, fp); 78 | UT::LoadB(m_idx, fp); 79 | UT::LoadB(m_start, fp); 80 | UT::LoadB(m_avg, fp); 81 | UT::VectorLoadB(m_times, fp); 82 | } 83 | 84 | inline static double GetTime() { 85 | #ifndef WIN32 86 | struct timeval time; 87 | gettimeofday(&time, NULL); 88 | return time.tv_sec + (time.tv_usec / 1000000.0); 89 | #else 90 | SYSTEMTIME systime; 91 | GetSystemTime(&systime); 92 | return 0.001 * static_cast(systime.wMilliseconds) 93 | + 1.0 * static_cast(systime.wSecond) 94 | + 60.0 * static_cast(systime.wMinute) 95 | + 3600.0 * static_cast(systime.wHour); 96 | #endif 97 | } 98 | 99 | private: 100 | bool m_first; 101 | int m_idx; 102 | double m_start, m_avg; 103 | std::vector m_times; 104 | }; 105 | #endif // UTILITY_TIMER_H_ 106 | -------------------------------------------------------------------------------- /Backend/Visualization/Arcball.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _ARCBALL_H_ 17 | #define _ARCBALL_H_ 18 | 19 | #include "Rotation.h" 20 | #include "Matrix4x4.h" 21 | #include 22 | #include 23 | 24 | class Arcball { 25 | 26 | public: 27 | 28 | inline void Initialize() { 29 | m_C.MakeZero(); 30 | m_OC = m_OC2 = 0.0f; 31 | m_nC.MakeZero(); 32 | //m_r = m_r2 = 0.0f; 33 | SetRadius(0.0f); 34 | m_R.MakeIdentity(); 35 | m_R.GetTranspose(m_RT); 36 | m_M = m_R; 37 | m_Cstart = m_C; 38 | m_Rstart = m_R; 39 | } 40 | inline void Initialize(const Rotation3D &R, const float zCenter, const float radius) { 41 | m_C.Set(0, 0, zCenter); 42 | m_OC = fabs(zCenter); 43 | m_OC2 = zCenter * zCenter; 44 | m_nC.Set(0.0f, 0.0f, 1.0f); 45 | SetRadius(radius); 46 | m_R = R; 47 | m_R.GetTranspose(m_RT); 48 | m_M = m_R; 49 | m_Cstart = m_C; 50 | m_rStart = m_r; 51 | m_Rstart = m_R; 52 | } 53 | inline void Initialize(const float zCenter, const float radius) { 54 | m_C.Set(0, 0, zCenter); 55 | m_OC = fabs(zCenter); 56 | m_OC2 = zCenter * zCenter; 57 | m_nC.Set(0.0f, 0.0f, 1.0f); 58 | SetRadius(radius); 59 | m_R.MakeIdentity(); 60 | m_R.GetTranspose(m_RT); 61 | m_M = m_R; 62 | m_Cstart = m_C; 63 | m_rStart = m_r; 64 | m_Rstart = m_R; 65 | } 66 | inline void Initialize(const Point3D ¢er, const float radius) { 67 | m_C = center; 68 | m_OC2 = m_C.SquaredLength(); 69 | m_OC = sqrtf(m_OC2); 70 | m_nC = m_C / m_OC; 71 | SetRadius(radius); 72 | m_R.MakeIdentity(); 73 | m_R.GetTranspose(m_RT); 74 | m_M = m_R; 75 | m_Cstart = m_C; 76 | m_rStart = m_r; 77 | m_Rstart = m_R; 78 | } 79 | inline void Set(const float centerZ, const float radius) { 80 | m_C.Set(0, 0, centerZ); 81 | m_OC = centerZ; 82 | m_OC2 = m_OC * m_OC; 83 | m_nC.Set(0, 0, 1); 84 | SetRadius(radius); 85 | } 86 | inline const Point3D& GetCenter() const { return m_C; } 87 | inline void SetCenter(const Point3D &C) { 88 | m_C = C; 89 | m_OC2 = m_C.SquaredLength(); 90 | m_OC = sqrtf(m_OC2); 91 | m_nC = m_C / m_OC; 92 | ComputeTangentFoot(); 93 | } 94 | inline float GetCenterZ() const { return m_C.z(); } 95 | inline float GetRadius() const { return m_r; } 96 | inline const Rotation3D& GetRotation() const { return m_RT; } 97 | inline const Rotation3D& GetRotationTranspose() const { return m_R; } 98 | 99 | inline void StartChangingRadius() { m_rStart = m_r; } 100 | inline void ChangeRadius(const float ratio) { SetRadius(m_rStart * ratio); } 101 | inline void StartTranslatingCenter() { m_Cstart = m_C; } 102 | inline void TranslateCenter(const Point3D &dC) { 103 | m_C = dC + m_Cstart; 104 | m_OC2 = m_C.SquaredLength(); 105 | m_OC = sqrtf(m_OC2); 106 | m_nC = m_C / m_OC; 107 | ComputeTangentFoot(); 108 | } 109 | inline void TranslateCenterXY(const float dx, const float dy) { 110 | m_C.x() = dx + m_Cstart.x(); 111 | m_C.y() = dy + m_Cstart.y(); 112 | m_OC2 = m_C.SquaredLength(); 113 | m_OC = sqrtf(m_OC2); 114 | m_nC = m_C / m_OC; 115 | ComputeTangentFoot(); 116 | } 117 | inline void StartRotation(const Point3D &Xwin) { 118 | m_Rstart = m_R; 119 | ComputeCenterToIntersectionDirection(Xwin, m_nCXstart); 120 | } 121 | inline void ComputeRotation(const Point3D &Xwin) { 122 | Point3D nCX; 123 | ComputeCenterToIntersectionDirection(Xwin, nCX); 124 | 125 | Rotation3D dR; 126 | AxisAngle kth; 127 | kth.FromVectors(m_nCXstart, nCX); 128 | dR.SetAxisAngle(kth); 129 | 130 | m_R = m_Rstart * dR; 131 | m_R.GetTranspose(m_RT); 132 | m_M = m_R; 133 | } 134 | inline void ApplyTransformation() const { 135 | glTranslatef(m_C.x(), m_C.y(), m_C.z()); 136 | glMultMatrixf(m_M); 137 | glTranslatef(-m_C.x(), -m_C.y(), -m_C.z()); 138 | } 139 | inline void Draw(const int slices, const int stacks) const { 140 | glPushMatrix(); 141 | glTranslatef(m_C.x(), m_C.y(), m_C.z()); 142 | glMultMatrixf(m_M); 143 | glutWireSphere(double(m_r), slices, stacks); 144 | glPopMatrix(); 145 | } 146 | 147 | protected: 148 | 149 | inline void SetRadius(const float radius) { 150 | m_r = radius; 151 | m_r2 = radius * radius; 152 | ComputeTangentFoot(); 153 | } 154 | inline void ComputeTangentFoot() { 155 | m_OTfoot = m_OC - m_r2 / m_OC; 156 | m_Tfoot = m_nC * m_OC; 157 | } 158 | inline void ComputeCenterToIntersectionDirection(const Point3D &Xwin, 159 | LA::AlignedVector3f &nCX) const { 160 | float a = Xwin.SquaredLength(); 161 | float b = -Xwin.Dot(m_C); 162 | float c = m_OC2 - m_r2; 163 | float root = b * b - a * c; 164 | float t; 165 | if (root > 0) { 166 | t = (-b - sqrtf(root)) / a; 167 | nCX = Xwin * t - m_C; 168 | nCX.Normalize(); 169 | } else { 170 | // Tfoot is perpendicular foot of the tangent point T on OC 171 | // Xext is the intersection of OXwin and TfootT 172 | // |OXext| / |OTfoot| = |OXwin| / |OXfoot| 173 | const Point3D Xext = Xwin * (m_OTfoot / Xwin.Dot(m_nC)); 174 | 175 | // T = Xext + XextTfoot * t 176 | // TC = XextT - CXext = XextTfoot * t - CXext 177 | // |TC| = |XextTfoot * t - CXext| = R 178 | //Point3D CXext, XextTfoot; 179 | const Point3D CXext = Xext - m_C, XextTfoot = m_Tfoot - Xext; 180 | a = b = XextTfoot.SquaredLength(); 181 | c = CXext.SquaredLength() - m_r2; 182 | t = (-b - sqrtf(b * b - a * c)) / a; 183 | nCX = XextTfoot * t + Xext - m_C; 184 | nCX.Normalize(); 185 | } 186 | } 187 | 188 | private: 189 | 190 | Point3D m_C, m_Cstart, m_Tfoot; 191 | LA::AlignedVector3f m_nC, m_nCXstart; 192 | Rotation3D m_R, m_RT, m_Rstart; 193 | LA::AlignedMatrix4x4f m_M; 194 | float m_OC, m_OC2, m_r, m_r2, m_rStart, m_OTfoot; 195 | 196 | }; 197 | 198 | #endif 199 | -------------------------------------------------------------------------------- /Backend/Visualization/EventHandler.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _EVENT_HANDLER_H_ 17 | #define _EVENT_HANDLER_H_ 18 | 19 | #include 20 | #ifdef WIN32 21 | #define VW_KEY_QUIT 27 22 | #else 23 | #define VW_KEY_QUIT 0xff1b // XK_Escape 24 | #endif 25 | 26 | #define VW_MOUSE_BUTTON_INVALID -1 27 | #define VW_MOUSE_BUTTON_LEFT 1 28 | #define VW_MOUSE_BUTTON_RIGHT 4 29 | #define VW_MOUSE_BUTTON_MIDDLE_UP 32 30 | #define VW_MOUSE_BUTTON_MIDDLE_DOWN 64 31 | 32 | template 33 | class EventHandler : public CVD::GLWindow::EventHandler { 34 | 35 | public: 36 | 37 | typedef bool (HANDLER::*CallBackKeyDown) (const int key); 38 | typedef bool (HANDLER::*CallBackKeyUp) (const int key); 39 | typedef void (HANDLER::*CallBackMouseMove) (const CVD::ImageRef &where); 40 | typedef void (HANDLER::*CallBackMouseDown) (const CVD::ImageRef &where, const int button); 41 | typedef void (HANDLER::*CallBackMouseUp) (const CVD::ImageRef &where, const int button); 42 | typedef bool (HANDLER::*CallBackMouseDraging) (const CVD::ImageRef &from, const CVD::ImageRef &to, 43 | const int button); 44 | typedef bool (HANDLER::*CallBackMouseDoubleClick)(const CVD::ImageRef &where, const int button); 45 | typedef void (HANDLER::*CallBackResize) (const CVD::ImageRef &size); 46 | 47 | public: 48 | 49 | inline EventHandler() : m_quit(false), m_keyDown(false), m_mouseDown(false), m_handler(NULL), 50 | m_keyDownCB(NULL), m_keyUpCB(NULL), m_mouseMoveCB(NULL), 51 | m_mouseDownCB(NULL), m_mouseUpCB(NULL), m_mouseDragingCB(NULL), m_mouseDoubleClickCB(NULL), 52 | m_resizeCB(NULL) {} 53 | 54 | /// Called for key press events 55 | virtual void on_key_down(CVD::GLWindow &win, int key) { 56 | m_keyDown = true; 57 | m_key = key; 58 | m_quit = (key == VW_KEY_QUIT); 59 | if (m_handler && m_keyDownCB) 60 | (m_handler->*m_keyDownCB)(key); 61 | } 62 | /// Called for key release events 63 | virtual void on_key_up(CVD::GLWindow &win, int key) { 64 | m_keyDown = false; 65 | m_key = -1; 66 | if (m_handler && m_keyUpCB) 67 | (m_handler->*m_keyUpCB)(key); 68 | } 69 | /// Called for mouse movement events 70 | virtual void on_mouse_move(CVD::GLWindow &win, CVD::ImageRef where, int state) { 71 | m_mouseDraging = m_mouseDown && where != m_whereMouseMove; 72 | m_whereMouseMove = where; 73 | if (m_mouseDown && m_handler && m_mouseDragingCB) 74 | (m_handler->*m_mouseDragingCB)(m_whereMouseDown, where, m_button); 75 | else if (m_handler && m_mouseMoveCB) 76 | (m_handler->*m_mouseMoveCB)(where); 77 | } 78 | /// Called for mouse button press events 79 | virtual void on_mouse_down(CVD::GLWindow &win, CVD::ImageRef where, int state, int button) { 80 | m_mouseDown = true; 81 | m_button = button; 82 | if (m_whereMouseDown == where) // Double click 83 | m_whereMouseDown.x = -1; 84 | else { 85 | m_whereMouseDown = where; 86 | if (m_handler && m_mouseDownCB) 87 | (m_handler->*m_mouseDownCB)(where, button); 88 | } 89 | } 90 | /// Called for mouse button release events 91 | virtual void on_mouse_up(CVD::GLWindow &win, CVD::ImageRef where, int state, int button) { 92 | if ((button == VW_MOUSE_BUTTON_LEFT || button == VW_MOUSE_BUTTON_RIGHT) && 93 | m_whereMouseDown.x == -1) { //Double click 94 | if (m_handler && m_mouseDoubleClickCB) 95 | (m_handler->*m_mouseDoubleClickCB)(where, button); 96 | } else { 97 | if (m_handler && m_mouseUpCB) 98 | (m_handler->*m_mouseUpCB)(where, button); 99 | } 100 | m_mouseDown = false; 101 | m_mouseDraging = false; 102 | } 103 | /// Called for window resize events 104 | virtual void on_resize(CVD::GLWindow &win, CVD::ImageRef size) { 105 | if (m_handler && m_resizeCB) 106 | (m_handler->*m_resizeCB)(size); 107 | } 108 | /// Called for general window events (such as EVENT_CLOSE) 109 | virtual void on_event(CVD::GLWindow &win, int event) { 110 | m_quit = (event == CVD::GLWindow::EVENT_CLOSE); 111 | } 112 | 113 | inline void Initialize(HANDLER *handler, CallBackKeyDown keyDownCB, CallBackKeyUp keyUpCB, 114 | CallBackMouseMove mouseMoveCB, CallBackMouseDown mouseDownCB, 115 | CallBackMouseUp mouseUpCB, CallBackMouseDraging mouseDragingCB, 116 | CallBackMouseDoubleClick mouseDoubleClickCB, CallBackResize resizeCB) { 117 | m_quit = false; 118 | m_keyDown = false; 119 | m_mouseDown = false; 120 | m_handler = handler; 121 | m_keyDownCB = keyDownCB; 122 | m_keyUpCB = keyUpCB; 123 | m_mouseMoveCB = mouseMoveCB; 124 | m_mouseDownCB = mouseDownCB; 125 | m_mouseUpCB = mouseUpCB; 126 | m_mouseDragingCB = mouseDragingCB; 127 | m_mouseDoubleClickCB = mouseDoubleClickCB; 128 | m_resizeCB = resizeCB; 129 | } 130 | 131 | inline void Restart() { m_quit = false; } 132 | inline void Quit() { m_quit = true; } 133 | inline const bool Quitted() const { return m_quit; } 134 | inline const bool& MouseDown() const { return m_mouseDown; } 135 | inline const bool& MouseDraging() const { return m_mouseDraging; } 136 | inline const CVD::ImageRef& MouseMove() const { return m_whereMouseMove; } 137 | 138 | protected: 139 | 140 | bool m_quit, m_keyDown, m_mouseDown, m_mouseDraging; 141 | int m_key, m_button; 142 | CVD::ImageRef m_whereMouseDown, m_whereMouseMove; 143 | HANDLER *m_handler; 144 | CallBackKeyDown m_keyDownCB; 145 | CallBackKeyUp m_keyUpCB; 146 | CallBackMouseMove m_mouseMoveCB; 147 | CallBackMouseDown m_mouseDownCB; 148 | CallBackMouseUp m_mouseUpCB; 149 | CallBackMouseDraging m_mouseDragingCB; 150 | CallBackMouseDoubleClick m_mouseDoubleClickCB; 151 | CallBackResize m_resizeCB; 152 | 153 | }; 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /Backend/Visualization/Frustrum.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _FRUSTRUM_H_ 17 | #define _FRUSTRUM_H_ 18 | 19 | #include "Rigid.h" 20 | #include 21 | #include 22 | 23 | class Frustrum { 24 | 25 | public: 26 | 27 | inline void SetShape(const float z, const float xzRatio, const float yzRatio, const float size, 28 | const float axisLen) { 29 | m_Xc.Set(z * xzRatio, z * yzRatio, z); 30 | SetSize(size); 31 | SetAxisLength(axisLen); 32 | } 33 | inline void SetSize(const float size) { m_XcScaled = m_Xc * size; } 34 | inline void SetAxisLength(const float len) { m_len.vdup_all_lane(m_Xc.z() * len); } 35 | inline void SetPose(const Rigid3D &T, const bool axis) { 36 | T.GetPosition(m_Xws[0]); 37 | 38 | // Xw = R^T * (Xc - t) = R^T * Xc + Cc 39 | xp128f XcR0 = T.r_00_01_02_x() * m_XcScaled.x(); 40 | xp128f YcR1 = T.r_10_11_12_x() * m_XcScaled.y(); 41 | xp128f ZcR2 = T.r_20_21_22_x() * m_XcScaled.z(); 42 | 43 | // 4 plane vertexes' world coordinate are: X[R0] + Y[R1] + Z[R2] + Cc, X[R0] - Y[R1] + Z[R2] + Cc, 44 | // - X[R0] + Y[R1] + Z[R2] + Cc, - X[R0] - Y[R1] + Z[R2] + Cc, 45 | ZcR2 += m_Xws[0].xyzw(); // Z[R2] + Cc 46 | m_Xws[1].xyzw() = XcR0; 47 | XcR0 += YcR1; // X[R0] + Y[R1] 48 | YcR1 = m_Xws[1].xyzw() - YcR1; // X[R0] - Y[R1] 49 | m_Xws[1].xyzw() = ZcR2 + XcR0; // X[R0] + Y[R1] + Z[R2] + Cc 50 | m_Xws[2].xyzw() = ZcR2 + YcR1; // X[R0] - Y[R1] + Z[R2] + Cc 51 | m_Xws[3].xyzw() = ZcR2 - YcR1; // - X[R0] + Y[R1] + Z[R2] + Cc 52 | m_Xws[4].xyzw() = ZcR2 - XcR0; // - X[R0] - Y[R1] + Z[R2] + Cc 53 | 54 | if (axis) { 55 | m_axisX.xyzw() = T.r_00_01_02_x() * m_len + m_Xws[0].xyzw(); 56 | m_axisY.xyzw() = T.r_10_11_12_x() * m_len + m_Xws[0].xyzw(); 57 | m_axisZ.xyzw() = T.r_20_21_22_x() * m_len + m_Xws[0].xyzw(); 58 | } 59 | } 60 | inline const Point3D& GetPosition() const { return m_Xws[0]; } 61 | inline void MakeOrigin(const bool axis) { 62 | m_Xws[0].MakeZero(); 63 | m_Xws[1].Set( m_XcScaled.x(), m_XcScaled.y(), m_XcScaled.z()); 64 | m_Xws[2].Set( m_XcScaled.x(), -m_XcScaled.y(), m_XcScaled.z()); 65 | m_Xws[3].Set(-m_XcScaled.x(), m_XcScaled.y(), m_XcScaled.z()); 66 | m_Xws[4].Set(-m_XcScaled.x(), -m_XcScaled.y(), m_XcScaled.z()); 67 | if (axis) { 68 | m_axisX.Set(m_len[0], 0.0f, 0.0f); 69 | m_axisY.Set(0.0f, m_len[0], 0.0f); 70 | m_axisZ.Set(0.0f, 0.0f, m_len[0]); 71 | 72 | } 73 | } 74 | inline void Draw(const bool axis) const { 75 | // [0;0;0], [X;Y;Z], [X;-Y;Z], [-X;Y;Z], [-X;-Y;Z] 76 | glBegin(GL_LINE_LOOP); 77 | glVertex3fv(m_Xws[0]); 78 | glVertex3fv(m_Xws[1]); 79 | glVertex3fv(m_Xws[3]); 80 | glVertex3fv(m_Xws[4]); 81 | glVertex3fv(m_Xws[2]); 82 | glEnd(); 83 | glBegin(GL_LINES); 84 | glVertex3fv(m_Xws[0]); 85 | glVertex3fv(m_Xws[3]); 86 | glEnd(); 87 | glBegin(GL_LINES); 88 | glVertex3fv(m_Xws[0]); 89 | glVertex3fv(m_Xws[4]); 90 | glEnd(); 91 | glBegin(GL_LINES); 92 | glVertex3fv(m_Xws[1]); 93 | glVertex3fv(m_Xws[2]); 94 | glEnd(); 95 | if (axis) 96 | DrawAxis(); 97 | } 98 | inline void DrawAxis() const { 99 | glColor3ub(255, 0, 0); glBegin(GL_LINES); glVertex3fv(m_Xws[0]); glVertex3fv(m_axisX); glEnd(); 100 | glColor3ub(0, 255, 0); glBegin(GL_LINES); glVertex3fv(m_Xws[0]); glVertex3fv(m_axisY); glEnd(); 101 | glColor3ub(0, 0, 255); glBegin(GL_LINES); glVertex3fv(m_Xws[0]); glVertex3fv(m_axisZ); glEnd(); 102 | } 103 | template 104 | inline void DrawTexture(const TextureGL &tex) const { 105 | tex.Bind(); 106 | glBegin(GL_QUADS); 107 | glTexCoord2i(0, 0); glVertex3fv(m_Xws[4]); 108 | glTexCoord2i(0, tex.GetHeight()); glVertex3fv(m_Xws[3]); 109 | glTexCoord2i(tex.GetWidth(), tex.GetHeight()); glVertex3fv(m_Xws[1]); 110 | glTexCoord2i(tex.GetWidth(), 0); glVertex3fv(m_Xws[2]); 111 | glEnd(); 112 | } 113 | 114 | protected: 115 | 116 | Point3D m_Xc, m_XcScaled, m_Xws[5], m_axisX, m_axisY, m_axisZ; 117 | xp128f m_len; 118 | 119 | }; 120 | 121 | #endif 122 | -------------------------------------------------------------------------------- /Backend/Visualization/TextureGL.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _TEXTURE_GL_H_ 17 | #define _TEXTURE_GL_H_ 18 | 19 | #include 20 | 21 | template 22 | class TextureGL { 23 | 24 | public: 25 | 26 | inline TextureGL() { memset(this, 0, sizeof(TextureGL)); } 27 | inline ~TextureGL() { Delete(); } 28 | inline void Generate(const GLint filterType = GL_NEAREST) { 29 | #ifdef CFG_DEBUG 30 | UT_ASSERT(m_texture == 0); 31 | #endif 32 | glGenTextures(1, &m_texture); 33 | Bind(); 34 | glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP); 35 | glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP); 36 | glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, filterType); 37 | glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, filterType); 38 | glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); 39 | } 40 | inline void Generate(const int w, const int h, const GLint filterType = GL_NEAREST) { 41 | if (m_texture == 0) 42 | Generate(filterType); 43 | else 44 | Bind(); 45 | Resize(w, h); 46 | } 47 | inline void Bind() const { 48 | #ifdef CFG_DEBUG 49 | UT_ASSERT(m_texture != 0); 50 | #endif 51 | GLint boundTexture; 52 | glGetIntegerv(GL_TEXTURE_BINDING_RECTANGLE_ARB, &boundTexture); 53 | if (boundTexture != m_texture) 54 | glBindTexture(GL_TEXTURE_RECTANGLE_ARB, m_texture); 55 | } 56 | inline void Unbind() const { 57 | #ifdef CFG_DEBUG 58 | UT_ASSERT(m_texture != 0); 59 | GLint boundTexture; 60 | glGetIntegerv(GL_TEXTURE_BINDING_RECTANGLE_ARB, &boundTexture); 61 | UT_ASSERT(boundTexture == m_texture); 62 | #endif 63 | glBindTexture(GL_TEXTURE_RECTANGLE_ARB, 0); 64 | } 65 | inline void SetFilterType(const GLint filterType) const { 66 | #ifdef CFG_DEBUG 67 | AssertBound(); 68 | #endif 69 | glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, filterType); 70 | glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, filterType); 71 | } 72 | inline void Delete() { 73 | if (m_texture != 0) { 74 | GLint boundTexture; 75 | glGetIntegerv(GL_TEXTURE_BINDING_RECTANGLE_ARB, &boundTexture); 76 | if (boundTexture == m_texture) 77 | glBindTexture(GL_TEXTURE_RECTANGLE_ARB, 0); 78 | glDeleteTextures(1, &m_texture); 79 | m_texture = 0; 80 | } 81 | } 82 | 83 | inline void Resize(const int w, const int h); 84 | 85 | template inline void UploadFromCPU(const TYPE *pixels) const; 86 | template inline void UploadFromCPU(const TYPE *pixels, 87 | const GLenum components) const; 88 | template inline void UploadFromCPU(const TYPE *pixels, const int w, 89 | const int h) const; 90 | template inline void UploadFromCPU(const TYPE *pixels, const int w, const int h, 91 | const GLenum components) const; 92 | template inline void UploadFromCPU(const TYPE *pixels, const int x, const int y, 93 | const int w, const int h) const; 94 | template inline void UploadFromCPU(const TYPE *pixels, const int x, const int y, 95 | const int w, const int h, const GLenum components) const; 96 | template inline void DownloadToCPU(TYPE *pixels) const; 97 | template inline void DownloadToCPU(TYPE *pixels, const GLenum components) const; 98 | template inline void DownloadToCPU(TYPE *pixels, const int w, const int h) const; 99 | template inline void DownloadToCPU(TYPE *pixels, const int w, const int h, 100 | const GLenum components) const; 101 | template inline void DownloadToCPU(TYPE *pixels, const int x, const int y, 102 | const int w, const int h) const; 103 | template inline void DownloadToCPU(TYPE *pixels, const int x, const int y, 104 | const int w, const int h, const GLenum components) const; 105 | 106 | inline GLenum GetAttachment() const { 107 | GLint attachedTexture; 108 | GLenum i = 0, attachment; 109 | for (i = 0, attachment = GL_COLOR_ATTACHMENT0_EXT; i < 8; ++i, ++attachment) { 110 | glGetFramebufferAttachmentParameterivEXT(GL_FRAMEBUFFER_EXT, attachment, 111 | GL_FRAMEBUFFER_ATTACHMENT_OBJECT_NAME, &attachedTexture); 112 | if (attachedTexture == m_texture) 113 | return attachment; 114 | } 115 | glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, GL_TEXTURE_RECTANGLE_ARB, 116 | m_texture, 0); 117 | return GL_COLOR_ATTACHMENT0_EXT; 118 | } 119 | inline int GetWidth() const { return m_w; } 120 | inline int GetHeight() const { return m_h; } 121 | inline int GetPixelsNumber() const { return m_nPixels; } 122 | inline int GetTotalSize() const { return m_totalSize; } 123 | inline GLuint GetTexture() const { return m_texture; } 124 | inline void ComputeTotalSize(); 125 | 126 | #ifdef CFG_DEBUG 127 | inline void AssertBound() const { 128 | UT_ASSERT(m_texture != 0); 129 | GLint boundTexture; 130 | glGetIntegerv(GL_TEXTURE_BINDING_RECTANGLE_ARB, &boundTexture); 131 | UT_ASSERT(boundTexture == m_texture); 132 | } 133 | inline void AssertComponentsCompitable(const GLenum components) const { 134 | UT_ASSERT(components == GL_RED || components == GL_GREEN || components == GL_BLUE || 135 | components == GL_ALPHA 136 | || CHANNELS >= 2 && components == GL_RG 137 | || CHANNELS >= 3 && components == GL_RGB 138 | || CHANNELS == 4 && components == GL_RGBA); 139 | } 140 | #endif 141 | 142 | protected: 143 | 144 | GLuint m_texture; 145 | int m_w, m_h, m_nPixels, m_totalSize; 146 | 147 | }; 148 | 149 | typedef TextureGL<1> TextureGL1; 150 | typedef TextureGL<2> TextureGL2; 151 | typedef TextureGL<3> TextureGL3; 152 | typedef TextureGL<4> TextureGL4; 153 | 154 | #include "TextureGL.hpp" 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /Backend/Visualization/TextureGL.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | template inline const GLenum GetFormat(); 17 | template<> inline const GLenum GetFormat<1>() { return GL_LUMINANCE; } 18 | template<> inline const GLenum GetFormat<2>() { return GL_RG; } 19 | template<> inline const GLenum GetFormat<3>() { return GL_RGB; } 20 | template<> inline const GLenum GetFormat<4>() { return GL_RGBA; } 21 | 22 | template inline const GLenum GetType(); 23 | template<> inline const GLenum GetType() { return GL_UNSIGNED_BYTE; } 24 | template<> inline const GLenum GetType() { return GL_UNSIGNED_SHORT; } 25 | template<> inline const GLenum GetType() { return GL_FLOAT; } 26 | 27 | template inline void TextureGL::Resize(const int w, const int h) { 28 | if (m_w == w && m_h == h) 29 | return; 30 | m_w = w; 31 | m_h = h; 32 | m_nPixels = w * h; 33 | ComputeTotalSize(); 34 | Bind(); 35 | glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GetFormat(), m_w, m_h, 0, GetFormat(), 36 | GL_FLOAT, NULL); 37 | } 38 | 39 | template template 40 | inline void TextureGL::UploadFromCPU(const TYPE *pixels) const { 41 | #ifdef CFG_DEBUG 42 | AssertBound(); 43 | #endif 44 | glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GetFormat(), m_w, m_h, 0, GetFormat(), 45 | GetType(), pixels); 46 | } 47 | 48 | template template 49 | inline void TextureGL::UploadFromCPU(const TYPE *pixels, const GLenum components) const { 50 | #ifdef CFG_DEBUG 51 | AssertBound(); 52 | AssertComponentsCompitable(components); 53 | #endif 54 | glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GetFormat(), m_w, m_h, 0, components, 55 | GetType(), pixels); 56 | } 57 | 58 | template template 59 | inline void TextureGL::UploadFromCPU(const TYPE *pixels, const int w, const int h) const { 60 | #ifdef CFG_DEBUG 61 | AssertBound(); 62 | #endif 63 | glTexSubImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, 0, 0, w, h, GetFormat(), GetType(), 64 | pixels); 65 | } 66 | 67 | template template 68 | inline void TextureGL::UploadFromCPU(const TYPE *pixels, const int w, const int h, 69 | const GLenum components) const { 70 | #ifdef CFG_DEBUG 71 | AssertBound(); 72 | AssertComponentsCompitable(components); 73 | #endif 74 | glTexSubImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, 0, 0, w, h, components, GetType(), pixels); 75 | } 76 | 77 | template template 78 | inline void TextureGL::UploadFromCPU(const TYPE *pixels, const int x, const int y, 79 | const int w, const int h) const { 80 | #ifdef CFG_DEBUG 81 | AssertBound(); 82 | #endif 83 | glTexSubImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, x, y, w, h, GetFormat(), GetType(), 84 | pixels); 85 | } 86 | 87 | template template 88 | inline void TextureGL::UploadFromCPU(const TYPE *pixels, const int x, const int y, 89 | const int w, const int h, const GLenum components) const { 90 | #ifdef CFG_DEBUG 91 | AssertBound(); 92 | AssertComponentsCompitable(components); 93 | #endif 94 | glTexSubImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, x, y, w, h, components, GetType(), pixels); 95 | } 96 | 97 | template template 98 | inline void TextureGL::DownloadToCPU(TYPE *pixels) const { 99 | glReadBuffer(GetAttachment()); 100 | glReadPixels(0, 0, m_w, m_h, GetFormat(), GetType(), pixels); 101 | } 102 | 103 | template template 104 | inline void TextureGL::DownloadToCPU(TYPE *pixels, const GLenum components) const { 105 | #ifdef CFG_DEBUG 106 | AssertComponentsCompitable(components); 107 | #endif 108 | glReadBuffer(GetAttachment()); 109 | glReadPixels(0, 0, m_w, m_h, components, GetType(), pixels); 110 | } 111 | 112 | template template 113 | inline void TextureGL::DownloadToCPU(TYPE *pixels, const int w, const int h) const { 114 | glReadBuffer(GetAttachment()); 115 | glReadPixels(0, 0, w, h, GetFormat(), GetType(), pixels); 116 | } 117 | 118 | template template 119 | inline void TextureGL::DownloadToCPU(TYPE *pixels, const int w, const int h, 120 | const GLenum components) const { 121 | #ifdef CFG_DEBUG 122 | AssertComponentsCompitable(components); 123 | #endif 124 | glReadBuffer(GetAttachment()); 125 | glReadPixels(0, 0, w, h, components, GetType(), pixels); 126 | } 127 | 128 | template template 129 | inline void TextureGL::DownloadToCPU(TYPE *pixels, const int x, const int y, const int w, 130 | const int h) const { 131 | glReadBuffer(GetAttachment()); 132 | glReadPixels(x, y, w, h, GetFormat(), GetType(), pixels); 133 | } 134 | 135 | template template 136 | inline void TextureGL::DownloadToCPU(TYPE *pixels, const int x, const int y, const int w, 137 | const int h, const GLenum components) const { 138 | #ifdef CFG_DEBUG 139 | AssertComponentsCompitable(components); 140 | #endif 141 | glReadBuffer(GetAttachment()); 142 | glReadPixels(x, y, w, h, components, GetType(), pixels); 143 | } 144 | template<> inline void TextureGL<1>::ComputeTotalSize() { m_totalSize = m_nPixels; } 145 | template<> inline void TextureGL<2>::ComputeTotalSize() { m_totalSize = (m_nPixels << 1); } 146 | template<> inline void TextureGL<3>::ComputeTotalSize() { m_totalSize = (m_nPixels << 1) + m_nPixels; } 147 | template<> inline void TextureGL<4>::ComputeTotalSize() { m_totalSize = (m_nPixels << 2); } 148 | -------------------------------------------------------------------------------- /Backend/Visualization/Viewer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _VIEWER_H_ 17 | #define _VIEWER_H_ 18 | 19 | #include "EventHandler.h" 20 | #include "Point.h" 21 | #include "TextureGL.h" 22 | #include "ViewerIBAKey.hpp" 23 | #include 24 | #include 25 | #include 26 | 27 | #define VW_STRING_FONT GLUT_BITMAP_HELVETICA_18 28 | #define VW_STRING_HEIGHT 18 29 | #define VW_STRING_BORDER_X 15 30 | #define VW_STRING_BORDER_Y 15 31 | #define VW_STRING_GAP_Y 10 32 | 33 | class Viewer { 34 | 35 | public: 36 | 37 | inline Viewer() : m_pWnd(NULL) { 38 | #ifdef __linux__ 39 | m_ctrlDown = false; 40 | #endif 41 | } 42 | inline ~Viewer() { if (m_pWnd) delete m_pWnd; } 43 | 44 | virtual void Initialize(const int w, const int h); 45 | virtual bool Run(const bool visualize = true); 46 | 47 | protected: 48 | 49 | int GetCharacterWidth(void *front, const char c); 50 | int GetStringWidth(void *font, const char *str); 51 | int GetStringHeight(void *font); 52 | int GetStringYTopLeft(const int row, void *font); 53 | void DrawString(void *font, const char *str); 54 | void DrawStringCurrent(const char *format, ...); 55 | void DrawStringCurrent(void *font, const char *format, ...); 56 | void DrawStringAt(const int x, const int y, void *font, const char *format, ...); 57 | void DrawStringTopLeft(const char *format, ...); 58 | void DrawStringTopLeft(const int row, void *font, const char *format, ...); 59 | void DrawStringBottomLeft(const char *format, ...); 60 | //void DrawStringBottomLeft(const int row, const char *format, ...); 61 | 62 | template inline void DrawTexture(const TextureGL &tex) { 63 | tex.Bind(); 64 | glBegin(GL_QUADS); 65 | glTexCoord2i(0, 0); glVertex2i(0, 0); 66 | glTexCoord2i(0, tex.GetHeight()); glVertex2i(0, m_pWnd->size().y); 67 | glTexCoord2i(tex.GetWidth(), tex.GetHeight()); glVertex2i(m_pWnd->size().x, m_pWnd->size().y); 68 | glTexCoord2i(tex.GetWidth(), 0); glVertex2i(m_pWnd->size().x, 0); 69 | glEnd(); 70 | } 71 | template inline void DrawTexture(const TextureGL &tex, const Point2D &x, 72 | const LA::Vector2f size) { 73 | tex.Bind(); 74 | glBegin(GL_QUADS); 75 | glTexCoord2i(0, 0); glVertex2f(x.x() - size.x(), x.y() - size.y()); 76 | glTexCoord2i(0, tex.GetHeight()); glVertex2f(x.x() - size.x(), x.y() + size.y()); 77 | glTexCoord2i(tex.GetWidth(), tex.GetHeight()); glVertex2i(x.x() + size.x(), x.y() + size.y()); 78 | glTexCoord2i(tex.GetWidth(), 0); glVertex2i(x.x() + size.x(), x.y() - size.y()); 79 | glEnd(); 80 | } 81 | template inline void DrawTexture(const TextureGL &tex, 82 | const Point3D *Xs) { 83 | tex.Bind(); 84 | glBegin(GL_QUADS); 85 | glTexCoord2i(0, 0); glVertex3fv(Xs[0]); 86 | glTexCoord2i(0, tex.GetHeight()); glVertex3fv(Xs[1]); 87 | glTexCoord2i(tex.GetWidth(), tex.GetHeight()); glVertex3fv(Xs[2]); 88 | glTexCoord2i(tex.GetWidth(), 0); glVertex3fv(Xs[3]); 89 | glEnd(); 90 | } 91 | 92 | static void DrawBox(const Point2D &x1, const Point2D &x2); 93 | static void DrawBox(const float x, const float y, const float size); 94 | static void DrawBox(const float x, const float y, const LA::Vector2f &size); 95 | 96 | static void DrawQuad(const Point2D &x1, const Point2D &x2); 97 | static void DrawQuad(const float x, const float y, const LA::Vector2f &size); 98 | 99 | static void DrawCross(const Point2D &x, const LA::Vector2f &size); 100 | static void DrawCross(const int x, const int y, const int size); 101 | 102 | static void DrawWireCircle(const Point2D &x, const float r); 103 | static void DrawSolidCircle(const Point2D &x, const float r); 104 | static void DrawCovariance(const Point2D &x, const Point2DCovariance &S, const float X2); 105 | static void PrepareCircle(); 106 | 107 | protected: 108 | 109 | virtual void Draw(const bool swapBuffers = true); 110 | virtual void SwapBuffers(); 111 | 112 | virtual bool OnKeyDown(const int key); 113 | virtual bool OnKeyUp(const int key); 114 | virtual void OnMouseMove(const CVD::ImageRef &where); 115 | virtual void OnMouseDown(const CVD::ImageRef &where, const int button); 116 | virtual void OnMouseUp(const CVD::ImageRef &where, const int button); 117 | virtual bool OnMouseDraging(const CVD::ImageRef &from, const CVD::ImageRef &to, const int button); 118 | virtual bool OnMouseDoubleClicked(const CVD::ImageRef &where, const int button); 119 | virtual void OnResize(const CVD::ImageRef &size); 120 | 121 | protected: 122 | 123 | CVD::GLWindow *m_pWnd; 124 | EventHandler m_handler; 125 | #ifdef __linux__ 126 | bool m_ctrlDown; 127 | #endif 128 | 129 | private: 130 | 131 | static std::vector g_xsCircle; 132 | }; 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /Backend/Visualization/ViewerIBAKey.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef _VIEWER_IBA_KEY_H_ 17 | #define _VIEWER_IBA_KEY_H_ 18 | 19 | #ifdef __linux__ 20 | #define VW_KEY_CTRL_KEY_VALUE 65507 // CTRL 21 | #define VW_KEY_XD_DRAW_VIEW_TYPE 65289 // Tab 22 | #else 23 | #define VW_KEY_XD_DRAW_VIEW_TYPE 9 // Tab 24 | #endif 25 | 26 | #define VW_KEY_XD_PAUSE 19 // Ctrl + s 27 | #define VW_KEY_XD_STEP 1 // Ctrl + a 28 | #define VW_KEY_XD_SAVE 's' 29 | #define VW_KEY_XD_SCREEN 'S' 30 | #define VW_KEY_XD_ACTIVATE_NEXT_FRAME 'f' 31 | #define VW_KEY_XD_ACTIVATE_LAST_FRAME 'd' 32 | #define VW_KEY_XD_ACTIVATE_KEY_FRAME 'r' 33 | #define VW_KEY_XD_ACTIVATE_LOCAL_FRAME 't' 34 | #define VW_KEY_XD_ACTIVATE_CURRENT_FRAME 'y' 35 | #define VW_KEY_XD_DRAW_CAMERA_TYPE_GROUND_TRUTH 7 // Ctrl + g 36 | #define VW_KEY_XD_DRAW_CAMERA_TYPE_NEXT 'V' 37 | #define VW_KEY_XD_DRAW_CAMERA_TYPE_LAST 'C' 38 | #define VW_KEY_XD_DRAW_DEPTH_TYPE_NEXT 'N' 39 | #define VW_KEY_XD_DRAW_DEPTH_TYPE_LAST 'B' 40 | #define VW_KEY_XD_DRAW_STRING 12 // Ctrl + l 41 | #define VW_KEY_XD_DRAW_TIME_LINE_NEXT 'l' 42 | #define VW_KEY_XD_DRAW_TIME_LINE_LAST 'k' 43 | #define VW_KEY_XD_DRAW_TIME_LINE_BRIGHTNESS_INCREASE_1 ']' 44 | #define VW_KEY_XD_DRAW_TIME_LINE_BRIGHTNESS_DECREASE_1 '[' 45 | #define VW_KEY_XD_DRAW_TIME_LINE_BRIGHTNESS_INCREASE_2 125 // Ctrl + ] 46 | #define VW_KEY_XD_DRAW_TIME_LINE_BRIGHTNESS_DECREASE_2 123 // Ctrl + [ 47 | #define VM_KEY_XD_PRINT_CALIBRATION 3 // Ctrl + c 48 | #define VW_KEY_XD_INPUT_ACTIVE_FEATURE 24 // Ctrl + x 49 | 50 | #define VW_KEY_XD_DRAW_AXIS 'a' 51 | #define VW_KEY_XD_DRAW_AXIS_LENGTH_INCREASE '.' 52 | #define VW_KEY_XD_DRAW_AXIS_LENGTH_DECREASE ',' 53 | #define VW_KEY_XD_DRAW_DEPTH_PLANE 'n' 54 | #define VW_KEY_XD_DRAW_DEPTH_VARIANCE_INCREASE '+' 55 | #define VW_KEY_XD_DRAW_DEPTH_VARIANCE_DECREASE '-' 56 | 57 | #ifdef __linux__ 58 | #define VW_KEY_XD_DRAW_COVARIANCE_PROBABILITY_INCREASE 65361 // Left 59 | #define VW_KEY_XD_DRAW_COVARIANCE_PROBABILITY_DECREASE 65363 // Right 60 | #define VW_KEY_XD_DRAW_COVARIANCE_SCALE_INCREASE 65362 // Up 61 | #define VW_KEY_XD_DRAW_COVARIANCE_SCALE_DECREASE 65364 // Down 62 | #else 63 | #define VW_KEY_XD_DRAW_COVARIANCE_PROBABILITY_INCREASE 37 // Left 64 | #define VW_KEY_XD_DRAW_COVARIANCE_PROBABILITY_DECREASE 39 // Right 65 | #define VW_KEY_XD_DRAW_COVARIANCE_SCALE_INCREASE 38 // Up 66 | #define VW_KEY_XD_DRAW_COVARIANCE_SCALE_DECREASE 40 // Down 67 | #endif 68 | 69 | #define VW_KEY_PROFILE_ACTIVATE ' ' 70 | #define VM_KEY_PROFILE_LEVEL_1_NEXT 'R' 71 | #define VM_KEY_PROFILE_LEVEL_1_LAST 'E' 72 | #define VM_KEY_PROFILE_LEVEL_2_NEXT 'F' 73 | #define VM_KEY_PROFILE_LEVEL_2_LAST 'D' 74 | #define VM_KEY_PROFILE_LEVEL_3_NEXT 6 // Ctrl + f 75 | #define VM_KEY_PROFILE_LEVEL_3_LAST 4 // Ctrl + d 76 | 77 | #define VW_KEY_2D_DRAW_WARP_IMAGE 23 // Ctrl + w 78 | #define VW_KEY_2D_DRAW_FEATURE_TYPE 'x' 79 | #define VW_KEY_2D_DRAW_PROJECTION_TYPE 'j' 80 | #define VW_KEY_2D_DRAW_PROJECTION_RANGE 10 // Ctrl + j 81 | #define VW_KEY_2D_DRAW_ERROR_TYPE 'e' 82 | 83 | #define VW_KEY_3D_DRAW_MOTION_TYPE 'm' 84 | #define VW_KEY_3D_DRAW_STRUCTURE_TYPE 'M' 85 | #define VW_KEY_3D_DRAW_DEPTH_COMPARE_TYPE 'G' 86 | #define VW_KEY_3D_DRAW_CAMERA_SIZE_INCREASE '.' 87 | #define VW_KEY_3D_DRAW_CAMERA_SIZE_DECREASE ',' 88 | #define VW_KEY_3D_DRAW_CAMERA_VELOCITY 'v' 89 | #define VW_KEY_3D_DRAW_CAMERA_TEXTURE 'i' 90 | #define VW_KEY_3D_DRAW_CAMERA_GROUND_TRUTH 'g' 91 | #define VW_KEY_3D_DRAW_ERROR_TYPE 'e' 92 | #define VW_KEY_3D_DRAW_BACKGROUND_COLOR 11 // Ctrl + k 93 | #define VW_KEY_3D_RESET_VIEW_POINT_ACTIVE 18 // Ctrl + r 94 | #define VW_KEY_3D_RESET_VIEW_POINT_VIRTUAL 'R' 95 | #endif 96 | -------------------------------------------------------------------------------- /Backend/Visualization/stdafx.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | // stdafx.cpp : source file that includes just the standard includes 17 | // Visualization.pch will be the pre-compiled header 18 | // stdafx.obj will contain the pre-compiled type information 19 | 20 | #include "stdafx.h" 21 | 22 | // TODO: reference any additional headers you need in STDAFX.H 23 | // and not in this file 24 | -------------------------------------------------------------------------------- /Backend/Visualization/stdafx.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | // stdafx.h : include file for standard system include files, 17 | // or project specific include files that are used frequently, but 18 | // are changed infrequently 19 | // 20 | 21 | #pragma once 22 | 23 | #include "targetver.h" 24 | 25 | #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers 26 | 27 | 28 | 29 | // TODO: reference additional headers your program requires here 30 | #include "IBA_config.h" 31 | -------------------------------------------------------------------------------- /Backend/Visualization/targetver.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #pragma once 17 | 18 | // Including SDKDDKVer.h defines the highest available Windows platform. 19 | 20 | // If you wish to build your application for a previous Windows platform, include WinSDKVer.h and 21 | // set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h. 22 | 23 | #ifdef WIN32 24 | #include 25 | #endif 26 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | 3 | # Cross Compile 4 | if ("$ENV{CROSS_COMPILE_FLAG}" STREQUAL "XU4") 5 | message("Cross Compile For XU4") 6 | SET(CMAKE_SYSTEM_NAME Linux) 7 | SET(CMAKE_SYSTEM_PROCESSOR armv7l) 8 | SET(CMAKE_C_COMPILER /usr/bin/arm-linux-gnueabihf-gcc) 9 | SET(CMAKE_CXX_COMPILER /usr/bin/arm-linux-gnueabihf-g++) 10 | SET(CMAKE_BUILD_TYPE Release) 11 | endif() 12 | 13 | # If on OSX, force cmake to use gcc-6/5 instead of the built-in gcc (AppleClang) 14 | # The compiler overriden has to happen before calling "project" 15 | if (APPLE) 16 | if (EXISTS /usr/local/bin/g++-6) 17 | message("Override CXX compiler to /usr/local/bin/g++-6!") 18 | set(CMAKE_C_COMPILER /usr/local/bin/gcc-6) 19 | set(CMAKE_CXX_COMPILER /usr/local/bin/g++-6) 20 | elseif (EXISTS /usr/local/bin/g++-5) 21 | message("Override CXX compiler to /usr/local/bin/g++-5!") 22 | set(CMAKE_C_COMPILER /usr/local/bin/gcc-5) 23 | set(CMAKE_CXX_COMPILER /usr/local/bin/g++-5) 24 | elseif (EXISTS /usr/local/bin/g++-7) 25 | message("Override CXX compiler to /usr/local/bin/g++-7!") 26 | set(CMAKE_C_COMPILER /usr/local/bin/gcc-7) 27 | set(CMAKE_CXX_COMPILER /usr/local/bin/g++-7) 28 | else () 29 | message(FATAL_ERROR "Cannot find GNU gcc on Mac!") 30 | endif() 31 | set(CMAKE_MACOSX_RPATH 0) 32 | endif() 33 | 34 | project(ICE_BA) 35 | 36 | # Configure cxx flags 37 | if(CYGWIN) 38 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -msse2 -Wno-unused-result") 39 | else() 40 | # NEON or SSE 41 | if((CMAKE_SYSTEM_PROCESSOR MATCHES "^arm")) 42 | set(CMAKE_CXX_FLAGS "-D__ARM_NEON__ -DENABLE_NEON -mfloat-abi=hard -mfpu=neon -Ofast -std=c++11 -Wno-unused-result") 43 | message("Enabling neon for armv7 " ${CMAKE_CXX_FLAGS}) 44 | elseif (CMAKE_SYSTEM_PROCESSOR MATCHES "^aarch64") 45 | set(CMAKE_CXX_FLAGS "-D__ARM_NEON__ -DENABLE_NEON -Ofast -std=c++11 -Wno-unused-result") 46 | message("Detected armv8 " ${CMAKE_CXX_FLAGS}) 47 | else() 48 | # SSE4.1 seems to be not supported by GCC 4.8.4 49 | # TODO(mingyu): See if we still need sse4.1 50 | message(STATUS "Enable SSE2 and SSE4.1") 51 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -msse2 -msse4.1 -Wno-unused-result") 52 | # Export compile_commands.json 53 | set(CMAKE_EXPORT_COMPILE_COMMANDS 1) 54 | endif() 55 | endif() 56 | 57 | # Suppress warnings for deprecated declarations for GCC 5 and above 58 | if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0) 59 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations") 60 | endif() 61 | 62 | # Debug / Release / RelWithDebInfo 63 | set(CMAKE_BUILD_TYPE Release) 64 | 65 | # Thirdparty dependencies 66 | # If CFG_VIEW is turned on, need to install glut (freeglut3-dev) and glew (libglew-dev) 67 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake") 68 | 69 | find_package(CVD) 70 | find_package(Boost COMPONENTS thread system filesystem REQUIRED) 71 | find_package(Eigen REQUIRED) 72 | if (CVD_FOUND) 73 | add_definitions(-DIBA_WITH_CVD) 74 | find_package(OpenGL REQUIRED) 75 | find_package(GLUT REQUIRED) 76 | find_package(GLEW REQUIRED) 77 | if (UNIX) 78 | find_package(X11 REQUIRED) 79 | endif() 80 | endif() 81 | 82 | # ========================= 83 | # Frontend 84 | # ========================= 85 | add_subdirectory(Frontend) 86 | 87 | # ========================= 88 | # Backend 89 | # ========================= 90 | add_subdirectory(Backend) 91 | 92 | # ========================= 93 | # App 94 | # ========================= 95 | add_subdirectory(App) 96 | -------------------------------------------------------------------------------- /Frontend/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(euroc_tracking) 3 | 4 | list(FIND OpenCV_LIBS "opencv_viz" find_index) 5 | if (find_index GREATER -1) 6 | message(STATUS "Found opencv_viz") 7 | add_definitions(-DHAS_OPENCV_VIZ) 8 | endif() 9 | 10 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../cmake) 11 | 12 | find_package(Glog REQUIRED) 13 | find_package(GFlags REQUIRED) 14 | find_package(OpenCV REQUIRED) 15 | find_package(Yaml REQUIRED) 16 | 17 | #include_directories (${GLOG_INCLUDE_DIR}) 18 | 19 | set(HEADERS 20 | cameras/CameraBase.hpp 21 | cameras/DistortionBase.hpp 22 | cameras/PinholeCamera.hpp 23 | cameras/RadialTangentialDistortion.hpp 24 | cameras/RadialTangentialDistortion8.hpp 25 | cameras/implementation/CameraBase.hpp 26 | cameras/implementation/PinholeCamera.hpp 27 | cameras/implementation/RadialTangentialDistortion8.hpp 28 | cameras/implementation/RadialTangentialDistortion.hpp 29 | basic_datatype.h 30 | feature_utils.h 31 | iba_helper.h 32 | ORBextractor.h 33 | param.h 34 | patch_score.h 35 | plotting_utils.h 36 | pose_viewer.h 37 | rotation.h 38 | shared_queue.h 39 | xppyramid.hpp 40 | image_utils.h 41 | timer.h 42 | xp_quaternion.h 43 | ) 44 | 45 | set(SOURCES 46 | feature_utils.cc 47 | feature_utils_direct_matcher.cc 48 | feature_utils_feature_track.cc 49 | feature_utils_warp.cc 50 | feature_utils_uniformity.cc 51 | feature_utils_align.cc 52 | iba_helper.cc 53 | ORBextractor.cc 54 | plotting_utils.cpp 55 | pose_viewer.cc 56 | timer.cc 57 | rotation.cc 58 | param.cc 59 | xppyramid.cpp 60 | image_utils.cc 61 | CameraBase.cpp 62 | xp_quaternion.cc 63 | ) 64 | 65 | add_library(OF SHARED 66 | ${SOURCES} 67 | ) 68 | target_include_directories(OF PUBLIC 69 | ${GFLAGS_INCLUDE_DIR} 70 | ${OpenCV_INCLUDE_DIR} 71 | ${YAMLCPP_INCLUDE_DIR} 72 | ${CMAKE_CURRENT_SOURCE_DIR} 73 | ${CMAKE_CURRENT_SOURCE_DIR}/cameras 74 | ) 75 | target_link_libraries(OF 76 | ${GFLAGS_LIBRARIES} 77 | ${OpenCV_LIBS} 78 | ${YAMLCPP_LIBRARIES} 79 | IBA 80 | glog 81 | ) 82 | -------------------------------------------------------------------------------- /Frontend/CameraBase.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 2, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file CameraBase.cpp 35 | * @brief Source file for the CameraBase class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include "cameras/CameraBase.hpp" 40 | 41 | /// \brief vio Main namespace of this package. 42 | namespace vio { 43 | /// \brief cameras Namespace for camera-related functionality. 44 | namespace cameras { 45 | 46 | // Creates a random (uniform distribution) image point. 47 | Eigen::Vector2d CameraBase::createRandomImagePoint() const { 48 | // Uniform random sample in image coordinates. 49 | // Add safety boundary for later inaccurate backprojection 50 | Eigen::Vector2d outPoint = Eigen::Vector2d::Random(); 51 | outPoint += Eigen::Vector2d::Ones(); 52 | outPoint *= 0.5; 53 | outPoint[0] *= static_cast(imageWidth_ - 0.022); 54 | outPoint[0] += 0.011; 55 | outPoint[1] *= static_cast(imageHeight_ - 0.022); 56 | outPoint[1] += 0.011; 57 | return outPoint; 58 | } 59 | 60 | // Creates a random (uniform distribution) image point. 61 | Eigen::Vector2f CameraBase::createRandomImagePoint2f() const { 62 | // Uniform random sample in image coordinates. 63 | // Add safety boundary for later inaccurate backprojection 64 | Eigen::Vector2f outPoint = Eigen::Vector2f::Random(); 65 | outPoint += Eigen::Vector2f::Ones(); 66 | outPoint *= 0.5; 67 | outPoint[0] *= static_cast(imageWidth_ - 0.022); 68 | outPoint[0] += 0.011; 69 | outPoint[1] *= static_cast(imageHeight_ - 0.022); 70 | outPoint[1] += 0.011; 71 | return outPoint; 72 | } 73 | 74 | 75 | Eigen::Vector2d CameraBase::createRandomImagePointAtRadius(double radius) const { 76 | Eigen::Vector2d outPoint = Eigen::Vector2d::Random(); 77 | outPoint.normalize(); 78 | outPoint *= radius; 79 | outPoint[0] += imageWidth_ / 2; 80 | outPoint[1] += imageHeight_ / 2; 81 | return outPoint; 82 | } 83 | 84 | // Creates a random visible point in Euclidean coordinates. 85 | Eigen::Vector3d CameraBase::createRandomVisiblePoint(double minDist, 86 | double maxDist) const { 87 | // random image point first: 88 | Eigen::Vector2d imagePoint = createRandomImagePoint(); 89 | // now sample random depth: 90 | Eigen::Vector2d depth = Eigen::Vector2d::Random(); 91 | Eigen::Vector3d ray; 92 | while (!backProject(imagePoint, &ray)) { 93 | // In case backProject return false (E.g., RadialTangentialDistortion8) 94 | imagePoint = createRandomImagePoint(); 95 | } 96 | ray.normalize(); 97 | ray *= (0.5 * (maxDist - minDist) * (depth[0] + 1.0) + minDist); // rescale and offset 98 | return ray; 99 | } 100 | 101 | // Creates a random visible point in homogeneous coordinates. 102 | Eigen::Vector4d CameraBase::createRandomVisibleHomogeneousPoint( 103 | double minDist, double maxDist) const { 104 | Eigen::Vector3d point = createRandomVisiblePoint(minDist, maxDist); 105 | return Eigen::Vector4d(point[0], point[1], point[2], 1.0); 106 | } 107 | 108 | } // namespace cameras 109 | } // namespace vio 110 | -------------------------------------------------------------------------------- /Frontend/basic_datatype.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef XP_INCLUDE_XP_DATA_ATOM_BASIC_DATATYPE_H_ 17 | #define XP_INCLUDE_XP_DATA_ATOM_BASIC_DATATYPE_H_ 18 | 19 | #include 20 | #include 21 | 22 | namespace XP { 23 | 24 | class ImuData { 25 | public: 26 | Eigen::Vector3f accel; 27 | Eigen::Vector3f ang_v; 28 | float time_stamp; 29 | public: 30 | #ifndef __ANDROID__ 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | #endif 33 | }; 34 | 35 | } // namespace XP 36 | #endif // XP_INCLUDE_XP_DATA_ATOM_BASIC_DATATYPE_H_ 37 | -------------------------------------------------------------------------------- /Frontend/cameras/implementation/CameraBase.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 3, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file implementation/CameraBase.hpp 35 | * @brief Header implementation file for the CameraBase class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief vio Main namespace of this package. 42 | namespace vio { 43 | /// \brief cameras Namespace for camera-related functionality. 44 | namespace cameras { 45 | 46 | // Set the mask. It must be the same size as the image and 47 | bool CameraBase::setMask(const cv::Mat & mask) { 48 | // check type 49 | if (mask.type() != CV_8UC1) { 50 | return false; 51 | } 52 | // check size 53 | if (mask.rows != imageHeight_) { 54 | return false; 55 | } 56 | if (mask.cols != imageWidth_) { 57 | return false; 58 | } 59 | mask_ = mask; 60 | return true; 61 | } 62 | 63 | /// Was a nonzero mask set? 64 | bool CameraBase::removeMask() { 65 | mask_.resize(0); 66 | return true; 67 | } 68 | 69 | // Was a nonzero mask set? 70 | bool CameraBase::hasMask() const { 71 | return (mask_.data); 72 | } 73 | 74 | // Get the mask. 75 | const cv::Mat & CameraBase::mask() const { 76 | return mask_; 77 | } 78 | 79 | bool CameraBase::isMasked(const Eigen::Vector2d& imagePoint) const { 80 | if (!isInImage(imagePoint)) { 81 | return true; 82 | } 83 | if (!hasMask()) { 84 | return false; 85 | } 86 | return mask_.at(static_cast(imagePoint[1]), static_cast(imagePoint[0])); 87 | } 88 | bool CameraBase::isMasked(const Eigen::Vector2f& imagePoint) const { 89 | if (!isInImage(imagePoint)) { 90 | return true; 91 | } 92 | if (!hasMask()) { 93 | return false; 94 | } 95 | return mask_.at(static_cast(imagePoint[1]), static_cast(imagePoint[0])); 96 | } 97 | 98 | // Check if the keypoint is in the image. 99 | bool CameraBase::isInImage(const Eigen::Vector2d& imagePoint) const { 100 | if (imagePoint[0] < 0.0 || imagePoint[1] < 0.0) { 101 | return false; 102 | } 103 | if (imagePoint[0] >= imageWidth_ || imagePoint[1] >= imageHeight_) { 104 | return false; 105 | } 106 | return true; 107 | } 108 | bool CameraBase::isInImage(const Eigen::Vector2f& imagePoint) const { 109 | if (imagePoint[0] < 0.0 || imagePoint[1] < 0.0) { 110 | return false; 111 | } 112 | if (imagePoint[0] >= imageWidth_ || imagePoint[1] >= imageHeight_) { 113 | return false; 114 | } 115 | return true; 116 | } 117 | #ifdef __ARM_NEON__ 118 | /****************************************************************************** 119 | * ipts -> layout: u0, u1, u2, u3, v0, v1, v2, v3 120 | * status -> 0 for in image range, or 0xffffffff for out of range 121 | */ 122 | void CameraBase::isInImage(const float* ipts, unsigned int status[4]) const { 123 | uint32x4_t vres0, vres1; 124 | float32x4_t img_us = vld1q_f32(ipts); 125 | float32x4_t img_vs = vld1q_f32(ipts + 4); 126 | vres0 = vcltq_f32(img_us, vmovq_n_f32(0.f)); 127 | vres1 = vcltq_f32(img_vs, vmovq_n_f32(0.f)); 128 | vres0 = vorrq_u32(vcgeq_f32(img_us, vmovq_n_f32(imageWidth_)), vres0); 129 | vres1 = vorrq_u32(vcgeq_f32(img_vs, vmovq_n_f32(imageHeight_)), vres1); 130 | vres0 = vorrq_u32(vres0, vres1); 131 | vst1q_u32(status, vres0); 132 | } 133 | #endif 134 | 135 | } // namespace cameras 136 | } // namespace vio 137 | 138 | -------------------------------------------------------------------------------- /Frontend/feature_utils_warp.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #include "feature_utils.h" 17 | #include 18 | 19 | namespace XP { 20 | namespace warp { 21 | 22 | using Eigen::Vector2f; 23 | using Eigen::Vector3f; 24 | using Eigen::Matrix2f; 25 | using Eigen::Matrix3f; 26 | 27 | // Compute affine warp matrix A_ref_cur 28 | // The warping matrix is warping the ref patch (at level_ref) to the current frame (at pyr 0) 29 | bool getWarpMatrixAffine(const vio::cameras::CameraBase& cam_ref, 30 | const vio::cameras::CameraBase& cam_cur, 31 | const Vector2f& px_ref, // distorted pixel at pyr0 32 | const Vector3f& f_ref, // undist ray in unit plane 33 | const float depth_ref, 34 | const Matrix3f& R_cur_ref, 35 | const Vector3f& t_cur_ref, 36 | const int level_ref, 37 | Eigen::Matrix2f* A_cur_ref) { 38 | CHECK_NOTNULL(A_cur_ref); 39 | // TODO(mingyu): tune the *d_unit* size in pixel for different 1st order approximation 40 | const int halfpatch_size = 5; 41 | const Vector3f xyz_ref(f_ref * depth_ref); 42 | float d_unit = halfpatch_size * (1 << level_ref); 43 | Vector2f du_ref(px_ref + Vector2f(d_unit, 0)); 44 | Vector2f dv_ref(px_ref + Vector2f(0, d_unit)); 45 | Vector3f xyz_du_ref, xyz_dv_ref; 46 | if (cam_ref.backProject(du_ref, &xyz_du_ref) && cam_ref.backProject(dv_ref, &xyz_dv_ref)) { 47 | // Make sure the back project succeed for both du_ref & dv_ref 48 | xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2]; 49 | xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2]; 50 | Vector2f px_cur, du_cur, dv_cur; 51 | if (vio::cameras::CameraBase::ProjectionStatus::Successful == 52 | cam_cur.project(R_cur_ref * xyz_ref + t_cur_ref, &px_cur) && 53 | vio::cameras::CameraBase::ProjectionStatus::Successful == 54 | cam_cur.project(R_cur_ref * xyz_du_ref + t_cur_ref, &du_cur) && 55 | vio::cameras::CameraBase::ProjectionStatus::Successful == 56 | cam_cur.project(R_cur_ref * xyz_dv_ref + t_cur_ref, &dv_cur)) { 57 | A_cur_ref->col(0) = (du_cur - px_cur) / halfpatch_size; 58 | A_cur_ref->col(1) = (dv_cur - px_cur) / halfpatch_size; 59 | return true; 60 | } 61 | } 62 | A_cur_ref->setIdentity(); // No warping 63 | return false; 64 | } 65 | 66 | // Compute patch level in other image (based on pyramid level 0) 67 | int getBestSearchLevel(const Eigen::Matrix2f& A_cur_ref, 68 | const int max_level) { 69 | int search_level = 0; 70 | float D = A_cur_ref.determinant(); 71 | while (D > 3.f && search_level < max_level) { 72 | ++search_level; 73 | D *= 0.25; 74 | } 75 | return search_level; 76 | } 77 | 78 | namespace { 79 | // Return value between 0 and 255 80 | // [NOTE] Does not check whether the x/y is within the border 81 | inline float interpolateMat_8u(const cv::Mat& mat, float u, float v) { 82 | CHECK_EQ(mat.type(), CV_8U); 83 | int x = floor(u); 84 | int y = floor(v); 85 | float subpix_x = u - x; 86 | float subpix_y = v - y; 87 | 88 | float w00 = (1.0f - subpix_x) * (1.0f - subpix_y); 89 | float w01 = (1.0f - subpix_x) * subpix_y; 90 | float w10 = subpix_x * (1.0f - subpix_y); 91 | float w11 = 1.0f - w00 - w01 - w10; 92 | 93 | const int stride = mat.step.p[0]; 94 | uint8_t* ptr = mat.data + y * stride + x; 95 | return w00 * ptr[0] + w01 * ptr[stride] + w10 * ptr[1] + w11 * ptr[stride+1]; 96 | } 97 | } // namespace 98 | 99 | // Compute a squared patch that is *warperd* from img_ref with A_cur_ref. 100 | bool warpAffine(const Eigen::Matrix2f& A_cur_ref, 101 | const cv::Mat& img_ref, // at pyramid level_ref 102 | const Eigen::Vector2f& px_ref, // at pyramid 0 103 | const int level_ref, 104 | const int level_cur, 105 | const int halfpatch_size, 106 | uint8_t* patch) { 107 | const int patch_size = halfpatch_size * 2; 108 | const Matrix2f A_ref_cur = A_cur_ref.inverse(); 109 | if (std::isnan(A_ref_cur(0, 0))) { 110 | // TODO(mingyu): Use looser criteria for invalid affine warp? 111 | // I suspect A_ref_cur can barely hit NaN. 112 | LOG(ERROR) << "Invalid affine warp matrix (NaN)"; 113 | return false; 114 | } 115 | 116 | // px_ref is at pyr0, img_ref is at level_ref pyr already 117 | CHECK_NOTNULL(patch); 118 | uint8_t* patch_ptr = patch; 119 | const Vector2f px_ref_pyr = px_ref / (1<< level_ref); // pixel at pyramid level_ref 120 | for (int y = 0; y < patch_size; ++y) { 121 | for (int x = 0; x < patch_size; ++x, ++patch_ptr) { 122 | Vector2f px_patch(x - halfpatch_size, y - halfpatch_size); 123 | px_patch *= (1 << level_cur); 124 | const Vector2f px(A_ref_cur * px_patch + px_ref_pyr); // pixel at pyramid level_ref 125 | if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || px[1] >= img_ref.rows - 1) { 126 | *patch_ptr = 0; 127 | } else { 128 | *patch_ptr = interpolateMat_8u(img_ref, px[0], px[1]); 129 | } 130 | } 131 | } 132 | return true; 133 | } 134 | 135 | } // namespace warp 136 | } // namespace XP 137 | -------------------------------------------------------------------------------- /Frontend/iba_helper.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #include "iba_helper.h" 17 | #include // for CFG_STEREO 18 | #include 19 | #include 20 | 21 | #include "../Geometry/Rotation.h" 22 | 23 | namespace XP { 24 | IBA::Intrinsic to_iba_intrinsic(const DuoCalibParam::Camera_t& cam, const int lr) { 25 | IBA::Intrinsic K; 26 | K.fx = cam.cameraK_lr[lr](0, 0); 27 | K.fy = cam.cameraK_lr[lr](1, 1); 28 | K.cx = cam.cameraK_lr[lr](0, 2); 29 | K.cy = cam.cameraK_lr[lr](1, 2); 30 | for (int i = 0; i < 8; ++i) { 31 | if (i < cam.cv_dist_coeff_lr[lr].rows) { 32 | K.ds[i] = cam.cv_dist_coeff_lr[lr].at(i); 33 | } else { 34 | K.ds[i] = 0.f; 35 | } 36 | } 37 | return K; 38 | } 39 | 40 | IBA::Calibration to_iba_calibration(const DuoCalibParam& calib) { 41 | IBA::Calibration iba_calib; 42 | iba_calib.w = calib.Camera.img_size.width; 43 | iba_calib.h = calib.Camera.img_size.height; 44 | iba_calib.fishEye = false; 45 | Eigen::Matrix4f Cl_T_I = calib.Camera.D_T_C_lr[0].inverse() * calib.Imu.D_T_I; 46 | for (int i = 0; i < 3; ++i) { 47 | for (int j = 0; j < 4; ++j) { 48 | iba_calib.Tu[i][j] = Cl_T_I(i, j); 49 | } 50 | } 51 | Rotation3D R; 52 | R.Set(iba_calib.Tu[0], iba_calib.Tu[1], iba_calib.Tu[2]); 53 | R.MakeOrthogonal(); 54 | R.Get(iba_calib.Tu[0], iba_calib.Tu[1], iba_calib.Tu[2]); 55 | iba_calib.K = to_iba_intrinsic(calib.Camera, 0); 56 | 57 | #ifdef CFG_STEREO 58 | Eigen::Matrix4f Cl_T_Cr = calib.Camera.D_T_C_lr[0].inverse() * calib.Camera.D_T_C_lr[1]; 59 | for (int i = 0; i < 3; ++i) { 60 | for (int j = 0; j < 4; ++j) { 61 | iba_calib.Tr[i][j] = Cl_T_Cr(i, j); 62 | } 63 | } 64 | R.Set(iba_calib.Tr[0], iba_calib.Tr[1], iba_calib.Tr[2]); 65 | R.MakeOrthogonal(); 66 | R.Get(iba_calib.Tr[0], iba_calib.Tr[1], iba_calib.Tr[2]); 67 | iba_calib.Kr = to_iba_intrinsic(calib.Camera, 1); 68 | #endif 69 | 70 | for (int i = 0; i < 3; ++i) { 71 | iba_calib.ba[i] = calib.Imu.accel_bias(i); 72 | iba_calib.bw[i] = calib.Imu.gyro_bias(i); 73 | } 74 | return iba_calib; 75 | } 76 | 77 | IBA::IMUMeasurement to_iba_imu(const ImuData& imu_in) { 78 | IBA::IMUMeasurement imu_out; 79 | imu_out.t = imu_in.time_stamp; 80 | for (size_t i = 0; i < 3; ++i) { 81 | imu_out.a[i] = imu_in.accel(i); 82 | imu_out.w[i] = imu_in.ang_v(i); 83 | } 84 | return imu_out; 85 | } 86 | 87 | 88 | /* 89 | 90 | IBA::CameraPose vio_to_iba_pose(const Eigen::Matrix4f& T) { 91 | IBA::CameraPose pose; 92 | for (size_t i = 0; i < 3; ++i) { 93 | for (size_t j = 0; j < 3; ++j) { 94 | pose.R[i][j] = T(i, j); 95 | } 96 | pose.p[i] = T(i, 3); 97 | } 98 | return pose; 99 | } 100 | 101 | 102 | 103 | IBA::CameraIMUState vio_to_iba_state(const Eigen::Matrix4f& W_T_C, 104 | const VIO::CalibParam& calib) { 105 | // TODO(mingyu): Need to save ground-truth velocity from synthetic data 106 | IBA::CameraIMUState state; 107 | for (int i = 0; i < 3; ++i) { 108 | // Check IBA_datatype.h for the CameraPose notation: 109 | // R is R_CW, p is the camera position in W 110 | state.C.p[i] = W_T_C(i, 3); 111 | for (int j = 0; j < 3; ++j) { 112 | state.C.R[i][j] = W_T_C(j, i); // Transpose here! 113 | } 114 | state.v[i] = 0.f; // WRONG velocity for now 115 | state.ba[i] = calib.Imu.accel_bias(i); 116 | state.bw[i] = calib.Imu.gyro_bias(i); 117 | } 118 | return state; 119 | } 120 | */ 121 | 122 | } // namespace XP 123 | -------------------------------------------------------------------------------- /Frontend/iba_helper.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef XP_INCLUDE_XP_HELPER_IBA_HELPER_H_ 17 | #define XP_INCLUDE_XP_HELPER_IBA_HELPER_H_ 18 | 19 | #include "param.h" 20 | #include "basic_datatype.h" // for ImuData 21 | #include 22 | 23 | namespace XP { 24 | 25 | // Helper functions to convert from DuoCalibParam to IBA::Calibration 26 | IBA::Calibration to_iba_calibration(const DuoCalibParam& calib); 27 | 28 | IBA::Intrinsic to_iba_intrinsic(const DuoCalibParam::Camera_t& cam, const int lr); 29 | 30 | IBA::IMUMeasurement to_iba_imu(const ImuData& imu_in); 31 | 32 | } // namespace XP 33 | #endif // XP_INCLUDE_XP_HELPER_IBA_HELPER_H_ 34 | -------------------------------------------------------------------------------- /Frontend/image_utils.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include 18 | #include "image_utils.h" 19 | //#include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #ifndef __DEVELOPMENT_DEBUG_MODE__ 26 | #define __IMAGE_UTILS_NO_DEBUG__ 27 | #endif 28 | 29 | namespace XP { 30 | 31 | // only use central area in the image 32 | constexpr int kMarginRow = 50; 33 | constexpr int kMarginCol = 100; 34 | constexpr int kPixelStep = 2; 35 | 36 | // Compute the histogram of a sampled area of the input image and return the number of 37 | // sampled pixels 38 | // [NOTE] This function is hardcoded for VGA / WVGA images for now 39 | int sampleBrightnessHistogram(const cv::Mat& raw_img, 40 | std::vector* histogram, 41 | int* avg_pixel_val_ptr) { 42 | const int end_row = raw_img.rows - kMarginRow; 43 | const int end_col = raw_img.cols - kMarginCol; 44 | 45 | // Given the current algorithm, collecting histogram is not 46 | // necessary. But we still do so in case later we switch to a better 47 | // algorithm 48 | int pixel_num = 0; 49 | int avg_pixel_val = 0; 50 | histogram->clear(); 51 | histogram->resize(256, 0); 52 | int over_exposure_pixel_num = 0; 53 | for (int i = kMarginRow; i < end_row; i += kPixelStep) { 54 | for (int j = kMarginCol; j < end_col; j += kPixelStep) { 55 | const uint8_t pixel_val = raw_img.data[i * raw_img.cols + j]; 56 | avg_pixel_val += pixel_val; 57 | (*histogram)[pixel_val]++; 58 | ++pixel_num; 59 | } 60 | } 61 | if (avg_pixel_val_ptr) { 62 | *avg_pixel_val_ptr = avg_pixel_val / pixel_num; 63 | } 64 | return pixel_num; 65 | } 66 | 67 | // [NOTE] Instead of matching the cdf(s), we brute-force scale the histograms and match them 68 | // directly. This matchingHistogram is intended to match two histograms of images taken with 69 | // different gain/exposure settings. 70 | float matchingHistogram(const std::vector& hist_src, 71 | const std::vector& hist_tgt, 72 | const float init_scale) { 73 | std::vector cdf_tgt(256); 74 | cdf_tgt[0] = hist_tgt[0]; 75 | for (int i = 1; i < 256; ++i) { 76 | cdf_tgt[i] = hist_tgt[i] + cdf_tgt[i - 1]; 77 | } 78 | constexpr float delta_scale = 0.02; 79 | float best_scale = -1.f; // an invalid value 80 | int best_cdf_L1_dist = std::numeric_limits::max(); 81 | for (int s = -4; s < 5; ++s) { 82 | float scale = init_scale + s * delta_scale; 83 | std::vector hist_src_scale(256, 0); 84 | for (int i = 0; i < 256; ++i) { 85 | int si = i * scale; 86 | if (si >= 255) { 87 | int tmp_acc = 0; 88 | for (int j = i; j < 256; ++j) { 89 | tmp_acc += hist_src[j]; 90 | } 91 | hist_src_scale[255] = tmp_acc; 92 | break; 93 | } 94 | hist_src_scale[si] = hist_src_scale[si] + hist_src[i]; 95 | } 96 | 97 | int cdf_L1_dist = 0; 98 | int cdf_src_cumsum = 0; 99 | for (int i = 0; i < 256; ++i) { 100 | cdf_src_cumsum += hist_src_scale[i]; 101 | int L1_dist = std::abs(cdf_src_cumsum - cdf_tgt[i]); 102 | cdf_L1_dist += L1_dist; 103 | } 104 | // We simply assume these histograms are sampled from the same size of images 105 | CHECK_EQ(cdf_src_cumsum, cdf_tgt[255]); 106 | VLOG(1) << "scale = " << scale << " cdf_L1_dist = " << cdf_L1_dist; 107 | if (cdf_L1_dist < best_cdf_L1_dist) { 108 | best_scale = scale; 109 | best_cdf_L1_dist = cdf_L1_dist; 110 | 111 | if (VLOG_IS_ON(3)) { 112 | cv::Mat hist_canvas; 113 | const int scale = 2; 114 | const int height = 64 * scale; 115 | const int width = 256 * scale; 116 | hist_canvas.create(height * 2, width, CV_8UC3); 117 | hist_canvas.setTo(0x00); 118 | cv::Mat hist_img_src = hist_canvas(cv::Rect(0, 0, width, height)); 119 | drawHistogram(&hist_img_src, hist_src_scale); 120 | cv::Mat hist_img_tgt = hist_canvas(cv::Rect(0, height, width, height)); 121 | drawHistogram(&hist_img_tgt, hist_tgt); 122 | cv::imshow("matchingHistogram", hist_canvas); 123 | cv::waitKey(0); 124 | } 125 | } 126 | } 127 | CHECK_GT(best_scale, 0.f); 128 | VLOG(1) << "best scale = " << best_scale << " cdf_L1_dist = " << best_cdf_L1_dist; 129 | return best_scale; 130 | } 131 | 132 | void drawHistogram(cv::Mat* img_hist, const std::vector& histogram) { 133 | // Get some stats of this histogram 134 | const int N = static_cast(histogram.size()); 135 | int total_num = 0; 136 | int avg_pixel_val = 0; 137 | for (int i = 0; i < N; ++i) { 138 | total_num += histogram[i]; 139 | avg_pixel_val += histogram[i] * i; 140 | } 141 | avg_pixel_val /= total_num; 142 | 143 | int acc_pixel_counts = 0; 144 | int median_pixel_val; 145 | for (int i = 0; i < N; ++i) { 146 | acc_pixel_counts += histogram[i]; 147 | if (acc_pixel_counts >= total_num / 2) { 148 | median_pixel_val = i; 149 | break; 150 | } 151 | } 152 | 153 | const int scale = 2; 154 | const int width = N * scale + 1; 155 | const int height = 64 * scale; 156 | CHECK_NOTNULL(img_hist); 157 | if (img_hist->rows == 0) { 158 | img_hist->create(height, width, CV_8UC3); 159 | } 160 | img_hist->setTo(0x00); 161 | float hist_max = 0.1f; // scale the max y axis to 10% 162 | for (int i = 0; i < N; ++i) { 163 | float val = static_cast(histogram[i]) / total_num / hist_max; 164 | cv::Rect rect(i * scale, height * (1 - val), scale, height * val); 165 | cv::rectangle(*img_hist, rect, cv::Scalar(0, 0, 255)); 166 | } 167 | cv::putText(*img_hist, "mean: " + boost::lexical_cast(avg_pixel_val), 168 | cv::Point(15, 15), cv::FONT_HERSHEY_SIMPLEX, .5, cv::Scalar(255, 255, 255)); 169 | cv::putText(*img_hist, "median: " + boost::lexical_cast(median_pixel_val), 170 | cv::Point(15, 30), cv::FONT_HERSHEY_SIMPLEX, .5, cv::Scalar(255, 255, 255)); 171 | } 172 | 173 | } // namespace XP 174 | -------------------------------------------------------------------------------- /Frontend/image_utils.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef XP_INCLUDE_XP_UTIL_IMAGE_UTILS_H_ 17 | #define XP_INCLUDE_XP_UTIL_IMAGE_UTILS_H_ 18 | 19 | #include 20 | #include 21 | 22 | namespace XP { 23 | 24 | int sampleBrightnessHistogram(const cv::Mat& raw_img, 25 | std::vector* histogram, 26 | int* avg_pixel_val_ptr = nullptr); 27 | 28 | float matchingHistogram(const std::vector& hist_src, 29 | const std::vector& hist_tgt, 30 | const float init_scale); 31 | 32 | void drawHistogram(cv::Mat* img_hist, const std::vector& histogram); 33 | 34 | } // namespace XP 35 | 36 | #endif // XP_INCLUDE_XP_UTIL_IMAGE_UTILS_H_ 37 | -------------------------------------------------------------------------------- /Frontend/param.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef XP_INCLUDE_XP_HELPER_PARAM_H_ 17 | #define XP_INCLUDE_XP_HELPER_PARAM_H_ 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | namespace XP { 27 | 28 | class ParamBase { 29 | public: 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 31 | 32 | public: 33 | ParamBase() {} 34 | virtual bool LoadFromYaml(const std::string& filename) = 0; 35 | virtual bool WriteToYaml(const std::string& filename) = 0; 36 | virtual bool LoadFromCvYaml(const std::string& filename) = 0; 37 | virtual bool WriteToCvYaml(const std::string& filename) = 0; 38 | 39 | protected: 40 | void serialize(const std::string& filename, const YAML::Emitter& emitter); 41 | YAML::Node deserialize(const std::string& filename); 42 | 43 | YAML::Node base_node_; 44 | }; 45 | 46 | class AlgorithmParam : public ParamBase { 47 | public: 48 | AlgorithmParam() { 49 | } 50 | 51 | bool LoadFromYaml(const std::string& filename) override; 52 | bool WriteToYaml(const std::string& filename) override; 53 | bool LoadFromCvYaml(const std::string& filename) override; 54 | bool WriteToCvYaml(const std::string& filename) override; 55 | struct FeatDetParam_t { 56 | int request_feat_num = 70; 57 | int pyra_level = 2; 58 | int fast_det_thresh = 10; 59 | int uniform_radius = 40; 60 | float min_feature_distance_over_baseline_ratio = 3; 61 | float max_feature_distance_over_baseline_ratio = 3000; 62 | int feature_track_length_thresh = 25; 63 | float feature_track_dropout_rate = 0.3f; 64 | } FeatDetParam; 65 | struct Tracking_t { 66 | float max_feature_search_range = 10.f / 400.f; 67 | float orb_match_dist_thresh = 0.3 * 255; 68 | float orb_match_thresh_test_ratio = 0.9; 69 | float feature_uncertainty = 5; 70 | int imaging_FPS = 20; 71 | int imaging_exposure = 100; 72 | int imaging_gain = 100; 73 | int aec_index = 100; 74 | bool use_of_id = true; 75 | bool use_april_tag = false; 76 | std::string slave_det = "direct"; 77 | bool undistort_before_vio = true; 78 | } Tracking; 79 | }; 80 | 81 | class DuoCalibParam : public ParamBase { 82 | public: 83 | DuoCalibParam(); 84 | ~DuoCalibParam() {} 85 | 86 | bool LoadFromYaml(const std::string& filename) override; 87 | bool WriteToYaml(const std::string& filename) override; 88 | bool LoadFromCvYaml(const std::string& filename) override; 89 | bool WriteToCvYaml(const std::string& filename) override; 90 | bool LoadCamCalibFromYaml(const std::string& filename); 91 | bool LoadImuCalibFromYaml(const std::string& filename); 92 | 93 | // init undistort map from camera K and distort 94 | bool initUndistortMap(const cv::Size& new_img_size); 95 | 96 | struct Imu_t { 97 | Eigen::Matrix3f accel_TK; 98 | Eigen::Vector3f accel_bias; 99 | Eigen::Matrix3f gyro_TK; 100 | Eigen::Vector3f gyro_bias; 101 | Eigen::Vector3f accel_noise_var; // m / sec^2 102 | Eigen::Vector3f angv_noise_var; // rad / sec 103 | Eigen::Matrix4f D_T_I; 104 | Eigen::Matrix4f undist_D_T_I; 105 | } Imu; 106 | 107 | struct Camera_t { 108 | std::vector> D_T_C_lr; 109 | std::vector> cameraK_lr; 110 | std::vector cv_camK_lr; 111 | std::vector> cv_dist_coeff_lr; 112 | // the boundary of images in uv coordinate 113 | std::vector> lurd_lr; 114 | std::vector undistort_map_op1_lr; 115 | std::vector undistort_map_op2_lr; 116 | std::vector cv_undist_K_lr; 117 | std::vector> undist_D_T_C_lr; 118 | cv::Matx44f Q; // 4x4 convert disparity to depth. See cv::reprojectImageTo3D 119 | cv::Size img_size; 120 | } Camera; 121 | 122 | Eigen::Matrix3f C_R_B; 123 | Eigen::Vector3f C_p_B; 124 | std::string device_id; 125 | enum SensorType { 126 | UNKNOWN = 0, 127 | LI = 2, 128 | XP = 3, 129 | XP2 = 4, 130 | XP3 = 5, 131 | } sensor_type; 132 | }; 133 | 134 | // Helper functions to load calibration parameters from calib.yaml 135 | bool get_calib_file_from_device_id(const std::string& device_id, 136 | std::string* calib_file_ptr); 137 | bool load_imu_calib_param(const std::string& device_id, 138 | DuoCalibParam* duo_calib_param_ptr); 139 | 140 | bool load_camera_calib_param(const std::string& calib_file, 141 | XP::DuoCalibParam* duo_calib_param_ptr); 142 | bool save_camera_calib_param(const std::string& calib_file, 143 | const XP::DuoCalibParam& duo_calib_param); 144 | } // namespace XP 145 | #endif // XP_INCLUDE_XP_HELPER_PARAM_H_ 146 | -------------------------------------------------------------------------------- /Frontend/plotting_utils.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | /// \file 17 | #ifndef __PLOTTING_UTILS_H__ // NOLINT 18 | #define __PLOTTING_UTILS_H__ 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #ifdef HAS_OPENCV_VIZ // defined in CMakeLists 25 | #include 26 | #endif 27 | // Stl used in this sample 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include // unique_ptr 35 | class PoseDrawer2D { 36 | public: 37 | constexpr static const int imageSize = 480; // the same as camera image height 38 | PoseDrawer2D(); 39 | /** 40 | * Add a new pose for drawing 41 | */ 42 | void addPose(float x, float y, float alpha); 43 | /** Drawing function called by external thread 44 | * Draw all the positions in history. 45 | */ 46 | bool drawTo(cv::Mat* img_ptr); 47 | 48 | private: 49 | cv::Point2f convertToImageCoordinates(const cv::Point2f & pointInMeters); 50 | void drawPath(cv::Mat* img_ptr); 51 | std::mutex data_io_mutex_; 52 | std::list paths_; 53 | struct { 54 | float x; 55 | float y; 56 | float alpha; 57 | } latest_pose_; 58 | std::atomic scale_; 59 | std::atomic min_x_; 60 | std::atomic min_y_; 61 | std::atomic max_x_; 62 | std::atomic max_y_; 63 | const float frameScale_ = 0.2; // the scale of the axis in plot[m] 64 | }; 65 | #ifdef HAS_OPENCV_VIZ 66 | class PoseDrawer3D { 67 | public: 68 | /** \brief Initialize viz3d visualizer 69 | * 70 | * Create a bunch of widgets 71 | * \param viz_window pointer of the Viz3d window 72 | */ 73 | explicit PoseDrawer3D(float viz_cam_height, 74 | const cv::Matx33f& cam_K); 75 | /** \brief Render 3d widgets once 76 | * 77 | * \param viz_window pointer of the Viz3d window 78 | * \param viz_cam_height the height of the observing camera. 79 | * The larger the scene, the bigger this value ideally is. 80 | * This value is re-computed by this function. 81 | */ 82 | void viz3d_once(const cv::Affine3f& W_T_D, 83 | const cv::Mat& img = cv::Mat(), 84 | const cv::Mat& rig_xyz_mat = cv::Mat(), 85 | const cv::Mat_& depth_result_img = cv::Mat_()); 86 | uchar key_pressed() const; 87 | 88 | protected: 89 | std::shared_ptr viz_window_ptr_; 90 | cv::viz::WImage3D img3d_; 91 | cv::viz::WText viz3d_text_; 92 | enum { 93 | FREE_VIEW = 0, 94 | TOP_VIEW_FOLLOW = 1, 95 | CAM_FOLLOW = 2, 96 | } viz3d_view_mode_; 97 | uchar key_pressed_; 98 | float viz_cam_height_; 99 | }; 100 | #endif // HAS_OPENCV_VIZ 101 | #endif // __PLOTTING_UTILS_H__ // NOLINT 102 | -------------------------------------------------------------------------------- /Frontend/pose_viewer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef XP_INCLUDE_XP_HELPER_POSE_VIEWER_H_ 17 | #define XP_INCLUDE_XP_HELPER_POSE_VIEWER_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | namespace XP { 30 | class PoseViewer { 31 | public: 32 | typedef Eigen::Matrix SpeedAndBias; 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | constexpr static const int imageSize = 400; 35 | PoseViewer(); // this we can register as a callback 36 | void set_clear_canvas_before_draw(bool clear_canvas_before_draw); 37 | void addPose(const Eigen::Matrix4f & T_WS, 38 | const Eigen::Matrix & speedAndBiases, 39 | const float travel_dist); 40 | void addPose(const Eigen::Matrix4f & T_WS); 41 | void addPose(const Eigen::Matrix4f & T_WS, int path_id); 42 | void displayTo(const std::string& win_name); 43 | bool drawTo(cv::Mat* img); 44 | 45 | private: 46 | cv::Point2f convertToImageCoordinates(const cv::Point2f & pointInMeters); 47 | void drawPath(cv::Mat* img); 48 | bool clear_canvas_before_draw_; 49 | cv::Mat _image; // for display to 50 | struct XYH { 51 | cv::Point2f xy; 52 | float h; 53 | }; 54 | std::deque data_io_mutices_; // vector doesn't work 55 | std::vector> paths_; 56 | std::vector latest_T_WS_; 57 | std::mutex scale_mutex_; 58 | SpeedAndBias last_speedAndBiases_; 59 | float last_travel_dist_ = 0.f; 60 | std::atomic _scale; 61 | std::atomic _min_x; 62 | std::atomic _min_y; 63 | std::atomic _min_z; 64 | std::atomic _max_x; 65 | std::atomic _max_y; 66 | std::atomic _max_z; 67 | const float _frameScale = 0.2; // the scale of the axis in plot[m] 68 | std::atomic_bool pose_changed_after_last_display_; 69 | }; 70 | } // namespace XP 71 | #endif // XP_INCLUDE_XP_HELPER_POSE_VIEWER_H_ 72 | -------------------------------------------------------------------------------- /Frontend/rotation.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include "rotation.h" 18 | #include "xp_quaternion.h" 19 | #include 20 | 21 | namespace XP { 22 | 23 | // angle2dcm 'ZYX' 24 | // https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles1.pdf 25 | void RotationMatrixToEulerRadians(const Eigen::Matrix3f &R, Eigen::Vector3f *euler_rad) { 26 | CHECK_NOTNULL(euler_rad); 27 | (*euler_rad)(0) = std::atan2(R(1, 2), R(2, 2)); 28 | (*euler_rad)(1) =-std::atan2(R(0, 2), std::sqrt(1 - R(0, 2) * R(0, 2))); 29 | (*euler_rad)(2) = std::atan2(R(0, 1), R(0, 0)); 30 | } 31 | 32 | // dcm2angle 'ZYX' 33 | void EulerRadiansToRotationMatrix(const Eigen::Vector3f& euler_rad, Eigen::Matrix3f *R) { 34 | CHECK_NOTNULL(R); 35 | const float &phi = euler_rad(0); 36 | const float &theta = euler_rad(1); 37 | const float &psi = euler_rad(2); 38 | (*R)(0, 0) = cos(psi)*cos(theta); 39 | (*R)(0, 1) = sin(psi)*cos(theta); 40 | (*R)(0, 2) = -sin(theta); 41 | (*R)(1, 0) = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi); 42 | (*R)(1, 1) = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi); 43 | (*R)(1, 2) = cos(theta)*sin(phi); 44 | (*R)(2, 0) = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi); 45 | (*R)(2, 1) = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi); 46 | (*R)(2, 2) = cos(theta)*cos(phi); 47 | } 48 | 49 | // Runge–Kutta 4th method for quaternion integration 50 | void IntegrateQuaternion(const Eigen::Vector3f &omega_begin, 51 | const Eigen::Vector3f &omega_end, 52 | const Eigen::Quaternionf &q_begin, 53 | const float dt, 54 | Eigen::Quaternionf *q_end) { 55 | XpQuaternion xp_q_begin(q_begin); 56 | XpQuaternion xp_q_end; 57 | IntegrateQuaternion(omega_begin, 58 | omega_end, 59 | xp_q_begin, 60 | dt, 61 | &xp_q_end); 62 | *q_end = xp_q_end.ToEigenQuaternionf(); 63 | } 64 | 65 | } // namespace XP 66 | -------------------------------------------------------------------------------- /Frontend/rotation.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #ifndef XP_INCLUDE_XP_MATH_ROTATION_H_ 18 | #define XP_INCLUDE_XP_MATH_ROTATION_H_ 19 | 20 | #include 21 | #include 22 | 23 | namespace XP { 24 | 25 | /* 26 | * Common rotation-related utility functions 27 | */ 28 | // angle2dcm 'ZYX' 29 | // https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles1.pdf 30 | void RotationMatrixToEulerRadians(const Eigen::Matrix3f &R, Eigen::Vector3f *euler_rad); 31 | 32 | // dcm2angle 'ZYX' 33 | void EulerRadiansToRotationMatrix(const Eigen::Vector3f& euler_rad, Eigen::Matrix3f *R); 34 | 35 | // Runge–Kutta 4th method for quaternion integration 36 | // q_begin and q_end should be in the notation of L_q_W, where L is local coordinate, 37 | // and W is world coordinate. R(L_q_W) will be the top left 3x3 corner of T_LW, which 38 | // is the transformation from W to L: p_L = T_LW * p_W 39 | void IntegrateQuaternion(const Eigen::Vector3f &omega_begin, 40 | const Eigen::Vector3f &omega_end, 41 | const Eigen::Quaternionf &q_begin, 42 | const float dt, 43 | Eigen::Quaternionf *q_end); 44 | 45 | } // namespace XP 46 | #endif // XP_INCLUDE_XP_MATH_ROTATION_H_ 47 | -------------------------------------------------------------------------------- /Frontend/shared_queue.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #ifndef XP_INCLUDE_XP_HELPER_SHARED_QUEUE_H_ 17 | #define XP_INCLUDE_XP_HELPER_SHARED_QUEUE_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace XP { 26 | 27 | namespace internal { 28 | 29 | // Internal helper function to get the last element and then empty the container. 30 | // The caller should be responsible for checking non-empty before pop_to_back. 31 | // std::deque has back() 32 | template 33 | void pop_to_back(std::deque* container, T* elem) { 34 | CHECK(!container->empty()); 35 | *elem = std::move(container->back()); 36 | container->clear(); 37 | } 38 | 39 | // For other container types that may not have back() 40 | template 41 | void pop_to_back(OtherContainer* container, T* elem) { 42 | CHECK(!container->empty()); 43 | while (container->size() > 1) { 44 | container->pop_front(); 45 | } 46 | *elem = std::move(container->front()); 47 | container->pop_front(); 48 | } 49 | } // namespace internal 50 | 51 | template > 52 | class shared_queue { 53 | public: 54 | // Remove copy and assign 55 | shared_queue& operator=(const shared_queue&) = delete; 56 | shared_queue(const shared_queue& other) = delete; 57 | 58 | explicit shared_queue(const std::string& name) : name_(name), kill_(false) {} 59 | ~shared_queue() { 60 | // make sure you always call kill before destruction 61 | if (!kill_ && !queue_.empty()) { 62 | LOG(ERROR) << "shared_queue : " << name_ << " is destructed without getting killed"; 63 | } 64 | } 65 | 66 | // Use this function to kill the shared_queue before the application exists 67 | // to prevent potential deadlock. 68 | void kill() { 69 | kill_ = true; 70 | cond_.notify_one(); 71 | } 72 | 73 | T front() { 74 | std::lock_guard lock(m_); 75 | return queue_.front(); 76 | } 77 | 78 | void push_back(T elem) { 79 | { 80 | std::lock_guard lock(m_); 81 | queue_.push_back(std::move(elem)); 82 | } 83 | // Unlock mutex m_ before notifying 84 | cond_.notify_one(); 85 | } 86 | 87 | void pop_front() { 88 | std::lock_guard lock(m_); 89 | if (!queue_.empty()) { 90 | queue_.pop_front(); 91 | } 92 | } 93 | 94 | void pop_to_back(T* elem) { 95 | std::lock_guard lock(m_); 96 | internal::pop_to_back(&queue_, elem); 97 | } 98 | 99 | bool wait_and_pop_front(T* elem) { 100 | std::unique_lock lock(m_); 101 | cond_.wait(lock, [this](){return !queue_.empty() || kill_; }); 102 | if (kill_) { 103 | return false; 104 | } else { 105 | *elem = std::move(queue_.front()); 106 | queue_.pop_front(); 107 | return true; 108 | } 109 | } 110 | 111 | bool wait_and_pop_to_back(T* elem) { 112 | std::unique_lock lock(m_); 113 | cond_.wait(lock, [this](){ return !queue_.empty() || kill_; }); 114 | if (kill_) { 115 | return false; 116 | } else { 117 | internal::pop_to_back(&queue_, elem); 118 | return true; 119 | } 120 | } 121 | 122 | bool wait_and_peek_front(T* elem) { 123 | std::unique_lock lock(m_); 124 | cond_.wait(lock, [this](){ return !queue_.empty() || kill_; }); 125 | if (kill_) { 126 | return false; 127 | } else { 128 | *elem = queue_.front(); 129 | return true; 130 | } 131 | } 132 | 133 | bool empty() { 134 | std::lock_guard lock(m_); 135 | return queue_.empty(); 136 | } 137 | 138 | size_t size() { 139 | std::lock_guard lock(m_); 140 | return queue_.size(); 141 | } 142 | 143 | void clear() { 144 | std::lock_guard lock(m_); 145 | queue_.clear(); 146 | } 147 | 148 | private: 149 | Container queue_; 150 | std::mutex m_; 151 | std::condition_variable cond_; 152 | std::string name_; 153 | bool kill_; 154 | }; 155 | } // namespace XP 156 | #endif // XP_INCLUDE_XP_HELPER_SHARED_QUEUE_H_ 157 | -------------------------------------------------------------------------------- /Frontend/timer.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #include "timer.h" 17 | #include 18 | 19 | #ifndef __DEVELOPMENT_DEBUG_MODE__ 20 | #define __HELPER_TIMER_NO_DEBUG__ 21 | #endif 22 | 23 | namespace XP { 24 | ScopedMicrosecondTimer::ScopedMicrosecondTimer(const std::string& text_id, int vlog_level) : 25 | text_id_(text_id), 26 | vlog_level_(vlog_level), 27 | t_start_(std::chrono::steady_clock::now()) { 28 | } 29 | 30 | ScopedMicrosecondTimer::~ScopedMicrosecondTimer () { 31 | #ifndef __HELPER_TIMER_NO_DEBUG__ 32 | VLOG(vlog_level_) << "ScopedTimer " << text_id_ << "=[" 33 | << std::chrono::duration_cast( 34 | std::chrono::steady_clock::now() - t_start_).count() 35 | << "] microseconds"; 36 | #endif 37 | } 38 | ScopedLoopProfilingTimer::ScopedLoopProfilingTimer(const std::string& text_id, int vlog_level) : 39 | text_id_(text_id), 40 | vlog_level_(vlog_level), 41 | t_start_(std::chrono::steady_clock::now()){} 42 | ScopedLoopProfilingTimer::~ScopedLoopProfilingTimer() { 43 | using namespace std::chrono; 44 | // print timing info even if in release mode 45 | if (VLOG_IS_ON(vlog_level_)) { 46 | VLOG(vlog_level_) << "ScopedLoopProfilingTimer " << text_id_ 47 | << " start_end=[" << duration_cast(t_start_.time_since_epoch()).count() 48 | << " " << duration_cast(steady_clock::now().time_since_epoch()).count() << "]"; 49 | } 50 | } 51 | 52 | MicrosecondTimer::MicrosecondTimer(const std::string& text_id, int vlog_level) : 53 | has_ended_(false), 54 | text_id_(text_id), 55 | vlog_level_(vlog_level) { 56 | t_start_ = std::chrono::steady_clock::now(); 57 | } 58 | MicrosecondTimer::MicrosecondTimer() : 59 | has_ended_(false), 60 | text_id_(""), 61 | vlog_level_(99) { 62 | t_start_ = std::chrono::steady_clock::now(); 63 | } 64 | int MicrosecondTimer::end () { 65 | CHECK(!has_ended_); 66 | has_ended_ = true; 67 | int micro_sec_passed = std::chrono::duration_cast( 68 | std::chrono::steady_clock::now() - t_start_).count(); 69 | #ifndef __HELPER_TIMER_NO_DEBUG__ 70 | VLOG(vlog_level_) << "Timer " << text_id_ << "=[" << micro_sec_passed << "] microseconds"; 71 | #endif 72 | return micro_sec_passed; 73 | } 74 | MicrosecondTimer::~MicrosecondTimer() { 75 | if (!has_ended_) { 76 | VLOG(vlog_level_) << "MicrosecondTimer " << text_id_ << " is not used"; 77 | } 78 | } 79 | 80 | } // namespace XP 81 | -------------------------------------------------------------------------------- /Frontend/timer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #ifndef XP_HELPER_TIMER_H 18 | #define XP_HELPER_TIMER_H 19 | 20 | #include 21 | #include 22 | 23 | namespace XP { 24 | // Create a scoped timer. Result will be printed to vlog 25 | class ScopedMicrosecondTimer { 26 | public: 27 | explicit ScopedMicrosecondTimer(const std::string& text_id, int vlog_level); 28 | ~ScopedMicrosecondTimer(); 29 | private: 30 | const std::string text_id_; 31 | const int vlog_level_; 32 | std::chrono::time_point t_start_; 33 | }; 34 | 35 | // Create a scoped timer. 36 | // The construction time and the deconstruction timestamp will be logged 37 | class ScopedLoopProfilingTimer { 38 | public: 39 | explicit ScopedLoopProfilingTimer(const std::string& text_id, int vlog_level); 40 | ~ScopedLoopProfilingTimer(); 41 | private: 42 | const std::string text_id_; 43 | const int vlog_level_; 44 | std::chrono::time_point t_start_; 45 | }; 46 | 47 | // Create a timer. Result will be printed to vlog 48 | class MicrosecondTimer { 49 | public: 50 | explicit MicrosecondTimer(const std::string& text_id, int vlog_level); 51 | MicrosecondTimer(); 52 | int end(); 53 | ~MicrosecondTimer(); 54 | private: 55 | bool has_ended_; 56 | const std::string text_id_; 57 | const int vlog_level_; 58 | std::chrono::time_point t_start_; 59 | }; 60 | 61 | } // namespace XP 62 | #endif // XP_HELPER_TIMER_H 63 | -------------------------------------------------------------------------------- /Frontend/xp_quaternion.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #include "rotation.h" 17 | #include "xp_quaternion.h" 18 | #include 19 | #include 20 | namespace XP { 21 | 22 | // Quaternion multiplication 23 | XpQuaternion quat_multiply(const XpQuaternion& lhs, 24 | const XpQuaternion& rhs) { 25 | XpQuaternion result; 26 | result << lhs(0) * rhs(3) + lhs(1) * rhs(2) - lhs(2) * rhs(1) + lhs(3) * rhs(0), 27 | -lhs(0) * rhs(2) + lhs(1) * rhs(3) + lhs(2) * rhs(0) + lhs(3) * rhs(1), 28 | lhs(0) * rhs(1) - lhs(1) * rhs(0) + lhs(2) * rhs(3) + lhs(3) * rhs(2), 29 | -lhs(0) * rhs(0) - lhs(1) * rhs(1) - lhs(2) * rhs(2) + lhs(3) * rhs(3); 30 | result.normalize(); 31 | 32 | // keep scalar value non-negative 33 | if (result(3) < 0) { 34 | result = -result; 35 | } 36 | return result; 37 | } 38 | 39 | /* 40 | * XpQuaternion 41 | */ 42 | XpQuaternion::XpQuaternion() { 43 | *this = Eigen::Vector4f(0, 0, 0, 1); 44 | } 45 | 46 | // Hamiltonian representaiton 47 | Eigen::Quaternionf XpQuaternion::ToEigenQuaternionf() const { 48 | return Eigen::Quaternionf{(*this)(3), 49 | (*this)(0), 50 | (*this)(1), 51 | (*this)(2)}; 52 | } 53 | 54 | Eigen::Matrix3f XpQuaternion::ToRotationMatrix() const { 55 | const XpQuaternion &q = *this; 56 | Eigen::Matrix3f R; 57 | R << 1 - 2 * (q(1)*q(1) + q(2)*q(2)), 58 | 2 * (q(0)*q(1) - q(2)*q(3)), 59 | 2 * (q(0)*q(2) + q(1)*q(3)), 60 | 2 * (q(0)*q(1) + q(2)*q(3)), 61 | 1 - 2 * (q(0)*q(0) + q(2)*q(2)), 62 | 2 * (q(1)*q(2) - q(0)*q(3)), 63 | 2 * (q(0)*q(2) - q(1)*q(3)), 64 | 2 * (q(1)*q(2) + q(0)*q(3)), 65 | 1 - 2 * (q(0)*q(0) + q(1)*q(1)); 66 | return R; 67 | } 68 | 69 | Eigen::Vector3f XpQuaternion::ToEulerRadians() const { 70 | // pitch roll yaw 71 | Eigen::Vector3f euler_rad; 72 | RotationMatrixToEulerRadians(this->ToRotationMatrix(), &euler_rad); 73 | return euler_rad; 74 | } 75 | 76 | // set from delta theta during update 77 | // see page 8 on ref[1] 78 | void XpQuaternion::SetFromDeltaTheta(Eigen::Vector3f dtheta) { 79 | (*this).head<3>() = 0.5 * dtheta; 80 | (*this)(3) = std::sqrt(1 - (*this).head<3>().squaredNorm()); 81 | } 82 | 83 | void XpQuaternion::SetFromRotationMatrix(const Eigen::Matrix3f &R) { 84 | float trace = R(0, 0) + R(1, 1) + R(2, 2); 85 | if ( trace > 0 ) { 86 | float s = 2.0 * std::sqrt(trace + 1.0); 87 | (*this)(0) = ( R(2, 1) - R(1, 2) ) / s; 88 | (*this)(1) = ( R(0, 2) - R(2, 0) ) / s; 89 | (*this)(2) = ( R(1, 0) - R(0, 1) ) / s; 90 | (*this)(3) = 0.25 * s; 91 | } else { 92 | // find the biggest diagonal element 93 | if ( R(0, 0) >= R(1, 1) && R(0, 0) >= R(2, 2) ) { 94 | float s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)); 95 | (*this)(0) = 0.25 * s; 96 | (*this)(1) = (R(1, 0) + R(0, 1)) / s; 97 | (*this)(2) = (R(2, 0) + R(0, 2)) / s; 98 | (*this)(3) = (R(2, 1) - R(1, 2)) / s; 99 | } else if (R(1, 1) >= R(0, 0) && R(1, 1) >= R(2, 2)) { 100 | float s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)); 101 | (*this)(0) = (R(1, 0) + R(0, 1)) / s; 102 | (*this)(1) = 0.25 * s; 103 | (*this)(2) = (R(2, 1) + R(1, 2)) / s; 104 | (*this)(3) = (R(0, 2) - R(2, 0)) / s; 105 | } else { // in this case, R(2,2) is the biggest element 106 | float s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)); 107 | (*this)(0) = (R(2, 0) + R(0, 2)) / s; 108 | (*this)(1) = (R(2, 1) + R(1, 2)) / s; 109 | (*this)(2) = 0.25 * s; 110 | (*this)(3) = (R(1, 0) - R(0, 1)) / s; 111 | } 112 | } 113 | // keep scalar value non-negative 114 | if ((*this)(3) < 0) { 115 | (*this) = -(*this); 116 | } 117 | } 118 | 119 | void XpQuaternion::SetFromEulerRadians(const Eigen::Vector3f &euler_rad) { 120 | Eigen::Matrix3f R; 121 | EulerRadiansToRotationMatrix(euler_rad, &R); 122 | SetFromRotationMatrix(R); 123 | } 124 | 125 | // v_lhs = R(q) * v_rhs 126 | void XpQuaternion::SetFromTwoVectors( 127 | const Eigen::Vector3f& v_rhs, const Eigen::Vector3f& v_lhs) { 128 | Eigen::Vector3f v_rhs_unit = v_rhs / v_rhs.norm(); 129 | Eigen::Vector3f v_lhs_unit = v_lhs / v_lhs.norm(); 130 | this->topRows<3>() = v_lhs_unit.cross(v_rhs_unit); 131 | this->topRows<3>().normalize(); 132 | float cos_theta = v_rhs_unit.dot(v_lhs_unit); 133 | float theta = -std::acos(cos_theta); 134 | (*this)[3] = std::cos(theta / 2); 135 | this->topRows<3>() = this->topRows<3>() * std::sin(theta / 2); 136 | // keep scalar value non-negative 137 | if ((*this)(3) < 0) { 138 | (*this) = -(*this); 139 | } 140 | } 141 | 142 | // Hides Eigen::MatrixBase<>::inverse 143 | XpQuaternion XpQuaternion::inverse() const { 144 | return XpQuaternion{-(*this)(0), -(*this)(1), -(*this)(2), (*this)(3)}; 145 | } 146 | XpQuaternion XpQuaternion::mul(const XpQuaternion& rhs) const { 147 | return quat_multiply(*this, rhs); 148 | } 149 | 150 | void IntegrateQuaternion( 151 | const Eigen::Vector3f &omega_begin, const Eigen::Vector3f &omega_end, 152 | const XpQuaternion &q_begin, const float dt, 153 | XpQuaternion *q_end, 154 | XpQuaternion *q_mid, 155 | XpQuaternion *q_fourth, 156 | XpQuaternion *q_eighth) { 157 | CHECK_NOTNULL(q_end); 158 | XpQuaternion q_0 = q_begin; 159 | 160 | // divide dt time interval into num_segment segments 161 | // TODO(mingyu): Reduce to 2 or 4 segments as 8 segments may overkill 162 | const int num_segment = 8; 163 | 164 | // the time duration in each segment 165 | const float inv_num_segment = 1.0 / num_segment; 166 | const float dt_seg = dt * inv_num_segment; 167 | 168 | Eigen::Vector3f delta_omega = omega_end - omega_begin; 169 | for (int i = 0; i < num_segment; ++i) { 170 | // integrate in the region: [i/num_segment, (i+1)/num_segment] 171 | 172 | Eigen::Vector3f omega_tmp = omega_begin + (i * inv_num_segment) * delta_omega; 173 | Eigen::Vector4f k1 = XpQuaternionDerivative(q_0, omega_tmp); 174 | omega_tmp = omega_begin + 0.5 * (2 * i + 1) * inv_num_segment * delta_omega; 175 | Eigen::Vector4f k2 = XpQuaternionDerivative(q_0 + 0.5 * k1 * dt_seg, omega_tmp); 176 | Eigen::Vector4f k3 = XpQuaternionDerivative(q_0 + 0.5 * k2 * dt_seg, omega_tmp); 177 | omega_tmp = omega_begin + (i + 1) * inv_num_segment * delta_omega; 178 | Eigen::Vector4f k4 = XpQuaternionDerivative(q_0 + k3 * dt_seg, omega_tmp); 179 | (*q_end) = q_0 + (dt_seg / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4); 180 | 181 | // store the start point the next region of integration 182 | q_0 = (*q_end); 183 | 184 | // store the intermediate attitude as output 185 | if (i == 0 && q_eighth) { 186 | (*q_eighth) = (*q_end); 187 | } else if (i == 1 && q_fourth) { 188 | (*q_fourth) = (*q_end); 189 | } else if (i == 3 && q_mid) { 190 | (*q_mid) = (*q_end); 191 | } 192 | } 193 | } 194 | } // namespace XP 195 | -------------------------------------------------------------------------------- /Frontend/xp_quaternion.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #ifndef XP_INCLUDE_XP_MATH_XP_QUATERNION_H_ 18 | #define XP_INCLUDE_XP_MATH_XP_QUATERNION_H_ 19 | 20 | #include 21 | #include 22 | 23 | namespace XP { 24 | 25 | class XpQuaternion : public Eigen::Vector4f { 26 | // "Indirect Kalman Filter for 3D Attitude Estimation" 27 | // x y z w 28 | public: 29 | // REQUIRED BY EIGEN 30 | typedef Eigen::Vector4f Base; 31 | 32 | // constructors 33 | // XpQuaternion is Hamilton quaternion in the order of xyzw 34 | // Eigen::Quaternion is also Hamiton quaternion in the order of wxyz 35 | XpQuaternion(); 36 | template 37 | XpQuaternion(Scalar q0, Scalar q1, Scalar q2, Scalar q3) 38 | : Base{static_cast(q0), static_cast(q1), 39 | static_cast(q2), static_cast(q3)} { 40 | Eigen::Vector4f& q = *this; 41 | q = q * (1.f / q.norm()); 42 | if (q(3) < 0) { 43 | q = -q; 44 | } 45 | } 46 | 47 | template 48 | explicit XpQuaternion( 49 | const Eigen::QuaternionBase &eigen_quat) { 50 | SetFromEigenQuaternionf(eigen_quat); 51 | } 52 | 53 | // from MatrixBase - REQUIRED BY EIGEN 54 | template 55 | XpQuaternion(const MatrixBase &other) // NOLINT 56 | : Base{other} { 57 | Eigen::Vector4f& q = *this; 58 | q = q * (1.f / q.norm()); 59 | if (q(3) < 0) { 60 | q = -q; 61 | } 62 | } 63 | 64 | // assignment operator - REQUIRED BY EIGEN 65 | template 66 | XpQuaternion &operator=(const MatrixBase &other) { 67 | Base::operator=(other); 68 | Eigen::Vector4f& q = *this; 69 | q = q * (1.f / q.norm()); 70 | if (q(3) < 0) { 71 | q = -q; 72 | } 73 | return *this; 74 | } 75 | 76 | inline float x() const { return (*this)[0]; } 77 | inline float y() const { return (*this)[1]; } 78 | inline float z() const { return (*this)[2]; } 79 | inline float w() const { return (*this)[3]; } 80 | 81 | // For more details on conversion to and from Hamiltonian quaternions 82 | // see ref [2] and [3] 83 | Eigen::Quaternionf ToEigenQuaternionf() const; 84 | 85 | Eigen::Matrix3f ToRotationMatrix() const; 86 | 87 | Eigen::Vector3f ToEulerRadians() const; 88 | 89 | // set from Eigen Quaternion 90 | template 91 | void SetFromEigenQuaternionf( 92 | const Eigen::QuaternionBase &eigen_quat) { 93 | (*this)(0) = eigen_quat.x(); 94 | (*this)(1) = eigen_quat.y(); 95 | (*this)(2) = eigen_quat.z(); 96 | (*this)(3) = eigen_quat.w(); 97 | // keep scalar value non-negative 98 | if ((*this)(3) < 0) { 99 | (*this) = -(*this); 100 | } 101 | } 102 | 103 | // set from RotationMatrix. 104 | // http://www.euclideanspace.com/maths/geometry/rotations/conversions/ 105 | // matrixToQuaternion/ 106 | // Normalize the quaternion 107 | void SetFromRotationMatrix(const Eigen::Matrix3f &R); 108 | 109 | void SetFromEulerRadians(const Eigen::Vector3f& euler_rad); 110 | 111 | // set from delta theta 112 | // see page 8 on ref 113 | // Not const & because dtheta is changed inside the function 114 | void SetFromDeltaTheta(Eigen::Vector3f dtheta); 115 | 116 | // v_lhs = R(q) * v_rhs 117 | void SetFromTwoVectors(const Eigen::Vector3f& v_rhs, const Eigen::Vector3f& v_lhs); 118 | 119 | // Hides Eigen::MatrixBase<>::inverse 120 | XpQuaternion inverse() const; 121 | XpQuaternion mul(const XpQuaternion& rhs) const; 122 | }; 123 | 124 | inline Eigen::Matrix4f XpComposeOmega(const Eigen::Vector3f& w) { 125 | Eigen::Matrix4f Ohm; 126 | Ohm(0, 0) = 0; Ohm(0, 1) = -w(2); Ohm(0, 2) = w(1); Ohm(0, 3) = w(0); 127 | Ohm(1, 0) = w(2); Ohm(1, 1) = 0; Ohm(1, 2) = -w(0); Ohm(1, 3) = w(1); 128 | Ohm(2, 0) = -w(1); Ohm(2, 1) = w(0); Ohm(2, 2) = 0; Ohm(2, 3) = w(2); 129 | Ohm(3, 0) = -w(0); Ohm(3, 1) = -w(1); Ohm(3, 2) = -w(2); Ohm(3, 3) = 0; 130 | return Ohm; 131 | } 132 | 133 | // page 11 134 | // Note we use Vector4f to represent quaternion instead of quaternion 135 | // because it is better do not normalize q during integration 136 | inline Eigen::Vector4f XpQuaternionDerivative( 137 | const Eigen::Vector4f &q, 138 | const Eigen::Vector3f &omega) { 139 | return -0.5 * XpComposeOmega(omega) * q; 140 | } 141 | 142 | XpQuaternion quat_multiply(const XpQuaternion& lhs, const XpQuaternion& rhs); 143 | 144 | // page 11 145 | void IntegrateQuaternion(const Eigen::Vector3f &omega_begin, 146 | const Eigen::Vector3f &omega_end, 147 | const XpQuaternion &q_begin, 148 | const float dt, 149 | XpQuaternion *q_end, 150 | XpQuaternion *q_mid = nullptr, 151 | XpQuaternion *q_fourth = nullptr, 152 | XpQuaternion *q_eighth = nullptr); 153 | 154 | } // namespace XP 155 | #endif // XP_INCLUDE_XP_MATH_XP_QUATERNION_H_ 156 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ICE-BA 2 | ## ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM 3 | We present ICE-BA, an incremental, consistent and efficient bundle adjustment for visual-inertial SLAM, which takes feature tracks, IMU measurements and optionally the loop constraints as input, performs in parallel both local BA over the sliding window and global BA over all keyframes, and outputs camera pose and updated map points for each frame in real-time. The main contributions include: 4 | - a new BA solver that leverages the incremental nature of SLAM measurements to achieve more than 10x efficiency compared to the state-of-the-arts. 5 | - a new relative marginalization algorithm that resolves the conflicts between sliding window marginalization bias and global loop closure constraints. 6 | 7 | Beside the backend solver, the library also provides an optic flow based frontend, which can be easily replaced by other more complicated frontends like ORB-SLAM2. 8 | 9 | The original implementation of our ICE-BA is at https://github.com/ZJUCVG/EIBA, which only performs global BA and does not support IMU input. 10 | 11 | **Authors:** Haomin Liu, Mingyu Chen, Yingze Bao, Zhihao Wang 12 | **Related Publications:** 13 | Haomin Liu, Mingyu Chen, Guofeng Zhang, Hujun Bao and Yingze Bao. ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for 14 | Visual-Inertial SLAM. (Accepted by CVPR 2018).**[PDF](http://openaccess.thecvf.com/content_cvpr_2018/papers/Liu_ICE-BA_Incremental_Consistent_CVPR_2018_paper.pdf)**. 15 | Haomin Liu, Chen Li, Guojun Chen, Guofeng Zhang, Michael Kaess and Hujun Bao. Robust Keyframe-based Dense SLAM with an RGB-D Camera [J]. arXiv preprint arXiv:1711.05166, 2017. [arXiv report].**[PDF](https://arxiv.org/abs/1711.05166)**. 16 | 17 | 18 | ## 1. License 19 | Licensed under the Apache License, Version 2.0. 20 | Refer to LISENCE for more details. 21 | 22 | ## 2. Prerequisites 23 | We have tested the library in **Ubuntu 14.04** and **Ubuntu 16.04**. 24 | The following dependencies are needed: 25 | ### boost 26 | sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev 27 | 28 | ### Eigen 29 | sudo apt-get install libeigen3-dev 30 | 31 | ### Glog 32 | https://github.com/google/glog 33 | 34 | ### Gflags 35 | https://github.com/gflags/gflags 36 | 37 | ### OpenCV 38 | We use OpenCV 3.0.0. 39 | https://opencv.org/ 40 | 41 | ### Yaml 42 | https://github.com/jbeder/yaml-cpp 43 | 44 | ### brisk 45 | https://github.com/gwli/brisk 46 | 47 | ## 3. Build 48 | cd ice-ba 49 | chmod +x build.sh 50 | ./build.sh 51 | 52 | ## 4. Run 53 | We provide examples to run ice-ba with [EuRoC dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets#downloads). 54 | 55 | ### run ICE-BA stereo 56 | Run ICE-BA in stereo mode. Please refer to scripts/run_ice_ba_stereo.sh for more details about how to run the example. 57 | 58 | ### run ICE-BA monocular 59 | Run ICE-BA in monocular mode. Please refer to scripts/run_ice_ba_mono.sh for more details about how to run the example. 60 | 61 | ### run back-end only 62 | Front-end results can be saved into files. Back-end only mode loads these files and runs backend only. 63 | Please refer to scripts/run_backend_only.sh for more details about how to run the example. 64 | 65 | ## 5. Contribution 66 | You are very welcome to contribute to ICE-BA. 67 | Baidu requires the contributors to e-sign [CLA (Contributor License Agreement)](https://gist.github.com/tanzhongyibidu/6605bdef5f7bb03b9084dd8fed027037) before making a Pull Request. We have the CLA binding to Github so it will pop up before creating a PR. 68 | 69 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | mkdir build 3 | cd build 4 | cmake .. 5 | make -j3 6 | -------------------------------------------------------------------------------- /cmake/FindCVD.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find CVD 2 | # 3 | # The following variables are optionally searched for defaults 4 | # CVD_ROOT_DIR: Base directory where all CVD components are found 5 | # 6 | # The following are set after configuration is done: 7 | # CVD_FOUND 8 | # CVD_INCLUDE_DIRS 9 | # CVD_LIBRARIES 10 | 11 | include(FindPackageHandleStandardArgs) 12 | 13 | set(CVD_ROOT_DIR /usr/local/ 14 | CACHE PATH "Folder contains libcvd" 15 | ) 16 | 17 | find_path(CVD_INCLUDE_DIR cvd/config.h 18 | PATHS ${CVD_ROOT_DIR}/include) 19 | 20 | find_library(CVD_LIBRARY cvd 21 | PATHS ${CVD_ROOT_DIR} 22 | PATH_SUFFIXES 23 | lib 24 | lib64) 25 | 26 | find_package_handle_standard_args(CVD 27 | DEFAULT_MSG 28 | CVD_INCLUDE_DIR 29 | CVD_LIBRARY 30 | ) 31 | 32 | if(CVD_FOUND) 33 | set(CVD_INCLUDE_DIRS ${CVD_INCLUDE_DIR}) 34 | set(CVD_LIBRARIES ${CVD_LIBRARY}) 35 | message(STATUS "Found CVD:") 36 | message(STATUS "CVD_INCLUDE_DIRS=${CVD_INCLUDE_DIRS}") 37 | message(STATUS "CVD_LIBRARIES =${CVD_LIBRARIES}") 38 | else() 39 | message(STATUS "CVD is not found") 40 | endif() -------------------------------------------------------------------------------- /cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | 84 | -------------------------------------------------------------------------------- /cmake/FindGTest.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find GTest 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GTEST_ROOT_DIR: Base directory where all GTEST components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GTEST_FOUND 8 | # GTEST_INCLUDE_DIRS 9 | # GTEST_LIBRARIES 10 | 11 | include(FindPackageHandleStandardArgs) 12 | 13 | # GTest should be installed at /usr/local 14 | set(GTEST_ROOT_DIR /usr/local 15 | CACHE PATH "Folder contains Google gtest" 16 | ) 17 | 18 | find_path(GTEST_INCLUDE_DIR gtest/gtest.h 19 | PATHS ${GTEST_ROOT_DIR}/include) 20 | 21 | find_library(GTEST_LIBRARY gtest 22 | PATHS ${GTEST_ROOT_DIR} 23 | PATH_SUFFIXES 24 | lib 25 | lib64) 26 | 27 | find_package_handle_standard_args(GTEST 28 | DEFAULT_MSG 29 | GTEST_INCLUDE_DIR 30 | GTEST_LIBRARY 31 | ) 32 | 33 | if(GTEST_FOUND) 34 | set(GTEST_INCLUDE_DIRS ${GTEST_INCLUDE_DIR}) 35 | set(GTEST_LIBRARIES ${GTEST_LIBRARY}) 36 | message(STATUS "Found GTEST:") 37 | message(STATUS "GTEST_INCLUDE_DIRS=${GTEST_INCLUDE_DIRS}") 38 | message(STATUS "GTEST_LIBRARIES =${GTEST_LIBRARIES}") 39 | else() 40 | message(STATUS "GTEST is not found") 41 | endif() -------------------------------------------------------------------------------- /cmake/FindYaml.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Yaml 2 | # 3 | # The following variables are optionally searched for defaults 4 | # YAMLCPP_ROOT_DIR: Base directory where all YAMLCPP components are found 5 | # 6 | # The following are set after configuration is done: 7 | # YAMLCPP_FOUND 8 | # YAMLCPP_INCLUDE_DIRS 9 | # YAMLCPP_LIBRARIES 10 | 11 | include(FindPackageHandleStandardArgs) 12 | 13 | set(YAMLCPP_ROOT_DIR /usr/local/ 14 | CACHE PATH "Folder contains yaml-cpp" 15 | ) 16 | 17 | find_path(YAMLCPP_INCLUDE_DIR yaml-cpp/yaml.h 18 | PATHS ${YAMLCPP_ROOT_DIR}/include) 19 | 20 | find_library(YAMLCPP_LIBRARY yaml-cpp 21 | PATHS ${YAMLCPP_ROOT_DIR} 22 | PATH_SUFFIXES 23 | lib 24 | lib64) 25 | 26 | find_package_handle_standard_args(YAMLCPP 27 | DEFAULT_MSG 28 | YAMLCPP_INCLUDE_DIR 29 | YAMLCPP_LIBRARY 30 | ) 31 | 32 | if(YAMLCPP_FOUND) 33 | set(YAMLCPP_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIR}) 34 | set(YAMLCPP_LIBRARIES ${YAMLCPP_LIBRARY}) 35 | message(STATUS "Found YAMLCPP:") 36 | message(STATUS "YAMLCPP_INCLUDE_DIRS=${YAMLCPP_INCLUDE_DIRS}") 37 | message(STATUS "YAMLCPP_LIBRARIES =${YAMLCPP_LIBRARIES}") 38 | else() 39 | message(STATUS "YAMLCPP is not found") 40 | endif() -------------------------------------------------------------------------------- /config/config_of_mono.txt: -------------------------------------------------------------------------------- 1 | param_ba_variance_regularization_depth = 1.0e-1 2 | 3 | visualize = 2 4 | pause = 0 5 | print_camera = 0 6 | verbose_lba = 0 7 | verbose_gba = 0 8 | serial_lba = 1 9 | serial_gba = 1 10 | debug_lba = 0 11 | debug_gba = 0 12 | history_lba = 1 13 | history_gba = 1 14 | 15 | input_fps = 500 16 | 17 | input_directory = /home/jio/dataset/EuRoC/MH_01_easy/ 18 | input_image = mav0/cam0/data/*.png 19 | input_image_right = mav0/cam1/data/*.png 20 | input_feature = dat/*.dat 21 | input_ground_truth_max_time_difference = -1.0 22 | input_calibration_file = ./calibration.dat 23 | 24 | input_keyframe = *.txt 25 | input_relative_constraint = *.txt 26 | 27 | output_camera_file_gba = ../RMSE_mono/*.txt 28 | -------------------------------------------------------------------------------- /config/config_of_stereo.txt: -------------------------------------------------------------------------------- 1 | visualize = 2 2 | pause = 0 3 | print_camera = 0 4 | verbose_lba = 0 5 | verbose_gba = 0 6 | serial_lba = 1 7 | serial_gba = 1 8 | debug_lba = 0 9 | debug_gba = 0 10 | history_lba = 1 11 | history_gba = 1 12 | 13 | input_fps = 500 14 | random_seed = 10 15 | 16 | input_directory = /home/jio/dataset/EuRoC/MH_01_easy/ 17 | input_image = mav0/cam0/data/*.png 18 | input_image_right = mav0/cam1/data/*.png 19 | input_feature = dat/*.dat 20 | input_ground_truth_max_time_difference = -1.0 21 | input_calibration_file = ./calibration.dat 22 | 23 | input_keyframe = *.txt 24 | input_relative_constraint = *.txt 25 | 26 | output_camera_file_gba = ../RMSE_stereo/*.txt 27 | -------------------------------------------------------------------------------- /metadata: -------------------------------------------------------------------------------- 1 | version=0.1.27 2 | -------------------------------------------------------------------------------- /scripts/run_backend_only.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This script shows how to run back-end only mode. 4 | 5 | # You should provide feature message for each frame and a calibration file. 6 | # Our program 'ice_ba' support generating feature message and calibration file. 7 | # After running 'ice-ba' with flag 'save_feature', you will get feature message in your sequence folder: 8 | # 1. calibration.dat 9 | # 2. a folder called 'dat' which contains .dat files that correspond to frames one by one 10 | 11 | # For example, your dataset dir is /home/dataset/EuRoC/MH_01_easy 12 | # Then run: 13 | 14 | # ./bin/ice_ba --imgs_folder /home/dataset/EuRoC/MH_01_easy --start_idx 0 --end_idx -1 --iba_param_path ./config/config_of_stereo.txt --gba_camera_save_path /home/dataset/EuRoC/result/MH_01_easy.txt --stereo --save_feature 15 | 16 | # Then, there will be a calibration file: /home/dataset/EuRoC/calibration.dat, 17 | # and a folder contains lots of dat files correspond to your input images one by one: /home/dataset/EuRoC/dat/*.dat 18 | 19 | # Now, you can run back-end only by the following command 20 | # If your feature message generated by monocular mode, use config_of_mono.txt as your config file. Otherwise, config_of_stereo.txt. 21 | # Set your sequence path to 'input_directory' in config file. For the above example, the path should be: /home/dataset/EuRoC/MH_01_easy/ 22 | ../bin/back_end ../config/config_of_stereo.txt 23 | -------------------------------------------------------------------------------- /scripts/run_ice_ba_mono.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Set your own EuRoC_PATH path to run ice-ba. Use "./bin/ice_ba --help" to get the explanation for all of the flags. Flags [imgs_folder] and [iba_param_path] are necessary. 4 | # Add flag '--save_feature' to save feature message and calibration file for back-end only mode 5 | 6 | EuRoC_PATH=~/dataset/EuRoC 7 | 8 | mkdir $EuRoC_PATH/result 9 | 10 | cmd="../bin/ice_ba --imgs_folder $EuRoC_PATH/MH_01_easy --start_idx 0 --end_idx -1 --iba_param_path ../config/config_of_mono.txt --gba_camera_save_path $EuRoC_PATH/result/MH_01_easy.txt" 11 | echo $cmd 12 | eval $cmd 13 | -------------------------------------------------------------------------------- /scripts/run_ice_ba_stereo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Set your own EuRoC_PATH path to run ice-ba. Use './bin/ice_ba --help' to get the explanation for all of the flags. Flags [imgs_folder] and [iba_param_path] are necessary. 4 | # Add flag '--save_feature' to save feature message and calibration file for back-end only mode 5 | 6 | EuRoC_PATH=~/dataset/EuRoC 7 | 8 | mkdir $EuRoC_PATH/result 9 | 10 | cmd="../bin/ice_ba --imgs_folder $EuRoC_PATH/MH_01_easy --start_idx 0 --end_idx -1 --iba_param_path ../config/config_of_stereo.txt --gba_camera_save_path $EuRoC_PATH/result/MH_01_easy.txt --stereo --save_feature" 11 | echo $cmd 12 | eval $cmd 13 | --------------------------------------------------------------------------------