├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── vio.cfg └── vio_task.cfg ├── include └── tsif │ ├── element.h │ ├── element_vector.h │ ├── filter.h │ ├── filters │ ├── imu_gps.h │ └── vio.h │ ├── model.h │ ├── residual.h │ ├── residuals │ ├── accelerometer_prediction.h │ ├── attitude_findif.h │ ├── attitude_update.h │ ├── bearing_findif.h │ ├── distance_findif.h │ ├── gyroscope_update.h │ ├── image_update.h │ ├── pose_update.h │ ├── position_findif.h │ ├── position_update.h │ └── random_walk.h │ ├── timeline.h │ ├── unit_vector.h │ └── utils │ ├── camera.h │ ├── common.h │ ├── logging.h │ ├── option.h │ ├── random.h │ ├── rotation.h │ ├── simulator.h │ ├── timing.h │ └── typedefs.h └── src ├── camera.cpp ├── simulator.cpp ├── test.cpp ├── test_imu_gps.cpp ├── test_quaternion.cpp ├── test_state.cpp ├── test_timeline.cpp ├── test_unit_vector.cpp └── test_vio.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Folders 32 | /build 33 | /gtest 34 | /cmake 35 | /html 36 | /latex 37 | 38 | # Random 39 | .* 40 | *~ 41 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.8) 2 | project(two_state_implicit_filter) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native") 4 | set(TSIF_VERBOSE 0 CACHE STRING "Verbose flag") 5 | add_definitions(-DTSIF_VERBOSE=${TSIF_VERBOSE}) 6 | 7 | include_directories(include) 8 | 9 | find_package(Eigen3 REQUIRED) 10 | include_directories(${EIGEN3_INCLUDE_DIR}) 11 | 12 | add_executable(test_state src/test_state.cpp) 13 | add_executable(test_quaternion src/test_quaternion.cpp) 14 | add_executable(test_unit_vector src/test_unit_vector.cpp) 15 | add_executable(test_timeline src/test_timeline.cpp) 16 | add_executable(test_imu_gps src/test_imu_gps.cpp src/simulator.cpp src/camera.cpp) 17 | 18 | add_executable(mytest src/test.cpp) 19 | 20 | if(ENABLE_OPENCV) 21 | set(OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.2.0-dev) 22 | find_package(OpenCV REQUIRED) 23 | add_executable(test_vio src/test_vio.cpp src/simulator.cpp src/camera.cpp) 24 | target_link_libraries(test_vio ${OpenCV_LIBS}) 25 | endif() 26 | 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Autonomous Systems Lab 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # README # 2 | 3 | A modular filter library targeted at state estimation for mobile robots. -------------------------------------------------------------------------------- /cfg/vio.cfg: -------------------------------------------------------------------------------- 1 | ######### General ######### 2 | max_iter 1 3 | th_iter 10000.1 4 | use_sim 0 5 | timeOffsetCam 0 6 | timeOffsetGyr -0.05 7 | timeOffsetAcc 0 8 | do_draw 1 9 | draw_tracking 1 10 | draw_adding 0 11 | 12 | ######### Residual weighting ######### 13 | w_posfd 1e2 14 | w_attfd 1e3 15 | w_accpre 1e1 16 | w_gyrupd 1e3 17 | w_accbias 1e3 18 | w_gyrbias 1e3 19 | w_imgupd 3e2 20 | w_beapre 1e2 21 | w_dispre 1e2 22 | w_veppre 1e4 23 | w_veapre 1e4 24 | 25 | ######### State initialization ######### 26 | init_vep -0.0111674199187 -0.0574640920022 0.0207586947896 27 | init_vea 0.712115587266 0.00666398307551 -0.0079168224269 -0.701985972528 28 | init_dis 0.5 29 | initstd_pos 1e2 30 | initstd_att 1e1 31 | initstd_vel 1e1 32 | initstd_ror 1e1 33 | initstd_acb 1e1 34 | initstd_gyb 1e1 35 | initstd_vep 1e3 36 | initstd_vea 1e3 37 | initstd_bea 1e2 38 | initstd_dis 1e0 39 | 40 | ######### Landmark tracking ######### 41 | match_desc_distance 100 42 | match_geom_distance 3 43 | prune_count 5 44 | num_candidates_matching 10 45 | num_candidates_add 500 46 | bucket_count 6 47 | neighbor_suppression 50 48 | start_add 5 49 | scale_factor 1.2 50 | n_levels 1 51 | img_huber_th 0.01 52 | single_match_only 0 53 | 54 | -------------------------------------------------------------------------------- /cfg/vio_task.cfg: -------------------------------------------------------------------------------- 1 | data_folder /home/michael/datasets/euroc/MH_01_easy 2 | 3 | # std::string folder = "/home/michael/datasets/euroc/MH_01_easy"; 4 | # std::string folder = "/home/michael/datasets/euroc/MH_02_easy"; 5 | # std::string folder = "/home/michael/datasets/euroc/MH_03_medium"; 6 | # std::string folder = "/home/michael/datasets/euroc/MH_04_difficult"; 7 | # std::string folder = "/home/michael/datasets/euroc/MH_05_difficult"; -------------------------------------------------------------------------------- /include/tsif/element.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_ELEMENT_H_ 2 | #define TSIF_ELEMENT_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "unit_vector.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | struct ElementTraits{ 11 | static constexpr bool kIsVectorSpace = true; 12 | static constexpr int kDim = 0; 13 | static std::string Print(const T& x){ 14 | return ""; 15 | } 16 | static T Identity(){ 17 | T x; 18 | return x; 19 | } 20 | static void SetRandom(T& x){ 21 | } 22 | static void Boxplus(const T& ref, const VecCRef& vec, T& out){ 23 | out = ref; // Must be computable in-place 24 | } 25 | static void Boxminus(const T& in, const T& ref, VecRef vec){ 26 | } 27 | static Mat BoxplusJacInp(const T& in, const VecCRef& vec){ 28 | return Mat::Identity(); 29 | } 30 | static Mat BoxplusJacVec(const T& in, const VecCRef& vec){ 31 | return Mat::Identity(); 32 | } 33 | static Mat BoxminusJacInp(const T& in, const T& ref){ 34 | return Mat::Identity(); 35 | } 36 | static Mat BoxminusJacRef(const T& in, const T& ref){ 37 | return Mat::Identity(); 38 | } 39 | static Vec GetVec(const T& x){ 40 | return Vec::Zero(); 41 | } 42 | static void Scale(double w, T& x){ 43 | } 44 | }; 45 | 46 | template 47 | class Element{ 48 | private: 49 | alignas(16) T x_; 50 | typedef ElementTraits Traits; 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | typedef T Type; 54 | static const int kI = I; 55 | static const int kDim = Traits::kDim*(int)(I>=0); 56 | static const bool kIsVectorSpace = Traits::kIsVectorSpace; 57 | Element(){} 58 | Element(const T& x): x_(x){} 59 | T& Get(){ 60 | return x_; 61 | } 62 | const T& Get() const{ 63 | return x_; 64 | } 65 | std::string Print() const{ 66 | std::ostringstream out; 67 | out << (I>=0 ? "\033[32m" : "\033[33m"); 68 | out << Traits::Print(x_) << "\033[0m"; 69 | return out.str(); 70 | } 71 | void SetIdentity(){ 72 | x_ = Traits::Identity(); 73 | } 74 | void SetRandom(){ 75 | Traits::SetRandom(x_); 76 | } 77 | template=0,typename std::enable_if::type* = nullptr> 78 | void Boxplus(const VecCRef& vec, Element& out) const{ 79 | Traits::Boxplus(x_, vec, out.Get()); 80 | } 81 | template=0,typename std::enable_if::type* = nullptr> 82 | void Boxplus(const VecCRef& vec, Element& out) const{ 83 | out.Get() = x_; 84 | } 85 | template=0,typename std::enable_if::type* = nullptr> 86 | void Boxminus(const Element& ref, VecRef vec) const{ 87 | Traits::Boxminus(x_, ref.Get(), vec); 88 | } 89 | template=0,typename std::enable_if::type* = nullptr> 90 | void Boxminus(const Element& ref, VecRef vec) const{ 91 | } 92 | template=0,typename std::enable_if::type* = nullptr> 93 | Mat BoxplusJacInp(const VecCRef& vec) const{ 94 | return Traits::BoxplusJacInp(x_, vec); 95 | } 96 | template=0,typename std::enable_if::type* = nullptr> 97 | Mat BoxplusJacInp(const VecCRef& vec) const{ 98 | return Mat::Zero(); 99 | } 100 | template=0,typename std::enable_if::type* = nullptr> 101 | Mat BoxplusJacVec(const VecCRef& vec) const{ 102 | return Traits::BoxplusJacVec(x_, vec); 103 | } 104 | template=0,typename std::enable_if::type* = nullptr> 105 | Mat BoxplusJacVec(const VecCRef& vec) const{ 106 | return Mat::Zero(); 107 | } 108 | template=0,typename std::enable_if::type* = nullptr> 109 | Mat BoxminusJacInp(const Element& ref) const{ 110 | return Traits::BoxminusJacInp(x_, ref.Get()); 111 | } 112 | template=0,typename std::enable_if::type* = nullptr> 113 | Mat BoxminusJacInp(const Element& ref) const{ 114 | return Mat::Zero(); 115 | } 116 | template=0,typename std::enable_if::type* = nullptr> 117 | Mat BoxminusJacRef(const Element& ref) const{ 118 | return Traits::BoxminusJacRef(x_, ref.Get()); 119 | } 120 | template=0,typename std::enable_if::type* = nullptr> 121 | Mat BoxminusJacRef(const Element& ref) const{ 122 | return Mat::Zero(); 123 | } 124 | template=0,typename std::enable_if::type* = nullptr> 125 | Vec GetVec() const{ 126 | return Traits::GetVec(x_); 127 | } 128 | template=0,typename std::enable_if::type* = nullptr> 129 | Vec GetVec() const{ 130 | return Vec::Zero(); 131 | } 132 | void Scale(double w){ 133 | Traits::Scale(w,x_); 134 | } 135 | }; 136 | 137 | template 138 | const int Element::kI; 139 | 140 | // ==================== Traits Implementation ==================== // 141 | /*! \brief Scalar Trait. 142 | * Element trait for regular scalar. 143 | */ 144 | template<> 145 | class ElementTraits{ 146 | public: 147 | static constexpr bool kIsVectorSpace = true; 148 | static constexpr int kDim = 1; 149 | static std::string Print(const double& x){ 150 | std::ostringstream out; 151 | out << x; 152 | return out.str(); 153 | } 154 | static double Identity(){ 155 | return 0; 156 | } 157 | static void SetRandom(double& x){ 158 | x = NormalRandomNumberGenerator::Instance().Get(); 159 | } 160 | static void Boxplus(const double& in, const VecCRef& vec, double& out){ 161 | out = in + vec(0); 162 | } 163 | static void Boxminus(const double& in, const double& ref, VecRef vec){ 164 | vec(0) = in - ref; 165 | } 166 | static Mat BoxplusJacInp(const double& in, const VecCRef& vec){ 167 | return Mat::Identity(); 168 | } 169 | static Mat BoxplusJacVec(const double& in, const VecCRef& vec){ 170 | return Mat::Identity(); 171 | } 172 | static Mat BoxminusJacInp(const double& in, const double& ref){ 173 | return Mat::Identity(); 174 | } 175 | static Mat BoxminusJacRef(const double& in, const double& ref){ 176 | return -Mat::Identity(); 177 | } 178 | static Vec GetVec(const double& x){ 179 | return Vec<1>(x); 180 | } 181 | static void Scale(double w, double& x){ 182 | x *= w; 183 | } 184 | }; 185 | 186 | /*! \brief Vector Trait. 187 | * Element trait for vector of scalars 188 | */ 189 | template 190 | class ElementTraits>{ 191 | public: 192 | static constexpr bool kIsVectorSpace = true; 193 | static constexpr int kDim = N; 194 | static std::string Print(const Vec& x){ 195 | std::ostringstream out; 196 | out << x.transpose(); 197 | return out.str(); 198 | } 199 | static Vec Identity(){ 200 | return Vec::Zero(); 201 | } 202 | static void SetRandom(Vec& x){ 203 | x = NormalRandomNumberGenerator::Instance().GetVec(); 204 | } 205 | static void Boxplus(const Vec& in, const VecCRef& vec, Vec& out){ 206 | out = in + vec; 207 | } 208 | static void Boxminus(const Vec& in, const Vec& ref, VecRef vec){ 209 | vec = in - ref; 210 | } 211 | static Mat BoxplusJacInp(const Vec& in, const VecCRef& vec){ 212 | return Mat::Identity(); 213 | } 214 | static Mat BoxplusJacVec(const Vec& in, const VecCRef& vec){ 215 | return Mat::Identity(); 216 | } 217 | static Mat BoxminusJacInp(const Vec& in, const Vec& ref){ 218 | return Mat::Identity(); 219 | } 220 | static Mat BoxminusJacRef(const Vec& in, const Vec& ref){ 221 | return -Mat::Identity(); 222 | } 223 | static Vec GetVec(const Vec& x){ 224 | return x; 225 | } 226 | static void Scale(double w, Vec& x){ 227 | x *= w; 228 | } 229 | }; 230 | 231 | /*! \brief Unit Quaternion Trait. 232 | * Element trait for unit quaternion. Employed to represent 233 | * orientations. 234 | */ 235 | template<> 236 | class ElementTraits{ 237 | public: 238 | static constexpr bool kIsVectorSpace = false; 239 | static constexpr int kDim = 3; 240 | static std::string Print(const Quat& x){ 241 | std::ostringstream out; 242 | out << x.w() << " " << x.x() << " " << x.y() << " " << x.z(); 243 | return out.str(); 244 | } 245 | static Quat Identity(){ 246 | return Quat::Identity(); 247 | } 248 | static void SetRandom(Quat& x){ 249 | x.w() = NormalRandomNumberGenerator::Instance().Get(); 250 | x.x() = NormalRandomNumberGenerator::Instance().Get(); 251 | x.y() = NormalRandomNumberGenerator::Instance().Get(); 252 | x.z() = NormalRandomNumberGenerator::Instance().Get(); 253 | x.normalize(); 254 | } 255 | static void Boxplus(const Quat& in, const VecCRef& vec, Quat& out){ 256 | out = tsif::Boxplus(in,vec); 257 | out.normalize(); 258 | } 259 | static void Boxminus(const Quat& in, const Quat& ref, VecRef vec){ 260 | vec = tsif::Boxminus(in,ref); 261 | } 262 | static Mat BoxplusJacInp(const Quat& in, const VecCRef& vec){ 263 | return Exp(vec).toRotationMatrix(); 264 | } 265 | static Mat BoxplusJacVec(const Quat& in, const VecCRef& vec){ 266 | return GammaMat(vec); 267 | } 268 | static Mat BoxminusJacInp(const Quat& in, const Quat& ref){ 269 | return GammaMat(tsif::Boxminus(in,ref)).inverse(); 270 | } 271 | static Mat BoxminusJacRef(const Quat& in, const Quat& ref){ 272 | return -GammaMat(tsif::Boxminus(in,ref)).inverse() * (in*ref.inverse()).toRotationMatrix(); 273 | } 274 | static Vec GetVec(const Quat& x){ 275 | return Log(x); 276 | } 277 | static void Scale(double w, Quat& x){ 278 | x = Exp(w*Log(x)); 279 | } 280 | }; 281 | 282 | /*! \brief Unit Vector Trait. 283 | * Element trait for unit vectors. 284 | */ 285 | template<> 286 | class ElementTraits{ 287 | public: 288 | static constexpr bool kIsVectorSpace = false; 289 | static constexpr int kDim = 2; 290 | static std::string Print(const UnitVector& x){ 291 | std::ostringstream out; 292 | out << x.GetVec().transpose(); 293 | return out.str(); 294 | } 295 | static UnitVector Identity(){ 296 | return UnitVector(Quat::Identity()); 297 | } 298 | static void SetRandom(UnitVector& x){ 299 | x.SetRandom(); 300 | } 301 | static void Boxplus(const UnitVector& in, const VecCRef& vec, UnitVector& out){ 302 | in.Boxplus(vec,out); 303 | } 304 | static void Boxminus(const UnitVector& in, const UnitVector& ref, VecRef vec){ 305 | in.Boxminus(ref,vec); 306 | } 307 | static Mat BoxplusJacInp(const UnitVector& in, const VecCRef& vec){ 308 | Mat J; 309 | in.BoxplusJacInp(vec,J); 310 | return J; 311 | } 312 | static Mat BoxplusJacVec(const UnitVector& in, const VecCRef& vec){ 313 | Mat J; 314 | in.BoxplusJacVec(vec,J); 315 | return J; 316 | } 317 | static Mat BoxminusJacInp(const UnitVector& in, const UnitVector& ref){ 318 | Mat J; 319 | in.BoxminusJacInp(ref,J); 320 | return J; 321 | } 322 | static Mat BoxminusJacRef(const UnitVector& in, const UnitVector& ref){ 323 | Mat J; 324 | in.BoxminusJacRef(ref,J); 325 | return J; 326 | } 327 | static Vec GetVec(const UnitVector& x){ 328 | Vec vec; 329 | x.Boxminus(Identity(),vec); 330 | return vec; 331 | } 332 | static void Scale(double w, UnitVector& x){ 333 | Vec vec; 334 | x.Boxminus(Identity(),vec); 335 | vec *= w; 336 | Identity().Boxplus(vec,x); 337 | } 338 | }; 339 | 340 | /*! \brief Array Trait. 341 | * Element trait for array of specific sub-elements 342 | */ 343 | template 344 | class ElementTraits>{ 345 | public: 346 | typedef ElementTraits Traits; 347 | using array = std::array; 348 | static constexpr bool kIsVectorSpace = Traits::kIsVectorSpace; 349 | static constexpr int kElementDim = Traits::kDim; 350 | static constexpr int kDim = N * kElementDim; 351 | static std::string Print(const array& x){ 352 | std::ostringstream out; 353 | for (const T& i : x){ 354 | out << Traits::Print(i) << "\t"; 355 | } 356 | return out.str(); 357 | 358 | } 359 | static array Identity(){ 360 | array x; 361 | for (T& i : x){ 362 | i = Traits::Identity(); 363 | } 364 | return x; 365 | } 366 | static void SetRandom(array& x){ 367 | for (T& i : x){ 368 | Traits::SetRandom(i); 369 | } 370 | } 371 | static void Boxplus(const array& in, const VecCRef& vec, array& out){ 372 | for (int i = 0; i < N; i++){ 373 | Traits::Boxplus(in.at(i), vec.template block(i * kElementDim, 0), out.at(i)); 374 | } 375 | } 376 | static void Boxminus(const array& in, const array& ref, VecRef vec){ 377 | for (int i = 0; i < N; i++){ 378 | Traits::Boxminus(in.at(i), ref.at(i), vec.template block(i * kElementDim, 0)); 379 | } 380 | } 381 | static Mat BoxplusJacInp(const array& in, const VecCRef& vec){ 382 | Mat J = Mat::Zero(); 383 | for (int i = 0; i < N; i++){ 384 | J.template block(i * kElementDim, i * kElementDim) = 385 | Traits::BoxplusJacInp(in.at(i), vec.template block(i * kElementDim, 0)); 386 | } 387 | return J; 388 | } 389 | static Mat BoxplusJacVec(const array& in, const VecCRef& vec){ 390 | Mat J = Mat::Zero(); 391 | for (int i = 0; i < N; i++){ 392 | J.template block(i * kElementDim, i * kElementDim) = 393 | Traits::BoxplusJacVec(in.at(i), vec.template block(i * kElementDim, 0)); 394 | } 395 | return J; 396 | } 397 | static Mat BoxminusJacInp(const array& in, const array& ref){ 398 | Mat J = Mat::Zero(); 399 | for (int i = 0; i < N; i++){ 400 | J.template block(i * kElementDim, i * kElementDim) = 401 | Traits::BoxminusJacInp(in.at(i), ref.at(i)); 402 | } 403 | return J; 404 | } 405 | static Mat BoxminusJacRef(const array& in, const array& ref){ 406 | Mat J = Mat::Zero(); 407 | for (int i = 0; i < N; i++){ 408 | J.template block(i * kElementDim, i * kElementDim) = 409 | Traits::BoxminusJacRef(in.at(i), ref.at(i)); 410 | } 411 | return J; 412 | } 413 | static Vec GetVec(const array& x){ 414 | Vec vec; 415 | for (int i = 0; i < N; i++){ 416 | vec.template segment(i * kElementDim) = x.at(i); 417 | } 418 | return vec; 419 | } 420 | static void Scale(double w, array& x){ 421 | for (int i = 0; i < N; i++){ 422 | Traits::Scale(w,x.at(i)); 423 | } 424 | } 425 | }; 426 | 427 | } // namespace tsif 428 | 429 | #endif // TSIF_ELEMENT_H_ 430 | -------------------------------------------------------------------------------- /include/tsif/element_vector.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_ELEMENT_VECTOR_H_ 2 | #define TSIF_ELEMENT_VECTOR_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/element.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | struct DimensionTrait; 11 | template <> 12 | struct DimensionTrait<>{ 13 | static const int kDim = 0; 14 | }; 15 | template 16 | struct DimensionTrait{ 17 | static const int kDim = Element::kDim + DimensionTrait::kDim; 18 | }; 19 | 20 | template 21 | struct CheckAllTrue; 22 | template 23 | struct CheckAllTrue{ 24 | static const bool kVal = B; 25 | }; 26 | template 27 | struct CheckAllTrue{ 28 | static const bool kVal = B & CheckAllTrue::kVal; 29 | }; 30 | 31 | 32 | template 33 | class ElementVectorBase{ 34 | private: 35 | typedef std::tuple Tuple; 36 | 37 | public: 38 | static const int kN = sizeof...(Elements); 39 | static const bool kIsVectorSpace = CheckAllTrue::kVal; 40 | 41 | template::type::kI == I)>::type* = nullptr> 42 | static constexpr int GetC(){ 43 | return C; 44 | } 45 | template::type::kI != I))>::type* = nullptr> 46 | static constexpr int GetC(){ 47 | static_assert(C < kN-1, "Index not found in ElementVector"); 48 | return GetC(); 49 | } 50 | 51 | template 52 | typename std::tuple_element(),Tuple>::type::Type& Get(){ 53 | return static_cast(*this).Get(); 54 | } 55 | template 56 | const typename std::tuple_element(),Tuple>::type::Type& Get() const{ 57 | return static_cast(*this).Get(); 58 | } 59 | 60 | template 61 | typename std::tuple_element(),Tuple>::type& GetElement(){ 62 | return static_cast(*this).GetElement(); 63 | } 64 | template 65 | const typename std::tuple_element(),Tuple>::type& GetElement() const{ 66 | return static_cast(*this).GetElement(); 67 | } 68 | 69 | template::type* = nullptr> 70 | static constexpr bool HasElement(){ 71 | return std::is_same::type,Element>::value ? true : HasElement(); 72 | } 73 | template= kN)>::type* = nullptr> 74 | static constexpr bool HasElement(){ 75 | return false; 76 | } 77 | 78 | template::type* = nullptr> 79 | static constexpr bool HasId(){ 80 | return std::tuple_element::type::kI == I ? true : HasId(); 81 | } 82 | template= kN)>::type* = nullptr> 83 | static constexpr bool HasId(){ 84 | return false; 85 | } 86 | 87 | template 88 | static constexpr int GetId(){ 89 | return std::tuple_element::type::kI; 90 | } 91 | 92 | template 93 | static constexpr int GetElementDim(){ 94 | return std::tuple_element(),Tuple>::type::kDim; 95 | } 96 | 97 | template::type* = nullptr> 98 | std::string Print() const{ 99 | return GetElement()>().Print() + "\n" + Print(); 100 | } 101 | template= kN)>::type* = nullptr> 102 | std::string Print() const{ 103 | return ""; 104 | } 105 | 106 | template::type* = nullptr> 107 | void SetIdentity(){ 108 | GetElement()>().SetIdentity(); 109 | SetIdentity(); 110 | } 111 | template= kN)>::type* = nullptr> 112 | void SetIdentity(){} 113 | 114 | template::type* = nullptr> 115 | void SetRandom(){ 116 | GetElement()>().SetRandom(); 117 | SetRandom(); 118 | } 119 | template= kN)>::type* = nullptr> 120 | void SetRandom(){} 121 | 122 | template::type* = nullptr> 123 | void Boxplus(const VecCRefX& vec, ElementVectorBase& out) const{ 124 | assert(vec.size() == Dim()); 125 | typedef typename std::tuple_element::type E; 126 | if(E::kDim > 0){ 127 | GetElement().Boxplus(vec.template block(Start(E::kI),0),out.GetElement()); 128 | } 129 | Boxplus(vec,out); 130 | } 131 | template= kN)>::type* = nullptr> 132 | void Boxplus(const VecCRefX& vec, ElementVectorBase& out) const{} 133 | 134 | template::type* = nullptr> 135 | void Boxminus(const ElementVectorBase& ref, VecRefX out) const{ 136 | assert(out.size() == Dim()); 137 | typedef typename std::tuple_element::type E; 138 | if(E::kDim > 0){ 139 | GetElement().Boxminus(ref.GetElement(),out.template block(Start(E::kI),0)); 140 | } 141 | Boxminus(ref,out); 142 | } 143 | template= kN)>::type* = nullptr> 144 | void Boxminus(const ElementVectorBase& ref, VecRefX out) const{} 145 | 146 | template::type* = nullptr> 147 | void GetVec(VecRefX vec) const{ 148 | assert(vec.size() == Dim()); 149 | typedef typename std::tuple_element::type E; 150 | if(E::kDim>0){ 151 | vec.template block(Start(E::kI),0) = GetElement().GetVec(); 152 | } 153 | GetVec(vec); 154 | } 155 | template= kN)>::type* = nullptr> 156 | void GetVec(VecRefX vec) const{} 157 | 158 | template::type* = nullptr> 159 | void Scale(double w){ 160 | typedef typename std::tuple_element::type E; 161 | GetElement().Scale(w); 162 | Scale(w); 163 | } 164 | template= kN)>::type* = nullptr> 165 | void Scale(double w){} 166 | 167 | int Start(int I) const{ 168 | return static_cast(*this).Start(I); 169 | } 170 | int Dim() const{ 171 | return static_cast(*this).Dim(); 172 | } 173 | }; 174 | 175 | template 176 | class ElementVectorRef: public ElementVectorBase,Elements...>{ 177 | private: 178 | typedef ElementVectorBase,Elements...> Base; 179 | typedef std::tuple Tuple; 180 | std::tuple elements_; 181 | const std::map startMap_; 182 | const int Dim_; 183 | 184 | public: 185 | static constexpr int kN = Base::kN; 186 | template 187 | ElementVectorRef(ElementVectorBase& elementVector): 188 | elements_(elementVector.GetElement()...), 189 | startMap_{std::make_pair(Elements::kI,elementVector.Start(Elements::kI))...}, 190 | Dim_(elementVector.Dim()){ 191 | } 192 | ~ElementVectorRef(){} 193 | 194 | template 195 | typename std::tuple_element(),Tuple>::type::Type& Get(){ 196 | return std::gettemplate GetC()>(elements_).Get(); 197 | } 198 | template 199 | const typename std::tuple_element(),Tuple>::type::Type& Get() const{ 200 | return std::gettemplate GetC()>(elements_).Get(); 201 | } 202 | template 203 | typename std::tuple_element(),Tuple>::type& GetElement(){ 204 | return std::gettemplate GetC()>(elements_); 205 | } 206 | template 207 | const typename std::tuple_element(),Tuple>::type& GetElement() const{ 208 | return std::gettemplate GetC()>(elements_); 209 | } 210 | int Start(int I) const{ 211 | return startMap_.at(I); 212 | } 213 | int Dim() const{ 214 | return Dim_; 215 | } 216 | }; 217 | 218 | template 219 | class ElementVectorConstRef: public ElementVectorBase,Elements...>{ 220 | private: 221 | typedef ElementVectorBase,Elements...> Base; 222 | typedef std::tuple Tuple; 223 | std::tuple elements_; 224 | const std::map startMap_; 225 | const int Dim_; 226 | 227 | public: 228 | static constexpr int kN = Base::kN; 229 | template 230 | ElementVectorConstRef(const ElementVectorBase& elementVector): 231 | elements_(elementVector.GetElement()...), 232 | startMap_{std::make_pair(Elements::kI,elementVector.Start(Elements::kI))...}, 233 | Dim_(elementVector.Dim()){ 234 | } 235 | ~ElementVectorConstRef(){} 236 | 237 | template 238 | const typename std::tuple_element(),Tuple>::type::Type& Get() const{ 239 | return std::gettemplate GetC()>(elements_).Get(); 240 | } 241 | template 242 | const typename std::tuple_element(),Tuple>::type& GetElement() const{ 243 | return std::gettemplate GetC()>(elements_); 244 | } 245 | int Start(int I) const{ 246 | return startMap_.at(I); 247 | } 248 | int Dim() const{ 249 | return Dim_; 250 | } 251 | }; 252 | 253 | template 254 | class ElementVector: public ElementVectorBase,Elements...>{ 255 | private: 256 | typedef ElementVectorBase,Elements...> Base; 257 | typedef std::tuple Tuple; 258 | alignas(16) Tuple elements_; 259 | 260 | public: 261 | typedef ElementVectorRef Ref; 262 | typedef ElementVectorConstRef CRef; 263 | static constexpr int kN = Base::kN; 264 | ElementVector(){} 265 | template 266 | ElementVector(const ElementVectorBase& elementVector): 267 | elements_(elementVector.GetElement()...){} 268 | template C)>::type* = nullptr> 269 | ElementVector(const typename Elements::Type&... elements): 270 | elements_(elements...){} 271 | ~ElementVector(){} 272 | 273 | template 274 | typename std::tuple_element(),Tuple>::type::Type& Get(){ 275 | return std::gettemplate GetC()>(elements_).Get(); 276 | } 277 | template 278 | const typename std::tuple_element(),Tuple>::type::Type& Get() const{ 279 | return std::gettemplate GetC()>(elements_).Get(); 280 | } 281 | 282 | template 283 | typename std::tuple_element(),Tuple>::type& GetElement(){ 284 | return std::gettemplate GetC()>(elements_); 285 | } 286 | template 287 | const typename std::tuple_element(),Tuple>::type& GetElement() const{ 288 | return std::gettemplate GetC()>(elements_); 289 | } 290 | static constexpr int Start(int I){ 291 | return _Start(I); 292 | } 293 | template::type* = nullptr> 294 | static constexpr int _Start(int I){ 295 | return (std::tuple_element::type::kI == I) ? 0 : std::tuple_element::type::kDim + _Start(I); 296 | } 297 | template= kN)>::type* = nullptr> 298 | static constexpr int _Start(int I){ 299 | return -1; 300 | } 301 | static constexpr int Dim(){ 302 | return DimensionTrait::kDim; 303 | } 304 | }; 305 | 306 | template 307 | struct AddElementTrait; 308 | template 309 | struct AddElementTrait,Element,true>{ 310 | typedef ElementVector Type; 311 | }; 312 | template 313 | struct AddElementTrait,Element,false>{ 314 | typedef ElementVector Type; 315 | }; 316 | 317 | template 318 | struct MergeTwo; 319 | 320 | template 321 | struct MergeTwo,ElementVector<>>{ 322 | typedef ElementVector Type; 323 | }; 324 | template 325 | struct MergeTwo,ElementVector>{ 326 | static_assert(!ElementVector::template HasId() | 327 | ElementVector::template HasElement(),"Conflicting Merge"); 328 | typedef typename MergeTwo,Element, 329 | !ElementVector::template HasElement()>::Type, 330 | ElementVector>::Type Type; 331 | }; 332 | 333 | template 334 | struct MergeTrait; 335 | template 336 | struct MergeTrait{ 337 | typedef ElementVector Type; 338 | }; 339 | template 340 | struct MergeTrait{ 341 | typedef typename MergeTrait::Type, 342 | ElementVectors...>::Type Type; 343 | }; 344 | 345 | class MeasEmpty: public ElementVector<>{ 346 | }; 347 | 348 | } // namespace tsif 349 | 350 | #endif // TSIF_ELEMENT_VECTOR_H_ 351 | -------------------------------------------------------------------------------- /include/tsif/filter.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_FILTER_H_ 2 | #define TSIF_FILTER_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | #include "tsif/timeline.h" 7 | 8 | namespace tsif{ 9 | 10 | template 11 | class Filter{ 12 | public: 13 | static constexpr int kN = sizeof...(Residuals); 14 | typedef typename MergeTrait::Type State; 15 | typedef std::tuple ResidualTuple; 16 | typedef std::tuple...> TimelineTuple; 17 | alignas(16) ResidualTuple residuals_; 18 | alignas(16) TimelineTuple timelines_; 19 | 20 | Filter(): timelines_(Timeline(fromSec(0.1),fromSec(0.0))...), 21 | I_(State::Dim(),State::Dim()){ 22 | is_initialized_ = false; 23 | include_max_ = false; 24 | max_iter_ = 1; 25 | th_iter_ = 0.1; 26 | iter_ = 0; 27 | weightedDelta_ = 0; 28 | } 29 | virtual ~Filter(){} 30 | 31 | template 32 | void AddMeas(TimePoint t,std::shared_ptr::type::Measurement> m){ 33 | TSIF_LOGWIF(t < time_,"Warning: adding measurement in past!"); 34 | TSIF_LOGWIF(is_initialized_ && (t-time_ > fromSec(10)),"Warning: measurement far in future!"); 35 | std::get(timelines_).Add(t,m); 36 | } 37 | 38 | template::type* = nullptr> 39 | TimePoint GetMaxUpdateTime(TimePoint current){ 40 | return std::min(std::get(timelines_).GetMaximalUpdateTime(current), GetMaxUpdateTime(current)); 41 | } 42 | template= kN)>::type* = nullptr> 43 | TimePoint GetMaxUpdateTime(TimePoint current){ 44 | return current; 45 | } 46 | 47 | template::type* = nullptr> 48 | TimePoint GetCurrentTime(){ 49 | TimePoint lastTime = std::get(timelines_).GetLastTime(); 50 | return lastTime == TimePoint::max() ? GetCurrentTime() : std::max(std::get(timelines_).GetLastTime(), GetCurrentTime()); 51 | } 52 | template= kN)>::type* = nullptr> 53 | TimePoint GetCurrentTime(){ 54 | return TimePoint::min(); 55 | } 56 | 57 | template::type* = nullptr> 58 | TimePoint GetMinMaxTime(){ 59 | return std::min(std::get(timelines_).GetLastTime(), GetMinMaxTime()); 60 | } 61 | template= kN)>::type* = nullptr> 62 | TimePoint GetMinMaxTime(){ 63 | return TimePoint::max(); 64 | } 65 | 66 | template::type* = nullptr> 67 | TimePoint GetMaxMinTime(){ 68 | return std::max(std::get(timelines_).GetFirstTime(), GetMaxMinTime()); 69 | } 70 | template= kN)>::type* = nullptr> 71 | TimePoint GetMaxMinTime(){ 72 | return TimePoint::min(); 73 | } 74 | 75 | template::type* = nullptr> 76 | void PrintTimelines(const TimePoint& start, int startOffset, double resolution){ 77 | PrintTimelines(start,startOffset,resolution); 78 | TSIF_LOG(std::get(timelines_).Print(start, startOffset, resolution)); 79 | } 80 | template= kN)>::type* = nullptr> 81 | void PrintTimelines(const TimePoint& start, int startOffset, double resolution){ 82 | std::ostringstream out; 83 | for (int i = 0; i < startOffset; i++){ 84 | out << " "; 85 | } 86 | out << "|"; 87 | TSIF_LOG(out.str()); 88 | } 89 | 90 | template::type* = nullptr> 91 | void GetTimeList(std::set& times, TimePoint maxUpdateTime, bool includeMax) const{ 92 | if (!std::get(residuals_).isMergeable_){ 93 | std::get(timelines_).GetAllInRange(times, time_, maxUpdateTime); 94 | } 95 | GetTimeList(times,maxUpdateTime,includeMax); 96 | } 97 | template= kN)>::type* = nullptr> 98 | void GetTimeList(std::set& times, TimePoint maxUpdateTime, bool includeMax) const{ 99 | if (includeMax && maxUpdateTime > time_){ 100 | times.insert(maxUpdateTime); 101 | } 102 | } 103 | 104 | template::type* = nullptr> 105 | void SplitAndMerge(TimePoint time, const std::set& times){ 106 | std::get(timelines_).SplitAndMerge(time,times,std::get(residuals_)); 107 | SplitAndMerge(time,times); 108 | } 109 | template= kN)>::type* = nullptr> 110 | void SplitAndMerge(TimePoint time, const std::set& times){} 111 | 112 | template::type* = nullptr> 113 | void Clean(TimePoint time){ 114 | std::get(timelines_).Clean(time); 115 | Clean(time); 116 | } 117 | template= kN)>::type* = nullptr> 118 | void Clean(TimePoint time){} 119 | 120 | virtual void Init(TimePoint t){ 121 | if(GetMinMaxTime() != TimePoint::min()){ 122 | TSIF_LOG("Initializing state at t = " << Print(t)); 123 | startTime_ = t; 124 | time_ = t; 125 | state_.SetIdentity(); 126 | I_.setIdentity(); 127 | is_initialized_ = true; 128 | } 129 | } 130 | virtual void ComputeLinearizationPoint(const TimePoint& t){ 131 | curLinState_ = state_; 132 | } 133 | virtual void PreProcess(){}; 134 | virtual void PostProcess(){}; 135 | 136 | void Update(){ 137 | // Initialize if possible 138 | if(!is_initialized_){ 139 | Init(GetMaxMinTime()); 140 | } 141 | 142 | if(is_initialized_){ 143 | TSIF_LOG("Timelines before processing:"); 144 | PrintTimelines(time_, 20, 0.001); 145 | TSIF_LOG("State time:\t" << Print(time_)); 146 | TimePoint current = GetCurrentTime(); 147 | TSIF_LOG("Current time:\t" << Print(current)); 148 | TimePoint maxUpdateTime = GetMaxUpdateTime(current); 149 | TSIF_LOG("Maximal update time:\t" << Print(maxUpdateTime)); 150 | std::set times; 151 | GetTimeList(times, maxUpdateTime, include_max_); 152 | std::ostringstream out; 153 | out << "Update times:\t"; 154 | for (const auto& t : times){ 155 | out << Print(t) << " "; 156 | } 157 | TSIF_LOG(out.str()); 158 | SplitAndMerge(time_,times); 159 | TSIF_LOG("Timelines after split and merging:"); 160 | PrintTimelines(time_, 20, 0.001); 161 | 162 | // Carry out updates 163 | for (const auto& t : times){ 164 | MakeUpdateStep(t); 165 | } 166 | Clean(time_); 167 | TSIF_LOG("Timelines after cleaning:"); 168 | PrintTimelines(time_, 20, 0.001); 169 | } 170 | } 171 | 172 | void MakeUpdateStep(TimePoint t){ 173 | // Compute linearisation point 174 | ComputeLinearizationPoint(t); 175 | 176 | // Check available measurements and prepare residuals 177 | int innDim = PreProcessResidual(t); 178 | PreProcess(); 179 | 180 | // Temporaries 181 | y_.resize(innDim); 182 | y_.setZero(); 183 | JacPre_.resize(innDim, State::Dim()); 184 | JacPre_.setZero(); 185 | JacCur_.resize(innDim, State::Dim()); 186 | JacCur_.setZero(); 187 | 188 | weightedDelta_ = th_iter_; 189 | MatX newInf(State::Dim(),State::Dim()); 190 | for(iter_=0;iter_= th_iter_;iter_++){ 191 | ConstructProblem(0); 192 | TSIF_LOG("Innovation:\t" << y_.transpose()); 193 | TSIF_LOG("JacPre:\n" << JacPre_); 194 | TSIF_LOG("JacCur:\n" << JacCur_); 195 | 196 | // Compute Kalman Update // TODO use more efficient form 197 | MatX D = I_ + JacPre_.transpose() * JacPre_; 198 | MatX J(innDim,innDim); J.setIdentity(); 199 | #if TSIF_VERBOSE > 0 200 | Eigen::JacobiSVD svdD(D); 201 | const double condD = svdD.singularValues()(0) / svdD.singularValues()(svdD.singularValues().size()-1); 202 | TSIF_LOG("D condition number:\n" << condD); 203 | #endif 204 | MatX S = JacCur_.transpose() * (J - JacPre_ * D.inverse() * JacPre_.transpose()); 205 | newInf = S * JacCur_; 206 | newInf = 0.5*(newInf + newInf.transpose().eval()); 207 | Eigen::LDLT I_LDLT(newInf); 208 | #if TSIF_VERBOSE > 0 209 | Eigen::JacobiSVD svdI(newInf); 210 | const double condI = svdI.singularValues()(0) / svdI.singularValues()(svdI.singularValues().size()-1); 211 | TSIF_LOG("I condition number:\n" << condI); 212 | #endif 213 | TSIF_LOGEIF((I_LDLT.info() != Eigen::Success),"Computation of Iinv failed"); 214 | VecX dx = -I_LDLT.solve(S * y_); 215 | 216 | // Apply Kalman Update 217 | State newState = curLinState_; 218 | curLinState_.Boxplus(dx, newState); 219 | curLinState_ = newState; 220 | weightedDelta_ = sqrt((dx.dot(newInf*dx))/dx.size()); 221 | TSIF_LOG("iter: " << iter_ << "\tw: " << sqrt((dx.dot(dx))/dx.size()) << "\twd: " << weightedDelta_); 222 | } 223 | TSIF_LOGWIF(weightedDelta_ >= th_iter_, "Reached maximal iterations:" << iter_); 224 | 225 | state_ = curLinState_; 226 | I_ = newInf; 227 | TSIF_LOG("State after Update:\n" << state_.Print()); 228 | TSIF_LOG("Information matrix:\n" << I_); 229 | 230 | // Post Processing 231 | PostProcess(); 232 | time_ = t; 233 | } 234 | 235 | template::type* = nullptr> 236 | int PreProcessResidual(TimePoint t){ 237 | std::get(residuals_).isActive_ = std::get(timelines_).HasMeas(t); 238 | assert(std::get(residuals_).isActive_ || !std::get(residuals_).isMandatory_); 239 | if(std::get(residuals_).isActive_){ 240 | std::get(residuals_).dt_ = toSec(t-time_); 241 | std::get(residuals_).meas_ = std::get(timelines_).Get(t); 242 | } 243 | return PreProcessResidual(t) + std::tuple_element::type::Output::Dim(); 244 | } 245 | template= kN)>::type* = nullptr> 246 | int PreProcessResidual(TimePoint t){ 247 | return 0; 248 | } 249 | 250 | template::type* = nullptr> 251 | void ConstructProblem(int start){ 252 | typedef typename std::tuple_element::type::Output Output; 253 | static_assert(Output::kIsVectorSpace, "Residual must be vector space!"); 254 | if(std::get(residuals_).isActive_ && Output::Dim() > 0){ 255 | Output ySub; 256 | std::get(residuals_).EvalRes(ySub,state_,curLinState_); 257 | std::get(residuals_).JacPre(JacPre_.block(start,0),state_,curLinState_); 258 | std::get(residuals_).JacCur(JacCur_.block(start,0),state_,curLinState_); 259 | std::get(residuals_).AddNoise(ySub, JacPre_.block(start,0), 260 | JacCur_.block(start,0)); 261 | ySub.GetVec(y_.block(start, 0)); 262 | } 263 | ConstructProblem(start+Output::Dim()*std::get(residuals_).isActive_); 264 | } 265 | template= kN)>::type* = nullptr> 266 | void ConstructProblem(int start){ 267 | } 268 | 269 | template::type* = nullptr> 270 | int JacTestAll(double th, double d, const State& pre, const State& cur){ 271 | std::get(residuals_).JacPreTest(th,d,pre,cur); 272 | std::get(residuals_).JacCurTest(th,d,pre,cur); 273 | return JacTestAll(th,d,pre,cur); 274 | } 275 | 276 | template= kN)>::type* = nullptr> 277 | int JacTestAll(double th, double d, const State& pre, const State& cur){ 278 | return 0; 279 | } 280 | 281 | template::type* = nullptr> 282 | int JacTestAll(double th, double d){ 283 | std::get(residuals_).JacPreTest(th,d); 284 | std::get(residuals_).JacCurTest(th,d); 285 | return JacTestAll(th,d); 286 | } 287 | 288 | template= kN)>::type* = nullptr> 289 | int JacTestAll(double th, double d){ 290 | return 0; 291 | } 292 | 293 | const State& GetState() const{ 294 | return state_; 295 | } 296 | State& GetState(){ 297 | return state_; 298 | } 299 | 300 | template::type* = nullptr> 301 | std::string PrintConnectivityRes(){ 302 | std::ostringstream out; 303 | if(!B){ 304 | if(std::tuple_element::type::Previous::template HasId()>()){ 305 | out << "--" << " "; 306 | }else{ 307 | out << " "; 308 | } 309 | if(C==State::kN-1){ 310 | if(N<10) out << " "; 311 | out << N << " "; 312 | } 313 | } else { 314 | if(std::tuple_element::type::Current::template HasId()>()){ 315 | out << "--" << " "; 316 | }else{ 317 | out << " "; 318 | } 319 | if(C==State::kN-1) out << std::endl; 320 | } 321 | out << PrintConnectivityRes(); 322 | return out.str(); 323 | } 324 | template= kN)>::type* = nullptr> 325 | std::string PrintConnectivityRes(){ 326 | return ""; 327 | } 328 | 329 | std::string PrintConnectivity(){ 330 | std::ostringstream out; 331 | for(int i=0;i, 16 | AttitudeFindif<0,1,3>, 17 | AccelerometerPrediction<0,2,1,3,4>, 18 | GyroscopeUpdate<0,3,5>, 19 | RandomWalk>, 20 | RandomWalk>, 21 | PositionUpdate<0,0,1,-6,-7,-8>, 22 | RandomWalk,Element,Element>, 23 | AttitudeUpdate<-1,1,-7,-9>>{ 24 | public: 25 | ImuGpsFilter(){ 26 | max_iter_ = 10; 27 | th_iter_ = 0.1; 28 | std::get<0>(residuals_).w_ = 1e4; // PositionFindif 29 | std::get<1>(residuals_).w_ = 1e4; // AttitudeFindif 30 | std::get<2>(residuals_).w_ = 1e2; // AccelerometerPrediction 31 | std::get<3>(residuals_).w_ = 1/(2e-3); // GyroscopeUpdate 32 | std::get<4>(residuals_).w_ = 1e4; // AccelerometerBiasPrediction 33 | std::get<5>(residuals_).w_ = 1e4; // GyroscopeBiasPrediction 34 | std::get<6>(residuals_).w_ = 1e2; // PositionUpdate 35 | std::get<7>(residuals_).w_ = 1e4; // GravityCorrectionPrediction 36 | std::get<8>(residuals_).w_ = 1e2; // AttitudeUpdate 37 | } 38 | virtual ~ImuGpsFilter(){} 39 | virtual void Init(TimePoint t){ 40 | if(GetMinMaxTime() != TimePoint::min()){ 41 | TSIF_LOG("Initializing state at t = " << Print(t)); 42 | startTime_ = t; 43 | time_ = t; 44 | state_.SetIdentity(); 45 | state_.Get<1>() = Quat(0.7,0,0,sqrt(1-0.7*0.7)); 46 | I_.setIdentity(); 47 | I_.block<3,3>(State::Start(0),State::Start(0)) = Mat3::Identity()*1e2; 48 | I_.block<3,3>(State::Start(1),State::Start(1)) = Mat3::Identity()*1e0; 49 | I_.block<3,3>(State::Start(2),State::Start(2)) = Mat3::Identity()*1e2; 50 | I_.block<3,3>(State::Start(3),State::Start(3)) = Mat3::Identity()*1e2; 51 | I_.block<3,3>(State::Start(4),State::Start(4)) = Mat3::Identity()*1e0; 52 | I_.block<3,3>(State::Start(5),State::Start(5)) = Mat3::Identity()*1e0; 53 | is_initialized_ = true; 54 | } 55 | } 56 | virtual void ComputeLinearizationPoint(const TimePoint& t){ 57 | curLinState_ = state_; 58 | double dt = toSec(t-time_); 59 | std::shared_ptr acc = std::get<2>(timelines_).Get(t); 60 | std::shared_ptr gyr = std::get<3>(timelines_).Get(t); 61 | curLinState_.Get<0>() = state_.Get<0>() + dt*state_.Get<1>().toRotationMatrix()*state_.Get<2>(); 62 | curLinState_.Get<1>() = state_.Get<1>()*Exp(dt*state_.Get<3>()); 63 | curLinState_.Get<2>() = (Mat3::Identity() - SSM(dt*state_.template Get<3>()))*state_.template Get<2>() 64 | + dt*(acc->GetAcc()-state_.template Get<4>()+state_.template Get<1>().inverse().toRotationMatrix()*std::get<2>(residuals_).g_); 65 | curLinState_.Get<3>() = gyr->GetGyr()-state_.template Get<5>(); 66 | } 67 | }; 68 | 69 | } // namespace tsif 70 | 71 | #endif // TSIF_IMU_GPS_H_ 72 | -------------------------------------------------------------------------------- /include/tsif/filters/vio.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_VIO_H_ 2 | #define TSIF_VIO_H_ 3 | 4 | #include 5 | 6 | #include "tsif/residuals/accelerometer_prediction.h" 7 | #include "tsif/residuals/attitude_findif.h" 8 | #include "tsif/residuals/bearing_findif.h" 9 | #include "tsif/residuals/distance_findif.h" 10 | #include "tsif/residuals/gyroscope_update.h" 11 | #include "tsif/residuals/image_update.h" 12 | #include "tsif/residuals/position_findif.h" 13 | #include "tsif/residuals/random_walk.h" 14 | #include "tsif/utils/common.h" 15 | #include "tsif/filter.h" 16 | 17 | namespace tsif{ 18 | 19 | template 20 | class MeasImg: public ElementVector<>{ 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | MeasImg(){ 24 | for(int i=0;i n_; 44 | mutable std::vector keyPoints_; 45 | bool isSim_; 46 | }; 47 | 48 | class LandmarkData{ 49 | public: 50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 51 | LandmarkData(){ 52 | id_ = -1; 53 | } 54 | Vec<2> prePoint_; 55 | Mat<2> preCov_; 56 | cv::Mat desc_; 57 | double trackingQuality_; 58 | std::vector> matches_; 59 | int count_; 60 | double sigmaAngle_; 61 | double sigma0_; 62 | double sigma1_; 63 | int id_; 64 | 65 | void SetPrediction(const Camera& cam, const UnitVector& vec, MatCRef<2> P){ 66 | UnitVector vec0, vec1; 67 | Vec<2> pt_pred0, pt_pred1; 68 | cam.BearingToPixel(vec.GetVec(),prePoint_); 69 | Eigen::EigenSolver> ES(P); 70 | const double delta = 0.01; 71 | vec.Boxplus(ES.eigenvectors().col(0).real()*delta,vec0); 72 | vec.Boxplus(ES.eigenvectors().col(1).real()*delta,vec1); 73 | cam.BearingToPixel(vec0.GetVec(),pt_pred0); 74 | cam.BearingToPixel(vec1.GetVec(),pt_pred1); 75 | Mat<2> U; 76 | U.col(0) = (pt_pred0-prePoint_)/delta*sqrt(ES.eigenvalues()(0).real()); 77 | U.col(1) = (pt_pred1-prePoint_)/delta*sqrt(ES.eigenvalues()(1).real()); 78 | preCov_ = (U*U.transpose()); 79 | ES.compute(preCov_); 80 | sigmaAngle_ = std::atan2(ES.eigenvectors()(1,0).real(),ES.eigenvectors()(0,0).real()); 81 | sigma0_ = std::min(1000.0,sqrt(ES.eigenvalues()(0).real())); 82 | sigma1_ = std::min(1000.0,sqrt(ES.eigenvalues()(1).real())); 83 | } 84 | }; 85 | 86 | template 87 | using VioFilterBase = Filter, 88 | AttitudeFindif<0,1,3>, 89 | AccelerometerPrediction<0,2,1,3,4>, 90 | GyroscopeUpdate<0,3,5>, 91 | RandomWalk>, 92 | RandomWalk>, 93 | ImageUpdate<0,6,N,MeasImg>, 94 | BearingFindif<0,6,7,2,3,8,9,N>, 95 | DistanceFindif<0,6,7,2,3,8,9,N>, 96 | RandomWalk>, 97 | RandomWalk>>; 98 | 99 | template 100 | class VioFilter: public VioFilterBase { 101 | typedef VioFilterBase Base; 102 | using Base::max_iter_; 103 | using Base::th_iter_; 104 | using Base::include_max_; 105 | using Base::residuals_; 106 | using Base::startTime_; 107 | using Base::time_; 108 | using Base::state_; 109 | using Base::I_; 110 | using Base::is_initialized_; 111 | using Base::curLinState_; 112 | using Base::timelines_; 113 | using Base::GetMinMaxTime; 114 | using typename Base::State; 115 | public: 116 | VioFilter(const std::string& optionFile){ 117 | FD_ = tsif::OptionLoader::Instance().LoadFile(optionFile); 118 | max_iter_ = FD_->Get("max_iter"); 119 | th_iter_ = FD_->Get("th_iter"); 120 | include_max_ = false; 121 | std::get<0>(residuals_).w_ = FD_->Get("w_posfd"); 122 | std::get<1>(residuals_).w_ = FD_->Get("w_attfd"); 123 | std::get<2>(residuals_).w_ = FD_->Get("w_accpre"); 124 | std::get<3>(residuals_).w_ = FD_->Get("w_gyrupd"); 125 | std::get<4>(residuals_).w_ = FD_->Get("w_accbias"); 126 | std::get<5>(residuals_).w_ = FD_->Get("w_gyrbias"); 127 | std::get<6>(residuals_).w_ = FD_->Get("w_imgupd"); 128 | std::get<6>(residuals_).huberTh_ = FD_->Get("img_huber_th"); 129 | std::get<7>(residuals_).w_ = FD_->Get("w_beapre"); 130 | std::get<8>(residuals_).w_ = FD_->Get("w_dispre"); 131 | std::get<9>(residuals_).w_ = FD_->Get("w_veppre"); 132 | std::get<10>(residuals_).w_ = FD_->Get("w_veapre"); 133 | drawAdding_ = FD_->Get("draw_adding"); 134 | drawTracking_ = FD_->Get("draw_tracking"); 135 | doDraw_ = FD_->Get("do_draw"); 136 | singleMatchOnly_ = FD_->Get("single_match_only"); 137 | } 138 | virtual ~VioFilter(){} 139 | virtual void Init(TimePoint t){ 140 | if(GetMinMaxTime() != TimePoint::min()){ 141 | TSIF_LOG("Initializing state at t = " << Print(t)); 142 | startTime_ = t; 143 | time_ = t; 144 | state_.SetIdentity(); 145 | state_.template Get<1>() = Quat::FromTwoVectors(std::get<2>(timelines_).Get(std::get<2>(timelines_).GetFirstTime())->GetAcc(),Vec3(0,0,1)); 146 | state_.template Get<8>() = FD_->Get("init_vep"); 147 | state_.template Get<9>() = FD_->Get("init_vea"); 148 | I_.setIdentity(); 149 | I_.template block<3,3>(State::Start(0),State::Start(0)) = Mat3::Identity()*pow(FD_->Get("initstd_pos"),2); 150 | I_.template block<3,3>(State::Start(1),State::Start(1)) = Mat3::Identity()*pow(FD_->Get("initstd_att"),2); 151 | I_.template block<3,3>(State::Start(2),State::Start(2)) = Mat3::Identity()*pow(FD_->Get("initstd_vel"),2); 152 | I_.template block<3,3>(State::Start(3),State::Start(3)) = Mat3::Identity()*pow(FD_->Get("initstd_ror"),2); 153 | I_.template block<3,3>(State::Start(4),State::Start(4)) = Mat3::Identity()*pow(FD_->Get("initstd_acb"),2); 154 | I_.template block<3,3>(State::Start(5),State::Start(5)) = Mat3::Identity()*pow(FD_->Get("initstd_gyb"),2); 155 | I_.template block<3,3>(State::Start(8),State::Start(8)) = Mat3::Identity()*pow(FD_->Get("initstd_vep"),2); 156 | I_.template block<3,3>(State::Start(9),State::Start(9)) = Mat3::Identity()*pow(FD_->Get("initstd_vea"),2); 157 | is_initialized_ = true; 158 | } 159 | } 160 | virtual void ComputeLinearizationPoint(const TimePoint& t){ 161 | curLinState_ = state_; 162 | double dt = toSec(t-time_); 163 | std::shared_ptr acc = std::get<2>(timelines_).Get(t); 164 | std::shared_ptr gyr = std::get<3>(timelines_).Get(t); 165 | curLinState_.template Get<0>() = state_.template Get<0>() + dt*state_.template Get<1>().toRotationMatrix()*state_.template Get<2>(); 166 | curLinState_.template Get<1>() = state_.template Get<1>()*Exp(dt*state_.template Get<3>()); 167 | curLinState_.template Get<2>() = (Mat3::Identity() - SSM(dt*state_.template Get<3>()))*state_.template Get<2>() 168 | + dt*(acc->GetAcc()-state_.template Get<4>()+state_.template Get<1>().inverse().toRotationMatrix()*std::get<2>(residuals_).g_); 169 | curLinState_.template Get<3>() = gyr->GetGyr()-state_.template Get<5>(); 170 | 171 | const Mat3 C_VI = state_.template Get<9>().toRotationMatrix(); 172 | const Vec3 ror = C_VI*state_.template Get<3>(); 173 | const Vec3 vel = C_VI*(state_.template Get<2>() + state_.template Get<3>().cross(state_.template Get<8>())); 174 | for(int i=0;i()[i]; 176 | const Vec3 beaVec = bea.GetVec(); 177 | const Mat<3,2> beaN = bea.GetN(); 178 | const double& invDis = state_.template Get<7>()[i]; 179 | const Vec<2> dn = -beaN.transpose()*(ror + invDis * beaVec.cross(vel)); 180 | bea.Boxplus(dn*dt,curLinState_.template Get<6>()[i]); 181 | curLinState_.template Get<7>()[i] = invDis + dt*beaVec.dot(vel)*invDis*invDis; 182 | } 183 | } 184 | virtual void PreProcess(){ 185 | const auto& m = std::get<6>(residuals_).meas_; 186 | for(int i=0;i(residuals_).active_[i] = false; 188 | } 189 | 190 | Timer timer; 191 | if(doDraw_){ 192 | if(!m->isSim_){ 193 | cv::cvtColor(m->img_,drawImg_,CV_GRAY2BGR); 194 | } else { 195 | drawImg_ = cv::Mat::zeros(480, 752, CV_8U); 196 | cv::drawKeypoints(drawImg_,m->keyPoints_,drawImg_,cv::Scalar(255,50,50)); 197 | } 198 | } 199 | TSIF_LOG("Img: " << 1000*timer.GetIncr()); 200 | 201 | // Compute covariance of landmark predition // TODO: make more efficient 202 | MatX P = I_.inverse(); 203 | MatX JBearingPrediction(2*N,State::Dim()); 204 | JBearingPrediction.setZero(); 205 | std::get<7>(residuals_).JacPreCustom(JBearingPrediction,state_,curLinState_,true); 206 | MatX Pbearing = (JBearingPrediction*P*JBearingPrediction.transpose()) + MatX(Vec<2*N>::Ones().asDiagonal()*pow(FD_->Get("w_imgupd"),-2)); 207 | TSIF_LOG("Cov: " << 1000*timer.GetIncr()); 208 | 209 | // Detect and compute 210 | if(!m->isSim_){ 211 | mask_ = cv::Mat::zeros(m->img_.size(), CV_8U); 212 | std::vector keyPoints; 213 | cv::Mat desc; 214 | for(int i=0;i()[i],Pbearing.block<2,2>(2*i,2*i)); 218 | cv::ellipse(mask_, cv::Point2f(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1)), cv::Size(l_.at(i).sigma0_*FD_->Get("match_geom_distance"),l_.at(i).sigma1_*FD_->Get("match_geom_distance")), l_.at(i).sigmaAngle_/M_PI*180, 0, 360, cv::Scalar(255), -1); 219 | } 220 | } 221 | TSIF_LOG("Mask: " << 1000*timer.GetIncr()); 222 | cv::Ptr orb = cv::ORB::create(FD_->Get("num_candidates_matching")*NumLandmarks(),FD_->Get("scale_factor"),FD_->Get("n_levels")); 223 | // cv::ORB orb(FD_->Get("num_candidates_matching")*NumLandmarks(),FD_->Get("scale_factor"),FD_->Get("n_levels")); 224 | orb->detect(m->img_,keyPoints,mask_); 225 | TSIF_LOG("Detection: " << 1000*timer.GetIncr()); 226 | orb->compute(m->img_,keyPoints,desc); 227 | TSIF_LOG("Compute: " << 1000*timer.GetIncr()); 228 | if(doDraw_ && drawTracking_){ 229 | cv::drawKeypoints(drawImg_,keyPoints,drawImg_,cv::Scalar(255,0,0)); 230 | } 231 | 232 | for(int i=0;iGet("match_desc_distance")); 241 | double best_score = 0; 242 | for(int j=0;j pt_meas(keyPoints[l_.at(i).matches_[0][j].trainIdx].pt.x,keyPoints[l_.at(i).matches_[0][j].trainIdx].pt.y); 244 | const double geomScore = std::sqrt(((l_.at(i).prePoint_-pt_meas).transpose()*l_.at(i).preCov_.inverse()*(l_.at(i).prePoint_-pt_meas))(0))/FD_->Get("match_geom_distance"); 245 | const double descScore = l_.at(i).matches_[0][j].distance/FD_->Get("match_desc_distance"); 246 | if(geomScore + descScore < 1){ 247 | count ++; 248 | if(best_match == -1 || geomScore + descScore < best_score){ 249 | best_match = j; 250 | best_score = geomScore + descScore; 251 | } 252 | } 253 | } 254 | } 255 | if(best_match != -1 && (!singleMatchOnly_ || count == 1)){ 256 | TSIF_LOGW("Found measurement for landmark " << i); 257 | Vec<2> pt_meas(keyPoints[l_.at(i).matches_[0][best_match].trainIdx].pt.x,keyPoints[l_.at(i).matches_[0][best_match].trainIdx].pt.y); 258 | Vec3 vec; 259 | cam_.PixelToBearing(pt_meas,vec); 260 | m->SetBea(i,UnitVector(vec)); 261 | std::get<6>(residuals_).active_[i] = true; 262 | if(doDraw_ && drawTracking_){ 263 | cv::ellipse(drawImg_, cv::Point2f(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1)), cv::Size(l_.at(i).sigma0_*FD_->Get("match_geom_distance"),l_.at(i).sigma1_*FD_->Get("match_geom_distance")), l_.at(i).sigmaAngle_/M_PI*180, 0, 360, cv::Scalar(0,255,0)); 264 | cv::line(drawImg_, cv::Point2f(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1)), cv::Point2f(pt_meas(0),pt_meas(1)), cv::Scalar(0,255,0)); 265 | } 266 | } else { 267 | if(doDraw_ && drawTracking_){ 268 | if(count == 0){ 269 | cv::ellipse(drawImg_, cv::Point2f(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1)), cv::Size(l_.at(i).sigma0_*FD_->Get("match_geom_distance"),l_.at(i).sigma1_*FD_->Get("match_geom_distance")), l_.at(i).sigmaAngle_/M_PI*180, 0, 360, cv::Scalar(0,0,255)); 270 | } else { 271 | cv::ellipse(drawImg_, cv::Point2f(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1)), cv::Size(l_.at(i).sigma0_*FD_->Get("match_geom_distance"),l_.at(i).sigma1_*FD_->Get("match_geom_distance")), l_.at(i).sigmaAngle_/M_PI*180, 0, 360, cv::Scalar(255,0,255)); 272 | } 273 | } 274 | } 275 | } 276 | } 277 | } else { 278 | for(int i=0;i matches; 280 | for(const auto& kp : std::get<6>(residuals_).meas_->keyPoints_){ 281 | for(int i=0;i()[i],Pbearing.block<2,2>(2*i,2*i)); // TODO: change to curLinState_ 285 | matches.emplace_back(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1),0,0,0,0,l_.at(i).id_); 286 | Vec3 vec; 287 | cam_.PixelToBearing(Vec<2>(kp.pt.x,kp.pt.y),vec); 288 | m->SetBea(i,UnitVector(vec)); 289 | std::get<6>(residuals_).active_[i] = true; 290 | if(doDraw_ && drawTracking_){ 291 | cv::ellipse(drawImg_, cv::Point2f(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1)), cv::Size(l_.at(i).sigma0_,l_.at(i).sigma1_), l_.at(i).sigmaAngle_/M_PI*180, 0, 360, cv::Scalar(0,255,0)); 292 | cv::line(drawImg_, cv::Point2f(l_.at(i).prePoint_(0),l_.at(i).prePoint_(1)), kp.pt, cv::Scalar(0,255,0)); 293 | } 294 | } 295 | } 296 | } 297 | } 298 | } 299 | TSIF_LOG("Matching: " << 1000*timer.GetIncr()); 300 | 301 | if(doDraw_){ 302 | DrawVirtualHorizon(drawImg_,state_.template Get<9>()*state_.template Get<1>().inverse()); 303 | DrawVelocity(drawImg_,state_.template Get<9>().toRotationMatrix()*state_.template Get<2>()); 304 | } 305 | TSIF_LOG("Draw Horizon: " << 1000*timer.GetIncr()); 306 | TSIF_LOG("=== Preprocess: " << 1000*timer.GetFull()); 307 | }; 308 | void DrawVirtualHorizon(cv::Mat& img,const Quat& att){ 309 | cv::rectangle(img,cv::Point2f(0,0),cv::Point2f(82,92),cv::Scalar(50,50,50),-1,8,0); 310 | cv::rectangle(img,cv::Point2f(0,0),cv::Point2f(80,90),cv::Scalar(100,100,100),-1,8,0); 311 | cv::Point2f rollCenter = cv::Point2f(40,40); 312 | cv::Scalar rollColor1(50,50,50); 313 | cv::Scalar rollColor2(200,200,200); 314 | cv::Scalar rollColor3(120,120,120); 315 | cv::circle(img,rollCenter,32,rollColor1,-1,8,0); 316 | cv::circle(img,rollCenter,30,rollColor2,-1,8,0); 317 | Eigen::Vector3d Vg = att.toRotationMatrix()*Eigen::Vector3d(0,0,-1); 318 | double roll = atan2(Vg(1),Vg(0))-0.5*M_PI; 319 | double pitch = acos(Vg.dot(Eigen::Vector3d(0,0,1)))-0.5*M_PI; 320 | double pixelFor10Pitch = 5.0; 321 | double pitchOffsetAngle = -asin(pitch/M_PI*180.0/10.0*pixelFor10Pitch/30.0); 322 | cv::Point2f rollVector1 = 30*cv::Point2f(cos(roll),sin(roll)); 323 | cv::Point2f rollVector2 = cv::Point2f(25,0); 324 | cv::Point2f rollVector3 = cv::Point2f(10,0); 325 | std::vector pts; 326 | cv::ellipse2Poly(rollCenter,cv::Size(30,30),0,(roll-pitchOffsetAngle)/M_PI*180,(roll+pitchOffsetAngle)/M_PI*180+180,1,pts); 327 | cv::Point *points; 328 | points = &pts[0]; 329 | int nbtab = pts.size(); 330 | cv::fillPoly(img,(const cv::Point**)&points,&nbtab,1,rollColor3); 331 | cv::line(img,rollCenter+rollVector2,rollCenter+rollVector3,rollColor1, 2); 332 | cv::line(img,rollCenter-rollVector2,rollCenter-rollVector3,rollColor1, 2); 333 | cv::ellipse(img,rollCenter,cv::Size(10,10),0,0,180,rollColor1,2,8,0); 334 | cv::circle(img,rollCenter,2,rollColor1,-1,8,0); 335 | } 336 | void DrawVelocity(cv::Mat& img,const Vec3& vel){ 337 | cv::Scalar color(0,0,150); 338 | const double arrowSize = 0.05; 339 | const double scale = 0.2; 340 | const Vec3 p0_3d = Vec3(0,0.3,1); 341 | 342 | const Vec3 p1_3d = p0_3d+scale*vel; 343 | const double yaw = atan2(vel(2),vel(0)); 344 | const double norm = vel.norm(); 345 | const Vec3 p2_3d = norm > 1e-3 ? p1_3d-scale*arrowSize*(vel/norm + Vec3(cos(yaw),0,-sin(yaw))) : p1_3d; 346 | const Vec3 p3_3d = norm > 1e-3 ? p1_3d-scale*arrowSize*(vel/norm - Vec3(cos(yaw),0,-sin(yaw))) : p1_3d; 347 | 348 | Vec<2> p0_2d, p1_2d, p2_2d, p3_2d; 349 | cam_.BearingToPixel(p0_3d, p0_2d); 350 | cam_.BearingToPixel(p1_3d, p1_2d); 351 | cam_.BearingToPixel(p2_3d, p2_2d); 352 | cam_.BearingToPixel(p3_3d, p3_2d); 353 | 354 | cv::line(img,cv::Point2f(p0_2d(0),p0_2d(1)),cv::Point2f(p1_2d(0),p1_2d(1)),color, 1); 355 | cv::line(img,cv::Point2f(p1_2d(0),p1_2d(1)),cv::Point2f(p2_2d(0),p2_2d(1)),color, 1); 356 | cv::line(img,cv::Point2f(p1_2d(0),p1_2d(1)),cv::Point2f(p3_2d(0),p3_2d(1)),color, 1); 357 | } 358 | virtual void PostProcess(){ 359 | Timer timer; 360 | const auto& m = std::get<6>(residuals_).meas_; 361 | // Remove if not tracked for more then prune_count frames 362 | for(int i=0;i(residuals_).active_[i] || (!m->isSim_ && l_.at(i).desc_.empty()) || (m->isSim_ && l_.at(i).id_ == -1)){ 364 | l_.at(i).count_ = 0; 365 | } else { 366 | l_.at(i).count_++; 367 | } 368 | if(l_.at(i).count_ > FD_->Get("prune_count")){ 369 | TSIF_LOGW("Removing landmark " << i); 370 | l_.at(i).desc_ = cv::Mat(); 371 | l_.at(i).id_ = -1; 372 | } 373 | if(state_.template Get<7>()[i] <= 1e-6){ 374 | state_.template Get<7>()[i] = 1e-6; 375 | } 376 | } 377 | TSIF_LOG("Removing: " << 1000*timer.GetIncr()); 378 | 379 | // Add new landmarks 380 | if(N - NumLandmarks() > FD_->Get("start_add")){ 381 | 382 | // Create mask 383 | std::vector keyPoints; 384 | cv::Mat desc; 385 | mask_ = cv::Mat::ones(m->img_.size(), CV_8U); // TODO: adapt size to sim 386 | for(int i=0;iGet("neighbor_suppression"), cv::Scalar(0), -1); 389 | } 390 | } 391 | TSIF_LOG("Mask: " << 1000*timer.GetIncr()); 392 | 393 | if(!m->isSim_){ 394 | cv::Ptr orb = cv::ORB::create(FD_->Get("num_candidates_add"),FD_->Get("scale_factor"),FD_->Get("n_levels")); 395 | // cv::ORB orb(FD_->Get("num_candidates_add"),FD_->Get("scale_factor"),FD_->Get("n_levels")); 396 | orb->detect(m->img_,keyPoints,mask_); 397 | TSIF_LOG("Detection: " << 1000*timer.GetIncr()); 398 | orb->compute(m->img_,keyPoints,desc); 399 | TSIF_LOG("Compute: " << 1000*timer.GetIncr()); 400 | } else { 401 | keyPoints = m->keyPoints_; 402 | } 403 | 404 | if(doDraw_ && drawAdding_){ 405 | cv::drawKeypoints(drawImg_,keyPoints,drawImg_,cv::Scalar(255,50,50)); 406 | } 407 | TSIF_LOG("Drawing Candidates: " << 1000*timer.GetIncr()); 408 | const int B = FD_->Get("bucket_count"); 409 | int bucketsCandidates[B*B]; 410 | for(int i=0;i= 0 && x_ind < B && y_ind >= 0 && y_ind < B){ 426 | if(m->isSim_){ 427 | bool found = false; 428 | for(int j=0;j keyPoints[bucketsCandidates[y_ind*B+x_ind]].response){ 436 | bucketsCandidates[y_ind*B+x_ind] = i; 437 | } 438 | } 439 | } 440 | TSIF_LOG("Bucketing: " << 1000*timer.GetIncr()); 441 | 442 | std::vector newKeyPoints; 443 | for(int i=0;iisSim_ && l_.at(i).desc_.empty()) || (m->isSim_ && l_.at(i).id_ == -1)){ 445 | const int start = abs((int)(NormalRandomNumberGenerator::Instance().Get()*1e6)); 446 | for(int j=0;j= 0){ 448 | const int newKeyPointInd = bucketsCandidates[(j+start)%(B*B)]; 449 | newKeyPoints.push_back(keyPoints[newKeyPointInd]); 450 | bucketsCandidates[(j+start)%(B*B)] = -1; 451 | if(!m->isSim_){ 452 | desc.row(newKeyPointInd).copyTo(l_.at(i).desc_); 453 | } else { 454 | l_.at(i).id_ = keyPoints[newKeyPointInd].class_id; 455 | } 456 | TSIF_LOGW("Adding landmark " << i); 457 | Vec3 vec; 458 | cam_.PixelToBearing(Vec<2>(keyPoints[newKeyPointInd].pt.x,keyPoints[newKeyPointInd].pt.y),vec); 459 | AddNewLandmark(i,UnitVector(vec),FD_->Get("init_dis")); 460 | break; 461 | } 462 | } 463 | } 464 | } 465 | TSIF_LOG("Adding: " << 1000*timer.GetIncr()); 466 | if(doDraw_ && drawAdding_){ 467 | cv::drawKeypoints(drawImg_,newKeyPoints,drawImg_,cv::Scalar(255,50,255)); 468 | } 469 | } 470 | 471 | if(doDraw_){ 472 | cv::namedWindow("VIO", cv::WINDOW_AUTOSIZE); 473 | cv::imshow("VIO", drawImg_); 474 | cv::waitKey(2); 475 | } 476 | TSIF_LOG("Drawing: " << 1000*timer.GetIncr()); 477 | TSIF_LOG("=== Postprocess: " << 1000*timer.GetFull()); 478 | }; 479 | void AddNewLandmark(int i,const UnitVector& n, double indDis){ 480 | l_.at(i).count_= 0; 481 | state_.template Get<6>()[i] = n; 482 | state_.template Get<7>()[i] = indDis; 483 | I_.template block<2,State::Dim()>(State::Start(6)+2*i,0).setZero(); 484 | I_.template block<1,State::Dim()>(State::Start(7)+1*i,0).setZero(); 485 | I_.template block(0,State::Start(6)+2*i).setZero(); 486 | I_.template block(0,State::Start(7)+1*i).setZero(); 487 | I_.template block<2,2>(State::Start(6)+2*i,State::Start(6)+2*i) = Mat<2>::Identity()*pow(FD_->Get("initstd_bea"),2); 488 | I_.template block<1,1>(State::Start(7)+1*i,State::Start(7)+1*i) = Mat<1>::Identity()*pow(FD_->Get("initstd_dis"),2); 489 | } 490 | int NumLandmarks(){ 491 | int count = 0; 492 | for(int i=0;i l_; 501 | bool drawAdding_; 502 | bool drawTracking_; 503 | bool doDraw_; 504 | bool singleMatchOnly_; 505 | cv::Mat mask_; 506 | const FileData* FD_; 507 | }; 508 | 509 | } // namespace tsif 510 | 511 | #endif // TSIF_VIO_H_ 512 | -------------------------------------------------------------------------------- /include/tsif/model.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_MODEL_H_ 2 | #define TSIF_MODEL_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "element_vector.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | class Model{ 11 | public: 12 | ~Model(){}; 13 | int Eval(typename Out::Ref out, const std::tuple ins){ 14 | return static_cast(*this).template EvalImpl(out,ins); 15 | } 16 | template 17 | int Jac(MatRefX J, const std::tuple ins){ 18 | return static_cast(*this).template JacImpl(J,ins); 19 | } 20 | template 21 | int JacFindif(double d, MatRefX J, const std::tuple insRef){ 22 | const int outDim = Out::Dim(); 23 | const int I = std::tuple_element>::type::template GetId(); 24 | const int inDim = std::tuple_element>::type::template GetElementDim(); 25 | Out outRef,outDis; 26 | Eval(outRef,insRef); 27 | std::tuple insDis = insRef; 28 | Vec inDif; 29 | Vec outDif; 30 | for(unsigned int j=0;j(insRef).template GetElement().Boxplus(inDif,std::get(insDis).template GetElement()); 34 | Eval(outDis,insDis); 35 | outDis.Boxminus(outRef,outDif); 36 | J.col(std::get(insRef).Start(I)+j) = outDif/d; 37 | } 38 | return 0; 39 | } 40 | template>::type::kN)>::type* = nullptr> 41 | int JacFindifFull(double d, MatRefX J, const std::tuple insRef){ 42 | JacFindif(d,J,insRef); 43 | JacFindifFull(d,J,insRef); 44 | return 0; 45 | } 46 | template= std::tuple_element>::type::kN)>::type* = nullptr> 47 | int JacFindifFull(double d, MatRefX J, const std::tuple insRef){ 48 | return 0; 49 | } 50 | template 51 | int JacTest(double th, double d, const std::tuple ins){ 52 | const int outDim = Out::Dim(); 53 | const int inDim = std::get(ins).Dim(); 54 | if(outDim > 0 & inDim > 0){ 55 | MatX J(outDim,inDim); 56 | J.setZero(); 57 | MatX J_FD(outDim,inDim); 58 | J_FD.setZero(); 59 | Jac(J,ins); 60 | JacFindifFull(d,J_FD,ins); 61 | TSIF_LOG("Analytical Jacobian:\n" << J); 62 | TSIF_LOG("Numerical Jacobian:\n" << J_FD); 63 | typename MatX::Index maxRow, maxCol = 0; 64 | const double r = (J-J_FD).array().abs().maxCoeff(&maxRow, &maxCol); 65 | if(r>th){ 66 | std::cout << "\033[31m==== Model jacInput (" << N << ") Test failed: " << r 67 | << " is larger than " << th << " at row " 68 | << maxRow << " and col " << maxCol << " ====" 69 | << " " << J(maxRow,maxCol) << " vs " << J_FD(maxRow,maxCol) 70 | << "\033[0m" << std::endl; 71 | return 1; 72 | } else{ 73 | std::cout << "\033[32m==== Test successful (" << r << ") ====\033[0m" << std::endl; 74 | return 0; 75 | } 76 | } else { 77 | std::cout << "\033[32m==== Test successful ( dimension 0 ) ====\033[0m" << std::endl; 78 | } 79 | } 80 | }; 81 | 82 | } // namespace tsif 83 | 84 | #endif // TSIF_MODEL_H_ 85 | -------------------------------------------------------------------------------- /include/tsif/residual.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_RESIDUAL_H_ 2 | #define TSIF_RESIDUAL_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/model.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | class Residual: public Model,Out,Pre,Cur>{ 11 | public: 12 | typedef Out Output; 13 | typedef Pre Previous; 14 | typedef Cur Current; 15 | typedef Meas Measurement; 16 | std::shared_ptr meas_; 17 | double dt_; 18 | double w_; 19 | const bool isSplitable_; // Can measurements be split into two 20 | const bool isMergeable_; // Can two measurements be merged into one (should be same as isSplitable) 21 | const bool isMandatory_; // Is this measurement required at every timestep (should then typically be splitable) 22 | bool isActive_; // Temporary, is a measurement currently available 23 | Residual(bool isSplitable = true,bool isMergeable = true,bool isMandatory = true): 24 | isSplitable_(isSplitable), 25 | isMergeable_(isMergeable), 26 | isMandatory_(isMandatory), 27 | meas_(nullptr){ 28 | dt_ = 0.1; 29 | w_ = 1.0; 30 | isActive_ = false; 31 | std::shared_ptr meas(new Meas()); 32 | meas->SetRandom(); 33 | meas_ = meas; 34 | } 35 | virtual ~Residual(){}; 36 | virtual int EvalRes(typename Out::Ref out, const typename Pre::CRef pre, const typename Cur::CRef cur){ 37 | return 1; 38 | } 39 | virtual int JacPre(MatRefX J, const typename Pre::CRef pre, const typename Cur::CRef cur){ 40 | return 1; 41 | } 42 | virtual int JacCur(MatRefX J, const typename Pre::CRef pre, const typename Cur::CRef cur){ 43 | return 1; 44 | } 45 | int EvalImpl(typename Out::Ref out, const std::tuple ins){ 46 | return EvalRes(out,std::get<0>(ins),std::get<1>(ins)); 47 | } 48 | template::type* = nullptr> 49 | int JacImpl(MatRefX J, const std::tuple ins){ 50 | return JacPre(J,std::get<0>(ins),std::get<1>(ins)); 51 | } 52 | template::type* = nullptr> 53 | int JacImpl(MatRefX J, const std::tuple ins){ 54 | return JacCur(J,std::get<0>(ins),std::get<1>(ins)); 55 | } 56 | int JacPreTest(double th, double d, const typename Pre::CRef pre, const typename Cur::CRef cur){ 57 | return this->template JacTest<0>(th,d,std::forward_as_tuple(pre,cur)); 58 | } 59 | int JacCurTest(double th, double d, const typename Pre::CRef pre, const typename Cur::CRef cur){ 60 | return this->template JacTest<1>(th,d,std::forward_as_tuple(pre,cur)); 61 | } 62 | int JacPreTest(double th, double d){ 63 | Pre pre; 64 | pre.SetRandom(); 65 | Cur cur; 66 | cur.SetRandom(); 67 | return JacPreTest(th,d,pre,cur); 68 | } 69 | int JacCurTest(double th, double d){ 70 | Pre pre; 71 | pre.SetRandom(); 72 | Cur cur; 73 | cur.SetRandom(); 74 | return JacCurTest(th,d,pre,cur); 75 | } 76 | virtual void SplitMeasurements(const TimePoint& t0, const TimePoint& t1, const TimePoint& t2, 77 | std::shared_ptr& m0, 78 | std::shared_ptr& m1, 79 | std::shared_ptr& m2) const{ 80 | if (isSplitable_){ 81 | m1 = m2; 82 | } else{ 83 | assert(false); 84 | } 85 | } 86 | virtual void MergeMeasurements(const TimePoint& t0, const TimePoint& t1, const TimePoint& t2, 87 | std::shared_ptr& m0, 88 | std::shared_ptr& m1, 89 | std::shared_ptr& m2) const{ 90 | if (isMergeable_){ 91 | std::shared_ptr newMeas(new Meas()); 92 | Vec dif; 93 | m1->Boxminus(*m2,dif); 94 | m2->Boxplus(toSec(t1 - t0) / toSec(t2 - t0) * dif, *newMeas); 95 | m2 = newMeas; 96 | } else{ 97 | assert(false); 98 | } 99 | } 100 | virtual void AddNoise(typename Out::Ref out, MatRefX J_pre, MatRefX J_cur){ 101 | AddWeight(GetWeight(),out,J_pre,J_cur); 102 | } 103 | void AddWeight(double w, typename Out::Ref out, MatRefX J_pre, MatRefX J_cur){ 104 | out.Scale(w); 105 | J_pre *= w; 106 | J_cur *= w; 107 | } 108 | virtual double GetWeight(){ 109 | return w_; 110 | } 111 | template=0 & OUT>=0)>::type* = nullptr> 112 | void SetJacCur(MatRefX J, const typename Current::CRef cur, MatCRef(),Current::template GetElementDim()> Jsub){ 113 | J.block(),Current::template GetElementDim()>( 114 | Output::Start(OUT),cur.Start(STA)) = Jsub; 115 | } 116 | template::type* = nullptr> 117 | void SetJacCur(MatRefX J, const typename Current::CRef cur, MatCRefX Jsub){ 118 | } 119 | template=0 & OUT>=0)>::type* = nullptr> 120 | void SetJacPre(MatRefX J, const typename Previous::CRef pre, MatCRef(),Previous::template GetElementDim()> Jsub){ 121 | J.block(),Previous::template GetElementDim()>( 122 | Output::Start(OUT),pre.Start(STA)) = Jsub; 123 | } 124 | template::type* = nullptr> 125 | void SetJacPre(MatRefX J, const typename Previous::CRef pre, MatCRefX Jsub){ 126 | } 127 | }; 128 | 129 | } // namespace tsif 130 | 131 | #endif // TSIF_RESIDUAL_H_ 132 | -------------------------------------------------------------------------------- /include/tsif/residuals/accelerometer_prediction.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_ACCELEROMETER_PREDICTION_H_ 2 | #define TSIF_ACCELEROMETER_PREDICTION_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | class MeasAcc: public ElementVector>{ 10 | public: 11 | MeasAcc(): ElementVector>(Vec3(0,0,0)){} 12 | MeasAcc(const Vec3& acc): ElementVector>(acc){} 13 | const Vec3& GetAcc() const{ 14 | return Get<0>(); 15 | } 16 | Vec3& GetAcc(){ 17 | return Get<0>(); 18 | } 19 | }; 20 | 21 | template 22 | using AccelerometerPredictionBase = Residual>, 23 | ElementVector,Element,Element,Element>, 24 | ElementVector>, 25 | MeasAcc>; 26 | 27 | template 28 | class AccelerometerPrediction: public AccelerometerPredictionBase{ 29 | public: 30 | typedef AccelerometerPredictionBase Base; 31 | using Base::dt_; 32 | using Base::w_; 33 | using Base::meas_; 34 | typedef typename Base::Output Output; 35 | typedef typename Base::Previous Previous; 36 | typedef typename Base::Current Current; 37 | const Vec3 g_; 38 | AccelerometerPrediction(): Base(true,true,true), g_(0,0,-9.81){} 39 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 40 | out.template Get() = cur.template Get() 41 | - (Mat3::Identity() - SSM(dt_*pre.template Get()))*pre.template Get() 42 | - dt_*(meas_->GetAcc()-pre.template Get()+pre.template Get().inverse().toRotationMatrix()*g_); 43 | return 0; 44 | } 45 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 46 | J.block<3,3>(Output::Start(OUT_VEL),pre.Start(STA_VEL)) = - (Mat3::Identity() - SSM(dt_*pre.template Get())); 47 | J.block<3,3>(Output::Start(OUT_VEL),pre.Start(STA_ATT)) = -pre.template Get().inverse().toRotationMatrix()*SSM(g_*dt_); 48 | J.block<3,3>(Output::Start(OUT_VEL),pre.Start(STA_ROR)) = - SSM(dt_*pre.template Get()); 49 | J.block<3,3>(Output::Start(OUT_VEL),pre.Start(STA_ACB)) = dt_*Mat3::Identity(); 50 | return 0; 51 | } 52 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 53 | J.block<3,3>(Output::Start(OUT_VEL),cur.Start(STA_VEL)) = Mat3::Identity(); 54 | return 0; 55 | } 56 | double GetWeight(){ 57 | return w_/sqrt(dt_); 58 | } 59 | }; 60 | 61 | } // namespace tsif 62 | 63 | #endif // TSIF_ACCELEROMETER_PREDICTION_H_ 64 | -------------------------------------------------------------------------------- /include/tsif/residuals/attitude_findif.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_ATTITUDE_FINDIF_H_ 2 | #define TSIF_ATTITUDE_FINDIF_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | using AttitudeFindifBase = Residual>, 11 | ElementVector,Element>, 12 | ElementVector>, 13 | MeasEmpty>; 14 | 15 | template 16 | class AttitudeFindif: public AttitudeFindifBase{ 17 | public: 18 | typedef AttitudeFindifBase Base; 19 | using Base::dt_; 20 | using Base::w_; 21 | typedef typename Base::Output Output; 22 | typedef typename Base::Previous Previous; 23 | typedef typename Base::Current Current; 24 | AttitudeFindif(): Base(true,true,true){} 25 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 26 | out.template Get() = Boxminus(cur.template Get(), 27 | pre.template Get()*Exp(dt_*pre.template Get())); 28 | return 0; 29 | } 30 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 31 | const Vec3 err = Boxminus(cur.template Get(), 32 | pre.template Get()*Exp(dt_*pre.template Get())); 33 | J.block<3,3>(Output::Start(OUT_ATT),pre.Start(STA_ATT)) = 34 | -GammaMat(err).inverse()*(Exp(err)).toRotationMatrix(); 35 | J.block<3,3>(Output::Start(OUT_ATT),pre.Start(STA_ROR)) = 36 | -GammaMat(err).inverse()*(cur.template Get()*Exp(-dt_*pre.template Get())).toRotationMatrix() 37 | *GammaMat(dt_*pre.template Get())*dt_; 38 | return 0; 39 | } 40 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 41 | const Vec3 err = Boxminus(cur.template Get(), 42 | pre.template Get()*Exp(dt_*pre.template Get())); 43 | J.block<3,3>(Output::Start(OUT_ATT),cur.Start(STA_ATT)) = GammaMat(err).inverse(); 44 | return 0; 45 | } 46 | virtual double GetWeight(){ 47 | return w_/sqrt(dt_); 48 | } 49 | }; 50 | 51 | } // namespace tsif 52 | 53 | #endif // TSIF_ATTITUDE_FINDIF_H_ 54 | -------------------------------------------------------------------------------- /include/tsif/residuals/attitude_update.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_ATTITUDE_UPDATE_H_ 2 | #define TSIF_ATTITUDE_UPDATE_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | class MeasAtt: public ElementVector>{ 10 | public: 11 | MeasAtt(): ElementVector>(Quat(1,0,0,0)){} 12 | MeasAtt(const Quat& att): ElementVector>(att){} 13 | const Quat& GetAtt() const{ 14 | return Get<0>(); 15 | } 16 | Quat& GetAtt(){ 17 | return Get<0>(); 18 | } 19 | }; 20 | 21 | template 22 | using AttitudeUpdateBase = Residual>, 23 | ElementVector<>, 24 | ElementVector, 25 | Element, 26 | Element>, 27 | MeasAtt>; 28 | 29 | template 30 | class AttitudeUpdate: public AttitudeUpdateBase{ 31 | public: 32 | typedef AttitudeUpdateBase Base; 33 | using Base::meas_; 34 | typedef typename Base::Output Output; 35 | typedef typename Base::Previous Previous; 36 | typedef typename Base::Current Current; 37 | AttitudeUpdate(): Base(false,false,false){} 38 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 39 | out.template Get() = Log(cur.template Get().inverse()* 40 | cur.template Get()*cur.template Get()* 41 | meas_->GetAtt().inverse()); 42 | return 0; 43 | } 44 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 45 | return 0; 46 | } 47 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 48 | const Vec3 attErr = Log(cur.template Get().inverse()* 49 | cur.template Get()*cur.template Get()* 50 | meas_->GetAtt().inverse()); 51 | const Mat3 mJI = cur.template Get().inverse().toRotationMatrix(); 52 | const Mat3 mIB = cur.template Get().toRotationMatrix(); 53 | const Mat3 GI = GammaMat(attErr).inverse(); 54 | this->template SetJacCur(J,cur,GI*mJI); 55 | this->template SetJacCur(J,cur,-GI*mJI); 56 | this->template SetJacCur(J,cur,GI*mJI*mIB); 57 | return 0; 58 | } 59 | }; 60 | 61 | } // namespace tsif 62 | 63 | #endif // TSIF_ATTITUDE_UPDATE_H_ 64 | -------------------------------------------------------------------------------- /include/tsif/residuals/bearing_findif.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_BEARING_FINDIF_H_ 2 | #define TSIF_BEARING_FINDIF_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | using BearingFindifBase = Residual,N>,OUT_BEA>>, 11 | ElementVector,STA_BEA>, 12 | Element,STA_DIS>, 13 | Element, 14 | Element, 15 | Element, 16 | Element>, 17 | ElementVector,STA_BEA>>, 18 | MeasEmpty>; 19 | 20 | template 21 | class BearingFindif: public BearingFindifBase{ 22 | public: 23 | typedef BearingFindifBase Base; 24 | using Base::dt_; 25 | using Base::w_; 26 | typedef typename Base::Output Output; 27 | typedef typename Base::Previous Previous; 28 | typedef typename Base::Current Current; 29 | BearingFindif(): Base(true,true,true){} 30 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 31 | UnitVector n_predicted; 32 | const Mat3 C_VI = pre.template Get().toRotationMatrix(); 33 | const Vec3 ror = C_VI*pre.template Get(); 34 | const Vec3 vel = C_VI*(pre.template Get() + pre.template Get().cross(pre.template Get())); 35 | for(int i=0;i()[i]; 37 | const Vec3 beaVec = bea.GetVec(); 38 | const Mat<3,2> beaN = bea.GetN(); 39 | const double& invDis = pre.template Get()[i]; 40 | const Vec<2> dn = -beaN.transpose()*(ror + invDis * beaVec.cross(vel)); 41 | bea.Boxplus(dn*dt_,n_predicted); 42 | n_predicted.Boxminus(cur.template Get()[i],out.template Get()[i]); // CAREFUL: DO NOT INVERSE BOXMINUS ORDER (CHAIN-RULE NOT VALID) 43 | } 44 | return 0; 45 | } 46 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 47 | JacPreCustom(J,pre,cur,false); 48 | return 0; 49 | } 50 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 51 | UnitVector n_predicted; 52 | const Mat3 C_VI = pre.template Get().toRotationMatrix(); 53 | const Vec3 ror = C_VI*pre.template Get(); 54 | const Vec3 vel = C_VI*(pre.template Get() + pre.template Get().cross(pre.template Get())); 55 | for(int i=0;i()[i]; 57 | const Vec3 beaVec = bea.GetVec(); 58 | const Mat<3,2> beaN = bea.GetN(); 59 | const double& invDis = pre.template Get()[i]; 60 | const Vec<2> dn = -beaN.transpose()*(ror + invDis * beaVec.cross(vel)); 61 | bea.Boxplus(dn*dt_,n_predicted); 62 | Mat<2> Jsub_1; 63 | n_predicted.BoxminusJacRef(cur.template Get()[i],Jsub_1); 64 | J.block<2,2>(Output::Start(OUT_BEA)+2*i,cur.Start(STA_BEA)+2*i) = Jsub_1; 65 | } 66 | return 0; 67 | } 68 | void JacPreCustom(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur, bool predictionOnly){ 69 | UnitVector n_predicted; 70 | const Mat3 C_VI = pre.template Get().toRotationMatrix(); 71 | const Vec3 ror = C_VI*pre.template Get(); 72 | const Vec3 vel = C_VI*(pre.template Get() + pre.template Get().cross(pre.template Get())); 73 | for(int i=0;i()[i]; 75 | const Vec3 beaVec = bea.GetVec(); 76 | const Mat<3,2> beaN = bea.GetN(); 77 | const Mat<3,2> beaM = bea.GetM(); 78 | const double& invDis = pre.template Get()[i]; 79 | const Vec<2> dn = -beaN.transpose()*(ror + invDis * beaVec.cross(vel)); 80 | bea.Boxplus(dn*dt_,n_predicted); 81 | Mat<2> Jsub_1,Jsub_2a, Jsub_2b; 82 | bea.BoxplusJacInp(dn*dt_,Jsub_2a); 83 | bea.BoxplusJacVec(dn*dt_,Jsub_2b); 84 | if(predictionOnly){ 85 | Jsub_1.setIdentity(); 86 | } else { 87 | n_predicted.BoxminusJacInp(cur.template Get()[i],Jsub_1); 88 | } 89 | const Mat<2,3> J_ror = -dt_*Jsub_1*Jsub_2b*beaN.transpose(); 90 | const Mat<2,3> J_vel = -dt_*invDis*Jsub_1*Jsub_2b*beaN.transpose()*SSM(beaVec); 91 | J.block<2,3>(Output::Start(OUT_BEA)+2*i,pre.Start(STA_VEL)) = J_vel*C_VI; 92 | J.block<2,3>(Output::Start(OUT_BEA)+2*i,pre.Start(STA_ROR)) = J_ror*C_VI - J_vel*C_VI*SSM(pre.template Get()); 93 | J.block<2,1>(Output::Start(OUT_BEA)+2*i,pre.Start(STA_DIS)+i) = -dt_*Jsub_1*Jsub_2b*beaN.transpose()*beaVec.cross(vel); 94 | J.block<2,2>(Output::Start(OUT_BEA)+2*i,pre.Start(STA_BEA)+2*i) = -dt_*Jsub_1*Jsub_2b*(-invDis*beaN.transpose()*SSM(vel)*beaM 95 | +beaN.transpose()*SSM(ror + invDis * beaVec.cross(vel))*beaN) + Jsub_1*Jsub_2a; 96 | J.block<2,3>(Output::Start(OUT_BEA)+2*i,pre.Start(STA_VEP)) = J_vel*C_VI*SSM(pre.template Get()); 97 | J.block<2,3>(Output::Start(OUT_BEA)+2*i,pre.Start(STA_VEA)) = - J_ror*SSM(ror) - J_vel*SSM(vel); 98 | } 99 | } 100 | double GetWeight(){ 101 | return w_/sqrt(dt_); 102 | } 103 | }; 104 | 105 | } // namespace tsif 106 | 107 | #endif // TSIF_BEARING_FINDIF_H_ 108 | -------------------------------------------------------------------------------- /include/tsif/residuals/distance_findif.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_DISTANCE_FINDIF_H_ 2 | #define TSIF_DISTANCE_FINDIF_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | using DistanceFindifBase = Residual,N>,OUT_DIS>>, 11 | ElementVector,STA_BEA>, 12 | Element,STA_DIS>, 13 | Element, 14 | Element, 15 | Element, 16 | Element>, 17 | ElementVector,STA_DIS>>, 18 | MeasEmpty>; 19 | 20 | template 21 | class DistanceFindif: public DistanceFindifBase{ 22 | public: 23 | typedef DistanceFindifBase Base; 24 | using Base::dt_; 25 | using Base::w_; 26 | typedef typename Base::Output Output; 27 | typedef typename Base::Previous Previous; 28 | typedef typename Base::Current Current; 29 | DistanceFindif(): Base(true,true,true){} 30 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 31 | const Mat3 C_VI = pre.template Get().toRotationMatrix(); 32 | const Vec3 vel = C_VI*(pre.template Get() + pre.template Get().cross(pre.template Get())); 33 | for(int i=0;i()[i]; 35 | const Vec3 beaVec = bea.GetVec(); 36 | const double& invDis = pre.template Get()[i]; 37 | out.template Get()[i](0) = invDis + dt_*beaVec.dot(vel)*invDis*invDis - cur.template Get()[i]; 38 | } 39 | return 0; 40 | } 41 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 42 | const Mat3 C_VI = pre.template Get().toRotationMatrix(); 43 | const Vec3 vel = C_VI*(pre.template Get() + pre.template Get().cross(pre.template Get())); 44 | for(int i=0;i()[i]; 46 | const Vec3 beaVec = bea.GetVec(); 47 | const double& invDis = pre.template Get()[i]; 48 | const Mat<1,3> J_vel = dt_*beaVec.transpose()*invDis*invDis; 49 | J.block<1,3>(Output::Start(OUT_DIS)+1*i,pre.Start(STA_VEL)) = J_vel*C_VI; 50 | J.block<1,3>(Output::Start(OUT_DIS)+1*i,pre.Start(STA_ROR)) = -J_vel*C_VI*SSM(pre.template Get()); 51 | J.block<1,1>(Output::Start(OUT_DIS)+1*i,pre.Start(STA_DIS)+1*i) = Mat<1>::Identity()+dt_*2*beaVec.transpose()*vel*invDis; 52 | J.block<1,2>(Output::Start(OUT_DIS)+1*i,pre.Start(STA_BEA)+2*i) = dt_*vel.transpose()*invDis*invDis*bea.GetM(); 53 | J.block<1,3>(Output::Start(OUT_DIS)+1*i,pre.Start(STA_VEP)) = J_vel*C_VI*SSM(pre.template Get()); 54 | J.block<1,3>(Output::Start(OUT_DIS)+1*i,pre.Start(STA_VEA)) = - J_vel*SSM(vel); 55 | } 56 | return 0; 57 | } 58 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 59 | for(int i=0;i(Output::Start(OUT_DIS)+1*i,cur.Start(STA_DIS)+1*i) = -Mat<1>::Identity(); 61 | } 62 | return 0; 63 | } 64 | double GetWeight(){ 65 | return w_/sqrt(dt_); 66 | } 67 | }; 68 | 69 | } // namespace tsif 70 | 71 | #endif // TSIF_DISTANCE_FINDIF_H_ 72 | -------------------------------------------------------------------------------- /include/tsif/residuals/gyroscope_update.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_GYROSCOPE_UPDATE_H_ 2 | #define TSIF_GYROSCOPE_UPDATE_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | class MeasGyr: public ElementVector>{ 10 | public: 11 | MeasGyr(): ElementVector>(Vec3(0,0,0)){} 12 | MeasGyr(const Vec3& acc): ElementVector>(acc){} 13 | const Vec3& GetGyr() const{ 14 | return Get<0>(); 15 | } 16 | Vec3& GetGyr(){ 17 | return Get<0>(); 18 | } 19 | }; 20 | 21 | template 22 | using GyroscopeUpdateBase = Residual>, 23 | ElementVector<>, 24 | ElementVector,Element>, 25 | MeasGyr>; 26 | 27 | template 28 | class GyroscopeUpdate: public GyroscopeUpdateBase{ 29 | public: 30 | typedef GyroscopeUpdateBase Base; 31 | using Base::dt_; 32 | using Base::w_; 33 | using Base::meas_; 34 | typedef typename Base::Output Output; 35 | typedef typename Base::Previous Previous; 36 | typedef typename Base::Current Current; 37 | GyroscopeUpdate(): Base(true,true,true){} 38 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 39 | out.template Get() = cur.template Get() + cur.template Get() - meas_->GetGyr(); 40 | return 0; 41 | } 42 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 43 | return 0; 44 | } 45 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 46 | J.block<3,3>(Output::Start(OUT_ROR),cur.Start(STA_ROR)) = Mat3::Identity(); 47 | J.block<3,3>(Output::Start(OUT_ROR),cur.Start(STA_GYB)) = Mat3::Identity(); 48 | return 0; 49 | } 50 | double GetWeight(){ 51 | return w_*sqrt(dt_); 52 | } 53 | }; 54 | 55 | } // namespace tsif 56 | 57 | #endif // TSIF_GYROSCOPE_UPDATE_H_ 58 | -------------------------------------------------------------------------------- /include/tsif/residuals/image_update.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_IMAGE_UPDATE_H_ 2 | #define TSIF_IMAGE_UPDATE_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | using ImageUpdateBase = Residual,N>,OUT_RES>>, 11 | ElementVector<>, 12 | ElementVector,STA_BEA>>, 13 | MEAS>; 14 | 15 | template 16 | class ImageUpdate: public ImageUpdateBase{ // TODO: rename to bearing update 17 | public: 18 | typedef ImageUpdateBase Base; 19 | using Base::meas_; 20 | using Base::w_; 21 | using Base::GetWeight; 22 | typedef typename Base::Output Output; 23 | typedef typename Base::Previous Previous; 24 | typedef typename Base::Current Current; 25 | ImageUpdate(): Base(false,false,false){ 26 | for(int i=0;i()[i].Boxminus(meas_->GetBea(i),out.template Get()[i]); 36 | } else { 37 | out.template Get()[i].setZero(); 38 | } 39 | } 40 | return 0; 41 | } 42 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 43 | return 0; 44 | } 45 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 46 | for(int i=0;i()[i].BoxminusJacInp(meas_->GetBea(i),J.block<2,2>(Output::Start(OUT_RES)+i*2,cur.Start(STA_BEA)+i*2)); 49 | } 50 | } 51 | return 0; 52 | } 53 | virtual void AddNoise(typename Output::Ref out, MatRefX J_pre, MatRefX J_cur){ 54 | for(int i=0;i()[i].norm(); 57 | if(norm > huberTh_){ 58 | w *= sqrt(2 * huberTh_ * (norm - 0.5 * huberTh_)/(norm*norm)); 59 | } 60 | out.template Get()[i] *= w; 61 | J_cur.block(Output::Start(OUT_RES)+i*2,0,2,J_cur.cols()) *= w; 62 | } 63 | } 64 | std::array active_; 65 | double huberTh_; 66 | }; 67 | 68 | } // namespace tsif 69 | 70 | #endif // TSIF_IMAGE_UPDATE_H_ 71 | -------------------------------------------------------------------------------- /include/tsif/residuals/pose_update.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_POSE_UPDATE_H_ 2 | #define TSIF_POSE_UPDATE_H_ 3 | 4 | #include "attitude_update.h" 5 | #include "position_update.h" 6 | #include "tsif/utils/common.h" 7 | #include "tsif/residual.h" 8 | 9 | namespace tsif{ 10 | 11 | class MeasPose: public ElementVector,Element>{ 12 | public: 13 | MeasPose(): ElementVector,Element>(Vec3(0,0,0),Quat(1,0,0,0)){} 14 | MeasPose(const Vec3& pos, const Quat& att): ElementVector,Element>(pos,att){} 15 | const Vec3& GetPos() const{ 16 | return Get<0>(); 17 | } 18 | Vec3& GetPos(){ 19 | return Get<0>(); 20 | } 21 | const Quat& GetAtt() const{ 22 | return Get<1>(); 23 | } 24 | Quat& GetAtt(){ 25 | return Get<1>(); 26 | } 27 | }; 28 | 29 | template 30 | using PoseUpdateBase = Residual,Element>, 31 | ElementVector<>, 32 | ElementVector, 33 | Element, 34 | Element, 35 | Element, 36 | Element, 37 | Element>, 38 | MeasPose>; 39 | 40 | template 41 | class PoseUpdate: public PoseUpdateBase{ 42 | public: 43 | typedef PoseUpdateBase Base; 44 | using Base::meas_; 45 | typedef typename Base::Output Output; 46 | typedef typename Base::Previous Previous; 47 | typedef typename Base::Current Current; 48 | PositionUpdate posUpd_; 49 | AttitudeUpdate attUpd_; 50 | PoseUpdate(): Base(false,false,false){} 51 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 52 | posUpd_.EvalRes(out,pre,cur); 53 | attUpd_.EvalRes(out,pre,cur); 54 | return 0; 55 | } 56 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 57 | return 0; 58 | } 59 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 60 | posUpd_.JacCur(J.block(Output::Start(OUT_POS),0,3,cur.Dim()),pre,cur); 61 | attUpd_.JacCur(J.block(Output::Start(OUT_ATT),0,3,cur.Dim()),pre,cur); 62 | return 0; 63 | } 64 | }; 65 | 66 | } // namespace tsif 67 | 68 | #endif // TSIF_POSE_UPDATE_H_ 69 | -------------------------------------------------------------------------------- /include/tsif/residuals/position_findif.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_POSITION_FINDIF_H_ 2 | #define TSIF_POSITION_FINDIF_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | using PositionFindifBase = Residual>, 11 | ElementVector,Element,Element>, 12 | ElementVector>, 13 | MeasEmpty>; 14 | 15 | template 16 | class PositionFindif: public PositionFindifBase{ 17 | public: 18 | typedef PositionFindifBase Base; 19 | using Base::dt_; 20 | using Base::w_; 21 | typedef typename Base::Output Output; 22 | typedef typename Base::Previous Previous; 23 | typedef typename Base::Current Current; 24 | PositionFindif(): Base(true,true,true){} 25 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 26 | out.template Get() = cur.template Get() - pre.template Get() 27 | - dt_*pre.template Get().toRotationMatrix()*pre.template Get(); 28 | return 0; 29 | } 30 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 31 | J.block<3,3>(Output::Start(OUT_POS),pre.Start(STA_POS)) = -Mat3::Identity(); 32 | J.block<3,3>(Output::Start(OUT_POS),pre.Start(STA_VEL)) = -pre.template Get().toRotationMatrix()*dt_; 33 | J.block<3,3>(Output::Start(OUT_POS),pre.Start(STA_ATT)) = SSM(dt_*pre.template Get().toRotationMatrix()*pre.template Get()); 34 | return 0; 35 | } 36 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 37 | J.block<3,3>(Output::Start(OUT_POS),cur.Start(STA_POS)) = Mat3::Identity(); 38 | return 0; 39 | } 40 | double GetWeight(){ 41 | return w_/sqrt(dt_); 42 | } 43 | }; 44 | 45 | } // namespace tsif 46 | 47 | #endif // TSIF_POSITION_FINDIF_H_ 48 | -------------------------------------------------------------------------------- /include/tsif/residuals/position_update.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_POSITION_UPDATE_H_ 2 | #define TSIF_POSITION_UPDATE_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | class MeasPos: public ElementVector>{ 10 | public: 11 | MeasPos(): ElementVector>(Vec3(0,0,0)){} 12 | MeasPos(const Vec3& pos): ElementVector>(pos){} 13 | const Vec3& GetPos() const{ 14 | return Get<0>(); 15 | } 16 | Vec3& GetPos(){ 17 | return Get<0>(); 18 | } 19 | }; 20 | 21 | template 22 | using PositionUpdateBase = Residual>, 23 | ElementVector<>, 24 | ElementVector, 25 | Element, 26 | Element, 27 | Element, 28 | Element>, 29 | MeasPos>; 30 | 31 | template 32 | class PositionUpdate: public PositionUpdateBase{ 33 | public: 34 | typedef PositionUpdateBase Base; 35 | using Base::meas_; 36 | typedef typename Base::Output Output; 37 | typedef typename Base::Previous Previous; 38 | typedef typename Base::Current Current; 39 | PositionUpdate(): Base(false,false,false){} 40 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 41 | out.template Get() = cur.template Get().inverse().toRotationMatrix()*( 42 | cur.template Get() - cur.template Get() 43 | + cur.template Get().toRotationMatrix()*cur.template Get()) - meas_->GetPos(); 44 | return 0; 45 | } 46 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 47 | return 0; 48 | } 49 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 50 | const Vec3 pos = cur.template Get().inverse().toRotationMatrix()*( 51 | cur.template Get() - cur.template Get() 52 | + cur.template Get().toRotationMatrix()*cur.template Get()); 53 | const Mat3 mJI = cur.template Get().inverse().toRotationMatrix(); 54 | const Mat3 mIB = cur.template Get().toRotationMatrix(); 55 | this->template SetJacCur(J,cur,mJI); 56 | this->template SetJacCur(J,cur,-mJI*SSM(mIB*cur.template Get())); 57 | this->template SetJacCur(J,cur,-mJI); 58 | this->template SetJacCur(J,cur,SSM(pos)*mJI); 59 | this->template SetJacCur(J,cur,mJI*mIB); 60 | return 0; 61 | } 62 | }; 63 | 64 | } // namespace tsif 65 | 66 | #endif // TSIF_POSITION_UPDATE_H_ 67 | -------------------------------------------------------------------------------- /include/tsif/residuals/random_walk.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_RANDOM_WALK_H_ 2 | #define TSIF_RANDOM_WALK_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/residual.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | using RandomWalkBase = Residual,Elements::kI>...>, 11 | ElementVector, 12 | ElementVector, 13 | MeasEmpty>; 14 | 15 | template 16 | class RandomWalk: public RandomWalkBase{ 17 | public: 18 | typedef ElementVector MyElementVector; 19 | typedef RandomWalkBase Base; 20 | using Base::dt_; 21 | using Base::w_; 22 | typedef typename Base::Output Output; 23 | typedef typename Base::Previous Previous; 24 | typedef typename Base::Current Current; 25 | RandomWalk(): Base(true,true,true){} 26 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 27 | _EvalRes(out,pre,cur); 28 | return 0; 29 | } 30 | template::type* = nullptr> 31 | int _EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 32 | const int I = MyElementVector::template GetId(); 33 | cur.template GetElement().Boxminus(pre.template GetElement(),out.template Get()); 34 | _EvalRes(out,pre,cur); 35 | return 0; 36 | } 37 | template= MyElementVector::kN)>::type* = nullptr> 38 | int _EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 39 | return 0; 40 | } 41 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 42 | _JacPre(J,pre,cur); 43 | return 0; 44 | } 45 | template::type* = nullptr> 46 | int _JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 47 | const int I = MyElementVector::template GetId(); 48 | this->template SetJacPre(J,pre,cur.template GetElement().BoxminusJacRef(pre.template GetElement())); 49 | _JacPre(J,pre,cur); 50 | return 0; 51 | } 52 | template= MyElementVector::kN)>::type* = nullptr> 53 | int _JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 54 | return 0; 55 | } 56 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 57 | _JacCur(J,pre,cur); 58 | return 0; 59 | } 60 | template::type* = nullptr> 61 | int _JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 62 | const int I = MyElementVector::template GetId(); 63 | this->template SetJacCur(J,cur,cur.template GetElement().BoxminusJacInp(pre.template GetElement())); 64 | _JacCur(J,pre,cur); 65 | return 0; 66 | } 67 | template= MyElementVector::kN)>::type* = nullptr> 68 | int _JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 69 | return 0; 70 | } 71 | virtual double GetWeight(){ 72 | return w_/sqrt(dt_); 73 | } 74 | }; 75 | 76 | } // namespace tsif 77 | 78 | #endif // TSIF_RANDOM_WALK_H_ 79 | -------------------------------------------------------------------------------- /include/tsif/timeline.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_TIMELINE_H_ 2 | #define TSIF_TIMELINE_H_ 3 | 4 | #include "element_vector.h" 5 | #include "tsif/utils/common.h" 6 | 7 | namespace tsif{ 8 | 9 | template 10 | class Timeline{ 11 | private: 12 | std::map> mm_; 13 | void RemoveFirst(){ 14 | if(mm_.size() > 0){ 15 | mm_.erase(mm_.begin()); 16 | } 17 | } 18 | public: 19 | Timeline(Duration max_wait_time,Duration min_wait_time): 20 | max_wait_time_(max_wait_time),min_wait_time_(min_wait_time){ 21 | } 22 | void Add(TimePoint t,std::shared_ptr m){ 23 | TSIF_LOG("Add measurement at " << tsif::Print(t)); 24 | TSIF_LOGWIF(mm_.count(t) > 0, "Entry already exists for measurement!"); 25 | mm_[t] = m; 26 | } 27 | bool HasMeas(TimePoint t){ 28 | return mm_.count(t) > 0; 29 | } 30 | std::shared_ptr Get(TimePoint t){ 31 | return mm_.at(t); 32 | } 33 | void Clean(TimePoint t){ 34 | while (CountSmallerOrEqual(t) > 1){ // Leave at least one measurement 35 | RemoveFirst(); 36 | } 37 | } 38 | int CountSmallerOrEqual(TimePoint t){ 39 | int count = 0; 40 | auto it = mm_.upper_bound(t); 41 | while(it != mm_.begin()){ 42 | count++; 43 | it--; 44 | } 45 | return count; 46 | } 47 | TimePoint GetLastTime() const{ 48 | if(mm_.empty()){ 49 | return TimePoint::min(); 50 | } else{ 51 | return mm_.rbegin()->first; 52 | } 53 | } 54 | TimePoint GetFirstTime() const{ 55 | if(mm_.empty()){ 56 | return TimePoint::max(); 57 | } else{ 58 | return mm_.begin()->first; 59 | } 60 | } 61 | TimePoint GetMaximalUpdateTime(TimePoint current) const{ 62 | return std::max(current-max_wait_time_,GetLastTime()+min_wait_time_); 63 | } 64 | void GetAllInRange(std::set& times, TimePoint start, TimePoint end) const{ 65 | auto it = mm_.upper_bound(start); 66 | while (it != mm_.end() && it->first <= end){ 67 | times.insert(it->first); 68 | ++it; 69 | } 70 | } 71 | template 72 | void Split(TimePoint t0, TimePoint t1, TimePoint t2, const Residual& res){ 73 | assert(t0 <= t1 && t1 <= t2); 74 | TSIF_LOG("Split measurement at " << tsif::Print(t1)); 75 | Add(t1,std::make_shared()); 76 | res.SplitMeasurements(t0, t1, t2, mm_.at(t0), mm_.at(t1), mm_.at(t2)); 77 | } 78 | template 79 | void Merge(TimePoint t0, TimePoint t1, TimePoint t2, const Residual& res){ 80 | assert(t0 <= t1 && t1 <= t2); 81 | TSIF_LOG("Merging measurement at " << tsif::Print(t1)); 82 | res.MergeMeasurements(t0, t1, t2, mm_.at(t0), mm_.at(t1), mm_.at(t2)); 83 | mm_.erase(t1); 84 | } 85 | template 86 | void SplitAndMerge(TimePoint t0, const std::set& times,const Residual& res){ 87 | if (times.size() == 0){ 88 | return; 89 | } 90 | if (res.isSplitable_){ 91 | for (const auto& t : times){ 92 | auto it = mm_.lower_bound(t); 93 | if (it == mm_.end()){ 94 | TSIF_LOGW("Partial splitting only!"); 95 | break; 96 | } 97 | if (it->first == t){ 98 | continue; // Measurement already available 99 | } 100 | assert(it != mm_.begin()); 101 | TimePoint previous = std::prev(it)->first; 102 | Split(previous, t, it->first, res); 103 | } 104 | } 105 | if (res.isMergeable_){ 106 | for (auto it = mm_.begin(); it != mm_.end();){ 107 | if (it->first <= t0 || times.count(it->first) > 0){ 108 | ++it; 109 | continue; // Ignore the first or if in times 110 | } 111 | if (it->first > *times.rbegin()){ 112 | break; // Ignore the last 113 | } 114 | if (std::next(it) == mm_.end()){ 115 | TSIF_LOGW("Partial merging only!"); 116 | break; 117 | } 118 | assert(it != mm_.begin()); 119 | TimePoint previous = std::prev(it)->first; 120 | ++it; // Needs to be increment before erase 121 | Merge(previous, std::prev(it)->first, it->first, res); 122 | } 123 | } 124 | } 125 | 126 | std::string Print(const TimePoint& start, int start_offset, double resolution) const{ 127 | std::ostringstream out; 128 | const int width = mm_.empty() ? start_offset : start_offset 129 | + std::max(0,(int)(std::ceil(toSec(mm_.rbegin()->first - start) / resolution)) + 1); 130 | std::vector counts(width, 0); 131 | for (auto it = mm_.begin(); it != mm_.end(); ++it){ 132 | const int x = start_offset + ceil(toSec(it->first - start) / resolution); 133 | if (x >= 0){ 134 | counts.at(x)++; 135 | } 136 | } 137 | for (const auto& c : counts){ 138 | if (c == 0){ 139 | out << "-"; 140 | } else{ 141 | out << c; 142 | } 143 | }; 144 | return out.str(); 145 | } 146 | const Duration max_wait_time_; 147 | const Duration min_wait_time_; 148 | }; 149 | 150 | template<> 151 | class Timeline{ 152 | public: 153 | Timeline(Duration max_wait_time,Duration min_wait_time): 154 | max_wait_time_(max_wait_time),min_wait_time_(min_wait_time){ 155 | } 156 | void Add(TimePoint t,std::shared_ptr m){ 157 | TSIF_LOGW("Unnecessary addition of empty measurement!"); 158 | } 159 | bool HasMeas(TimePoint t){ 160 | return true; 161 | } 162 | std::shared_ptr Get(TimePoint t){ 163 | return std::make_shared(); 164 | } 165 | void Clean(TimePoint t){ 166 | } 167 | TimePoint GetLastTime() const{ 168 | return TimePoint::max(); 169 | } 170 | TimePoint GetFirstTime() const{ 171 | return TimePoint::min(); 172 | } 173 | TimePoint GetMaximalUpdateTime(TimePoint current) const{ 174 | return TimePoint::max(); 175 | } 176 | void GetAllInRange(std::set& times, TimePoint start, TimePoint end) const{ 177 | } 178 | template 179 | void SplitAndMerge(TimePoint time, const std::set& times,const Residual& res){ 180 | } 181 | std::string Print(const TimePoint& start, int start_offset, double resolution) const{ 182 | const int c = start_offset > 4 ? start_offset - 4 : 0; 183 | std::ostringstream out; 184 | for(int i=0;i()); 42 | } 43 | Eigen::Matrix GetM() const { 44 | Eigen::Matrix M; 45 | M.col(0) = -GetPerp2(); 46 | M.col(1) = GetPerp1(); 47 | return M; 48 | } 49 | Eigen::Matrix GetN() const { 50 | Eigen::Matrix M; 51 | M.col(0) = GetPerp1(); 52 | M.col(1) = GetPerp2(); 53 | return M; 54 | } 55 | void Boxplus(const VecCRef<2>& dif, UnitVector& out) const{ 56 | out.SetQuat(Exp(dif(0)*GetPerp1()+dif(1)*GetPerp2())*q_); 57 | } 58 | void Boxminus(const UnitVector& ref, VecRef<2> dif) const{ 59 | dif = ref.GetN().transpose()*Log(Quat::FromTwoVectors(ref.GetVec(),GetVec())); 60 | } 61 | void BoxminusJacRef(const UnitVector& ref, MatRef<2> J) const{ 62 | J = ref.GetN().transpose()*FromTwoVectorsJac(ref.GetVec(),GetVec())*ref.GetM(); 63 | } 64 | void BoxminusJacInp(const UnitVector& ref, MatRef<2> J) const{ 65 | J = -ref.GetN().transpose()*FromTwoVectorsJac(GetVec(),ref.GetVec())*GetM(); 66 | } 67 | void BoxplusJacVec(const VecCRef<2>& dif, MatRef<2> J) const{ 68 | UnitVector out; 69 | Boxplus(dif,out); 70 | J = out.GetN().transpose()*GammaMat(dif(0)*GetPerp1()+dif(1)*GetPerp2())*GetN(); 71 | } 72 | void BoxplusJacInp(const VecCRef<2>& dif, MatRef<2> J) const{ 73 | UnitVector out; 74 | Boxplus(dif,out); 75 | J = out.GetN().transpose()*GetN(); 76 | } 77 | }; 78 | 79 | } // namespace tsif 80 | 81 | #endif // TSIF_UNIT_VECTOR_H_ 82 | -------------------------------------------------------------------------------- /include/tsif/utils/camera.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_CAMERA_H_ 2 | #define TSIF_CAMERA_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | 6 | namespace tsif{ 7 | 8 | class Camera{ 9 | public: 10 | enum ModelType{ 11 | RADTAN, 12 | EQUIDIST 13 | } type_; 14 | 15 | Eigen::Matrix3d K_; 16 | 17 | double k1_,k2_,k3_,k4_,k5_,k6_; 18 | double p1_,p2_,s1_,s2_,s3_,s4_; 19 | 20 | Camera(); 21 | ~Camera(); 22 | void LoadCameraMatrix(const std::string& filename); 23 | void LoadRadtan(const std::string& filename); 24 | void LoadEquidist(const std::string& filename); 25 | void Load(const std::string& filename); 26 | void DistortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 27 | void DistortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 28 | void DistortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 29 | void DistortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 30 | void Distort(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 31 | void Distort(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 32 | bool BearingToPixel(const Eigen::Vector3d& vec, Eigen::Vector2d& c) const; 33 | bool BearingToPixel(const Eigen::Vector3d& vec, Eigen::Vector2d& c, Eigen::Matrix& J) const; 34 | bool PixelToBearing(const Eigen::Vector2d& c,Eigen::Vector3d& vec) const; 35 | void TestCameraModel(); 36 | }; 37 | 38 | } 39 | 40 | 41 | #endif /* TSIF_CAMERA_H_ */ 42 | -------------------------------------------------------------------------------- /include/tsif/utils/common.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_COMMON_HPP_ 2 | #define TSIF_COMMON_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "tsif/utils/logging.h" 10 | #include "tsif/utils/option.h" 11 | #include "tsif/utils/random.h" 12 | #include "tsif/utils/rotation.h" 13 | #include "tsif/utils/timing.h" 14 | #include "tsif/utils/typedefs.h" 15 | 16 | #endif /* TSIF_COMMON_HPP_ */ 17 | -------------------------------------------------------------------------------- /include/tsif/utils/logging.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_LOGGING_HPP_ 2 | #define TSIF_LOGGING_HPP_ 3 | 4 | #include 5 | 6 | #if TSIF_VERBOSE > 0 7 | #define TSIF_LOG(msg) std::cout << msg << std::endl 8 | #define TSIF_LOGIF(con,msg) if(con) TSIF_LOG(msg) 9 | #define TSIF_LOGW(msg) std::cout << "\033[33m" << __FILE__ << "(" << __LINE__ << "): " << msg << "\033[0m" << std::endl 10 | #define TSIF_LOGWIF(con,msg) if(con) TSIF_LOGW(msg) 11 | #define TSIF_LOGE(msg) std::cout << "\033[31m" << __FILE__ << "(" << __LINE__ << "): " << msg << "\033[0m" << std::endl 12 | #define TSIF_LOGEIF(con,msg) if(con) TSIF_LOGE(msg) 13 | #else 14 | #define TSIF_LOG(msg) 15 | #define TSIF_LOGIF(con,msg) 16 | #define TSIF_LOGW(msg) 17 | #define TSIF_LOGWIF(con,msg) 18 | #define TSIF_LOGE(msg) 19 | #define TSIF_LOGEIF(con,msg) 20 | #endif 21 | 22 | #endif /* TSIF_LOGGING_HPP_ */ 23 | -------------------------------------------------------------------------------- /include/tsif/utils/option.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_OPTION_HPP_ 2 | #define TSIF_OPTION_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "tsif/utils/typedefs.h" 9 | 10 | namespace tsif{ 11 | 12 | template 13 | struct OptionLoaderTraits{ 14 | static bool Get(T& x, const std::vector& data){ 15 | return false; 16 | } 17 | }; 18 | template<> 19 | struct OptionLoaderTraits{ 20 | static bool Get(int& x, const std::vector& data){ 21 | assert(data.size() == 1); 22 | x = stoi(data[0]); 23 | return true; 24 | } 25 | }; 26 | template<> 27 | struct OptionLoaderTraits{ 28 | static bool Get(float& x, const std::vector& data){ 29 | assert(data.size() == 1); 30 | x = stof(data[0]); 31 | return true; 32 | } 33 | }; 34 | template<> 35 | struct OptionLoaderTraits{ 36 | static bool Get(double& x, const std::vector& data){ 37 | assert(data.size() == 1); 38 | x = stod(data[0]); 39 | return true; 40 | } 41 | }; 42 | template 43 | struct OptionLoaderTraits>{ 44 | static bool Get(Vec& x, const std::vector& data){ 45 | assert(data.size() == N); 46 | for(int i=0;i 53 | struct OptionLoaderTraits{ 54 | static bool Get(Quat& x, const std::vector& data){ 55 | assert(data.size() == 4); 56 | x.w() = stod(data[0]); 57 | x.x() = stod(data[1]); 58 | x.y() = stod(data[2]); 59 | x.z() = stod(data[3]); 60 | return true; 61 | } 62 | }; 63 | template<> 64 | struct OptionLoaderTraits{ 65 | static bool Get(std::string& x, const std::vector& data){\ 66 | x = data[0]; 67 | return true; 68 | } 69 | }; 70 | 71 | typedef std::vector optionData; 72 | class FileData{ 73 | public: 74 | template 75 | bool Get(const std::string& name, T& x) const{ 76 | return OptionLoaderTraits::Get(x,data_.at(name)); 77 | } 78 | template 79 | T Get(const std::string& name) const{ 80 | T x; 81 | Get(name,x); 82 | return x; 83 | } 84 | void Add(std::string name, optionData x){ 85 | data_[name] = x; 86 | } 87 | void Print() const{ 88 | for(const auto& entry : data_){ 89 | std::cout << entry.first << std::endl; 90 | for(const auto& value : entry.second){ 91 | std::cout << value << "|"; 92 | } 93 | std::cout << std::endl; 94 | } 95 | } 96 | 97 | private: 98 | std::map data_; 99 | }; 100 | 101 | /*! \brief Option Loader 102 | * Singleton class for interfacing option files. 103 | */ 104 | class OptionLoader{ 105 | public: 106 | std::map data_; 107 | const FileData* LoadFile(const std::string& filename){ 108 | if(data_.count(filename) == 0){ 109 | FileData fileData; 110 | std::ifstream data(filename); 111 | std::string str; 112 | while(getline(data, str)){ 113 | size_t prev = 0; 114 | std::vector dataVector; 115 | bool isFirst = true; 116 | std::string name; 117 | while(prev <= str.size()){ 118 | if(str[prev] == '#'){ 119 | break; 120 | } 121 | const size_t next = str.find_first_of("\t ",prev); 122 | if(next>prev){ 123 | if(isFirst){ 124 | name = str.substr(prev,next-prev); 125 | } else { 126 | dataVector.push_back(str.substr(prev,next-prev)); 127 | } 128 | isFirst = false; 129 | } 130 | prev = next != std::string::npos ? next + 1 : next; 131 | } 132 | if(!isFirst){ 133 | fileData.Add(name,dataVector); 134 | } 135 | } 136 | data_[filename] = fileData; 137 | } 138 | return &data_[filename]; 139 | } 140 | template 141 | bool Get(const std::string& filename, const std::string& name, T& x){ 142 | LoadFile(filename); 143 | return data_.at(filename).Get(name,x); 144 | } 145 | template 146 | T Get(const std::string& filename, const std::string& name){ 147 | LoadFile(filename); 148 | return data_.at(filename).Get(name); 149 | } 150 | static OptionLoader& Instance(){ 151 | static OptionLoader instance; 152 | return instance; 153 | } 154 | protected: 155 | OptionLoader(){ 156 | } 157 | }; 158 | 159 | } // namespace tsif 160 | 161 | #endif /* TSIF_OPTION_HPP_ */ 162 | -------------------------------------------------------------------------------- /include/tsif/utils/random.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_RANDOM_HPP_ 2 | #define TSIF_RANDOM_HPP_ 3 | 4 | #include "tsif/utils/typedefs.h" 5 | 6 | namespace tsif{ 7 | 8 | /*! \brief Normal Random Number Generator 9 | * Singleton class for generating normal random numbers (N(0,1)). Allows setting of seed. 10 | */ 11 | class NormalRandomNumberGenerator{ 12 | public: 13 | void SetSeed(int s){ 14 | generator_.seed(s); 15 | } 16 | double Get(){ 17 | return distribution_(generator_); 18 | } 19 | template 20 | Vec GetVec(){ 21 | Vec n; 22 | for(int i=0;i distribution_; 37 | NormalRandomNumberGenerator(): generator_(0), distribution_(0.0,1.0){ 38 | } 39 | }; 40 | 41 | } // namespace tsif 42 | 43 | #endif /* TSIF_RANDOM_HPP_ */ 44 | -------------------------------------------------------------------------------- /include/tsif/utils/rotation.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_ROTATION_HPP_ 2 | #define TSIF_ROTATION_HPP_ 3 | 4 | #include "tsif/utils/logging.h" 5 | #include "tsif/utils/typedefs.h" 6 | 7 | namespace tsif{ 8 | 9 | static constexpr double Sinc(const double x){ 10 | return fabs(x) < 1e-8 ? 1 : sin(x)/x; 11 | } 12 | static Quat Exp(const Vec3& v){ 13 | const double ha = 0.5*v.norm(); 14 | const double re = cos(ha); 15 | const Vec3 im = 0.5*Sinc(ha)*v; 16 | return Quat(re,im(0),im(1),im(2)); 17 | } 18 | static Vec3 Log(const Quat& q){ 19 | const double re = q.w(); 20 | const Vec3 im(q.x(),q.y(),q.z()); 21 | const double sha = im.norm(); 22 | return sha < 1e-8 ? ((std::signbit(re)!=0)*-2+1)*2*im : 2*atan(sha/re)/sha*im; 23 | } 24 | static Quat Boxplus(const Quat& q, const Vec3& v){ 25 | return Exp(v)*q; 26 | } 27 | static Vec3 Boxminus(const Quat& q, const Quat& p){ 28 | return Log(q*p.inverse()); 29 | } 30 | static Mat3 SSM(const Vec3& vec){ 31 | Mat3 mat; 32 | mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0; 33 | return mat; 34 | } 35 | static Mat3 RotMat(const Vec3& vec){ 36 | const double a = vec.norm(); 37 | if(a < 1e-8){ 38 | return Mat3::Identity() + SSM(vec); 39 | } else{ 40 | const Vec3 axis = vec.normalized(); 41 | return Mat3::Identity() + sin(a)*SSM(axis) + (1-cos(a))*SSM(axis)*SSM(axis); 42 | } 43 | } 44 | static Mat3 GammaMat(const Vec3& vec){ 45 | const double a = vec.norm(); 46 | if(a < 1e-8){ 47 | return Mat3::Identity() + 0.5 * SSM(vec); 48 | } else{ 49 | const Vec3 axis = vec.normalized(); 50 | return Mat3::Identity() + (1-cos(a))/a*SSM(axis) + (a-sin(a))/a*SSM(axis)*SSM(axis); 51 | } 52 | } 53 | static Mat3 FromTwoVectorsJac(const Vec3& a, const Vec3& b){ 54 | const Vec3 cross = a.cross(b); 55 | const double crossNorm = cross.norm(); 56 | Vec3 crossNormalized = cross/crossNorm; 57 | Mat3 crossNormalizedSqew = SSM(crossNormalized); 58 | const double c = a.dot(b); 59 | const double angle = std::acos(c); 60 | if(crossNorm<1e-6){ 61 | if(c>0){ 62 | return -SSM(b); 63 | } else { 64 | TSIF_LOGW("Warning: instable FromTwoVectorsJac!"); 65 | return Mat3::Zero(); 66 | } 67 | } else { 68 | return -1/crossNorm*(crossNormalized*b.transpose()-(crossNormalizedSqew*crossNormalizedSqew*SSM(b)*angle)); 69 | } 70 | } 71 | 72 | } // namespace tsif 73 | 74 | #endif /* TSIF_ROTATION_HPP_ */ 75 | -------------------------------------------------------------------------------- /include/tsif/utils/simulator.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_SIMULATOR_H_ 2 | #define TSIF_SIMULATOR_H_ 3 | 4 | #include "tsif/utils/common.h" 5 | #include "tsif/utils/camera.h" 6 | 7 | class Simulator{ 8 | public: 9 | // Simulated parameters 10 | tsif::Vec3 sim_bw_; 11 | tsif::Vec3 sim_bf_; 12 | tsif::Vec3 Ig_; 13 | tsif::Vec3 sim_IrIJ_; 14 | tsif::Quat sim_qIJ_; 15 | tsif::Vec3 sim_MrMC_; 16 | tsif::Quat sim_qMC_; 17 | tsif::Quat sim_qIM_des_; 18 | 19 | // Simulated trajectory 20 | double t_; 21 | tsif::Vec3 sim_IrIM_; 22 | tsif::Quat sim_qIM_; 23 | tsif::Vec3 sim_IvM_; 24 | tsif::Vec3 sim_IwIM_; 25 | tsif::Vec3 sim_IaM_; 26 | 27 | // IMU measurement 28 | tsif::Vec3 meas_MwM_; 29 | tsif::Vec3 meas_MfM_; 30 | tsif::Vec3 meas_MwM2_; 31 | tsif::Vec3 meas_MfM2_; 32 | 33 | // Pose measurement 34 | tsif::Vec3 meas_JrJC_; 35 | tsif::Quat meas_qJC_; 36 | 37 | // Landmarks 38 | tsif::Camera cam_; 39 | static const int kL = 1000; 40 | std::array JrJL_; 41 | std::array,kL> meas_CcCL_; 42 | std::array meas_CdCL_; 43 | std::array meas_CcCL_isVisible_; 44 | 45 | Simulator(); 46 | ~Simulator(); 47 | void step(); 48 | void init(); 49 | void genMeas(); 50 | 51 | // Simulation parameters 52 | int sim_seed_; 53 | double sim_dt_; 54 | double sim_aa_; 55 | double sim_ar_; 56 | double sim_av_; 57 | double sim_an_; 58 | double sim_ww_; 59 | double sim_wq_; 60 | double sim_wn_; 61 | double sim_noise_gyr_; 62 | double sim_outlier_noise_gyr_; 63 | double sim_outlier_rate_gyr_; 64 | double sim_noise_acc_; 65 | double sim_outlier_noise_acc_; 66 | double sim_outlier_rate_acc_; 67 | double sim_noise_pos_; 68 | double sim_outlier_noise_pos_; 69 | double sim_outlier_rate_pos_; 70 | double sim_noise_rot_; 71 | double sim_landmark_spread_; 72 | bool allowOutlier_; 73 | }; 74 | 75 | #endif /* TSIF_SIMULATOR_H_ */ 76 | -------------------------------------------------------------------------------- /include/tsif/utils/timing.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_TIMING_HPP_ 2 | #define TSIF_TIMING_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | namespace tsif{ 8 | 9 | typedef std::chrono::high_resolution_clock Clock; 10 | typedef Clock::time_point TimePoint; 11 | typedef Clock::duration Duration; 12 | inline Duration fromSec(const double sec){ 13 | return std::chrono::duration_cast < Duration 14 | > (std::chrono::duration(sec)); 15 | } 16 | inline double toSec(const Duration& duration){ 17 | return std::chrono::duration_cast>(duration).count(); 18 | } 19 | static std::string Print(TimePoint t){ 20 | std::ostringstream out; 21 | out.precision(15); 22 | out << ((double)t.time_since_epoch().count()*Duration::period::num)/(Duration::period::den); 23 | return out.str(); 24 | } 25 | 26 | class Timer{ 27 | public: 28 | Timer(){ 29 | start_ = Clock::now(); 30 | last_ = start_; 31 | } 32 | double GetIncr(){ 33 | TimePoint now = Clock::now(); 34 | double incr = toSec(now-last_); 35 | last_ = now; 36 | return incr; 37 | } 38 | double GetFull(){ 39 | last_ = Clock::now(); 40 | return toSec(last_-start_); 41 | } 42 | TimePoint start_; 43 | TimePoint last_; 44 | }; 45 | 46 | } // namespace tsif 47 | 48 | #endif /* TSIF_TIMING_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/tsif/utils/typedefs.h: -------------------------------------------------------------------------------- 1 | #ifndef TSIF_TYPEDEFS_HPP_ 2 | #define TSIF_TYPEDEFS_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | namespace tsif{ 8 | 9 | template 10 | using Vec = Eigen::Matrix; 11 | using Vec3 = Vec<3>; 12 | using VecX = Vec<>; 13 | template 14 | using VecRef = Eigen::Ref>; 15 | using VecRef3 = VecRef<3>; 16 | using VecRefX = VecRef<>; 17 | template 18 | using VecCRef = Eigen::Ref>; 19 | using VecCRef3 = VecCRef<3>; 20 | using VecCRefX = VecCRef<>; 21 | 22 | template 23 | using Mat = Eigen::Matrix; 24 | using Mat3 = Mat<3>; 25 | using MatX = Mat<>; 26 | template 27 | using MatRef = typename std::conditional<(N==1 & M>1), 28 | Eigen::Ref,0,Eigen::InnerStride<>>, 29 | Eigen::Ref>>::type; 30 | using MatRef3 = MatRef<3>; 31 | using MatRefX = MatRef<>; 32 | template 33 | using MatCRef = Eigen::Ref>; 34 | using MatCRef3 = MatCRef<3>; 35 | using MatCRefX = MatCRef<>; 36 | 37 | typedef Eigen::Quaterniond Quat; 38 | 39 | } // namespace tsif 40 | 41 | #endif /* TSIF_TYPEDEFS_HPP_ */ 42 | -------------------------------------------------------------------------------- /src/camera.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/utils/camera.h" 2 | 3 | namespace tsif{ 4 | 5 | Camera::Camera(){ 6 | k1_ = -0.28340811; k2_ = 0.07395907; k3_ = 0.0; k4_ = 0.0; k5_ = 0.0; k6_ = 0.0; 7 | p1_ = 0.00019359; p2_ = 1.76187114e-05; s1_ = 0.0; s2_ = 0.0; s3_ = 0.0; s4_ = 0.0; 8 | K_.setIdentity(); 9 | K_(0,0) = 458.654; 10 | K_(0,2) = 367.215; 11 | K_(1,1) = 457.296; 12 | K_(1,2) = 248.375; 13 | type_ = RADTAN; 14 | }; 15 | 16 | Camera::~Camera(){}; 17 | 18 | void Camera::LoadCameraMatrix(const std::string& filename){ 19 | } 20 | 21 | void Camera::LoadRadtan(const std::string& filename){ 22 | } 23 | 24 | void Camera::LoadEquidist(const std::string& filename){ 25 | } 26 | 27 | void Camera::Load(const std::string& filename){ 28 | } 29 | 30 | void Camera::DistortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out) const{ 31 | const double x2 = in(0) * in(0); 32 | const double y2 = in(1) * in(1); 33 | const double xy = in(0) * in(1); 34 | const double r2 = x2 + y2; 35 | const double kr = (1 + ((k3_ * r2 + k2_) * r2 + k1_) * r2); 36 | out(0) = in(0) * kr + p1_ * 2 * xy + p2_ * (r2 + 2 * x2); 37 | out(1) = in(1) * kr + p1_ * (r2 + 2 * y2) + p2_ * 2 * xy; 38 | } 39 | void Camera::DistortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const{ 40 | const double x2 = in(0) * in(0); 41 | const double y2 = in(1) * in(1); 42 | const double xy = in(0) * in(1); 43 | const double r2 = x2 + y2; 44 | const double kr = (1 + ((k3_ * r2 + k2_) * r2 + k1_) * r2); 45 | out(0) = in(0) * kr + p1_ * 2 * xy + p2_ * (r2 + 2 * x2); 46 | out(1) = in(1) * kr + p1_ * (r2 + 2 * y2) + p2_ * 2 * xy; 47 | J(0,0) = kr + 2.0 * k1_ * x2 + 4.0 * k2_ * x2 * r2 + 6.0 * k3_ * x2 * r2 * r2 + 2.0 * p1_ * in(1) + 6.0 * p2_ * in(0); 48 | J(0,1) = 2.0 * k1_ * xy + 4.0 * k2_ * xy * r2 + 6.0 * k3_ * xy * r2 * r2 + 2 * p1_ * in(0) + 2 * p2_ * in(1); 49 | J(1,0) = J(0,1); 50 | J(1,1) = kr + 2.0 * k1_ * y2 + 4.0 * k2_ * y2 * r2 + 6.0 * k3_ * y2 * r2 * r2 + 6.0 * p1_ * in(1) + 2.0 * p2_ * in(0); 51 | } 52 | 53 | void Camera::DistortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out) const{ 54 | const double x2 = in(0) * in(0); 55 | const double y2 = in(1) * in(1); 56 | const double r = std::sqrt(x2 + y2); // 1/r*x 57 | 58 | if(r < 1e-8){ 59 | out(0) = in(0); 60 | out(1) = in(1); 61 | return; 62 | } 63 | 64 | const double th = atan(r); // 1/(r^2 + 1) 65 | const double th2 = th*th; 66 | const double th4 = th2*th2; 67 | const double th6 = th2*th4; 68 | const double th8 = th2*th6; 69 | const double thd = th * (1.0 + k1_ * th2 + k2_ * th4 + k3_ * th6 + k4_ * th8); 70 | const double s = thd/r; 71 | 72 | out(0) = in(0) * s; 73 | out(1) = in(1) * s; 74 | } 75 | void Camera::DistortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const{ 76 | const double x2 = in(0) * in(0); 77 | const double y2 = in(1) * in(1); 78 | const double r = std::sqrt(x2 + y2); 79 | 80 | if(r < 1e-8){ 81 | out(0) = in(0); 82 | out(1) = in(1); 83 | J.setIdentity(); 84 | return; 85 | } 86 | 87 | const double r_x = 1/r*in(0); 88 | const double r_y = 1/r*in(1); 89 | 90 | const double th = atan(r); // 1/(r^2 + 1) 91 | const double th_r = 1/(r*r+1); 92 | const double th2 = th*th; 93 | const double th4 = th2*th2; 94 | const double th6 = th2*th4; 95 | const double th8 = th2*th6; 96 | const double thd = th * (1.0 + k1_ * th2 + k2_ * th4 + k3_ * th6 + k4_ * th8); 97 | const double thd_th = 1.0 + 3 * k1_ * th2 + 5* k2_ * th4 + 7 * k3_ * th6 + 9 * k4_ * th8; 98 | const double s = thd/r; 99 | const double s_r = thd_th*th_r/r - thd/(r*r); 100 | 101 | out(0) = in(0) * s; 102 | out(1) = in(1) * s; 103 | 104 | J(0,0) = s + in(0)*s_r*r_x; 105 | J(0,1) = in(0)*s_r*r_y; 106 | J(1,0) = in(1)*s_r*r_x; 107 | J(1,1) = s + in(1)*s_r*r_y; 108 | } 109 | 110 | void Camera::Distort(const Eigen::Vector2d& in, Eigen::Vector2d& out) const{ 111 | switch(type_){ 112 | case RADTAN: 113 | DistortRadtan(in,out); 114 | break; 115 | case EQUIDIST: 116 | DistortEquidist(in,out); 117 | break; 118 | default: 119 | DistortRadtan(in,out); 120 | break; 121 | } 122 | } 123 | void Camera::Distort(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const{ 124 | switch(type_){ 125 | case RADTAN: 126 | DistortRadtan(in,out,J); 127 | break; 128 | case EQUIDIST: 129 | DistortEquidist(in,out,J); 130 | break; 131 | default: 132 | DistortRadtan(in,out,J); 133 | break; 134 | } 135 | } 136 | 137 | bool Camera::BearingToPixel(const Eigen::Vector3d& vec, Eigen::Vector2d& c) const{ 138 | // Project 139 | if(vec(2)<=1e-8) return false; 140 | const Eigen::Vector2d undistorted = Eigen::Vector2d(vec(0)/vec(2),vec(1)/vec(2)); 141 | 142 | // Distort 143 | Eigen::Vector2d distorted; 144 | Distort(undistorted,distorted); 145 | 146 | // Shift origin and scale 147 | c(0) = K_(0, 0)*distorted(0) + K_(0, 2); 148 | c(1) = K_(1, 1)*distorted(1) + K_(1, 2); 149 | return true; 150 | } 151 | bool Camera::BearingToPixel(const Eigen::Vector3d& vec, Eigen::Vector2d& c, Eigen::Matrix& J) const{ 152 | // Project 153 | if(vec(2)<=1e-8) return false; 154 | const Eigen::Vector2d undistorted = Eigen::Vector2d(vec(0)/vec(2),vec(1)/vec(2)); 155 | Eigen::Matrix J1; J1.setZero(); 156 | J1(0,0) = 1.0/vec(2); 157 | J1(0,2) = -vec(0)/pow(vec(2),2); 158 | J1(1,1) = 1.0/vec(2); 159 | J1(1,2) = -vec(1)/pow(vec(2),2); 160 | 161 | // Distort 162 | Eigen::Vector2d distorted; 163 | Eigen::Matrix2d J2; 164 | Distort(undistorted,distorted,J2); 165 | 166 | // Shift origin and scale 167 | c(0) = K_(0, 0)*distorted(0) + K_(0, 2); 168 | c(1) = K_(1, 1)*distorted(1) + K_(1, 2); 169 | Eigen::Matrix2d J3; J3.setZero(); 170 | J3(0,0) = K_(0, 0); 171 | J3(1,1) = K_(1, 1); 172 | 173 | J = J3*J2*J1; 174 | 175 | return true; 176 | } 177 | 178 | bool Camera::PixelToBearing(const Eigen::Vector2d& c,Eigen::Vector3d& vec) const{ 179 | // Shift origin and scale 180 | Eigen::Vector2d y; 181 | y(0) = (c(0) - K_(0, 2)) / K_(0, 0); 182 | y(1) = (c(1) - K_(1, 2)) / K_(1, 1); 183 | 184 | // UnDistort by optimizing 185 | const int max_iter = 100; 186 | const double tolerance = 1e-10; 187 | Eigen::Vector2d ybar = y; // current guess (undistorted) 188 | Eigen::Matrix2d J; 189 | Eigen::Vector2d y_tmp; // current guess (distorted) 190 | Eigen::Vector2d e; 191 | Eigen::Vector2d du; 192 | bool success = false; 193 | for (int i = 0; i < max_iter; i++) { 194 | Distort(ybar,y_tmp,J); 195 | e = y - y_tmp; 196 | du = (J.transpose() * J).inverse() * J.transpose() * e; 197 | ybar += du; 198 | if (e.dot(e) <= tolerance){ 199 | success = true; 200 | break; 201 | } 202 | } 203 | if(success){ 204 | y = ybar; 205 | vec = Eigen::Vector3d(y(0),y(1),1.0).normalized(); 206 | } 207 | return success; 208 | } 209 | 210 | void Camera::TestCameraModel(){ 211 | // double d = 1e-4; 212 | // LWF::NormalVectorElement b_s; 213 | // LWF::NormalVectorElement b_s1; 214 | // LWF::NormalVectorElement b_s2; 215 | // Eigen::Vector3d v_s; 216 | // Eigen::Vector3d v_s1; 217 | // Eigen::Vector3d v_s2; 218 | // LWF::NormalVectorElement b_e; 219 | // Eigen::Matrix2d J1; 220 | // Eigen::Matrix2d J1_FD; 221 | // Eigen::Matrix J2; 222 | // Eigen::Matrix J2_FD; 223 | // Eigen::Vector2d p_s; 224 | // Eigen::Vector2d p_s1; 225 | // Eigen::Vector2d p_s2; 226 | // Eigen::Vector2d p_s3; 227 | // Eigen::Vector2d diff; 228 | // for(unsigned int s = 1; s<10;){ 229 | // b_s.setRandom(s); 230 | // if(b_s.getVec()(2)<0) b_s = b_s.inverted(); 231 | // BearingToPixel(b_s,p_s,J1); 232 | // pixelToBearing(p_s,b_e); 233 | // b_s.boxMinus(b_e,diff); 234 | // std::cout << b_s.getVec().transpose() << std::endl; 235 | // std::cout << "Error after back and forward mapping: " << diff.norm() << std::endl; 236 | // diff = Eigen::Vector2d(d,0); 237 | // b_s.boxPlus(diff,b_s1); 238 | // BearingToPixel(b_s1,p_s1); 239 | // J1_FD(0,0) = static_cast((p_s1-p_s).x)/d; 240 | // J1_FD(1,0) = static_cast((p_s1-p_s).y)/d; 241 | // diff = Eigen::Vector2d(0,d); 242 | // b_s.boxPlus(diff,b_s2); 243 | // BearingToPixel(b_s2,p_s2); 244 | // J1_FD(0,1) = static_cast((p_s2-p_s).x)/d; 245 | // J1_FD(1,1) = static_cast((p_s2-p_s).y)/d; 246 | // std::cout << J1 << std::endl; 247 | // std::cout << J1_FD << std::endl; 248 | // 249 | // v_s = b_s.getVec(); 250 | // BearingToPixel(v_s,p_s,J2); 251 | // BearingToPixel(v_s + Eigen::Vector3d(d,0,0),p_s1); 252 | // BearingToPixel(v_s + Eigen::Vector3d(0,d,0),p_s2); 253 | // BearingToPixel(v_s + Eigen::Vector3d(0,0,d),p_s3); 254 | // J2_FD(0,0) = static_cast((p_s1-p_s).x)/d; 255 | // J2_FD(1,0) = static_cast((p_s1-p_s).y)/d; 256 | // J2_FD(0,1) = static_cast((p_s2-p_s).x)/d; 257 | // J2_FD(1,1) = static_cast((p_s2-p_s).y)/d; 258 | // J2_FD(0,2) = static_cast((p_s3-p_s).x)/d; 259 | // J2_FD(1,2) = static_cast((p_s3-p_s).y)/d; 260 | // std::cout << J2 << std::endl; 261 | // std::cout << J2_FD << std::endl; 262 | // } 263 | } 264 | } 265 | -------------------------------------------------------------------------------- /src/simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/utils/simulator.h" 2 | 3 | Simulator::Simulator(){ 4 | // Simulated parameters 5 | sim_bw_ = tsif::Vec3(0.2,-0.1,0.1); 6 | sim_bf_ = tsif::Vec3(0.3,0.1,-0.1); 7 | Ig_ = tsif::Vec3(0,0,-9.81); 8 | sim_IrIJ_ = tsif::Vec3(0,0,0); 9 | sim_qIJ_ = tsif::Quat(1,0,0,0); 10 | sim_MrMC_ = tsif::Vec3(-0.0111674199187,-0.0574640920022,0.0207586947896); 11 | sim_qMC_ = tsif::Quat(-0.712115587266,0.00666398307551,-0.0079168224269,-0.701985972528); 12 | sim_qIM_des_ = tsif::Quat(1,0,0,0); 13 | 14 | // Simulation parameters 15 | sim_seed_ = 0; 16 | sim_dt_ = 0.01; 17 | sim_aa_ = 1; 18 | sim_ar_ = 0.1; 19 | sim_av_ = 1; 20 | sim_an_ = 5; 21 | sim_wq_ = 0.1; 22 | sim_ww_ = 0.1; 23 | sim_wn_ = 0.5; 24 | 25 | sim_noise_gyr_ = 2e-3; 26 | sim_outlier_noise_gyr_ = 1; 27 | sim_outlier_rate_gyr_ = 1000; 28 | sim_noise_acc_ = 1e-2; 29 | sim_outlier_noise_acc_ = 10; 30 | sim_outlier_rate_acc_ = 1000; 31 | sim_noise_pos_ = 1e-2; 32 | sim_outlier_noise_pos_ = 10; 33 | sim_outlier_rate_pos_ = 1000; 34 | sim_noise_rot_ = 1e-2; 35 | sim_landmark_spread_ = 10; 36 | allowOutlier_ = false; 37 | 38 | for(int i=0;i(); 40 | } 41 | 42 | init(); 43 | } 44 | 45 | Simulator::~Simulator(){ 46 | } 47 | 48 | void Simulator::step(){ 49 | tsif::NormalRandomNumberGenerator& gen = tsif::NormalRandomNumberGenerator::Instance(); 50 | t_ = t_ + sim_dt_; 51 | sim_IaM_ = (1-sim_aa_*sim_dt_) * sim_IaM_ 52 | - sim_ar_*sim_dt_ * sim_IrIM_ 53 | - sim_av_*sim_dt_ * sim_IvM_ 54 | + sim_an_*std::sqrt(sim_dt_)*gen.GetVec<3>(); 55 | sim_IvM_ = sim_IvM_ + sim_dt_ * sim_IaM_; 56 | sim_IrIM_ = sim_IrIM_ + sim_dt_ * sim_IvM_; 57 | 58 | sim_IwIM_ = (1-sim_ww_*sim_dt_) * sim_IwIM_ 59 | + sim_wq_*sim_dt_ * tsif::Boxminus(sim_qIM_des_,sim_qIM_) 60 | + sim_wn_*std::sqrt(sim_dt_)*gen.GetVec<3>(); 61 | tsif::Quat sim_qIM_next = tsif::Boxplus(sim_qIM_,sim_IwIM_*sim_dt_); 62 | sim_qIM_ = sim_qIM_next; 63 | 64 | genMeas(); 65 | } 66 | void Simulator::init(){ 67 | tsif::NormalRandomNumberGenerator::Instance().SetSeed(sim_seed_); 68 | 69 | // Simulated trajectory 70 | t_ = 0; 71 | sim_IrIM_ = tsif::Vec3(0,0,0); 72 | sim_qIM_ = tsif::Quat(1,0,0,0); 73 | sim_IvM_ = tsif::Vec3(0,0,0); 74 | sim_IwIM_ = tsif::Vec3(0,0,0); 75 | sim_IaM_ = tsif::Vec3(0,0,0); 76 | 77 | // Generate measurements 78 | genMeas(); 79 | } 80 | void Simulator::genMeas(){ 81 | tsif::NormalRandomNumberGenerator& gen = tsif::NormalRandomNumberGenerator::Instance(); 82 | 83 | // IMU measurement 84 | std::bernoulli_distribution distribution_gyr(1/sim_outlier_rate_gyr_); 85 | double noise_gyr = allowOutlier_ && distribution_gyr(gen.GetGenerator()) ? sim_outlier_noise_gyr_ : sim_noise_gyr_; 86 | meas_MwM_ = sim_qIM_.toRotationMatrix().transpose()*(sim_IwIM_) + sim_bw_ + noise_gyr/std::sqrt(sim_dt_)*gen.GetVec<3>(); 87 | std::bernoulli_distribution distribution_acc(1/sim_outlier_rate_acc_); 88 | double noise_acc = allowOutlier_ && distribution_acc(gen.GetGenerator()) ? sim_outlier_noise_acc_ : sim_noise_acc_; 89 | meas_MfM_ = sim_qIM_.toRotationMatrix().transpose()*(tsif::Vec3(sim_IaM_ - Ig_)) + sim_bf_ + noise_acc/std::sqrt(sim_dt_)*gen.GetVec<3>(); 90 | 91 | // IMU measurement 92 | noise_gyr = allowOutlier_ && distribution_gyr(gen.GetGenerator()) ? sim_outlier_noise_gyr_ : sim_noise_gyr_; 93 | meas_MwM2_ = sim_qIM_.toRotationMatrix().transpose()*(sim_IwIM_) + sim_bw_ + noise_gyr/std::sqrt(sim_dt_)*gen.GetVec<3>(); 94 | noise_acc = allowOutlier_ && distribution_acc(gen.GetGenerator()) ? sim_outlier_noise_acc_ : sim_noise_acc_; 95 | meas_MfM2_ = sim_qIM_.toRotationMatrix().transpose()*(tsif::Vec3(sim_IaM_ - Ig_)) + sim_bf_ + noise_acc/std::sqrt(sim_dt_)*gen.GetVec<3>(); 96 | 97 | // Pose measurement 98 | std::bernoulli_distribution distribution_pos(1/sim_outlier_rate_pos_); 99 | double noise_pos = allowOutlier_ && distribution_pos(gen.GetGenerator()) ? sim_outlier_noise_pos_ : sim_noise_pos_; 100 | const tsif::Vec3 JrJC = sim_qIJ_.toRotationMatrix().transpose()*(tsif::Vec3(sim_IrIM_ - sim_IrIJ_ + sim_qIM_.toRotationMatrix()*(sim_MrMC_))); 101 | meas_JrJC_ = JrJC+noise_pos*gen.GetVec<3>(); 102 | meas_qJC_ = tsif::Boxplus((sim_qIJ_.inverse()*sim_qIM_*sim_qMC_),sim_noise_rot_*gen.GetVec<3>()); 103 | 104 | // Image measurement 105 | for(int i=0;i test; 6 | test.JacPreTest(1e-6,1e-8); 7 | test.JacCurTest(1e-6,1e-8); 8 | 9 | 10 | tsif::UnitVector test_uv0,test_uv1,test_uv0_pert,test_uv1_pert; 11 | tsif::Vec<2> vec2,dir,pert; 12 | vec2 = tsif::NormalRandomNumberGenerator::Instance().GetVec<2>(); 13 | dir = tsif::NormalRandomNumberGenerator::Instance().GetVec<2>(); 14 | test_uv0.SetRandom(); 15 | test_uv0.Boxplus(vec2,test_uv1); 16 | test_uv0.Boxplus(vec2+dir*1e-8,test_uv1_pert); 17 | test_uv1_pert.Boxminus(test_uv1,pert); 18 | const tsif::Vec<2> der1 = pert*1e8; 19 | tsif::Mat<2> J; 20 | test_uv0.BoxplusJacVec(vec2,J); 21 | const tsif::Vec<2> der2 = J*dir; 22 | std::cout << "BoxplusJavVec" << std::endl; 23 | std::cout << der1.transpose() << std::endl; 24 | std::cout << der2.transpose() << std::endl; 25 | test_uv0.Boxplus(vec2,test_uv1); 26 | test_uv0.Boxplus(dir*1e-8,test_uv0_pert); 27 | test_uv0_pert.Boxplus(vec2,test_uv1_pert); 28 | test_uv1_pert.Boxminus(test_uv1,pert); 29 | test_uv0.BoxplusJacInp(vec2,J); 30 | std::cout << "BoxplusJacInp" << std::endl; 31 | std::cout << (pert).transpose()*1e8 << std::endl; 32 | std::cout << (J*dir).transpose() << std::endl; 33 | 34 | 35 | 36 | tsif::Vec<2> out2,out2_pert; 37 | tsif::UnitVector test_uv2; 38 | tsif::Mat<2> J1,J2; 39 | const double d = 1e-8; 40 | // Reference 41 | test_uv0.Boxplus(vec2,test_uv1); 42 | test_uv2.Boxminus(test_uv1,out2); 43 | test_uv0.BoxplusJacInp(vec2,J1); 44 | test_uv2.BoxminusJacRef(test_uv1,J2); 45 | 46 | // Perturbed 47 | test_uv0.Boxplus(dir*d,test_uv0_pert); 48 | test_uv0_pert.Boxplus(vec2,test_uv1_pert); 49 | test_uv2.Boxminus(test_uv1_pert,out2_pert); 50 | 51 | std::cout << "Jac Test 1" << std::endl; 52 | tsif::Vec<2> dif; 53 | test_uv1_pert.Boxminus(test_uv1,dif); 54 | std::cout << test_uv1_pert.GetQuat().coeffs().transpose() << std::endl; 55 | std::cout << (dif).transpose()/d << std::endl; 56 | std::cout << (J1*dir).transpose() << std::endl; 57 | 58 | std::cout << "Jac Test 2" << std::endl; 59 | std::cout << (out2_pert-out2).transpose()/d << std::endl; 60 | std::cout << (J2*J1*dir).transpose() << std::endl; 61 | 62 | 63 | 64 | dir = dif/d; 65 | test_uv1.Boxplus(dir*d,test_uv1_pert); 66 | std::cout << test_uv1_pert.GetQuat().coeffs().transpose() << std::endl; 67 | test_uv2.Boxminus(test_uv1,out2); 68 | test_uv2.Boxminus(test_uv1_pert,out2_pert); 69 | 70 | std::cout << "Jac Test 3" << std::endl; 71 | std::cout << (out2_pert-out2).transpose()/d << std::endl; 72 | std::cout << (J2*dir).transpose() << std::endl; 73 | 74 | 75 | 76 | 77 | 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /src/test_imu_gps.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/filters/imu_gps.h" 2 | #include "tsif/utils/simulator.h" 3 | 4 | int main(int argc, char** argv){ 5 | tsif::ImuGpsFilter filter; 6 | Simulator sim; 7 | sim.allowOutlier_ = false; 8 | sim.init(); 9 | 10 | for(int i=0;i<10000;i++){ 11 | sim.step(); 12 | filter.AddMeas<2>(tsif::TimePoint(tsif::fromSec(sim.t_)),std::make_shared(sim.meas_MfM_)); 13 | filter.AddMeas<3>(tsif::TimePoint(tsif::fromSec(sim.t_-sim.sim_dt_)),std::make_shared(sim.meas_MwM_)); 14 | filter.AddMeas<6>(tsif::TimePoint(tsif::fromSec(sim.t_)),std::make_shared(sim.meas_JrJC_)); 15 | filter.AddMeas<8>(tsif::TimePoint(tsif::fromSec(sim.t_)),std::make_shared(sim.meas_qJC_)); 16 | filter.Update(); 17 | } 18 | std::cout << "=================== State ===================" << std::endl; 19 | std::cout << filter.GetState().Print(); 20 | std::cout << "=================== GT ===================" << std::endl; 21 | std::cout << sim.meas_JrJC_.transpose() << std::endl; 22 | std::cout << sim.meas_qJC_.coeffs().transpose() << std::endl; 23 | std::cout << (sim.sim_qIM_.toRotationMatrix().transpose()*sim.sim_IvM_).transpose() << std::endl; 24 | std::cout << (sim.sim_qIM_.toRotationMatrix().transpose()*sim.sim_IwIM_).transpose() << std::endl; 25 | std::cout << filter.PrintConnectivity(); 26 | filter.JacTestAll(1e-6,1e-8); 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /src/test_quaternion.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/utils/common.h" 2 | #include "tsif/element.h" 3 | 4 | using namespace tsif; 5 | 6 | int main(int argc, char** argv){ 7 | Vec3 vec = NormalRandomNumberGenerator::Instance().GetVec<3>(); 8 | Quat q(1,0,0,0); 9 | ElementTraits::SetRandom(q); 10 | Quat p(1,0,0,0); 11 | ElementTraits::SetRandom(p); 12 | std::cout << (q.toRotationMatrix()*p.toRotationMatrix()*vec).transpose() << std::endl; 13 | std::cout << ((q*p).toRotationMatrix()*vec).transpose() << std::endl; 14 | std::cout << q.coeffs().transpose() << std::endl; 15 | std::cout << Exp(Log(q)).coeffs().transpose() << std::endl; 16 | std::cout << RotMat(Log(q)) << std::endl; 17 | std::cout << q.toRotationMatrix() << std::endl; 18 | 19 | for(int i=0;i<10000;i++){ 20 | Quat q1; 21 | ElementTraits::SetRandom(q1); 22 | Quat q2 = Exp(Log(q1)); 23 | if(Boxminus(q1,q2).norm() > 1e-8){ 24 | std::cout << "Fault" << std::endl; 25 | } 26 | } 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /src/test_state.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/element_vector.h" 2 | 3 | using namespace tsif; 4 | 5 | int main(int argc, char** argv){ 6 | 7 | typedef ElementVector,Element> ElementVectorA; 8 | typedef ElementVector,Element> ElementVectorB; 9 | typedef ElementVector,Element,2>,4>> ElementVectorC; 10 | typedef typename MergeTrait::Type ElementVectorD; 11 | 12 | ElementVectorD vec; 13 | vec.Get<0>() = Vec3(1,-2,3); 14 | vec.Get<1>() = 1.5; 15 | std::cout << "==== ElementVector ====\n" << vec.Print() << "=======================" << std::endl; 16 | vec.SetIdentity(); 17 | std::cout << "==== ElementVector ====\n" << vec.Print() << "=======================" << std::endl; 18 | vec.SetRandom(); 19 | std::cout << "==== ElementVector ====\n" << vec.Print() << "=======================" << std::endl; 20 | 21 | std::cout << ElementVectorD::Start(0) << std::endl; 22 | std::cout << ElementVectorD::Start(1) << std::endl; 23 | std::cout << ElementVectorD::Start(2) << std::endl; 24 | std::cout << ElementVectorD::Start(3) << std::endl; 25 | std::cout << ElementVectorD::Start(4) << std::endl; 26 | std::cout << ElementVectorD::Dim() << std::endl; 27 | 28 | ElementVectorD vec2; 29 | vec2.SetRandom(); 30 | std::cout << "==== ElementVector ====\n" << vec2.Print() << "=======================" << std::endl; 31 | Vec dif; 32 | vec2.Boxminus(vec,dif); 33 | std::cout << dif.transpose() << std::endl; 34 | vec2.SetIdentity(); 35 | std::cout << "==== ElementVector ====\n" << vec2.Print() << "=======================" << std::endl; 36 | vec.Boxplus(dif,vec2); 37 | std::cout << "==== ElementVector ====\n" << vec2.Print() << "=======================" << std::endl; 38 | 39 | ElementVector> cur; 40 | cur.SetRandom(); 41 | ElementVector,Element> pre; 42 | pre.SetRandom(); 43 | std::cout << pre.Print() << std::endl; 44 | ElementVectorRef> pre_ref(pre); 45 | std::cout << pre_ref.Print() << std::endl; 46 | 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /src/test_timeline.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/filter.h" 2 | 3 | using namespace tsif; 4 | 5 | class MeasA: public ElementVector>{ 6 | public: 7 | MeasA(): ElementVector>(Vec3(0,0,0)){} 8 | MeasA(const Vec3& acc): ElementVector>(acc){} 9 | const Vec3& GetAcc() const{ 10 | return Get<0>(); 11 | } 12 | Vec3& GetAcc(){ 13 | return Get<0>(); 14 | } 15 | }; 16 | 17 | template 18 | using ResidualPosBase = Residual>, 19 | ElementVector,Element>, 20 | ElementVector>, 21 | MeasEmpty>; 22 | 23 | template 24 | class ResidualPos: public ResidualPosBase{ 25 | public: 26 | typedef ResidualPosBase Base; 27 | using Base::dt_; 28 | typedef typename Base::Output Output; 29 | typedef typename Base::Previous Previous; 30 | typedef typename Base::Current Current; 31 | ResidualPos(): ResidualPosBase(true,true,true){} 32 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 33 | out.template Get() = cur.template Get() - pre.template Get() 34 | - dt_*pre.template Get(); 35 | return 0; 36 | } 37 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 38 | J.block<3,3>(Output::Start(OUT_POS),pre.Start(STA_POS)) = -Mat3::Identity(); 39 | J.block<3,3>(Output::Start(OUT_POS),pre.Start(STA_VEL)) = -Mat3::Identity()*dt_; 40 | return 0; 41 | } 42 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 43 | J.block<3,3>(Output::Start(OUT_POS),pre.Start(STA_POS)) = Mat3::Identity(); 44 | return 0; 45 | } 46 | }; 47 | 48 | template 49 | using ResdidualVelBase = Residual>, 50 | ElementVector>, 51 | ElementVector>, 52 | MeasA>; 53 | 54 | template 55 | class ResidualVel: public ResdidualVelBase{ 56 | public: 57 | typedef ResdidualVelBase Base; 58 | using Base::dt_; 59 | using Base::meas_; 60 | typedef typename Base::Output Output; 61 | typedef typename Base::Previous Previous; 62 | typedef typename Base::Current Current; 63 | ResidualVel(): ResdidualVelBase(true,false,true){} 64 | int EvalRes(typename Output::Ref out, const typename Previous::CRef pre, const typename Current::CRef cur){ 65 | out.template Get() = cur.template Get() - pre.template Get() - dt_*meas_->GetAcc(); 66 | return 0; 67 | } 68 | int JacPre(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 69 | J.block<3,3>(Output::Start(OUT_VEL),pre.Start(STA_VEL)) = -Mat3::Identity(); 70 | return 0; 71 | } 72 | int JacCur(MatRefX J, const typename Previous::CRef pre, const typename Current::CRef cur){ 73 | J.block<3,3>(Output::Start(OUT_VEL),pre.Start(STA_VEL)) = Mat3::Identity(); 74 | return 0; 75 | } 76 | }; 77 | 78 | 79 | int main(int argc, char** argv){ 80 | typedef Filter,ResidualVel<0,1>> FilterA; 81 | FilterA filter; 82 | FilterA::State state_pre; 83 | state_pre.SetRandom(); 84 | FilterA::State state_cur; 85 | state_cur.SetRandom(); 86 | filter.JacTestAll(1e-6,1e-8,state_pre,state_cur); 87 | std::cout << state_cur.Print() << std::endl; 88 | 89 | std::cout << Print(filter.GetMinMaxTime()) << std::endl; 90 | TimePoint start = Clock::now(); 91 | filter.Update(); 92 | std::cout << Print(filter.GetMinMaxTime()) << std::endl; 93 | filter.Update(); 94 | filter.AddMeas<1>(start+fromSec(0.01),std::make_shared(NormalRandomNumberGenerator::Instance().GetVec<3>())); 95 | std::cout << Print(filter.GetMinMaxTime()) << std::endl; 96 | filter.AddMeas<1>(start+fromSec(0.03),std::make_shared(NormalRandomNumberGenerator::Instance().GetVec<3>())); 97 | std::cout << Print(filter.GetMinMaxTime()) << std::endl; 98 | filter.Update(); 99 | std::cout << filter.GetState().Print() << std::endl; 100 | for(int i=0;i<10;i++){ 101 | filter.AddMeas<1>(start+fromSec(0.05+0.02*i),std::make_shared(NormalRandomNumberGenerator::Instance().GetVec<3>())); 102 | } 103 | filter.Update(); 104 | std::cout << filter.PrintConnectivity() << std::endl; 105 | 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /src/test_unit_vector.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/unit_vector.h" 2 | 3 | using namespace tsif; 4 | 5 | int main(int argc, char** argv){ 6 | tsif::UnitVector uv(Vec3(1,2,3)); 7 | std::cout << uv.GetVec().transpose() << std::endl; 8 | std::cout << Vec3(1,2,3).normalized().transpose() << std::endl; 9 | uv.SetIdentity(); 10 | std::cout << uv.GetVec().transpose() << std::endl; 11 | uv.SetFromVector(Vec3(1,2,3)*1e-12); 12 | std::cout << uv.GetVec().transpose() << std::endl; 13 | uv.SetFromVector(Vec3(0,0,0)); 14 | std::cout << uv.GetVec().transpose() << std::endl; 15 | uv.SetFromVector(Vec3(0,0,-1)); 16 | std::cout << uv.GetVec().transpose() << std::endl; 17 | 18 | tsif::UnitVector uv0(Vec3(0.1,0,1)); 19 | tsif::UnitVector uv1(Vec3(0.2,0,1)); 20 | std::cout << uv1.GetVec().transpose() << std::endl; 21 | Vec<2> unitVectorDif; 22 | uv1.Boxminus(uv0,unitVectorDif); 23 | std::cout << unitVectorDif.transpose() << std::endl; 24 | uv1.SetIdentity(); 25 | std::cout << uv1.GetVec().transpose() << std::endl; 26 | uv0.Boxplus(unitVectorDif,uv1); 27 | std::cout << uv1.GetVec().transpose() << std::endl; 28 | 29 | tsif::UnitVector test_uv0, test_uv0_pert, test_uv1, test_uv1_pert; 30 | test_uv0.SetRandom(); 31 | test_uv1.SetRandom(); 32 | Vec<2> dir = tsif::NormalRandomNumberGenerator::Instance().GetVec<2>(); 33 | Vec<2> ref,pert; 34 | test_uv1.Boxminus(test_uv0,ref); 35 | test_uv0.Boxplus(dir*1e-8,test_uv0_pert); 36 | test_uv1.Boxplus(dir*1e-8,test_uv1_pert); 37 | test_uv1.Boxminus(test_uv0_pert,pert); 38 | std::cout << "BoxminusJacRef" << std::endl; 39 | std::cout << (pert-ref).transpose()*1e8 << std::endl; 40 | Mat<2> J; 41 | test_uv1.BoxminusJacRef(test_uv0,J); 42 | std::cout << (J*dir).transpose() << std::endl; 43 | test_uv1_pert.Boxminus(test_uv0,pert); 44 | std::cout << "BoxminusJacInp" << std::endl; 45 | std::cout << (pert-ref).transpose()*1e8 << std::endl; 46 | test_uv1.BoxminusJacInp(test_uv0,J); 47 | std::cout << (J*dir).transpose() << std::endl; 48 | 49 | ref = tsif::NormalRandomNumberGenerator::Instance().GetVec<2>(); 50 | test_uv0.SetRandom(); 51 | test_uv0.Boxplus(ref,test_uv1); 52 | test_uv0.Boxplus(ref+dir*1e-8,test_uv1_pert); 53 | test_uv1_pert.Boxminus(test_uv1,pert); 54 | const Vec<2> der1 = pert*1e8; 55 | test_uv0.BoxplusJacVec(ref,J); 56 | const Vec<2> der2 = J*dir; 57 | std::cout << "BoxplusJavVec" << std::endl; 58 | std::cout << der1.transpose() << std::endl; 59 | std::cout << der2.transpose() << std::endl; 60 | test_uv0.Boxplus(ref,test_uv1); 61 | test_uv0.Boxplus(dir*1e-8,test_uv0_pert); 62 | test_uv0_pert.Boxplus(ref,test_uv1_pert); 63 | test_uv1_pert.Boxminus(test_uv1,pert); 64 | test_uv0.BoxplusJacInp(ref,J); 65 | std::cout << "BoxplusJacInp" << std::endl; 66 | std::cout << (pert).transpose()*1e8 << std::endl; 67 | std::cout << (J*dir).transpose() << std::endl; 68 | 69 | return 0; 70 | } 71 | -------------------------------------------------------------------------------- /src/test_vio.cpp: -------------------------------------------------------------------------------- 1 | #include "tsif/utils/simulator.h" 2 | #include "tsif/filters/vio.h" 3 | 4 | int main(int argc, char** argv){ 5 | std::string optionFile = "/home/michael/workspace/generalized_information_filter/cfg/vio.cfg"; 6 | std::string taskFile = "/home/michael/workspace/generalized_information_filter/cfg/vio_task.cfg"; 7 | bool useSim = tsif::OptionLoader::Instance().Get(optionFile,"use_sim"); 8 | const int L = 25; 9 | tsif::VioFilter filter(optionFile); 10 | filter.GetState().SetIdentity(); 11 | Simulator sim; 12 | sim.allowOutlier_ = false; 13 | sim.init(); 14 | 15 | std::string folder = tsif::OptionLoader::Instance().Get(taskFile,"data_folder"); 16 | 17 | std::ifstream cam0data(folder + "/mav0/cam0/data.csv"); 18 | std::string cam0string; 19 | getline(cam0data, cam0string); // Ignore first 20 | double cam0time; 21 | 22 | std::ifstream imu0data(folder + "/mav0/imu0/data.csv"); 23 | std::string imu0string; 24 | getline(imu0data, imu0string); // Ignore first 25 | double imu0time = std::numeric_limits::min(); 26 | 27 | for(int i=0;i<10000 && cam0data.good();i++){ 28 | if(useSim){ 29 | sim.step(); 30 | std::shared_ptr> meas(new tsif::MeasImg()); 31 | for(int i=0;iisSim_ = true; 33 | if(sim.meas_CcCL_isVisible_[i] && sim.meas_CcCL_[i](0) >= 0 && sim.meas_CcCL_[i](0) < 752 && sim.meas_CcCL_[i](1) >= 0 && sim.meas_CcCL_[i](1) < 480){ 34 | meas->keyPoints_.push_back(cv::KeyPoint(sim.meas_CcCL_[i](0),sim.meas_CcCL_[i](1),0,sim.meas_CdCL_[i],0,0,i)); 35 | } 36 | } 37 | filter.AddMeas<2>(tsif::TimePoint(tsif::fromSec(sim.t_+tsif::OptionLoader::Instance().Get(optionFile,"timeOffsetAcc"))),std::make_shared(sim.meas_MfM_)); 38 | filter.AddMeas<3>(tsif::TimePoint(tsif::fromSec(sim.t_+tsif::OptionLoader::Instance().Get(optionFile,"timeOffsetGyr"))),std::make_shared(sim.meas_MwM_)); 39 | filter.AddMeas<6>(tsif::TimePoint(tsif::fromSec(sim.t_+tsif::OptionLoader::Instance().Get(optionFile,"timeOffsetCam"))),meas); 40 | } else { 41 | getline (cam0data, cam0string, ','); 42 | if(!cam0data.good()){ 43 | break; 44 | } 45 | cam0time = stod(cam0string)*1e-9; 46 | getline (cam0data, cam0string); 47 | if(cam0string.back() == '\r'){ 48 | cam0string.pop_back(); 49 | } 50 | std::string imgFilename = folder + "/mav0/cam0/data/" + cam0string; 51 | std::shared_ptr> meas(new tsif::MeasImg(imgFilename.c_str())); 52 | for(int i=0;i= 0 && sim.meas_CcCL_[i](0) < 752 && sim.meas_CcCL_[i](1) >= 0 && sim.meas_CcCL_[i](1) < 480){ 54 | meas->keyPoints_.push_back(cv::KeyPoint(sim.meas_CcCL_[i](0),sim.meas_CcCL_[i](1),0,sim.meas_CdCL_[i],0,0,i)); 55 | } 56 | } 57 | filter.AddMeas<6>(tsif::TimePoint(tsif::fromSec(cam0time+tsif::OptionLoader::Instance().Get(optionFile,"timeOffsetCam"))),meas); 58 | 59 | while(imu0time < cam0time){ 60 | getline (imu0data, imu0string, ','); 61 | if(!imu0data.good()){ 62 | break; 63 | } 64 | imu0time = stod(imu0string)*1e-9; 65 | getline (imu0data, imu0string, ','); 66 | const double wx = stod(imu0string); 67 | getline (imu0data, imu0string, ','); 68 | const double wy = stod(imu0string); 69 | getline (imu0data, imu0string, ','); 70 | const double wz = stod(imu0string); 71 | getline (imu0data, imu0string, ','); 72 | const double ax = stod(imu0string); 73 | getline (imu0data, imu0string, ','); 74 | const double ay = stod(imu0string); 75 | getline (imu0data, imu0string); 76 | const double az = stod(imu0string); 77 | std::shared_ptr measGyr(new tsif::MeasGyr(tsif::Vec3(wx,wy,wz))); 78 | std::shared_ptr measAcc(new tsif::MeasAcc(tsif::Vec3(ax,ay,az))); 79 | filter.AddMeas<2>(tsif::TimePoint(tsif::fromSec(imu0time+tsif::OptionLoader::Instance().Get(optionFile,"timeOffsetAcc"))),measAcc); 80 | filter.AddMeas<3>(tsif::TimePoint(tsif::fromSec(imu0time+tsif::OptionLoader::Instance().Get(optionFile,"timeOffsetGyr"))),measGyr); // TODO timing 81 | } 82 | } 83 | 84 | filter.Update(); 85 | // std::cout << "====================\n" << filter.GetState().Print(); 86 | 87 | // const tsif::Vec3 IrIM = sim.sim_IrIM_; 88 | // const tsif::Quat qIM = sim.sim_qIM_; 89 | // const tsif::Vec3 vel = qIM.toRotationMatrix().transpose()*sim.sim_IvM_; 90 | // const tsif::Vec3 ror = qIM.toRotationMatrix().transpose()*sim.sim_IwIM_; 91 | // const tsif::Vec3 JrJC = sim.sim_qIJ_.toRotationMatrix().transpose()*(tsif::Vec3(IrIM - sim.sim_IrIJ_ + qIM.toRotationMatrix()*(sim.sim_MrMC_))); 92 | // const tsif::Vec3 CrCL = (sim.sim_qMC_.inverse()*qIM.inverse()*sim.sim_qIJ_).toRotationMatrix()*(sim.JrJL_[i]-JrJC); 93 | // const double invDis = 1/CrCL.norm(); 94 | // const tsif::UnitVector CnCL(CrCL); 95 | // 96 | // const double d = 1e-8; 97 | // const tsif::Vec3 IrIM_pert = IrIM + d*qIM.toRotationMatrix()*vel; 98 | // const tsif::Quat qIM_pert = qIM * tsif::Exp(ror*d); 99 | // const tsif::Vec3 JrJC_pert = sim.sim_qIJ_.toRotationMatrix().transpose()*(tsif::Vec3(IrIM_pert - sim.sim_IrIJ_ + qIM_pert.toRotationMatrix()*(sim.sim_MrMC_))); 100 | // const tsif::Vec3 CrCL_pert = (sim.sim_qMC_.inverse()*qIM_pert.inverse()*sim.sim_qIJ_).toRotationMatrix()*(sim.JrJL_[i]-JrJC_pert); 101 | // const double invDis_pert = 1/CrCL_pert.norm(); 102 | // const tsif::UnitVector CnCL_pert(CrCL_pert); 103 | // 104 | // tsif::Vec<2> dif; 105 | // CnCL_pert.Boxminus(CnCL,dif); 106 | // std::cout << dif.transpose()/d << std::endl; 107 | // std::cout << (CnCL.GetN().transpose()*(ror + invDis * CnCL.GetVec().cross(vel))).transpose() << std::endl; 108 | // std::cout << (invDis_pert-invDis)/d << std::endl; 109 | // std::cout << CnCL.GetVec().dot(vel)*invDis*invDis << std::endl; 110 | } 111 | std::cout << filter.PrintConnectivity(); 112 | filter.JacTestAll(1e-6,1e-8); 113 | std::cout << "=================== State ===================" << std::endl; 114 | std::cout << filter.GetState().Print(); 115 | std::cout << "=================== GT ===================" << std::endl; 116 | std::cout << sim.meas_JrJC_.transpose() << std::endl; 117 | std::cout << sim.meas_qJC_.coeffs().transpose() << std::endl; 118 | std::cout << (sim.sim_qIM_.toRotationMatrix().transpose()*sim.sim_IvM_).transpose() << std::endl; 119 | std::cout << (sim.sim_qIM_.toRotationMatrix().transpose()*sim.sim_IwIM_).transpose() << std::endl; 120 | 121 | return 0; 122 | } 123 | --------------------------------------------------------------------------------