├── params ├── rosbag.yaml ├── cal.yaml ├── salsa.yaml └── sim_params.yaml ├── include ├── factors │ ├── carrier_phase.h │ ├── xform.h │ ├── switch_dynamics.h │ ├── shield.h │ ├── mocap.h │ ├── zero_vel.h │ ├── clock_dynamics.h │ ├── feat.h │ ├── anchor.h │ ├── pseudorange.h │ └── imu.h └── salsa │ ├── num_diff.h │ ├── salsa_ros.h │ ├── state.h │ ├── meas.h │ ├── feat.h │ ├── sim_common.h │ ├── salsa_rosbag.h │ ├── misc.h │ ├── logger.h │ └── test_common.h ├── src ├── test │ ├── test_raw_gnss_factor.cpp │ ├── test_mocap_factor.cpp │ ├── test_xform_local_param.cpp │ ├── test_point_positioning.cpp │ ├── test_klt.cpp │ ├── test_misc.cpp │ ├── test_logger.cpp │ ├── test_salsa_mocap_indexing.cpp │ ├── test_salsa_gps_indexing.cpp │ └── test_imu_factor.cpp ├── salsa_node.cpp ├── examples │ ├── gnss_sim.cpp │ ├── mocap_sim.cpp │ ├── feat_sim.cpp │ ├── mixed_sim.cpp │ └── compare_sim.cpp ├── state.cpp ├── factors │ ├── mocap.cpp │ ├── zero_vel.cpp │ ├── switch_dynamics.cpp │ ├── xform.cpp │ ├── clock_dynamics.cpp │ ├── anchor.cpp │ ├── feat.cpp │ ├── pseudorange.cpp │ └── imu.cpp ├── salsa_mocap.cpp ├── meas.cpp ├── num_diff.cpp ├── rosbag_decompressor.cpp ├── feat.cpp ├── salsa_ros.cpp ├── salsa_gnss.cpp ├── salsa_vision.cpp └── graph.cpp ├── sample └── eph.dat ├── assets └── salsa.jpg ├── doc ├── new_node.png ├── new_node2.png ├── new_keyframe1.png ├── new_keyframe2.png ├── feat_frames.svg.png ├── feat_management.svg.png ├── IMU_Preint_factor_graph.svg.png └── vision_factor.lyx ├── python ├── rosbagResult.py ├── GNSSSimulation.py ├── FeatSimulation.py ├── MocapSimulation.py ├── MixedSimulation.py ├── compareSimulation.py ├── framerate.py ├── MocapHardware.py ├── MixedHardware.py ├── GNSSHardware.py ├── ImuFactor.MultiWindow.py ├── MocapFeatHardware.py ├── GNSSHardwareCamImu.py ├── Pseudorange.TrajectoryClockDynamics.py ├── ImuFactor.CheckPropagation.py ├── __init__.py ├── ImuFactor.CheckDynamics.py ├── plotWindow.py ├── rosbag_fixer.py └── typedefs.py ├── README.md ├── .gitignore ├── .gitmodules ├── scripts ├── tmp.params.yaml └── run_all_tests.sh ├── .gitlab-ci.yml ├── .travis.yml └── CMakeLists.txt /params/rosbag.yaml: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /include/factors/carrier_phase.h: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/test/test_raw_gnss_factor.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/salsa_node.cpp: -------------------------------------------------------------------------------- 1 | int main() 2 | { 3 | 4 | } -------------------------------------------------------------------------------- /sample/eph.dat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/sample/eph.dat -------------------------------------------------------------------------------- /assets/salsa.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/assets/salsa.jpg -------------------------------------------------------------------------------- /doc/new_node.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/doc/new_node.png -------------------------------------------------------------------------------- /doc/new_node2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/doc/new_node2.png -------------------------------------------------------------------------------- /doc/new_keyframe1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/doc/new_keyframe1.png -------------------------------------------------------------------------------- /doc/new_keyframe2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/doc/new_keyframe2.png -------------------------------------------------------------------------------- /python/rosbagResult.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | plotResults("/tmp/Salsa/") 3 | -------------------------------------------------------------------------------- /doc/feat_frames.svg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/doc/feat_frames.svg.png -------------------------------------------------------------------------------- /doc/feat_management.svg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/doc/feat_management.svg.png -------------------------------------------------------------------------------- /doc/IMU_Preint_factor_graph.svg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yxw027/salsa/HEAD/doc/IMU_Preint_factor_graph.svg.png -------------------------------------------------------------------------------- /src/test/test_mocap_factor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "factors/mocap.h" 4 | #include "factors/xform.h" 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Salsa 2 | [![Build Status](https://travis-ci.com/superjax/salsa.svg?branch=master)](https://travis-ci.com/superjax/salsa) 3 | 4 | 5 | GNSS-Visual-Inertial Moving Horizon Estimation 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.vscode 2 | *.swp 3 | *.user 4 | build/* 5 | *.pyc 6 | python/.idea/* 7 | .idea/* 8 | *.lyx# 9 | *.emergency 10 | *.lyx~ 11 | node_modules/ 12 | vid/* 13 | *.pdf 14 | *.8-pre1 15 | *.autosave 16 | results/* 17 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lib/multirotor_sim"] 2 | path = lib/multirotor_sim 3 | url = git@github.com:superjax/multirotor_sim 4 | [submodule "lib/geometry"] 5 | path = lib/geometry 6 | url = git@github.com:superjax/geometry 7 | [submodule "lib/gnss_utils"] 8 | path = lib/gnss_utils 9 | url = git@github.com:superjax/gnss_utils 10 | -------------------------------------------------------------------------------- /python/GNSSSimulation.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | 4 | subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja"), cwd="../build") 5 | subprocess.call(("ninja", "gnssSim"), cwd="../build") 6 | subprocess.call(("./gnssSim"), cwd="../build") 7 | 8 | plotResults("/tmp/Salsa/gnssSimulation/") -------------------------------------------------------------------------------- /python/FeatSimulation.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | 4 | subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja"), cwd="../build") 5 | subprocess.call(("ninja", "featSim"), cwd="../build") 6 | subprocess.call(("./featSim"), cwd="../build") 7 | 8 | plotResults("/tmp/Salsa/featSimulation/") 9 | -------------------------------------------------------------------------------- /python/MocapSimulation.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | 4 | # subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja"), cwd="../build") 5 | # subprocess.call(("ninja", "mocapSim"), cwd="../build") 6 | # subprocess.call(("./mocapSim"), cwd="../build") 7 | 8 | plotResults("/tmp/Salsa/mocapSimulation/", saveFig=True) -------------------------------------------------------------------------------- /python/MixedSimulation.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | import os 4 | 5 | directory = "/tmp/Salsa/mixedSimulation/" 6 | 7 | subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja"), cwd="../build") 8 | subprocess.call(("ninja", "mixedSim"), cwd="../build") 9 | subprocess.call(("./mixedSim"), cwd="../build") 10 | 11 | plotResults(directory, False) -------------------------------------------------------------------------------- /scripts/tmp.params.yaml: -------------------------------------------------------------------------------- 1 | clock_walk_stdev: 1e-07 2 | carrier_phase_stdev: 0.01 3 | start_time_week: 2026 4 | pseudorange_rate_stdev: 0.1 5 | ephemeris_filename: ../sample/eph.dat 6 | start_time_tow_sec: 165029 7 | ref_LLA: 8 | - 0.7024440952201444 9 | - -1.948621293729105 10 | - 1387.998309 11 | clock_init_stdev: 0.0001 12 | pseudorange_stdev: 3 13 | gnss_update_rate: 5 14 | use_raw_gnss_truth: true -------------------------------------------------------------------------------- /python/compareSimulation.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | import os 4 | 5 | directory = "/tmp/Salsa/compareSimulation/" 6 | 7 | # subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja"), cwd="../build") 8 | # subprocess.call(("ninja", "compareSim"), cwd="../build") 9 | # subprocess.call(("./compareSim"), cwd="../build") 10 | 11 | plotResults(directory, False) 12 | -------------------------------------------------------------------------------- /python/framerate.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | 4 | new_framerate = 13 5 | input = cv2.VideoCapture("../results/GNSSHardware/GVK/tracked.avi") 6 | 7 | fourcc = cv2.VideoWriter_fourcc(*'XVID') 8 | output = cv2.VideoWriter("../results/GNSSHardware/GVK/tracked" + str(new_framerate) + ".avi", fourcc, new_framerate, (752, 480)) 9 | 10 | while (input.isOpened()): 11 | ret, frame = input.read() 12 | if not ret: 13 | break 14 | output.write(frame) 15 | cv2.imshow("frame", frame) 16 | cv2.waitKey(1) 17 | 18 | 19 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | before_script: 2 | - apt update --qq 3 | - apt install cmake build_essential git libceres-dev 4 | - git clone https://github.com/google/googletest 5 | - cd googletest 6 | - mkdir build 7 | - cd build 8 | - cmake .. -DCMAKE_BUILD_TYPE=Release 9 | - make 10 | - make install 11 | - cd ../.. 12 | - git submodule update --init --recursive 13 | 14 | build: 15 | script: 16 | - mkdir build 17 | - cd build 18 | - cmake .. -DCMAKE_BUILD_TYPE=Release 19 | - make 20 | 21 | run_tests: 22 | script: 23 | - cd build 24 | - ./test_salsa 25 | 26 | -------------------------------------------------------------------------------- /include/salsa/num_diff.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | Eigen::MatrixXd calc_jac(std::function fun, const Eigen::MatrixXd &x, 6 | std::functionx_boxminus=nullptr, 7 | std::functionx_boxplus=nullptr, 8 | std::function y_boxminus=nullptr, double step_size=1e-8); 9 | 10 | -------------------------------------------------------------------------------- /scripts/run_all_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | EXIT_CODE=0 3 | 4 | function print_result() { 5 | if [ $1 -eq 0 ]; then 6 | echo_green "[Passed]" 7 | else 8 | echo_red "[Failed]" 9 | EXIT_CODE=1 10 | fi 11 | echo "" 12 | } 13 | 14 | FAILED=() 15 | for file in ../build/test* 16 | do 17 | if [ -x $file ]; then 18 | if [[ "$file" == *test_results* ]]; then 19 | : 20 | else 21 | echo_blue "./$file" 22 | ./$file 23 | if [ $? -ne 0 ]; then 24 | FAILED=("${FAILED[@]}" $file) 25 | fi 26 | fi 27 | fi 28 | done 29 | 30 | for i in "${FAILED[@]}"; do echo_red "FAILED $i"; done 31 | -------------------------------------------------------------------------------- /include/factors/xform.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "geometry/xform.h" 5 | 6 | struct XformPlus { 7 | template 8 | bool operator()(const T* x, const T* delta, T* x_plus_delta) const; 9 | }; 10 | typedef ceres::AutoDiffLocalParameterization XformParamAD; 11 | 12 | class XformParam : public ceres::LocalParameterization 13 | { 14 | public: 15 | bool Plus(const double* _x, const double* delta, double* x_plus_delta) const override; 16 | bool ComputeJacobian(const double* _x, double* jacobian) const override; 17 | int GlobalSize() const override; 18 | int LocalSize() const override; 19 | }; 20 | -------------------------------------------------------------------------------- /params/cal.yaml: -------------------------------------------------------------------------------- 1 | tm: -150527.278188551 2 | tc: -0.002305038072146955 3 | accel_bias: [-0.04717741613721455, -0.07034896670337663, -0.03231176628010501] 4 | gyro_bias: [-0.004936014393538678, 0.006832230548126756, 0.003299054277444827] 5 | p_u2m: [0.006228303245238449, 0.01936311380701126, -0.09925855920867407] 6 | q_u2m: [0.5905902763028437, -0.3475665976434256, -0.3213965937109184, 0.6535325663577318] 7 | p_u2c: [0.01666044727690726, 0.06831908058722323, -0.3523408903053932] 8 | q_u2c: [0.7060646666507441, 0.004429725140391003, 0.001812721421084533, 0.7081311870576004] 9 | q_u2b: [0.8831637786356926, -0.0001919734052057312, -0.4690647012033105, -9.662911492654515e-05] 10 | cd: 0.2167385760502608 -------------------------------------------------------------------------------- /include/factors/switch_dynamics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "factors/shield.h" 7 | 8 | namespace salsa 9 | { 10 | 11 | class SwitchFunctor 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | SwitchFunctor(const double Xi); 16 | 17 | template 18 | bool operator()(const T* _si, const T* _sj, T* _res) const; 19 | 20 | double Xi_; 21 | }; 22 | typedef ceres::AutoDiffCostFunction, 1, 1, 1> SwitchBiasFactorAD; 23 | 24 | 25 | class SwitchFactor : public ceres::SizedCostFunction<1,1,1> 26 | { 27 | public: 28 | SwitchFactor(double Xi); 29 | 30 | bool Evaluate(const double * const *parameters, double *residuals, double **jacobians) const override; 31 | 32 | double Xi_; 33 | }; 34 | } 35 | -------------------------------------------------------------------------------- /include/factors/shield.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // THIS IS HERE BECAUSE CERES DOESN'T USE SHARED POINTERS 4 | // What this does is it protects the functor from 5 | // ceres wanting to delete all the residual blocks 6 | // This gives ceres something to delete, but protects the actual 7 | // functor from being deleted 8 | 9 | namespace salsa 10 | { 11 | 12 | template 13 | struct FunctorShield 14 | { 15 | FunctorShield(const Functor* functor) : 16 | ptr(functor) 17 | {} 18 | 19 | template 20 | bool operator()(T... args) const 21 | { 22 | return ptr->operator ()(args...); 23 | } 24 | template 25 | bool Evaluate(T... args) const 26 | { 27 | return ptr->Evaluate(args...); 28 | } 29 | const Functor* ptr; 30 | }; 31 | 32 | } 33 | -------------------------------------------------------------------------------- /src/examples/gnss_sim.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa.h" 2 | #include "salsa/test_common.h" 3 | #include "salsa/sim_common.h" 4 | 5 | #include "multirotor_sim/simulator.h" 6 | 7 | using namespace salsa; 8 | using namespace std; 9 | using namespace multirotor_sim; 10 | 11 | int main() 12 | { 13 | std::string prefix = "/tmp/Salsa/gnssSimulation/"; 14 | std::experimental::filesystem::remove_all(prefix); 15 | 16 | Simulator sim(true); 17 | sim.load(imu_raw_gnss()); 18 | 19 | Salsa* salsa = initSalsa(prefix + "GNSS/", "$\\hat{x}$", sim); 20 | salsa->update_on_gnss_ = true; 21 | salsa->disable_gnss_ = false; 22 | salsa->disable_solver_ = false; 23 | 24 | Logger true_state_log(prefix + "Truth.log"); 25 | 26 | while (sim.run()) 27 | { 28 | logTruth(true_state_log, sim, *salsa); 29 | setInit(salsa, sim); 30 | } 31 | delete salsa; 32 | } 33 | -------------------------------------------------------------------------------- /src/examples/mocap_sim.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa.h" 2 | #include "salsa/test_common.h" 3 | #include "salsa/sim_common.h" 4 | 5 | #include "multirotor_sim/simulator.h" 6 | 7 | using namespace salsa; 8 | using namespace std; 9 | using namespace multirotor_sim; 10 | 11 | 12 | int main() 13 | { 14 | std::string prefix = "/tmp/Salsa/mocapSimulation/"; 15 | std::experimental::filesystem::remove_all(prefix); 16 | 17 | Simulator sim(true); 18 | sim.load(imu_mocap()); 19 | 20 | Salsa* salsa = initSalsa(prefix + "Mocap/", "$\\hat{x}$", sim); 21 | salsa->update_on_mocap_ = true; 22 | salsa->disable_mocap_ = false; 23 | salsa->disable_solver_ = false; 24 | 25 | Logger true_state_log(prefix + "Truth.log"); 26 | 27 | while (sim.run()) 28 | { 29 | logTruth(true_state_log, sim, *salsa); 30 | setInit(salsa, sim); 31 | } 32 | delete salsa; 33 | } 34 | -------------------------------------------------------------------------------- /src/state.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/state.h" 2 | #include "salsa/misc.h" 3 | 4 | using namespace Eigen; 5 | using namespace xform; 6 | 7 | namespace salsa 8 | { 9 | 10 | State::State() : 11 | t(buf_[0]), 12 | x(buf_+1), 13 | p(buf_+1), 14 | v(buf_+8), 15 | tau(buf_+11), 16 | bias(buf_+13) 17 | { 18 | for (int i = 0; i < 19; i++) 19 | buf_[i] = NAN; 20 | kf = -1; 21 | type = None; 22 | } 23 | 24 | State& State::operator=(const State& other) 25 | { 26 | t = other.t; 27 | x = other.x; 28 | v = other.v; 29 | tau = other.tau; 30 | bias = other.bias; 31 | kf = other.kf; 32 | node = other.node; 33 | type = other.type; 34 | n_cam = other.n_cam; 35 | } 36 | 37 | Obs::Obs() 38 | { 39 | sat_idx = -1; 40 | } 41 | bool Obs::operator <(const Obs& other) 42 | { 43 | return sat_idx < other.sat_idx; 44 | } 45 | 46 | 47 | 48 | 49 | 50 | 51 | } 52 | -------------------------------------------------------------------------------- /include/factors/mocap.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "geometry/xform.h" 5 | #include "factors/shield.h" 6 | 7 | namespace salsa 8 | { 9 | 10 | struct MocapFunctor 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | MocapFunctor(const double &dt_m, const xform::Xformd &x_u2m, const Vector7d& _xm, 14 | const Vector6d& _xmdot, const Matrix6d& _Xi, 15 | int idx, int node, int kf); 16 | 17 | template 18 | bool operator()(const T* _xu, T* _res) const; 19 | 20 | int idx_; 21 | int node_; 22 | int kf_; 23 | xform::Xformd xm_; 24 | Vector6d xmdot_; 25 | Matrix6d Xi_; 26 | const double& dt_m_; 27 | const xform::Xformd& x_u2m_; 28 | }; 29 | 30 | 31 | 32 | typedef ceres::AutoDiffCostFunction, 6, 7> MocapFactorAD; 33 | typedef ceres::AutoDiffCostFunction UnshiedledMocapFactorAD; 34 | 35 | } 36 | -------------------------------------------------------------------------------- /include/factors/zero_vel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "geometry/xform.h" 6 | #include "factors/shield.h" 7 | 8 | namespace salsa 9 | { 10 | 11 | class ZeroVelFunctor 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | ZeroVelFunctor(const xform::Xformd& x0, const Eigen::Vector3d& v0, const Matrix7d& _Xi); 16 | 17 | template 18 | bool operator() (const T* _x, const T* _v, T* _r) const; 19 | 20 | double yaw0_; 21 | Eigen::Vector3d p0_; 22 | Eigen::Vector3d v0_; 23 | Matrix7d Xi_; 24 | }; 25 | typedef ceres::AutoDiffCostFunction ZeroVelFactorAD; 26 | 27 | class ZeroVelFactor : public ceres::SizedCostFunction<7, 7, 3> 28 | { 29 | ZeroVelFactor(const ZeroVelFunctor* functor); 30 | bool Evaluate(const double* const* parameters, double* residuals, double ** jacobians) const; 31 | const ZeroVelFunctor* ptr; 32 | }; 33 | 34 | } 35 | 36 | -------------------------------------------------------------------------------- /src/examples/feat_sim.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa.h" 2 | #include "salsa/test_common.h" 3 | #include "salsa/sim_common.h" 4 | 5 | #include "multirotor_sim/simulator.h" 6 | 7 | using namespace salsa; 8 | using namespace std; 9 | using namespace multirotor_sim; 10 | 11 | int main() 12 | { 13 | std::string prefix = "/tmp/Salsa/featSimulation/"; 14 | std::experimental::filesystem::remove_all(prefix); 15 | 16 | Simulator sim(true); 17 | sim.load(imu_feat()); 18 | 19 | Salsa* salsa = initSalsa(prefix + "Feat/", "$\\hat{x}$", sim); 20 | salsa->enable_static_start_ = true; 21 | salsa->sim_KLT_ = true; 22 | salsa->update_on_camera_ = true; 23 | salsa->disable_vision_ = false; 24 | salsa->disable_solver_ = false; 25 | 26 | Logger true_state_log(prefix + "Truth.log"); 27 | 28 | setInit(salsa, sim); 29 | 30 | while (sim.run()) 31 | { 32 | logTruth(true_state_log, sim, *salsa); 33 | } 34 | delete salsa; 35 | } 36 | -------------------------------------------------------------------------------- /src/examples/mixed_sim.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "salsa/salsa.h" 4 | #include "salsa/test_common.h" 5 | #include "salsa/sim_common.h" 6 | 7 | #include "multirotor_sim/simulator.h" 8 | 9 | using namespace salsa; 10 | using namespace std; 11 | using namespace multirotor_sim; 12 | 13 | int main() 14 | { 15 | Simulator sim(true); 16 | sim.load(imu_feat_gnss()); 17 | std::string prefix = "/tmp/Salsa/mixedSimulation/"; 18 | std::experimental::filesystem::remove_all(prefix); 19 | 20 | Salsa* salsa = initSalsa(prefix + "GNSSSwitchingVision/", "GV+$\\kappa$", sim); 21 | salsa->disable_vision_ = false; 22 | salsa->disable_gnss_ = false; 23 | salsa->update_on_gnss_ = false; 24 | salsa->enable_switching_factors_ = true; 25 | 26 | 27 | Logger true_state_log(prefix + "Truth.log"); 28 | while (sim.run()) 29 | { 30 | logTruth(true_state_log, sim, *salsa); 31 | 32 | setInit(salsa, sim); 33 | } 34 | delete salsa; 35 | } 36 | -------------------------------------------------------------------------------- /include/salsa/salsa_ros.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "salsa/salsa.h" 10 | 11 | namespace salsa 12 | { 13 | 14 | class SalsaROS 15 | { 16 | public: 17 | SalsaROS(); 18 | 19 | void imuCB(const sensor_msgs::ImuConstPtr& msg); 20 | void odomCB(const nav_msgs::OdometryConstPtr& msg); 21 | void poseCB(const geometry_msgs::PoseStampedPtr& msg); 22 | void tformCB(const geometry_msgs::TransformStampedPtr& msg); 23 | 24 | ros::NodeHandle nh_; 25 | ros::NodeHandle nh_private_; 26 | 27 | ros::Subscriber imu_sub_; 28 | ros::Subscriber odom_sub_; 29 | ros::Subscriber pose_sub_; 30 | ros::Subscriber tform_sub_; 31 | 32 | ros::Publisher odom_pub_; 33 | 34 | Salsa salsa_; 35 | ros::Time start_time_; 36 | 37 | Matrix6d imu_R_; 38 | Matrix6d mocap_R_; 39 | }; 40 | 41 | } 42 | -------------------------------------------------------------------------------- /src/factors/mocap.cpp: -------------------------------------------------------------------------------- 1 | #include "factors/mocap.h" 2 | 3 | using namespace Eigen; 4 | using namespace xform; 5 | 6 | namespace salsa 7 | { 8 | 9 | MocapFunctor::MocapFunctor(const double& dt_m, const Xformd& x_u2m, const Vector7d& _xm, 10 | const Vector6d& _xmdot, const Matrix6d& _Xi, int idx, int node, int kf) : 11 | dt_m_(dt_m), 12 | x_u2m_{x_u2m}, 13 | idx_{idx}, 14 | node_{node}, 15 | kf_{kf} 16 | { 17 | xmdot_ = _xmdot; 18 | xm_ = _xm; 19 | Xi_ = _Xi; 20 | } 21 | 22 | template 23 | bool MocapFunctor::operator()(const T* _xu, T* _res) const 24 | { 25 | typedef Matrix Vec6; 26 | Map res(_res); 27 | Xform xu(_xu); 28 | 29 | res = Xi_ * (xm_ - (xu.otimes(x_u2m_))); 30 | 31 | return true; 32 | } 33 | 34 | template bool MocapFunctor::operator()(const double* _xu, double* _res) const; 35 | typedef ceres::Jet jactype; 36 | template bool MocapFunctor::operator()(const jactype* _xu, jactype* _res) const; 37 | 38 | } 39 | -------------------------------------------------------------------------------- /include/factors/clock_dynamics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "factors/shield.h" 7 | 8 | namespace salsa 9 | { 10 | 11 | class ClockBiasFunctor 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | ClockBiasFunctor(const Eigen::Matrix2d& Xi, int from_idx, int from_node); 16 | bool finished(double dt, int to_idx); 17 | ClockBiasFunctor split(double t); 18 | 19 | template 20 | bool operator()(const T* _taui, const T* _tauj, T* _res) const; 21 | 22 | int from_node_; 23 | int from_idx_; 24 | int to_idx_; 25 | double dt_; 26 | Eigen::Matrix2d Xi_; 27 | }; 28 | typedef ceres::AutoDiffCostFunction, 2, 2, 2> ClockBiasFactorAD; 29 | 30 | class ClockBiasFactor : public ceres::SizedCostFunction<2,2,2> 31 | { 32 | public: 33 | ClockBiasFunctor* ptr; 34 | 35 | ClockBiasFactor(ClockBiasFunctor* _ptr); 36 | bool Evaluate(const double * const *parameters, double *residuals, double **jacobians) const override; 37 | }; 38 | 39 | } 40 | -------------------------------------------------------------------------------- /python/MocapHardware.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | import yaml 4 | import os 5 | 6 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2.bag" 7 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2_adjust.bag" 8 | directory = "/tmp/Salsa/MocapHardware" 9 | prefix = "Mocap/" 10 | 11 | if not os.path.exists(os.path.join(directory, prefix)): 12 | os.makedirs(os.path.join(directory, prefix)) 13 | 14 | params = yaml.load(file("../params/salsa.yaml")) 15 | params['bagfile'] = "/home/superjax/rosbag/mynt_mocap/mocap2.bag" 16 | params['log_prefix'] = os.path.join(directory, prefix) 17 | params['update_on_mocap'] = False 18 | params['disable_mocap'] = True 19 | params['disable_vision'] = False 20 | params['static_start_imu_thresh'] = 15 21 | params['start_time'] = 10 22 | params['duration'] = 40 23 | param_filename = os.path.join(directory, "tmp.yaml") 24 | yaml.dump(params, file(param_filename, 'w')) 25 | 26 | 27 | process = subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja", "-DBUILD_ROS=ON"), cwd="../build") 28 | process = subprocess.call(("ninja", "salsa_rosbag"), cwd="../build") 29 | process = subprocess.call(("./salsa_rosbag", "-f", param_filename), cwd="../build") 30 | 31 | 32 | plotResults(directory, False) -------------------------------------------------------------------------------- /src/test/test_xform_local_param.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "factors/xform.h" 4 | #include "salsa/test_common.h" 5 | 6 | 7 | TEST (XformLocalParam, ADPlusAnalytical) 8 | { 9 | XformPlus ad; 10 | XformParam an; 11 | 12 | for (int i = 0; i < 1000; i++) 13 | { 14 | xform::Xformd x1 = xform::Xformd::Random(); 15 | Vector6d dx = Vector6d::Random(); 16 | 17 | xform::Xformd x2_ad = xform::Xformd::Identity(); 18 | xform::Xformd x2_an = xform::Xformd::Identity(); 19 | 20 | ad(x1.data(), dx.data(), x2_ad.data()); 21 | an.Plus(x1.data(), dx.data(), x2_an.data()); 22 | 23 | EXPECT_MAT_NEAR(x2_ad.arr(), x2_an.arr(), 1e-14); 24 | } 25 | } 26 | 27 | TEST (XformLocalParam, ADvsAnalyticalJac) 28 | { 29 | XformParamAD ad(new XformPlus()); 30 | XformParam an; 31 | 32 | for (int i = 0; i < 1000; i++) 33 | { 34 | xform::Xformd x1 = xform::Xformd::Random(); 35 | Vector6d dx = Vector6d::Random(); 36 | 37 | Eigen::Matrix Jad, Jan; 38 | Jad.setZero(); 39 | Jan.setZero(); 40 | 41 | ad.ComputeJacobian(x1.data(), Jad.data()); 42 | an.ComputeJacobian(x1.data(), Jan.data()); 43 | 44 | EXPECT_MAT_NEAR(Jan, Jad, 1e-14); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /src/factors/zero_vel.cpp: -------------------------------------------------------------------------------- 1 | #include "factors/zero_vel.h" 2 | 3 | using namespace xform; 4 | using namespace Eigen; 5 | 6 | namespace salsa 7 | { 8 | 9 | ZeroVelFunctor::ZeroVelFunctor(const xform::Xformd& x0, const Eigen::Vector3d& v0, 10 | const Matrix7d& _Xi) : 11 | v0_(v0), 12 | Xi_(_Xi) 13 | { 14 | p0_ = x0.t(); 15 | yaw0_ = x0.q().yaw(); 16 | } 17 | 18 | template 19 | bool ZeroVelFunctor::operator()(const T* _x, const T* _v, T* _r) const 20 | { 21 | typedef Matrix Vec3; 22 | typedef Matrix Vec7; 23 | Xform x(_x); 24 | Map v(_v); 25 | Map res(_r); 26 | 27 | res.template head<3>() = p0_ - x.t(); 28 | res.template tail<3>() = v0_ - v; 29 | 30 | T delta_yaw = yaw0_ - x.q().yaw(); 31 | // handle angle wrap in yaw error 32 | if (delta_yaw > (T)M_PI) 33 | delta_yaw -= (T)2.0*M_PI; 34 | else if (delta_yaw < -(T)M_PI) 35 | delta_yaw += (T)2.0*M_PI; 36 | res(3) = delta_yaw; 37 | 38 | res = Xi_ * res; 39 | return true; 40 | } 41 | 42 | template bool ZeroVelFunctor::operator()(const double* _xj, const double* _vj, double* _res) const; 43 | typedef ceres::Jet jactype; 44 | template bool ZeroVelFunctor::operator()(const jactype* _xj, const jactype* _vj, jactype* _res) const; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /python/MixedHardware.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | import yaml 4 | import os 5 | 6 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2.bag" 7 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2_adjust.bag" 8 | directory = "/tmp/Salsa/MixedHardware" 9 | prefix = "Est/" 10 | 11 | if not os.path.exists(os.path.join(directory, prefix)): 12 | os.makedirs(os.path.join(directory, prefix)) 13 | 14 | params = yaml.load(file("../params/salsa.yaml")) 15 | params['bagfile'] = "/home/superjax/rosbag/mynt_outdoors_walk/walk1_uc.bag" 16 | params['log_prefix'] = os.path.join(directory, prefix) 17 | params['update_on_mocap'] = False 18 | params['disable_mocap'] = True 19 | params['disable_vision'] = True 20 | params['update_on_gnss'] = True 21 | params['disable_gnss'] = False 22 | params['enable_static_start'] = True 23 | params['static_start_imu_thresh'] = 11 24 | 25 | params['start_time'] = 300 26 | params['duration'] = 300 27 | param_filename = os.path.join(directory, "tmp.yaml") 28 | yaml.dump(params, file(param_filename, 'w')) 29 | 30 | process = subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja", "-DBUILD_ROS=ON"), cwd="../build") 31 | process = subprocess.call(("ninja", "salsa_rosbag"), cwd="../build") 32 | process = subprocess.call(("./salsa_rosbag", "-f", param_filename), cwd="../build") 33 | 34 | plotResults(directory, False) -------------------------------------------------------------------------------- /src/test/test_point_positioning.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "multirotor_sim/simulator.h" 4 | #include "multirotor_sim/controller.h" 5 | #include "multirotor_sim/utils.h" 6 | #include "gnss_utils/wgs84.h" 7 | #include "salsa/test_common.h" 8 | #include "salsa/salsa.h" 9 | 10 | 11 | using namespace Eigen; 12 | using namespace xform; 13 | using namespace multirotor_sim; 14 | using namespace salsa; 15 | using namespace gnss_utils; 16 | 17 | TEST (Salsa, PointPositioningInit) 18 | { 19 | Simulator sim; 20 | salsa::Salsa salsa; 21 | salsa.init(default_params("/tmp/Salsa/PPInit/")); 22 | 23 | sim.load(imu_raw_gnss(false)); 24 | sim.register_estimator(&salsa); 25 | salsa.p_b2g_ = sim.p_b2g_; 26 | salsa.update_on_gnss_ = true; 27 | salsa.disable_solver_ = true; 28 | 29 | multirotor_sim::State x; 30 | x.p.setZero(); 31 | x.v.setZero(); 32 | sim.dyn_.set_state(x); 33 | sim.clock_bias_ = 1.4e-8; 34 | sim.clock_bias_rate_ = 2.6e-9; 35 | 36 | sim.t_ = 0.3; 37 | sim.update_raw_gnss_meas(); 38 | 39 | EXPECT_MAT_NEAR(sim.X_e2n_.arr(), salsa.x_e2n_.arr(), 1e-1); // PP is used for getting the first x_e2n estimate 40 | EXPECT_MAT_NEAR(salsa.xbuf_[0].x.t(), Vector3d::Zero(), 1e-8); 41 | EXPECT_MAT_NEAR(salsa.xbuf_[0].v, Vector3d::Zero(), 1e-8); 42 | EXPECT_NEAR(sim.clock_bias_, salsa.xbuf_[0].tau[0], 1e-5); 43 | EXPECT_NEAR(sim.clock_bias_rate_, salsa.xbuf_[0].tau[1], 1e-5); 44 | } 45 | -------------------------------------------------------------------------------- /src/salsa_mocap.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa.h" 2 | 3 | using namespace std; 4 | using namespace Eigen; 5 | using namespace xform; 6 | using namespace salsa; 7 | 8 | void Salsa::mocapCallback(const double &t, const Xformd &z, const Matrix6d &R) 9 | { 10 | if(disable_mocap_) 11 | return; 12 | addMeas(meas::Mocap(t, z, R)); 13 | } 14 | 15 | void Salsa::mocapUpdate(const meas::Mocap &m, int idx) 16 | { 17 | SD(2, "Mocap Update, t=%.3f", m.t); 18 | SALSA_ASSERT((xbuf_[idx].type & State::Mocap) == 0, "Cannot double-up with Mocap nodes"); 19 | 20 | int prev_x_idx = (idx + STATE_BUF_SIZE - 1) % STATE_BUF_SIZE; 21 | Vector6d zdot = (Xformd(xbuf_[idx].x) - Xformd(xbuf_[prev_x_idx].x)) 22 | / (xbuf_[idx].t - xbuf_[prev_x_idx].t); 23 | mocap_.emplace_back(dt_m_, x_b2m_, m.z.arr(), zdot, m.R.inverse().llt().matrixL().transpose(), 24 | idx, xbuf_[idx].node, xbuf_[idx].kf); 25 | xbuf_[idx].type |= State::Mocap; 26 | } 27 | 28 | void Salsa::initializeNodeWithMocap(const meas::Mocap& mocap) 29 | { 30 | xbuf_[xbuf_head_].t = mocap.t; 31 | xbuf_[xbuf_head_].x = mocap.z * x_b2m_.inverse(); 32 | } 33 | 34 | 35 | void Salsa::initializeStateMocap(const meas::Mocap &m) 36 | { 37 | SD_S(5, "x_b2m_" << 180.0/M_PI *x_b2m_.q_.euler().transpose() << " z_I2m " << 180.0/M_PI*m.z.q_.euler().transpose()); 38 | initialize(m.t, m.z*x_b2m_.inverse(), v0_, Vector2d::Zero()); 39 | } 40 | -------------------------------------------------------------------------------- /include/factors/feat.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "factors/shield.h" 7 | 8 | #include "salsa/misc.h" 9 | #include "geometry/xform.h" 10 | #include "geometry/cam.h" 11 | 12 | namespace salsa 13 | { 14 | 15 | class FeatFunctor 16 | { 17 | public: 18 | FeatFunctor(double t, const Eigen::Matrix2d& cov, const xform::Xformd& x_b2c, 19 | const Eigen::Vector3d &zetai_, const Eigen::Vector3d &zetaj_, 20 | const Eigen::Vector2d &pixj, int to_idx); 21 | 22 | template 23 | bool operator() (const T* _xi, const T* _xj, const T* _rho, T* _res) const; 24 | 25 | double t_; 26 | int to_idx_; 27 | const Eigen::Vector3d& zetai_; 28 | Eigen::Vector3d zetaj_; 29 | Eigen::Matrix2d Xi_; 30 | Eigen::Vector2d pixj_; 31 | Eigen::Matrix Pz_; 32 | const xform::Xformd& x_b2c_; 33 | 34 | double rho_true_; 35 | }; 36 | 37 | 38 | typedef ceres::AutoDiffCostFunction, 2, 7, 7, 1> FeatFactorAD; 39 | typedef ceres::AutoDiffCostFunction UnshieldedFeatFactorAD; 40 | 41 | class FeatFactor : public ceres::SizedCostFunction<2, 7, 7, 1> 42 | { 43 | public: 44 | FeatFactor(const FeatFunctor* functor); 45 | bool Evaluate(const double * const * parameters, double *residuals, double **jacobians) const; 46 | const FeatFunctor* ptr; 47 | }; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /src/factors/switch_dynamics.cpp: -------------------------------------------------------------------------------- 1 | #include "factors/switch_dynamics.h" 2 | 3 | using namespace Eigen; 4 | 5 | namespace salsa { 6 | 7 | 8 | SwitchFunctor::SwitchFunctor(const double Xi) 9 | { 10 | Xi_ = Xi; 11 | } 12 | 13 | 14 | template 15 | bool SwitchFunctor::operator()(const T* _si, const T* _sj, T* _res) const 16 | { 17 | const T &si(*_si); 18 | const T &sj(*_sj); 19 | T &r(*_res); 20 | 21 | r = Xi_ * (si - sj); 22 | 23 | return true; 24 | } 25 | template bool SwitchFunctor::operator()(const double* _si, const double* _sj, double* _res) const; 26 | typedef ceres::Jet jactype; 27 | template bool SwitchFunctor::operator()(const jactype* _si, const jactype* _sj, jactype* _res) const; 28 | 29 | 30 | SwitchFactor::SwitchFactor(double Xi) 31 | { 32 | Xi_ = Xi; 33 | } 34 | 35 | bool SwitchFactor::Evaluate(const double * const *parameters, double *residuals, double **jacobians) const 36 | { 37 | const double &si(*parameters[0]); 38 | const double &sj(*parameters[1]); 39 | double &r(*residuals); 40 | 41 | r = Xi_ * (si - sj); 42 | 43 | if (jacobians) 44 | { 45 | if (jacobians[0]) 46 | { 47 | double& drdsi(*jacobians[0]); 48 | drdsi = Xi_; 49 | } 50 | if (jacobians[1]) 51 | { 52 | double& drdsj(*jacobians[1]); 53 | drdsj = -Xi_; 54 | } 55 | } 56 | return true; 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /include/salsa/state.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "geometry/xform.h" 6 | 7 | #include "gnss_utils/gtime.h" 8 | 9 | namespace salsa 10 | { 11 | 12 | class State 13 | { 14 | public: 15 | enum { 16 | xSize = 18, 17 | dxSize = 17 18 | }; 19 | enum { 20 | None = 0x00, 21 | Gnss = 0x01, 22 | Mocap = 0x02, 23 | Camera = 0x04, 24 | }; 25 | typedef Eigen::Matrix dxMat; 26 | double buf_[19]; 27 | int kf; 28 | int node; 29 | uint8_t type; 30 | uint8_t n_cam; 31 | xform::Xformd x; 32 | double& t; 33 | Eigen::Map p; 34 | Eigen::Map v; 35 | Eigen::Map tau; 36 | Eigen::Map bias; 37 | 38 | State(); 39 | State& operator=(const State& other); 40 | }; 41 | typedef std::vector> StateVec; 42 | 43 | struct Obs 44 | { 45 | gnss_utils::GTime t; 46 | uint8_t sat_idx; // index in sats_ SatVec 47 | uint8_t sat; 48 | uint8_t rcv; 49 | uint8_t SNR; 50 | uint8_t LLI; // loss-of-lock indicator 51 | uint8_t code; 52 | double qualL; // carrier phase cov 53 | double qualP; // psuedorange cov 54 | Eigen::Vector3d z; // [prange, doppler, cphase] 55 | 56 | Obs(); 57 | 58 | bool operator < (const Obs& other); 59 | }; 60 | typedef std::vector ObsVec; 61 | 62 | } 63 | -------------------------------------------------------------------------------- /include/factors/anchor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "geometry/xform.h" 5 | #include "factors/shield.h" 6 | #include "salsa/state.h" 7 | #include "salsa/misc.h" 8 | 9 | namespace salsa 10 | { 11 | 12 | struct XformAnchor 13 | { 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | XformAnchor(const Matrix6d& Xi); 16 | void set(const xform::Xformd &x); 17 | 18 | template 19 | bool operator()(const T* _x, T* _res) const; 20 | 21 | xform::Xformd x_; 22 | const Matrix6d& Xi_; 23 | }; 24 | typedef ceres::AutoDiffCostFunction, 6, 7> XformAnchorFactorAD; 25 | 26 | struct StateAnchor 27 | { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | StateAnchor(const State::dxMat& Xi); 30 | void set(const State &x); 31 | 32 | template 33 | bool operator()(const T* _x, const T* _v, const T* _tau, const T* _bias, T* _res) const; 34 | 35 | salsa::State x_; 36 | const State::dxMat Xi_; 37 | 38 | }; 39 | typedef ceres::AutoDiffCostFunction, State::dxSize, 7, 3, 2, 6> StateAnchorFactorAD; 40 | 41 | 42 | class ImuBiasAnchor 43 | { 44 | public: 45 | ImuBiasAnchor(const Vector6d& bias_prev, const Matrix6d& xi); 46 | void setBias(const Vector6d& bias_prev); 47 | template 48 | bool operator() (const T* _b, T* _res) const; 49 | 50 | Vector6d bias_prev_; 51 | Matrix6d Xi_; 52 | }; 53 | typedef ceres::AutoDiffCostFunction, 6, 6> ImuBiasAnchorFactorAD; 54 | 55 | } 56 | -------------------------------------------------------------------------------- /python/GNSSHardware.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | import yaml 4 | import os 5 | 6 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2.bag" 7 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2_adjust.bag" 8 | directory = "/tmp/Salsa/GNSSHardware" 9 | prefix = "Est/" 10 | 11 | if not os.path.exists(os.path.join(directory, prefix)): 12 | os.makedirs(os.path.join(directory, prefix)) 13 | 14 | params = yaml.load(file("../params/salsa.yaml")) 15 | # params['bag_name'] = "/home/superjax/rosbag/outdoor_raw_gps/gps_raw1.bag" 16 | # params['bag_name'] = "/home/superjax/rosbag/outdoor_raw_gps/gps_raw2.bag" 17 | # params['bag_name'] = "/home/superjax/rosbag/outdoor_raw_gps/gps_raw3.bag" 18 | params['bag_name'] = "/home/superjax/rosbag/outdoor_raw_gps/gps_raw4_degraded.bag" 19 | params['log_prefix'] = os.path.join(directory, prefix) 20 | params['update_on_mocap'] = False 21 | params['disable_mocap'] = True 22 | params['disable_vision'] = True 23 | params['disable_gnss'] = False 24 | params['static_start_imu_thresh'] = 30 25 | params['start_time'] = 0 26 | params['duration'] = 200 27 | params['x_b2o'] = [0, 0, 0, 1, 0, 0, 0] 28 | params['x_b2m'] = [0, 0, 0, 1, 0, 0, 0] 29 | param_filename = os.path.join(directory, "tmp.yaml") 30 | yaml.dump(params, file(param_filename, 'w')) 31 | 32 | 33 | process = subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja", "-DBUILD_ROS=ON"), cwd="../build") 34 | process = subprocess.call(("ninja", "salsa_rosbag"), cwd="../build") 35 | process = subprocess.call(("./salsa_rosbag", "-f", param_filename), cwd="../build") 36 | 37 | 38 | plotResults(directory, False) -------------------------------------------------------------------------------- /include/salsa/meas.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "salsa/state.h" 9 | #include "salsa/feat.h" 10 | 11 | namespace salsa 12 | { 13 | 14 | namespace meas 15 | { 16 | struct Base 17 | { 18 | Base(); 19 | virtual ~Base() = default; 20 | enum 21 | { 22 | BASE, 23 | GNSS, 24 | IMU, 25 | MOCAP, 26 | IMG, 27 | ZERO_VEL 28 | }; 29 | double t; 30 | int type; 31 | std::string Type() const; 32 | }; 33 | 34 | bool basecmp(const Base *a, const Base *b); 35 | 36 | struct Gnss : public Base 37 | { 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | Gnss(double _t, const ObsVec& _z); 40 | Gnss(double _t, ObsVec&& _z); 41 | ObsVec obs; 42 | }; 43 | 44 | struct Imu : public Base 45 | { 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | Imu(double _t, const Vector6d& _z, const Matrix6d& _R); 48 | Vector6d z; 49 | Matrix6d R; 50 | }; 51 | 52 | struct Mocap : public Base 53 | { 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | Mocap(double _t, const xform::Xformd& _z, const Matrix6d& _R); 56 | xform::Xformd z; 57 | Matrix6d R; 58 | }; 59 | 60 | struct Img : public Base 61 | { 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 | Img(double _t, Features &&_z, const Eigen::Matrix2d& _R, bool _new_keyframe); 64 | Img(double _t, const Features &_z, const Eigen::Matrix2d& _R, bool _new_keyframe); 65 | Features z; 66 | Eigen::Matrix2d R_pix; 67 | bool new_keyframe; 68 | }; 69 | 70 | struct ZeroVel: public Base 71 | { 72 | ZeroVel(double _t); 73 | }; 74 | } 75 | 76 | 77 | } 78 | -------------------------------------------------------------------------------- /python/ImuFactor.MultiWindow.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from plotWindow import plotWindow 4 | 5 | np.set_printoptions(linewidth=150) 6 | plt.rc('text', usetex=True) 7 | plt.rc('font', family='serif') 8 | pw = plotWindow() 9 | 10 | LOG_WIDTH = 1 + 7 + 3 + 7 + 3 + 7 + 3 11 | 12 | data = np.reshape(np.fromfile('/tmp/ImuFactor.MultiWindow.log', dtype=np.float64), (-1, LOG_WIDTH)) 13 | 14 | t = data[:,0] 15 | xhat0 = data[:,1:8] 16 | vhat0 = data[:,8:11] 17 | xhatf = data[:, 11:18] 18 | vhatf = data[:, 18:21] 19 | x = data[:, 21:28] 20 | v = data[:, 28:31] 21 | 22 | xtitles = ['x', 'y', 'z', 'qw', 'qx', 'qy', 'qz'] 23 | vtitles = ['vx', 'vy', 'vz'] 24 | 25 | f = plt.figure(1) 26 | for i in range(3): 27 | plt.subplot(3, 1, i+1) 28 | plt.title(xtitles[i]) 29 | plt.plot(t, xhat0[:,i], label="$\hat{x}_0$") 30 | plt.plot(t, xhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 31 | plt.plot(t, x[:,i], label="$x$") 32 | plt.legend() 33 | pw.addPlot("Position", f) 34 | 35 | f = plt.figure(2) 36 | for i in range(4): 37 | plt.subplot(4, 1, i+1) 38 | plt.title(xtitles[i+3]) 39 | plt.plot(t, xhat0[:,i+3], label="$\hat{x}_0$") 40 | plt.plot(t, xhatf[:,i+3], '--', linewidth=3, label="$\hat{x}_f$") 41 | plt.plot(t, x[:,i+3], label="$x$") 42 | plt.legend() 43 | pw.addPlot("Attitude", f) 44 | 45 | f = plt.figure(3) 46 | for i in range(3): 47 | plt.subplot(3, 1, i+1) 48 | plt.title(vtitles[i]) 49 | plt.plot(t, vhat0[:,i], label="$\hat{x}_0$") 50 | plt.plot(t, vhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 51 | plt.plot(t, v[:,i], label="$x$") 52 | plt.legend() 53 | pw.addPlot("Velocity", f) 54 | 55 | pw.show() 56 | 57 | -------------------------------------------------------------------------------- /python/MocapFeatHardware.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | import yaml 4 | import os 5 | 6 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2.bag" 7 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2_adjust.bag" 8 | directory = "/tmp/Salsa/MocapFeatHardware/" 9 | prefix = "Est/" 10 | 11 | if not os.path.exists(os.path.join(directory, prefix)): 12 | os.makedirs(os.path.join(directory, prefix)) 13 | 14 | params = yaml.load(open("../params/salsa.yaml")) 15 | # params['bag_name'] = "/home/superjax/rosbag/mynt_mocap_ned/uncompressed/mocap3.bag" 16 | # params['bag_name'] = "/home/superjax/rosbag/mynt_mocap_ned/uncompressed/mocap4.bag" 17 | params['bag_name'] = "/home/superjax/rosbag/mynt_mocap_ned/uncompressed/mocap5.bag" 18 | # params['bag_name'] = "/home/superjax/rosbag/mynt_mocap_ned/uncompressed/mocap6.bag" 19 | params['log_prefix'] = os.path.join(directory, prefix) 20 | params['update_on_mocap'] = True 21 | params['disable_mocap'] = True 22 | params['disable_vision'] = False 23 | params['update_on_vision'] = True 24 | params['static_start_imu_thresh'] = 12 25 | params['start_time'] = 0 26 | params["mask_filename"] = "/home/superjax/rosbag/mynt_mocap_ned/mask.png" 27 | # params['duration'] = 66 # mocap3 28 | params['duration'] = 100 29 | params['enable_static_start'] = True 30 | param_filename = os.path.join(directory, "tmp.yaml") 31 | yaml.dump(params, open(param_filename, 'w')) 32 | 33 | 34 | process = subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja", "-DBUILD_ROS=ON"), cwd="../build") 35 | process = subprocess.call(("ninja", "salsa_rosbag"), cwd="../build") 36 | process = subprocess.call(("./salsa_rosbag", "-f", param_filename), cwd="../build") 37 | 38 | 39 | plotResults(directory, False) -------------------------------------------------------------------------------- /include/factors/pseudorange.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "gnss_utils/satellite.h" 7 | #include "gnss_utils/wgs84.h" 8 | 9 | #include "factors/shield.h" 10 | #include "factors/switch_dynamics.h" 11 | 12 | namespace salsa 13 | { 14 | 15 | class PseudorangeFunctor 16 | { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | PseudorangeFunctor(); 20 | void init(const gnss_utils::GTime& _t, const Eigen::Vector2d& _rho, gnss_utils::Satellite &sat, 21 | const Eigen::Vector3d& _rec_pos_ecef, const Eigen::Matrix2d& xi, double sw_xi, 22 | const Eigen::Vector3d& _p_b2g, int node, int idx); 23 | template 24 | bool operator()(const T* _x, const T* _v, const T* _clk, 25 | const T* _x_e2n, const T* _sw, T* _res) const; 26 | 27 | int sat_id_; 28 | bool active_ = false; 29 | int node_; 30 | int idx_; 31 | gnss_utils::GTime t; 32 | Eigen::Vector2d rho; 33 | Eigen::Vector3d sat_pos; 34 | Eigen::Vector3d sat_vel; 35 | Eigen::Vector2d sat_clk; 36 | Eigen::Vector3d p_b2g; 37 | double ion_delay; 38 | double trop_delay; 39 | double sagnac_comp; 40 | Eigen::Vector3d rec_pos; 41 | Eigen::Matrix2d Xi_; 42 | double sw_xi_; 43 | double sw; 44 | }; 45 | 46 | typedef ceres::AutoDiffCostFunction, 3, 7, 3, 2, 7, 1> PseudorangeFactorAD; 47 | 48 | class PseudorangeFactor : public ceres::SizedCostFunction<3,7,3,2,7,1> 49 | { 50 | public: 51 | PseudorangeFactor(const PseudorangeFunctor* functor); 52 | bool Evaluate(const double * const *parameters, double *residuals, double **jacobians) const; 53 | const PseudorangeFunctor* ptr; 54 | }; 55 | 56 | } 57 | -------------------------------------------------------------------------------- /src/factors/xform.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace Eigen; 5 | using namespace xform; 6 | 7 | 8 | bool XformParam::Plus(const double* _x, const double* delta, double* x_plus_delta) const 9 | { 10 | Xformd x(_x); 11 | Map d(delta); 12 | Xformd xp(x_plus_delta); 13 | xp = x + d; 14 | SALSA_ASSERT( abs(1 - xp.q().arr_.norm()) < 1e-8 , "Quaternion Left Manifold"); 15 | return true; 16 | } 17 | 18 | bool XformParam::ComputeJacobian(const double* _x, double* jacobian) const 19 | { 20 | Map> J(jacobian); 21 | Xformd x(_x); 22 | J.topLeftCorner<3,3>() = x.q().R().transpose(); 23 | J.topRightCorner<3,3>().setZero(); 24 | J.bottomLeftCorner<4,3>().setZero(); 25 | J.bottomRightCorner<4,3>() << -_x[4]/2.0, -_x[5]/2.0, -_x[6]/2.0, 26 | _x[3]/2.0, -_x[6]/2.0, _x[5]/2.0, 27 | _x[6]/2.0, _x[3]/2.0, -_x[4]/2.0, 28 | -_x[5]/2.0, _x[4]/2.0, _x[3]/2.0; 29 | return true; 30 | } 31 | int XformParam::GlobalSize() const 32 | { 33 | return 7; 34 | } 35 | int XformParam::LocalSize() const 36 | { 37 | return 6; 38 | } 39 | 40 | template 41 | bool XformPlus::operator()(const T* x, const T* delta, T* x_plus_delta) const 42 | { 43 | Xform q(x); 44 | Map> d(delta); 45 | Xform xp(x_plus_delta); 46 | xp = q + d; 47 | SALSA_ASSERT( abs((T)1.0 - xp.q().arr_.norm()) < 1e-8 , "Quaternion Left Manifold"); 48 | return true; 49 | } 50 | template bool XformPlus::operator()(const double* x, const double* delta, double* x_plus_delta) const; 51 | typedef ceres::Jet jactype; 52 | template bool XformPlus::operator()(const jactype* x, const jactype* delta, jactype* x_plus_delta) const; 53 | 54 | -------------------------------------------------------------------------------- /src/meas.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/meas.h" 2 | 3 | namespace salsa { 4 | namespace meas { 5 | 6 | Base::Base() 7 | { 8 | type = BASE; 9 | } 10 | 11 | std::string Base::Type() const 12 | { 13 | switch (type) 14 | { 15 | case BASE: 16 | return "Base"; 17 | break; 18 | case GNSS: 19 | return "Gnss"; 20 | break; 21 | case IMU: 22 | return "Imu"; 23 | break; 24 | case MOCAP: 25 | return "Mocap"; 26 | break; 27 | case IMG: 28 | return "Img"; 29 | break; 30 | case ZERO_VEL: 31 | return "ZeroVel"; 32 | break; 33 | } 34 | } 35 | 36 | bool basecmp(const Base* a, const Base* b) 37 | { 38 | return a->t < b->t; 39 | } 40 | 41 | 42 | 43 | Imu::Imu(double _t, const Vector6d &_z, const Matrix6d &_R) 44 | { 45 | t = _t; 46 | z = _z; 47 | R = _R; 48 | type = IMU; 49 | } 50 | 51 | Gnss::Gnss(double _t, const ObsVec& _z) 52 | { 53 | t = _t; 54 | type = GNSS; 55 | obs = _z; 56 | } 57 | 58 | //Gnss::Gnss(double _t, ObsVec&& _z) : 59 | // obs(std::move(_z)) 60 | //{ 61 | // t = _t; 62 | // type = GNSS; 63 | //} 64 | 65 | Mocap::Mocap(double _t, const xform::Xformd &_z, const Matrix6d &_R) : 66 | z(_z), 67 | R(_R) 68 | { 69 | t = _t; 70 | type = MOCAP; 71 | } 72 | 73 | Img::Img(double _t, const Features &_z, const Eigen::Matrix2d &_R, bool _new_keyframe) 74 | { 75 | t = _t; 76 | z = _z; 77 | R_pix = _R; 78 | new_keyframe = _new_keyframe; 79 | type = IMG; 80 | } 81 | //Img::Img(double _t, Features &&_z, const Eigen::Matrix2d &_R, bool _new_keyframe) : 82 | // z(std::move(_z)) 83 | //{ 84 | // t = _t; 85 | // R_pix = _R; 86 | // new_keyframe = _new_keyframe; 87 | // type = IMG; 88 | //} 89 | 90 | ZeroVel::ZeroVel(double _t) 91 | { 92 | t = _t; 93 | type = ZERO_VEL; 94 | } 95 | 96 | } 97 | } 98 | -------------------------------------------------------------------------------- /include/salsa/feat.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include "salsa/state.h" 7 | #include "factors/feat.h" 8 | #include "opencv2/opencv.hpp" 9 | 10 | namespace salsa 11 | { 12 | 13 | class Features 14 | { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | int id; // image label 18 | double t; // time stamp of this image 19 | std::vector> zetas; // unit vectors to features 20 | std::vector depths; // feature distances corresponding to feature measurements 21 | std::vector feat_ids; // feature ids corresonding to pixel measurements 22 | std::vector pix; // pixel measurements 23 | 24 | void reserve(const int& N); 25 | void resize(const int& N); 26 | void rm(const int& idx); 27 | void clear(); 28 | int size() const; 29 | }; 30 | 31 | typedef std::deque> FeatDeque; 32 | 33 | struct Feat 34 | { 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | int kf0; 37 | int idx0; 38 | double rho; 39 | double rho_true; 40 | double t0; 41 | int slide_count; 42 | Eigen::Vector3d z0; 43 | FeatDeque funcs; 44 | Eigen::Vector2d pix0; 45 | bool updated_in_last_image_; 46 | 47 | Feat(double _t0, int _idx, int _kf0, const Eigen::Vector3d& _z0, const Eigen::Vector2d& _pix0, double _rho, double _rho_true=NAN); 48 | 49 | void addMeas(double t, int to_idx, const Eigen::Matrix2d& cov, const xform::Xformd& x_b2c, 50 | const Eigen::Vector3d& zj, const Eigen::Vector2d &pixj); 51 | void moveMeas(double t, int to_idx, const Eigen::Vector3d& zj); 52 | bool slideAnchor(int new_from_idx, const StateVec& xbuf, const xform::Xformd& x_b2c); 53 | Eigen::Vector3d pos(const StateVec& xbuf, const xform::Xformd& x_b2c); 54 | }; 55 | } 56 | -------------------------------------------------------------------------------- /src/num_diff.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "salsa/num_diff.h" 3 | 4 | Eigen::MatrixXd calc_jac(std::function fun, const Eigen::MatrixXd& x, 5 | std::functionx_boxminus, 6 | std::functionx_boxplus, 7 | std::functiony_boxminus, 8 | double step_size) 9 | { 10 | if (x_boxminus == nullptr) 11 | { 12 | x_boxminus = [](const Eigen::MatrixXd& x1, const Eigen::MatrixXd& x2) 13 | { 14 | return x1 - x2; 15 | }; 16 | } 17 | if (y_boxminus == nullptr) 18 | { 19 | y_boxminus = [](const Eigen::MatrixXd& x1, const Eigen::MatrixXd& x2) 20 | { 21 | return x1 - x2; 22 | }; 23 | } 24 | 25 | if (x_boxplus == nullptr) 26 | { 27 | x_boxplus = [](const Eigen::MatrixXd& x1, const Eigen::MatrixXd& dx) 28 | { 29 | return x1 + dx; 30 | }; 31 | } 32 | 33 | Eigen::MatrixXd y = fun(x); 34 | Eigen::MatrixXd dy = y_boxminus(y,y); 35 | int rows = dy.rows(); 36 | 37 | Eigen::MatrixXd dx = x_boxminus(x, x); 38 | int cols = dx.rows(); 39 | 40 | Eigen::MatrixXd I; 41 | I.resize(cols, cols); 42 | I.setZero(cols, cols); 43 | for (int i = 0; i < cols; i++) 44 | I(i,i) = step_size; 45 | 46 | Eigen::MatrixXd JFD; 47 | JFD.setZero(rows, cols); 48 | for (int i =0; i < cols; i++) 49 | { 50 | Eigen::MatrixXd xp = x_boxplus(x, I.col(i)); 51 | Eigen::MatrixXd xm = x_boxplus(x, -1.0*I.col(i)); 52 | 53 | Eigen::MatrixXd yp = fun(xp); 54 | Eigen::MatrixXd ym = fun(xm); 55 | Eigen::MatrixXd dy = y_boxminus(yp,ym); 56 | 57 | JFD.col(i) = dy/(2*step_size); 58 | } 59 | return JFD; 60 | } 61 | 62 | -------------------------------------------------------------------------------- /include/salsa/sim_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "salsa/salsa.h" 4 | #include "salsa/test_common.h" 5 | 6 | #include "multirotor_sim/simulator.h" 7 | using namespace salsa; 8 | using namespace multirotor_sim; 9 | 10 | Salsa* initSalsa(const std::string& prefix, const std::string& label, Simulator& sim) 11 | { 12 | Salsa* salsa = new Salsa; 13 | salsa->init(default_params(prefix, label)); 14 | salsa->x0_ = sim.state().X; 15 | salsa->v0_ = sim.state().v; 16 | salsa->x_e2n_ = sim.X_e2n_; 17 | salsa->x_b2c_ = sim.x_b2c_; 18 | salsa->x_b2o_ = xform::Xformd::Identity(); 19 | 20 | salsa->cam_ = sim.cam_; 21 | salsa->mask_.create(cv::Size(salsa->cam_.image_size_(0), salsa->cam_.image_size_(1)), CV_8UC1); 22 | salsa->mask_ = 255; 23 | sim.register_estimator(salsa); 24 | return salsa; 25 | } 26 | 27 | void setInit(Salsa* salsa, Simulator& sim) 28 | { 29 | salsa->x0_ = sim.state().X; 30 | salsa->v0_ = sim.state().v; 31 | salsa->x_e2n_ = sim.X_e2n_; 32 | } 33 | 34 | inline void logTruth(salsa::Logger& log, multirotor_sim::Simulator& sim, salsa::Salsa& salsa) 35 | { 36 | log.log(sim.t_); 37 | Eigen::Vector3d lla = Eigen::Vector3d::Ones() * NAN; 38 | int32_t multipath = sim.multipath_area_.inside(sim.get_gps_position_ned()); 39 | int32_t denied = sim.gps_denied_area_.inside(sim.get_gps_position_ned()); 40 | log.logVectors(sim.state().X.arr(), sim.state().X.q().euler(), sim.state().v, sim.accel_bias_, 41 | sim.gyro_bias_, Vector2d{sim.clock_bias_, sim.clock_bias_rate_}, 42 | sim.X_e2n_.arr(), sim.x_b2c_.arr(), lla); 43 | log.log(multipath, denied); 44 | for (int i = 0; i < salsa.ns_; i++) 45 | { 46 | if (i < sim.satellites_.size()) 47 | { 48 | log.log((double)(sim.multipath_offset_[i] == 0)); 49 | } 50 | else 51 | { 52 | log.log((double)NAN); 53 | } 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/test/test_klt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "salsa/salsa.h" 5 | #include "salsa/test_common.h" 6 | 7 | using namespace Eigen; 8 | using namespace xform; 9 | 10 | 11 | TEST (DISABLED_KLT, TrackVideo) 12 | { 13 | // std::string video_file=SALSA_DIR"/vid/away.avi"; 14 | std::string video_file=SALSA_DIR"/vid/towards.avi"; 15 | // std::string video_file=SALSA_DIR"/vid/walk_across.avi"; 16 | 17 | cv::VideoCapture cap(video_file); 18 | ASSERT_TRUE(cap.isOpened()); 19 | double dt = 1.0 / cap.get(cv::CAP_PROP_FPS); 20 | 21 | salsa::Salsa salsa; 22 | salsa.init(default_params("/tmp/Salsa/KLT_TrackVideo", "$/hat{x}$")); 23 | salsa.disable_solver_ = true; 24 | 25 | Vector6d z; 26 | z << 0, 0, -9.80665, 0, 0, 0; 27 | Matrix6d R = Matrix6d::Identity(); 28 | double t = 0; 29 | salsa.imuCallback(t, z, R); 30 | while(1) 31 | { 32 | cv::Mat frame; 33 | cap >> frame; 34 | if (frame.empty()) 35 | break; 36 | salsa.imageCallback(t, frame, Matrix2d::Identity()); 37 | for (int i = 0; i < 3; i++) 38 | { 39 | t += dt/3.0; 40 | salsa.imuCallback(t, z, R); 41 | } 42 | cv::imshow("kf", salsa.kf_img_); 43 | cv::waitKey(1); 44 | 45 | } 46 | 47 | cap.release(); 48 | cv::destroyAllWindows(); 49 | } 50 | 51 | TEST (DISABLED_KLT, TrackCamera) 52 | { 53 | cv::VideoCapture cap(0); 54 | ASSERT_TRUE(cap.isOpened()); 55 | 56 | salsa::Salsa salsa; 57 | salsa.load(default_params("KLT.TrackVideo", "$/hat{x}$")); 58 | 59 | while(1) 60 | { 61 | cv::Mat frame; 62 | cap >> frame; 63 | if (frame.empty()) 64 | break; 65 | salsa.imageCallback(0, frame, Matrix2d::Identity()); 66 | // cv::imshow("vid", frame); 67 | char c = cv::waitKey(1); 68 | if (c == 'q') 69 | break; 70 | } 71 | 72 | cap.release(); 73 | cv::destroyAllWindows(); 74 | } 75 | -------------------------------------------------------------------------------- /python/GNSSHardwareCamImu.py: -------------------------------------------------------------------------------- 1 | from plotResults import plotResults 2 | import subprocess 3 | import yaml 4 | import os 5 | 6 | params = yaml.load(file("../params/salsa.yaml")) 7 | 8 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2.bag" 9 | # bagfile = "/home/superjax/rosbag/mynt_mocap/mocap2_adjust.bag" 10 | directory = "../results/GNSSHardware" 11 | prefix = "G/" 12 | 13 | if not os.path.exists(os.path.join(directory, prefix)): 14 | os.makedirs(os.path.join(directory, prefix)) 15 | params["label"] = "G" 16 | 17 | # params['bag_name'] = "/home/superjax/rosbag/gps_carry2_uc/small1.bag" 18 | # params['start_time'] = 80 # small1 19 | # params['duration'] = 80 # small 1 20 | # params['bag_name'] = "/home/superjax/rosbag/gps_carry2_uc/small2.bag" 21 | # params['start_time'] = 10 # small2 22 | # params['duration'] = 300 cuda klt vs cpu klt C++# small2 23 | # params['bag_name'] = "/home/superjax/rosbag/gps_carry2_uc/small3.bag" 24 | # params['start_time'] = 10 # small 3 25 | # params['duration'] = 80 # small 3 26 | # params['bag_name'] = "/home/superjax/rosbag/gps_flight_eve/uc/flight1.bag" 27 | # params['start_time'] = 36 28 | params['start_time'] = 60 # small 3 29 | params['duration'] = 220 # small 3 30 | params['bag_name'] = "/home/superjax/rosbag/gps_flight_eve/uc/flight5.bag" 31 | params['log_prefix'] = os.path.join(directory, prefix) 32 | params['update_on_mocap'] = False 33 | params['disable_mocap'] = True 34 | params['disable_vision'] = True 35 | params['update_on_vision'] = True 36 | params['disable_gnss'] = False 37 | params['update_on_gnss'] = True 38 | params['enable_switching_factors'] = False 39 | params['static_start_imu_thresh'] = 10.5 40 | param_filename = os.path.join(directory, "tmp.yaml") 41 | yaml.dump(params, file(param_filename, 'w')) 42 | 43 | 44 | process = subprocess.call(("cmake", "..", "-DCMAKE_BUILD_TYPE=RelWithDebInfo", "-GNinja", "-DBUILD_ROS=ON"), cwd="../build") 45 | process = subprocess.call(("ninja", "salsa_rosbag"), cwd="../build") 46 | process = subprocess.call(("./salsa_rosbag", "-f", param_filename), cwd="../build") 47 | 48 | 49 | plotResults(directory, False) -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: bionic 2 | language: cpp 3 | 4 | # Handle git submodules yourself 5 | git: 6 | submodules: false 7 | 8 | addons: 9 | apt: 10 | packages: 11 | - build-essential 12 | - cmake 13 | - libgtest-dev 14 | - libeigen3-dev 15 | - libyaml-cpp-dev 16 | - libceres-dev 17 | - libopencv-dev 18 | update: true 19 | 20 | before_install: 21 | - sudo apt update 22 | - sudo apt install -y cmake build-essential libgtest-dev libeigen3-dev libyaml-cpp-dev 23 | - sudo apt install -y libopenblas-dev libboost-thread-dev 24 | - sudo apt install -y libceres-dev 25 | - sudo apt install -y libopencv-dev 26 | - sed -i 's/git@github.com:/https:\/\/github.com\//' .gitmodules 27 | - git submodule update --init --recursive 28 | 29 | install: 30 | - cd /usr/src/gtest 31 | - sudo cmake CMakeLists.txt 32 | - sudo make 33 | - sudo cp *.a /usr/lib 34 | - cd "${TRAVIS_BUILD_DIR}" 35 | - gcc --version 36 | - mkdir lib 37 | - cd lib 38 | - git clone https://github.com/superjax/geometry && cd geometry 39 | - mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release && make && sudo make install && cd ../../ 40 | - git clone https://github.com/superjax/gnss_utils && cd gnss_utils 41 | - mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release && make && sudo make install && cd ../../ 42 | - git clone https://github.com/superjax/nanoflann_eigen && cd nanoflann_eigen 43 | - mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release && make && sudo make install && cd ../../ 44 | - git clone https://github.com/superjax/lin_alg_tools && cd lin_alg_tools 45 | - mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release && make && sudo make install && cd ../../ 46 | - git clone https://github.com/superjax/multirotor_sim && cd multirotor_sim 47 | - mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release && make && sudo make install && cd ../../ 48 | - cd .. 49 | 50 | 51 | script: 52 | - mkdir build 53 | - cd build 54 | - cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_TEST=ON 55 | - make 56 | - ./test_salsa 57 | 58 | -------------------------------------------------------------------------------- /python/Pseudorange.TrajectoryClockDynamics.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from plotWindow import plotWindow 4 | 5 | np.set_printoptions(linewidth=150) 6 | plt.rc('text', usetex=True) 7 | plt.rc('font', family='serif') 8 | pw = plotWindow() 9 | 10 | LOG_WIDTH = 1 + 7 + 3 + 7 + 3 + 7 + 3 + 2 + 2 + 2 11 | 12 | data = np.reshape(np.fromfile('/tmp/Pseudorange.TrajectoryClockDynamics.log', dtype=np.float64), (-1, LOG_WIDTH)) 13 | 14 | t = data[:,0] 15 | xhat0 = data[:,1:8] 16 | vhat0 = data[:,8:11] 17 | xhatf = data[:, 11:18] 18 | vhatf = data[:, 18:21] 19 | x = data[:, 21:28] 20 | v = data[:, 28:31] 21 | tauhat0 = data[:,31:33] 22 | tauhatf = data[:,33:35] 23 | tau = data[:,35:37] 24 | 25 | xtitles = ['x', 'y', 'z', 'qw', 'qx', 'qy', 'qz'] 26 | vtitles = ['vx', 'vy', 'vz'] 27 | tautitles = [r'$\tau$', r'$\dot{\tau}$'] 28 | 29 | 30 | 31 | f = plt.figure(dpi=150) 32 | for i in range(3): 33 | plt.subplot(3, 1, i+1) 34 | plt.title(xtitles[i]) 35 | plt.plot(t, xhat0[:,i], label="$\hat{x}_0$") 36 | plt.plot(t, xhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 37 | plt.plot(t, x[:,i], label="$x$") 38 | plt.legend() 39 | pw.addPlot("Position", f) 40 | 41 | f = plt.figure(dpi=150) 42 | for i in range(4): 43 | plt.subplot(4, 1, i+1) 44 | plt.title(xtitles[i+3]) 45 | plt.plot(t, xhat0[:,i+3], label="$\hat{x}_0$") 46 | plt.plot(t, xhatf[:,i+3], '--', linewidth=3, label="$\hat{x}_f$") 47 | plt.plot(t, x[:,i+3], label="$x$") 48 | plt.legend() 49 | pw.addPlot("Attitude", f) 50 | 51 | f = plt.figure(dpi=150) 52 | for i in range(3): 53 | plt.subplot(3, 1, i+1) 54 | plt.title(vtitles[i]) 55 | plt.plot(t, vhat0[:,i], label="$\hat{x}_0$") 56 | plt.plot(t, vhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 57 | plt.plot(t, v[:,i], label="$x$") 58 | plt.legend() 59 | pw.addPlot("Velocity", f) 60 | 61 | f = plt.figure(dpi=150) 62 | for i in range(2): 63 | plt.subplot(2, 1, i+1) 64 | plt.title(tautitles[i]) 65 | plt.plot(t, tauhat0[:,i], label="$\hat{x}_0$") 66 | plt.plot(t, tauhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 67 | plt.plot(t, tau[:,i], label="$x$") 68 | plt.legend() 69 | pw.addPlot("Clock Bias", f) 70 | 71 | pw.show() -------------------------------------------------------------------------------- /python/ImuFactor.CheckPropagation.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | from plotWindow import plotWindow 5 | 6 | np.set_printoptions(linewidth=150) 7 | plt.rc('text', usetex=True) 8 | plt.rc('font', family='serif') 9 | pw = plotWindow() 10 | 11 | data = np.reshape(np.fromfile('/tmp/ImuFactor.CheckPropagation.log', dtype=np.float64), (-1,27)) 12 | 13 | t = data[:, 0] 14 | xhat = data[:, 1:11] 15 | x = data[:, 11:21] 16 | u = data[:, 21:27] 17 | 18 | error = x - xhat 19 | 20 | xtitles = ['$p_x$', '$p_y$', '$p_z$', '$q_w$', '$q_x$', '$q_y$', '$q_z$'] 21 | vtitles = ['$v_x$', '$v_y$', '$v_z$'] 22 | tautitles = [r'$\tau$', r'$\dot{\tau}$'] 23 | 24 | f = plt.figure() 25 | plt.suptitle('Position') 26 | for i in range(3): 27 | plt.subplot(3, 1, i+1) 28 | plt.plot(t, x[:,i], label='x') 29 | plt.plot(t, xhat[:,i], label=r'\hat{x}') 30 | if i == 0: 31 | plt.legend() 32 | pw.addPlot("Position", f) 33 | 34 | f = plt.figure() 35 | plt.suptitle('Velocity') 36 | for i in range(3): 37 | plt.subplot(3, 1, i+1) 38 | plt.plot(t, x[:,i+7], label='x') 39 | plt.plot(t, xhat[:, i+7], label=r'$\hat{x}$') 40 | if i == 0: 41 | plt.legend() 42 | pw.addPlot("Velocity", f) 43 | 44 | f = plt.figure() 45 | plt.suptitle('Attitude') 46 | for i in range(4): 47 | plt.subplot(4, 1, i+1) 48 | plt.plot(t, x[:,i+3], label='x') 49 | plt.plot(t, xhat[:,i+3], label=r'\hat{x}') 50 | if i == 0: 51 | plt.legend() 52 | pw.addPlot("Attitude", f) 53 | 54 | f = plt.figure() 55 | plt.suptitle('Input') 56 | labels=[r'$a_{x}$', r'$a_{y}$', r'$a_{z}$', r'$\omega_{x}$', r'$\omega_{y}$', r'$\omega_{z}$'] 57 | for j in range(2): 58 | for i in range(3): 59 | plt.subplot(3, 2, i*2+1 + j) 60 | plt.plot(t, u[:,i+j*3], label=labels[i+j*3]) 61 | plt.legend() 62 | pw.addPlot("Input", f) 63 | 64 | f = plt.figure() 65 | plt.suptitle('Error') 66 | labels=[r'$p_x$', r'$p_y$', r'$p_z$', 67 | r'$v_x$', r'$v_y$', r'$v_z$', 68 | r'$q_x$', r'$q_y$', r'$q_z$'] 69 | for j in range(3): 70 | for i in range(3): 71 | plt.subplot(3, 3, i*3+1+j) 72 | plt.plot(t, error[:,i+j*3], label=labels[i+j*3]) 73 | plt.legend() 74 | 75 | pw.addPlot("Error", f) 76 | 77 | pw.show() 78 | -------------------------------------------------------------------------------- /python/__init__.py: -------------------------------------------------------------------------------- 1 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 2 | from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from PyQt5.QtWidgets import QMainWindow, QApplication, QPushButton, QWidget, QAction, QTabWidget,QVBoxLayout 6 | from PyQt5.QtGui import QIcon 7 | from PyQt5.QtCore import pyqtSlot 8 | import sys 9 | 10 | 11 | class plotWindow(): 12 | def __init__(self, parent=None): 13 | self.app = QApplication(sys.argv) 14 | self.MainWindow = QMainWindow() 15 | self.MainWindow.__init__() 16 | self.MainWindow.setWindowTitle("plot window") 17 | self.canvases = [] 18 | self.figure_handles = [] 19 | self.toolbar_handles = [] 20 | self.tab_handles = [] 21 | self.current_window = -1 22 | self.tabs = QTabWidget() 23 | self.MainWindow.setCentralWidget(self.tabs) 24 | self.MainWindow.resize(1280, 900) 25 | self.MainWindow.show() 26 | 27 | def addPlot(self, title, figure): 28 | new_tab = QWidget() 29 | layout = QVBoxLayout() 30 | new_tab.setLayout(layout) 31 | 32 | figure.subplots_adjust(left=0.05, right=0.99, bottom=0.05, top=0.91, wspace=0.2, hspace=0.2) 33 | new_canvas = FigureCanvas(figure) 34 | new_toolbar = NavigationToolbar(new_canvas, new_tab) 35 | 36 | layout.addWidget(new_canvas) 37 | layout.addWidget(new_toolbar) 38 | self.tabs.addTab(new_tab, title) 39 | 40 | self.toolbar_handles.append(new_toolbar) 41 | self.canvases.append(new_canvas) 42 | self.figure_handles.append(figure) 43 | self.tab_handles.append(new_tab) 44 | 45 | def show(self): 46 | self.app.exec_() 47 | 48 | if __name__ == '__main__': 49 | import numpy as np 50 | 51 | 52 | pw = plotWindow() 53 | 54 | x = np.arange(0, 10, 0.001) 55 | 56 | f = plt.figure() 57 | ysin = np.sin(x) 58 | plt.plot(x, ysin, '--') 59 | pw.addPlot("sin", f) 60 | 61 | f = plt.figure() 62 | ycos = np.cos(x) 63 | plt.plot(x, ycos, '--') 64 | pw.addPlot("cos", f) 65 | pw.show() 66 | 67 | # sys.exit(app.exec_()) 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /src/examples/compare_sim.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "salsa/salsa.h" 4 | #include "salsa/test_common.h" 5 | #include "salsa/sim_common.h" 6 | 7 | #include "multirotor_sim/simulator.h" 8 | 9 | using namespace salsa; 10 | using namespace std; 11 | using namespace multirotor_sim; 12 | 13 | int main() 14 | { 15 | Simulator sim(true); 16 | sim.load(imu_feat_gnss()); 17 | std::string prefix = "/tmp/Salsa/compareSimulation/"; 18 | std::experimental::filesystem::remove_all(prefix); 19 | 20 | Salsa* gnss_only = initSalsa(prefix + "GNSSOnly/", "G", sim); 21 | gnss_only->update_on_gnss_ = true; 22 | gnss_only->enable_switching_factors_ = false; 23 | gnss_only->disable_vision_ = true; 24 | 25 | Salsa* switching_salsa = initSalsa(prefix + "Switching/", "G+$\\kappa$", sim); 26 | switching_salsa->disable_vision_ = true; 27 | switching_salsa->update_on_gnss_ = true; 28 | 29 | Salsa* vision_salsa = initSalsa(prefix + "Vision/", "V", sim); 30 | vision_salsa->disable_vision_ = false; 31 | vision_salsa->disable_gnss_ = true; 32 | vision_salsa->update_on_gnss_ = false; 33 | 34 | Salsa* vision_gnss_salsa = initSalsa(prefix + "GNSSVision/", "GV", sim); 35 | vision_gnss_salsa->disable_vision_ = false; 36 | vision_gnss_salsa->disable_gnss_ = false; 37 | vision_gnss_salsa->update_on_gnss_ = false; 38 | vision_gnss_salsa->enable_switching_factors_ = false; 39 | 40 | Salsa* vision_switching_gnss_salsa = initSalsa(prefix + "GNSSSwitchingVision/", "GV+$\\kappa$", sim); 41 | vision_switching_gnss_salsa->disable_vision_ = false; 42 | vision_switching_gnss_salsa->disable_gnss_ = false; 43 | vision_switching_gnss_salsa->update_on_gnss_ = false; 44 | vision_switching_gnss_salsa->enable_switching_factors_ = true; 45 | 46 | 47 | Logger true_state_log(prefix + "Truth.log"); 48 | while (sim.run()) 49 | { 50 | logTruth(true_state_log, sim, *vision_salsa); 51 | 52 | setInit(gnss_only, sim); 53 | setInit(switching_salsa, sim); 54 | setInit(vision_salsa, sim); 55 | setInit(vision_gnss_salsa, sim); 56 | setInit(vision_switching_gnss_salsa, sim); 57 | } 58 | 59 | delete gnss_only; 60 | delete switching_salsa; 61 | delete vision_salsa; 62 | delete vision_gnss_salsa; 63 | delete vision_switching_gnss_salsa; 64 | } 65 | -------------------------------------------------------------------------------- /python/ImuFactor.CheckDynamics.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | from plotWindow import plotWindow 5 | 6 | np.set_printoptions(linewidth=150) 7 | plt.rc('text', usetex=True) 8 | plt.rc('font', family='serif') 9 | pw = plotWindow() 10 | 11 | LOG_WIDTH = 1 + 10 + 10 + 9 + 10 + 6 12 | 13 | data = np.reshape(np.fromfile('/tmp/ImuFactor.CheckDynamics.log', dtype=np.float64), (-1,LOG_WIDTH)) 14 | 15 | t = data[:, 0] 16 | y = data[:, 1:11] 17 | yhat = data[:, 11:21] 18 | dy = data[:, 21:30] 19 | ycheck = data[:, 30:40] 20 | u = data[:, 40:46] 21 | 22 | error = ycheck - yhat 23 | 24 | f = plt.figure() 25 | plt.suptitle('Position') 26 | for i in range(3): 27 | plt.subplot(3, 1, i+1) 28 | plt.plot(t, y[:,i], label='y', linewidth=2) 29 | plt.plot(t, yhat[:,i], label=r'$\hat{y}$') 30 | plt.plot(t, ycheck[:,i], '--', label=r'$y + \tilde{y}$') 31 | if i == 0: 32 | plt.legend() 33 | pw.addPlot("Position", f) 34 | 35 | f = plt.figure() 36 | plt.suptitle('Velocity') 37 | for i in range(3): 38 | plt.subplot(3, 1, i+1) 39 | plt.plot(t, y[:,i+7], label='y', linewidth=2) 40 | plt.plot(t, yhat[:,i+7], label=r'$\hat{y}$') 41 | plt.plot(t, ycheck[:,i+7], '--', label=r'$y + \tilde{y}$') 42 | if i == 0: 43 | plt.legend() 44 | pw.addPlot("Velocity", f) 45 | 46 | f = plt.figure() 47 | plt.suptitle('Attitude') 48 | for i in range(4): 49 | plt.subplot(4, 1, i+1) 50 | plt.plot(t, y[:,i+3], label='y', linewidth=2) 51 | plt.plot(t, yhat[:,i+3], label=r'$\hat{y}$') 52 | plt.plot(t, ycheck[:,i+3], '--', label=r'$y + \tilde{y}$') 53 | if i == 0: 54 | plt.legend() 55 | pw.addPlot("Attitude", f) 56 | 57 | f = plt.figure() 58 | plt.suptitle('Input') 59 | labels=['a_x', 'a_y', 'a_z', r'$\omega_x$', r'$\omega_{y}$', r'$\omega_{z}$'] 60 | for j in range(2): 61 | for i in range(3): 62 | plt.subplot(3, 2, i*2+1 + j) 63 | plt.plot(t, u[:,i+j*3], label=labels[i+j*3]) 64 | plt.legend() 65 | pw.addPlot("Input", f) 66 | 67 | f = plt.figure() 68 | plt.suptitle('Error') 69 | labels=[r'$p_x$', r'$p_y$', r'$p_z$', 70 | r'$v_x$', r'$v_y$', r'$v_z$', 71 | r'$q_x$', r'$q_y$', r'$q_z$'] 72 | for j in range(3): 73 | for i in range(3): 74 | plt.subplot(3, 3, i*3+1+j) 75 | plt.plot(t, error[:,i+j*3], label=labels[i+j*3]) 76 | plt.legend() 77 | 78 | pw.addPlot("Error", f) 79 | 80 | pw.show() 81 | -------------------------------------------------------------------------------- /include/salsa/salsa_rosbag.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "salsa/salsa.h" 19 | #include "multirotor_sim/utils.h" 20 | 21 | namespace salsa 22 | { 23 | 24 | class SalsaRosbag 25 | { 26 | public: 27 | SalsaRosbag(int argc, char **argv); 28 | 29 | void getArgs(int argc, char** argv); 30 | void loadParams(); 31 | void displayHelp(); 32 | void openBag(); 33 | void parseBag(); 34 | void loadEph(); 35 | 36 | // void getMocapOffset(); 37 | 38 | void imuCB(const rosbag::MessageInstance& m); 39 | void poseCB(const rosbag::MessageInstance& m); 40 | void obsCB(const rosbag::MessageInstance& m); 41 | void ephCB(const rosbag::MessageInstance& m); 42 | void odomCB(const rosbag::MessageInstance& m); 43 | void imgCB(const rosbag::MessageInstance& m); 44 | void compressedImgCB(const rosbag::MessageInstance& m); 45 | void imgCB(double tc, const cv_bridge::CvImageConstPtr &img); 46 | 47 | rosbag::Bag bag_; 48 | rosbag::View* view_; 49 | std::string bag_filename_; 50 | std::string param_filename_; 51 | std::string log_prefix_; 52 | double start_; 53 | double duration_; 54 | double end_; 55 | bool got_imu_; 56 | int imu_count_; 57 | bool wait_key_; 58 | 59 | 60 | quat::Quatd q_mocap_to_NED_pos_, q_mocap_to_NED_att_; 61 | ros::Time bag_start_; 62 | ros::Time bag_duration_; 63 | ros::Time bag_end_; 64 | double mocap_offset_; // t_b = t_m + dt 65 | double mocap_rate_; 66 | ros::Time prev_mocap_, prev_mocap_run_; 67 | xform::Xformd x_I2m_prev_; 68 | Eigen::Vector3d v_I2m_prev_; 69 | Salsa salsa_; 70 | 71 | std::string imu_topic_; 72 | std::string mocap_topic_; 73 | std::string image_topic_; 74 | 75 | Matrix6d imu_R_; 76 | Matrix6d mocap_R_; 77 | Eigen::Matrix2d pix_R_; 78 | 79 | Logger truth_log_; 80 | Logger imu_log_; 81 | 82 | std::vector eph_; 83 | 84 | xform::Xformd INS_ref_; 85 | }; 86 | 87 | } 88 | -------------------------------------------------------------------------------- /include/salsa/misc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #ifndef DEBUGPRINT 7 | #define DEBUGPRINT 1 8 | #endif 9 | 10 | #define DEBUGPRINTLEVEL 4 11 | #define DEBUGLOGLEVEL 1 12 | 13 | #if DEBUGPRINT 14 | #define SL std::cout << __LINE__ << std::endl 15 | #define SD(level, f_, ...) do{ \ 16 | if ((level) >= DEBUGPRINTLEVEL) {\ 17 | printf((f_), ##__VA_ARGS__);\ 18 | printf("\n");\ 19 | }\ 20 | if ((level) >= DEBUGLOGLEVEL) {\ 21 | char buf[100];\ 22 | int n = sprintf(buf, (f_), ##__VA_ARGS__);\ 23 | logs_[log::Graph]->file_.write(buf, n);\ 24 | logs_[log::Graph]->file_ << "\n";\ 25 | logs_[log::Graph]->file_.flush();\ 26 | }\ 27 | } while(false) 28 | 29 | #define SD_S(level, args) do{\ 30 | if ((level) >= DEBUGPRINTLEVEL) {\ 31 | std::cout << args << std::endl;\ 32 | }\ 33 | if ((level) >= DEBUGLOGLEVEL) {\ 34 | logs_[log::Graph]->file_ << args << std::endl;\ 35 | logs_[log::Graph]->file_.flush();\ 36 | }\ 37 | } while(false) 38 | #else 39 | #define SL 40 | #define SD(...) 41 | #define SD_S(...) 42 | #endif 43 | 44 | //#ifdef NDEBUG 45 | #define SALSA_ASSERT(condition, ...) \ 46 | do { \ 47 | if (! (condition)) { \ 48 | std::cerr << "Assertion `" #condition "` failed in " << __FILE__ \ 49 | << " line " << __LINE__ << ": "; \ 50 | fprintf(stderr, __VA_ARGS__);\ 51 | std::cerr << std::endl; \ 52 | assert(condition); \ 53 | throw std::runtime_error("ERROR:"); \ 54 | } \ 55 | } while (false) 56 | //#else 57 | //# define SALSA_ASSERT(...) 58 | //#endif 59 | 60 | /*************************************/ 61 | /* Round-Off helpers */ 62 | /*************************************/ 63 | constexpr double EPS = 1e-4; 64 | inline bool lt(double t0, double t1) { return t0 < t1-EPS; } 65 | inline bool le(double t0, double t1) { return t0 <= t1+EPS; } 66 | inline bool gt(double t0, double t1) { return t0 > t1+EPS; } 67 | inline bool ge(double t0, double t1) { return t0 >= t1-EPS; } 68 | inline bool eq(double t0, double t1) { return std::abs(t0 - t1) <= 2.0*EPS; } 69 | inline bool ne(double t0, double t1) { return std::abs(t0 - t1) > 2.0*EPS; } 70 | 71 | constexpr double rad2deg(double x) 72 | { 73 | return 180.0 / M_PI * x; 74 | } 75 | constexpr double deg2rad(double x) 76 | { 77 | return M_PI / 180.0 * x; 78 | } 79 | -------------------------------------------------------------------------------- /src/factors/clock_dynamics.cpp: -------------------------------------------------------------------------------- 1 | #include "factors/clock_dynamics.h" 2 | 3 | using namespace Eigen; 4 | 5 | namespace salsa { 6 | 7 | 8 | ClockBiasFunctor::ClockBiasFunctor(const Matrix2d& Xi, int from_idx, int from_node) 9 | { 10 | Xi_ = Xi; 11 | from_idx_ = from_idx; 12 | from_node_ = from_node; 13 | to_idx_ = -1; 14 | } 15 | 16 | bool ClockBiasFunctor::finished(double dt, int to_idx) 17 | { 18 | dt_ = dt; 19 | to_idx_ = to_idx; 20 | } 21 | 22 | ClockBiasFunctor ClockBiasFunctor::split(double t) 23 | { 24 | return ClockBiasFunctor(Xi_, from_idx_, from_node_); 25 | } 26 | 27 | 28 | template 29 | bool ClockBiasFunctor::operator()(const T* _taui, const T* _tauj, T* _res) const 30 | { 31 | typedef Matrix Vec2; 32 | 33 | Map tau_i(_taui); 34 | Map tau_j(_tauj); 35 | Map res(_res); 36 | 37 | res(0) = (tau_i(0) + tau_i(1) * (T)dt_) - tau_j(0); 38 | res(1) = (tau_i(1)) - tau_j(1); 39 | 40 | res = Xi_ * (T)1e9/dt_ * res; 41 | return true; 42 | } 43 | template bool ClockBiasFunctor::operator()(const double* _taui, const double* _tauj, double* _res) const; 44 | typedef ceres::Jet jactype; 45 | template bool ClockBiasFunctor::operator()(const jactype* _taui, const jactype* _tauj, jactype* _res) const; 46 | 47 | 48 | ClockBiasFactor::ClockBiasFactor(ClockBiasFunctor *_ptr) 49 | { 50 | ptr = _ptr; 51 | } 52 | 53 | bool ClockBiasFactor::Evaluate(const double * const *parameters, double *residuals, double **jacobians) const 54 | { 55 | typedef Matrix Vec2; 56 | 57 | Map tau_i(parameters[0]); 58 | Map tau_j(parameters[1]); 59 | Map res(residuals); 60 | 61 | res(0) = (tau_i(0) + tau_i(1) * ptr->dt_) - tau_j(0); 62 | res(1) = (tau_i(1)) - tau_j(1); 63 | 64 | res = ptr->Xi_ * 1e9/ptr->dt_ * res; 65 | 66 | if (jacobians) 67 | { 68 | if (jacobians[0]) 69 | { 70 | Map> drdtaui(jacobians[0]); 71 | drdtaui << 1, ptr->dt_, 72 | 0, 1; 73 | drdtaui = ptr->Xi_ * 1e9 * drdtaui; 74 | } 75 | if (jacobians[1]) 76 | { 77 | Map> drdtauj(jacobians[1]); 78 | drdtauj << -1, 0, 79 | 0, -1; 80 | drdtauj = ptr->Xi_ * 1e9 * drdtauj; 81 | } 82 | } 83 | return true; 84 | } 85 | } 86 | -------------------------------------------------------------------------------- /python/plotWindow.py: -------------------------------------------------------------------------------- 1 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 2 | from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar 3 | from mpl_toolkits.mplot3d import Axes3D 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from PyQt5.QtWidgets import QMainWindow, QApplication, QPushButton, QWidget, QAction, QTabWidget,QVBoxLayout 7 | from PyQt5.QtGui import QIcon 8 | from PyQt5.QtCore import pyqtSlot 9 | import sys 10 | import signal 11 | 12 | class plotWindow(): 13 | def __init__(self, parent=None): 14 | signal.signal(signal.SIGINT, signal.SIG_DFL) 15 | self.app = QApplication(sys.argv) 16 | self.MainWindow = QMainWindow() 17 | self.MainWindow.__init__() 18 | self.MainWindow.setWindowTitle("plot window") 19 | self.canvases = [] 20 | self.figure_handles = [] 21 | self.toolbar_handles = [] 22 | self.tab_handles = [] 23 | self.current_window = -1 24 | self.tabs = QTabWidget() 25 | self.MainWindow.setCentralWidget(self.tabs) 26 | self.MainWindow.resize(1400, 1200) 27 | self.MainWindow.show() 28 | 29 | def addPlot(self, title, figure, threeD=False): 30 | new_tab = QWidget() 31 | layout = QVBoxLayout() 32 | new_tab.setLayout(layout) 33 | 34 | figure.subplots_adjust(left=0.05, right=0.99, bottom=0.05, top=0.91, wspace=0.2, hspace=0.2) 35 | new_canvas = FigureCanvas(figure) 36 | 37 | new_toolbar = NavigationToolbar(new_canvas, new_tab) 38 | 39 | layout.addWidget(new_canvas) 40 | layout.addWidget(new_toolbar) 41 | self.tabs.addTab(new_tab, title) 42 | 43 | self.toolbar_handles.append(new_toolbar) 44 | self.canvases.append(new_canvas) 45 | self.figure_handles.append(figure) 46 | if threeD: 47 | figure.axes[0].mouse_init() 48 | self.tab_handles.append(new_tab) 49 | 50 | def show(self): 51 | self.app.exec_() 52 | 53 | if __name__ == '__main__': 54 | import numpy as np 55 | 56 | 57 | pw = plotWindow() 58 | 59 | x = np.arange(0, 10, 0.001) 60 | 61 | f = plt.figure() 62 | ysin = np.sin(x) 63 | plt.plot(x, ysin, '--') 64 | pw.addPlot("sin", f) 65 | 66 | f = plt.figure() 67 | ycos = np.cos(x) 68 | plt.plot(x, ycos, '--') 69 | pw.addPlot("cos", f) 70 | pw.show() 71 | 72 | # sys.exit(app.exec_()) 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/test/test_misc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "salsa/salsa.h" 3 | 4 | using namespace salsa; 5 | 6 | TEST (Salsa, IndexGeNoWrap) 7 | { 8 | Salsa salsa; 9 | salsa.STATE_BUF_SIZE = 25; 10 | salsa.xbuf_head_ = 20; 11 | salsa.xbuf_tail_ = 10; 12 | 13 | EXPECT_TRUE(salsa.stateIdxGe(12, 11)); 14 | EXPECT_TRUE(salsa.stateIdxGe(20, 11)); 15 | EXPECT_TRUE(salsa.stateIdxGe(20, 10)); 16 | EXPECT_TRUE(salsa.stateIdxGe(11, 10)); 17 | EXPECT_TRUE(salsa.stateIdxGe(20, 19)); 18 | EXPECT_FALSE(salsa.stateIdxGe(13, 19)); 19 | EXPECT_FALSE(salsa.stateIdxGe(10, 20)); 20 | EXPECT_FALSE(salsa.stateIdxGe(10, 11)); 21 | EXPECT_FALSE(salsa.stateIdxGe(19, 20)); 22 | 23 | salsa.xbuf_head_ = salsa.STATE_BUF_SIZE-1; 24 | salsa.xbuf_tail_ = 0; 25 | 26 | EXPECT_TRUE(salsa.stateIdxGe(salsa.STATE_BUF_SIZE-1, 0)); 27 | EXPECT_TRUE(salsa.stateIdxGe(1, 0)); 28 | EXPECT_TRUE(salsa.stateIdxGe(salsa.xbuf_head_, salsa.xbuf_tail_)); 29 | EXPECT_TRUE(salsa.stateIdxGe(salsa.STATE_BUF_SIZE-1, salsa.STATE_BUF_SIZE-2)); 30 | EXPECT_FALSE(salsa.stateIdxGe(0, salsa.STATE_BUF_SIZE-1)); 31 | EXPECT_FALSE(salsa.stateIdxGe(0, 1)); 32 | EXPECT_FALSE(salsa.stateIdxGe(salsa.xbuf_tail_, salsa.xbuf_head_)); 33 | EXPECT_FALSE(salsa.stateIdxGe(salsa.STATE_BUF_SIZE-2, salsa.STATE_BUF_SIZE-1)); 34 | } 35 | 36 | TEST (Salsa, IndexGeWithWrap) 37 | { 38 | Salsa salsa; 39 | salsa.STATE_BUF_SIZE = 25; 40 | salsa.xbuf_head_ = 10; 41 | salsa.xbuf_tail_ = 20; 42 | 43 | EXPECT_TRUE(salsa.stateIdxGe(8, 22)); 44 | EXPECT_TRUE(salsa.stateIdxGe(0, salsa.STATE_BUF_SIZE-1)); 45 | EXPECT_TRUE(salsa.stateIdxGe(8, 0)); 46 | EXPECT_TRUE(salsa.stateIdxGe(salsa.xbuf_head_, salsa.STATE_BUF_SIZE-1)); 47 | EXPECT_TRUE(salsa.stateIdxGe(salsa.xbuf_head_, salsa.xbuf_tail_)); 48 | EXPECT_TRUE(salsa.stateIdxGe(salsa.xbuf_head_, salsa.xbuf_head_-1)); 49 | EXPECT_TRUE(salsa.stateIdxGe(salsa.xbuf_tail_+1, salsa.xbuf_tail_)); 50 | EXPECT_TRUE(salsa.stateIdxGe(salsa.xbuf_tail_, salsa.xbuf_tail_)); 51 | EXPECT_TRUE(salsa.stateIdxGe(salsa.xbuf_head_, salsa.xbuf_head_)); 52 | EXPECT_TRUE(salsa.stateIdxGe(8,8)); 53 | 54 | EXPECT_FALSE(salsa.stateIdxGe(22, 8)); 55 | EXPECT_FALSE(salsa.stateIdxGe(salsa.STATE_BUF_SIZE-1, 0)); 56 | EXPECT_FALSE(salsa.stateIdxGe(0, 8)); 57 | EXPECT_FALSE(salsa.stateIdxGe(salsa.STATE_BUF_SIZE-1, salsa.xbuf_head_)); 58 | EXPECT_FALSE(salsa.stateIdxGe(salsa.xbuf_tail_, salsa.xbuf_head_)); 59 | EXPECT_FALSE(salsa.stateIdxGe(salsa.xbuf_head_-1, salsa.xbuf_head_)); 60 | EXPECT_FALSE(salsa.stateIdxGe(salsa.xbuf_tail_, salsa.xbuf_tail_+1)); 61 | 62 | salsa.STATE_BUF_SIZE = 1000; 63 | salsa.xbuf_head_ = 7; 64 | salsa.xbuf_tail_ = 986; 65 | 66 | EXPECT_TRUE(salsa.stateIdxGe(5, 994)); 67 | } 68 | -------------------------------------------------------------------------------- /src/factors/anchor.cpp: -------------------------------------------------------------------------------- 1 | #include "factors/anchor.h" 2 | 3 | using namespace Eigen; 4 | using namespace xform; 5 | 6 | namespace salsa 7 | { 8 | 9 | 10 | XformAnchor::XformAnchor(const Matrix6d &Xi) : 11 | Xi_(Xi) 12 | { 13 | x_ = xform::Xformd::Identity(); 14 | } 15 | 16 | void XformAnchor::set(const Xformd& x) 17 | { 18 | x_ = x; 19 | SALSA_ASSERT(std::abs(1.0 - x.q().arr_.norm()) < 1e-8, "Quat Left Manifold"); 20 | } 21 | 22 | template 23 | bool XformAnchor::operator ()(const T* _x, T* _res) const 24 | { 25 | Xform x(_x); 26 | Map> res(_res); 27 | 28 | res = Xi_ * x_.boxminus(x); 29 | return true; 30 | } 31 | template bool XformAnchor::operator ()(const double* _x, double* _res) const; 32 | typedef ceres::Jet jt1; 33 | template bool XformAnchor::operator ()(const jt1* _x, jt1* _res) const; 34 | 35 | 36 | StateAnchor::StateAnchor(const State::dxMat &Xi): 37 | Xi_(Xi) 38 | {} 39 | 40 | void StateAnchor::set(const salsa::State& x) 41 | { 42 | x_ = x; 43 | SALSA_ASSERT(std::abs(1.0 - x.x.q().arr_.norm()) < 1e-8, "Quat Left Manifold"); 44 | } 45 | 46 | template 47 | bool StateAnchor::operator()(const T* _x, const T* _v, const T* _tau, const T* _bias, T* _res) const 48 | { 49 | Map> res(_res); 50 | Xform x(_x); 51 | Map>v (_v); 52 | Map>tau (_tau); 53 | Map>bias (_bias); 54 | 55 | res.template segment<6>(0) = x_.x.boxminus(x); 56 | res.template segment<3>(6) = v - x_.v; 57 | res.template segment<2>(9) = 1e9*(tau - x_.tau); // convert to ns to avoid scaling issues 58 | res.template segment<6>(11) = bias - x_.bias; 59 | 60 | res = Xi_ * res; 61 | return true; 62 | } 63 | 64 | template bool StateAnchor::operator()(const double* _x, const double* v, const double* _tau, const double* _bias, double* _res) const; 65 | typedef ceres::Jet jactype; 66 | template bool StateAnchor::operator()(const jactype* _x, const jactype* _v, const jactype* _tau, const jactype* _bias, jactype* _res) const; 67 | 68 | 69 | ImuBiasAnchor::ImuBiasAnchor(const Vector6d& bias_prev, const Matrix6d& xi) : 70 | bias_prev_(bias_prev), 71 | Xi_(xi) 72 | {} 73 | void ImuBiasAnchor::setBias(const Vector6d& bias_prev) 74 | { 75 | bias_prev_ = bias_prev; 76 | } 77 | 78 | template 79 | bool ImuBiasAnchor::operator()(const T* _b, T* _res) const 80 | { 81 | typedef Matrix Vec6; 82 | Map b(_b); 83 | Map res(_res); 84 | res = Xi_ * (b - bias_prev_); 85 | return true; 86 | } 87 | template bool ImuBiasAnchor::operator ()(const double* _b, double* _res) const; 88 | typedef ceres::Jet jactype2; 89 | template bool ImuBiasAnchor::operator ()(const jactype2* _b, jactype2* _res) const; 90 | 91 | } 92 | -------------------------------------------------------------------------------- /src/rosbag_decompressor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | #include "multirotor_sim/utils.h" 12 | 13 | using namespace std; 14 | 15 | void displayHelp() 16 | { 17 | cout << "ROSbag Decompressor" < ROSbag to decompress" << endl; 20 | cout << "-o Output Filename" << endl; 21 | cout << "-s Start time (s) [0]" << endl; 22 | cout << "-u Duration (s) [Inf]" << endl; 23 | exit(0); 24 | } 25 | 26 | int main(int argc, char**argv) 27 | { 28 | InputParser argparse(argc, argv); 29 | \ 30 | std::string input_filename; 31 | std::string output_filename; 32 | double start = 0; 33 | double duration = INFINITY; 34 | 35 | if (argparse.cmdOptionExists("-h")) 36 | displayHelp(); 37 | if (!argparse.getCmdOption("-f", input_filename)) 38 | displayHelp(); 39 | if (!argparse.getCmdOption("-o", output_filename)) 40 | displayHelp(); 41 | argparse.getCmdOption("-s", start); 42 | argparse.getCmdOption("-u", duration); 43 | 44 | cout << "ROSbag Decompressor" <getBeginTime() + ros::Duration(start); 53 | ros::Time bag_end = view->getEndTime(); 54 | if (std::isfinite(duration)) 55 | bag_end = bag_start + ros::Duration(duration); 56 | delete view; 57 | 58 | view = new rosbag::View(inbag, bag_start, bag_end); 59 | 60 | ProgressBar prog(view->size(), 80); 61 | 62 | int i = 0; 63 | for (rosbag::MessageInstance const m : (*view)) 64 | { 65 | if (m.getTime() < bag_start) 66 | continue; 67 | if (m.getTime() > bag_end) 68 | break; 69 | 70 | prog.print(i++); 71 | 72 | if (m.isType()) 73 | { 74 | sensor_msgs::CompressedImageConstPtr img = m.instantiate(); 75 | cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvCopy(img); 76 | static int skip = 0; 77 | skip = (skip + 1) % 99; 78 | 79 | std::string topic = m.getTopic().substr(0, m.getTopic().find_last_of("\\/")); 80 | if ( skip == 0) 81 | { 82 | cv::imshow(topic, cv_ptr->image); 83 | cv::waitKey(1); 84 | } 85 | sensor_msgs::ImagePtr uncompressed = cv_ptr->toImageMsg(); 86 | outbag.write(topic, m.getTime(), uncompressed); 87 | } 88 | else 89 | { 90 | outbag.write(m.getTopic(), m.getTime(), m); 91 | } 92 | } 93 | prog.finished(); 94 | } 95 | -------------------------------------------------------------------------------- /include/factors/imu.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "factors/shield.h" 7 | 8 | #include "salsa/misc.h" 9 | #include "salsa/meas.h" 10 | #include "geometry/xform.h" 11 | 12 | namespace salsa 13 | { 14 | 15 | class ImuIntegrator 16 | { 17 | public: 18 | static constexpr double G = 9.80665; 19 | ImuIntegrator(); 20 | void reset(const double& _t); 21 | void dynamics(const Vector10d& y, const Vector6d& u, Vector9d& ydot); 22 | void estimateXj(const xform::Xformd& _xi, const Eigen::Vector3d& _vi, 23 | xform::Xformd& _xj, const Eigen::Ref &_vj) const; 24 | void integrateStateOnly(const double& _t, const Vector6d& u); 25 | static void boxplus(const Vector10d& y, const Vector9d& dy, Vector10d& yp); 26 | static void boxminus(const Vector10d& y1, const Vector10d& y2, Vector9d& d); 27 | 28 | enum : int 29 | { 30 | ALPHA = 0, 31 | BETA = 3, 32 | GAMMA = 6, 33 | }; 34 | 35 | enum :int 36 | { 37 | ACC = 0, 38 | OMEGA = 3 39 | }; 40 | 41 | enum : int 42 | { 43 | P = 0, 44 | V = 3, 45 | Q = 6, 46 | }; 47 | 48 | double t; 49 | double delta_t_; 50 | double t0_; 51 | Vector6d b_; 52 | Vector10d y_; 53 | static Eigen::Vector3d gravity_; 54 | }; 55 | 56 | class ImuFunctor : public ImuIntegrator 57 | { 58 | public: 59 | typedef Eigen::Matrix Matrix96; 60 | typedef Eigen::Matrix Matrix15d; 61 | 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 | ImuFunctor(const double& _t0, const Vector6d& b0, const Matrix6d& bias_Xi, int from_idx, int from_node); 64 | 65 | Vector6d avgImuOverInterval(); 66 | void errorStateDynamics(const Vector10d& y, const Vector9d& dy, 67 | const Vector6d& u, const Vector6d& eta, Vector9d& dydot); 68 | void dynamics(const Vector10d& y, const Vector6d& u, Vector9d& ydot, Matrix9d& A, Matrix96&B); 69 | void integrate(const double& _t, const Vector6d& u, const Matrix6d& cov, bool save=true); 70 | void integrate(const meas::Imu& z, bool save=true); 71 | void finished(int to_idx); 72 | ImuFunctor split(double t); 73 | 74 | template 75 | bool operator()(const T* _xi, const T* _xj, const T* _vi, const T* _vj, 76 | const T* _bi, const T* _bj, T* residuals) const; 77 | 78 | int from_idx_; 79 | int to_idx_; 80 | int from_node_; 81 | 82 | Vector6d u_; 83 | Matrix6d cov_; 84 | int n_updates_; 85 | 86 | Matrix9d P_; 87 | Matrix9d Xi_; 88 | Matrix6d bias_Xi_; 89 | 90 | Matrix96 J_; 91 | 92 | std::deque> meas_hist_; 93 | }; 94 | typedef ceres::AutoDiffCostFunction, 15, 7, 7, 3, 3, 6, 6> ImuFactorAD; 95 | typedef ceres::AutoDiffCostFunction UnshieldedImuFactorAD; 96 | 97 | } 98 | -------------------------------------------------------------------------------- /doc/vision_factor.lyx: -------------------------------------------------------------------------------- 1 | #LyX 2.3 created this file. For more info see http://www.lyx.org/ 2 | \lyxformat 544 3 | \begin_document 4 | \begin_header 5 | \save_transient_properties true 6 | \origin unavailable 7 | \textclass article 8 | \use_default_options true 9 | \maintain_unincluded_children false 10 | \language english 11 | \language_package default 12 | \inputencoding auto 13 | \fontencoding global 14 | \font_roman "default" "default" 15 | \font_sans "default" "default" 16 | \font_typewriter "default" "default" 17 | \font_math "auto" "auto" 18 | \font_default_family default 19 | \use_non_tex_fonts false 20 | \font_sc false 21 | \font_osf false 22 | \font_sf_scale 100 100 23 | \font_tt_scale 100 100 24 | \use_microtype false 25 | \use_dash_ligatures true 26 | \graphics default 27 | \default_output_format default 28 | \output_sync 0 29 | \bibtex_command default 30 | \index_command default 31 | \paperfontsize default 32 | \use_hyperref false 33 | \papersize default 34 | \use_geometry false 35 | \use_package amsmath 1 36 | \use_package amssymb 1 37 | \use_package cancel 1 38 | \use_package esint 1 39 | \use_package mathdots 1 40 | \use_package mathtools 1 41 | \use_package mhchem 1 42 | \use_package stackrel 1 43 | \use_package stmaryrd 1 44 | \use_package undertilde 1 45 | \cite_engine basic 46 | \cite_engine_type default 47 | \use_bibtopic false 48 | \use_indices false 49 | \paperorientation portrait 50 | \suppress_date false 51 | \justification true 52 | \use_refstyle 1 53 | \use_minted 0 54 | \index Index 55 | \shortcut idx 56 | \color #008000 57 | \end_index 58 | \secnumdepth 3 59 | \tocdepth 3 60 | \paragraph_separation indent 61 | \paragraph_indentation default 62 | \is_math_indent 0 63 | \math_numbering_side default 64 | \quotes_style english 65 | \dynamic_quotes 0 66 | \papercolumns 1 67 | \papersides 1 68 | \paperpagestyle default 69 | \tracking_changes false 70 | \output_changes false 71 | \html_math_output 0 72 | \html_css_as_file 0 73 | \html_be_strict false 74 | \end_header 75 | 76 | \begin_body 77 | 78 | \begin_layout Title 79 | Vision Factor 80 | \end_layout 81 | 82 | \begin_layout Author 83 | James Jackson 84 | \end_layout 85 | 86 | \begin_layout Section 87 | Derivation 88 | \end_layout 89 | 90 | \begin_layout Standard 91 | \begin_inset Float figure 92 | placement ht 93 | wide false 94 | sideways false 95 | status open 96 | 97 | \begin_layout Plain Layout 98 | \align center 99 | \begin_inset Graphics 100 | filename feat_frames.svg 101 | width 100col% 102 | 103 | \end_inset 104 | 105 | 106 | \begin_inset Caption Standard 107 | 108 | \begin_layout Plain Layout 109 | Vision Factor Geometry 110 | \end_layout 111 | 112 | \end_inset 113 | 114 | 115 | \begin_inset CommandInset label 116 | LatexCommand label 117 | name "fig:vision_factor_geometry" 118 | 119 | \end_inset 120 | 121 | 122 | \end_layout 123 | 124 | \end_inset 125 | 126 | 127 | \end_layout 128 | 129 | \begin_layout Standard 130 | 131 | \end_layout 132 | 133 | \end_body 134 | \end_document 135 | -------------------------------------------------------------------------------- /src/feat.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/feat.h" 2 | 3 | using namespace Eigen; 4 | using namespace xform; 5 | 6 | namespace salsa 7 | { 8 | 9 | void Features::reserve(const int &N) 10 | { 11 | zetas.reserve(N); 12 | depths.reserve(N); 13 | feat_ids.reserve(N); 14 | pix.reserve(N); 15 | } 16 | 17 | int Features::size() const 18 | { 19 | return zetas.size(); 20 | } 21 | 22 | void Features::resize(const int &N) 23 | { 24 | zetas.resize(N); 25 | depths.resize(N); 26 | feat_ids.resize(N); 27 | pix.resize(N); 28 | } 29 | 30 | void Features::clear() 31 | { 32 | zetas.clear(); 33 | depths.clear(); 34 | feat_ids.clear(); 35 | pix.clear(); 36 | } 37 | 38 | void Features::rm(const int &idx) 39 | { 40 | zetas.erase(zetas.begin() + idx); 41 | depths.erase(depths.begin() + idx); 42 | feat_ids.erase(feat_ids.begin() + idx); 43 | pix.erase(pix.begin() + idx); 44 | } 45 | 46 | Feat::Feat(double _t0, int _idx, int _kf0, const Vector3d &_z0, const Vector2d &_pix0, double _rho, double _rho_true) : 47 | t0(_t0), kf0(_kf0), idx0(_idx), z0(_z0), pix0(_pix0), rho(_rho), rho_true(_rho_true), slide_count(0) 48 | {} 49 | 50 | void Feat::addMeas(double t, int to_idx, const Matrix2d &cov, const xform::Xformd& x_b2c, const Vector3d &zj, const Vector2d & pixj) 51 | { 52 | SALSA_ASSERT(to_idx != idx0, "Cannot Point to the same id"); 53 | SALSA_ASSERT(funcs.size() > 0 ? funcs.back().to_idx_ != to_idx : true, "cannot add duplicate feat meas"); 54 | funcs.emplace_back(t, cov, x_b2c, z0, zj, pixj, to_idx); 55 | } 56 | 57 | void Feat::moveMeas(double t, int to_idx, const Vector3d &zj) 58 | { 59 | funcs.back().to_idx_ = to_idx; 60 | funcs.back().zetaj_ = zj; 61 | funcs.back().t_ = t; 62 | } 63 | 64 | bool Feat::slideAnchor(int new_from_idx, const StateVec &xbuf, const Xformd &x_b2c) 65 | { 66 | int new_from_kf = xbuf[new_from_idx].kf; 67 | assert (new_from_kf >= 0); 68 | if (new_from_kf <= kf0) 69 | return true; // Don't need to slide, this one is anchored ahead of the slide 70 | 71 | while (idx0 != new_from_idx) 72 | { 73 | if (funcs.size() <= 1) 74 | return false; // can't slide, no future measurements 75 | 76 | Xformd x_I2i(xbuf[idx0].x); 77 | Xformd x_I2j(xbuf[new_from_idx].x); 78 | 79 | Vector3d p_I2l_i = x_I2i.transforma(x_b2c.transforma(1.0/rho*z0)); 80 | Vector3d zi_j = x_b2c.transformp(x_I2j.transformp(p_I2l_i)); 81 | rho = 1.0/zi_j.norm(); 82 | zi_j.normalize(); 83 | z0 = funcs.front().zetaj_; 84 | rho_true = funcs.front().rho_true_; 85 | idx0 = funcs.front().to_idx_; 86 | pix0 = funcs.front().pixj_; 87 | t0 = funcs.front().t_; 88 | funcs.pop_front(); 89 | slide_count++; 90 | } 91 | kf0 = new_from_kf; 92 | SALSA_ASSERT (funcs.front().to_idx_ != idx0, "Trying to slide to zero"); 93 | return true; 94 | } 95 | 96 | Vector3d Feat::pos(const StateVec& xbuf, const Xformd& x_b2c) 97 | { 98 | Xformd x_I2i(xbuf[idx0].x); 99 | return x_I2i.transforma(x_b2c.transforma(1.0/rho*z0)); 100 | } 101 | } 102 | -------------------------------------------------------------------------------- /python/rosbag_fixer.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import rospy 3 | import sys 4 | from tqdm import tqdm 5 | from inertial_sense.msg import GNSSObsVec 6 | from geometry_msgs.msg import PoseStamped 7 | 8 | def adjustTime(inputfile, outputfile, gps_topics, ignored_topics): 9 | dt = gpsOffset(inputfile, gps_topics) 10 | 11 | 12 | inbag = rosbag.Bag(inputfile) 13 | outbag = rosbag.Bag(outputfile, 'w') 14 | 15 | topics = [x for x in inbag.get_type_and_topic_info()[1].keys() if x not in ignored_topics] 16 | 17 | last = rospy.Time(0) 18 | last_topic = "" 19 | for topic, msg, t in tqdm(inbag.read_messages(topics), total=inbag.get_message_count()): 20 | if hasattr(msg, "header") and msg.header.stamp.to_sec() < 1.0: 21 | msg.header.stamp = t 22 | 23 | new_time = [] 24 | if topic in gps_topics and hasattr(msg , "header"): 25 | new_time = msg.header.stamp 26 | else: 27 | if hasattr(msg , "header"): 28 | msg.header.stamp += dt 29 | new_time = msg.header.stamp 30 | else: 31 | new_time = t + dt 32 | outbag.write(topic, msg, new_time) 33 | if (last - new_time).to_sec() > 1.0: 34 | print((last - new_time).to_sec(), topic, last_topic) 35 | last = new_time 36 | last_topic = topic 37 | outbag.reindex() 38 | outbag.close() 39 | inbag.close() 40 | 41 | 42 | def gpsOffset(inputfile, gps_topics): 43 | bag = rosbag.Bag(inputfile) 44 | 45 | biggest_dt = rospy.Duration(0) 46 | last_topic= "" 47 | for topic, msg, t in tqdm(bag.read_messages(topics=gps_topics), total=bag.get_message_count()): 48 | if not hasattr(msg , "header"): 49 | continue 50 | dt = msg.header.stamp - t 51 | if abs((dt - biggest_dt).to_sec()) > 1.0: 52 | print ((dt - biggest_dt).to_sec(), topic, last_topic) 53 | if dt > biggest_dt: 54 | biggest_dt = dt 55 | last_topic = topic 56 | print biggest_dt.secs 57 | return biggest_dt 58 | 59 | 60 | def gtime2rostime(gtime): 61 | rostime = rospy.Time() 62 | rostime.secs = gtime.time 63 | rostime.nsecs = gtime.sec*1e9 64 | return rostime 65 | 66 | 67 | def convertObsType(inputfile, outputfile): 68 | print("input file: %s" % inputfile) 69 | print('output file: %s' % outputfile) 70 | 71 | outbag = rosbag.Bag(outputfile, 'w') 72 | 73 | try: 74 | bag = rosbag.Bag(inputfile) 75 | except: 76 | print("No bag file found at %s" % inputfile) 77 | quit(1) 78 | 79 | obs = GNSSObsVec() 80 | for topic, msg, t in tqdm(bag.read_messages(), total=bag.get_message_count()): 81 | if topic == "/gps/obs": 82 | if len(obs.obs) > 0 and (obs.obs[0].time.sec != msg.time.sec or obs.obs[0].time.time != msg.time.time): 83 | obs.header = gtime2rostime(obs.obs[0].time) 84 | outbag.write(topic, obs, gtime2rostime(obs.obs[0].time)) 85 | obs.obs = [] 86 | obs.obs.append(msg) 87 | else: 88 | outbag.write(topic, msg, msg.header.stamp) 89 | 90 | 91 | if __name__ == '__main__': 92 | adjustTime("/home/superjax/rosbag/gps_carry/bright_small1.bag", 93 | "/home/superjax/rosbag/gps_carry/bright_small1_adjust.bag", 94 | ["/gps", "/ins", "/gps/geph", "gps/obs", "/uins/imu"], 95 | ["/output_raw", "/rc_raw"]) 96 | 97 | 98 | -------------------------------------------------------------------------------- /src/salsa_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa_ros.h" 2 | 3 | using namespace xform; 4 | using namespace Eigen; 5 | 6 | namespace salsa 7 | { 8 | 9 | SalsaROS::SalsaROS() : 10 | nh_(), nh_private_("~") 11 | { 12 | start_time_ = ros::Time(0.0); 13 | pose_sub_ = nh_.subscribe("pose", 10, &SalsaROS::poseCB, this); 14 | tform_sub_ = nh_.subscribe("tform", 10, &SalsaROS::tformCB, this); 15 | odom_sub_ = nh_.subscribe("odom", 10, &SalsaROS::odomCB, this); 16 | imu_sub_ = nh_.subscribe("imu", 10, &SalsaROS::imuCB, this); 17 | 18 | odom_pub_ = nh_.advertise("state", 20); 19 | 20 | double acc_stdev, gyro_stdev; 21 | nh_private_.param("acc_stdev", acc_stdev, 1.0); 22 | nh_private_.param("gyro_stdev", gyro_stdev, 0.2); 23 | } 24 | 25 | 26 | void SalsaROS::imuCB(const sensor_msgs::ImuConstPtr& msg) 27 | { 28 | if (start_time_.sec == 0) 29 | start_time_ = msg->header.stamp; 30 | 31 | double t = (msg->header.stamp - start_time_).toSec(); 32 | Vector6d z; 33 | z << msg->linear_acceleration.x, 34 | msg->linear_acceleration.y, 35 | msg->linear_acceleration.z, 36 | msg->angular_velocity.x, 37 | msg->angular_velocity.y, 38 | msg->angular_velocity.z; 39 | salsa_.imuCallback(t, z, imu_R_); 40 | 41 | if (odom_pub_.getNumSubscribers() > 0) 42 | { 43 | nav_msgs::Odometry odom; 44 | odom.header.stamp = msg->header.stamp; 45 | odom.pose.pose.position.x = salsa_.current_state_.x.t().x(); 46 | odom.pose.pose.position.y = salsa_.current_state_.x.t().y(); 47 | odom.pose.pose.position.z = salsa_.current_state_.x.t().z(); 48 | odom.pose.pose.orientation.w = salsa_.current_state_.x.q().w(); 49 | odom.pose.pose.orientation.x = salsa_.current_state_.x.q().x(); 50 | odom.pose.pose.orientation.y = salsa_.current_state_.x.q().y(); 51 | odom.pose.pose.orientation.z = salsa_.current_state_.x.q().z(); 52 | odom.twist.twist.linear.x = salsa_.current_state_.v.x(); 53 | odom.twist.twist.linear.y = salsa_.current_state_.v.y(); 54 | odom.twist.twist.linear.z = salsa_.current_state_.v.z(); 55 | odom.twist.twist.angular.x = msg->angular_velocity.x; 56 | odom.twist.twist.angular.y = msg->angular_velocity.y; 57 | odom.twist.twist.angular.z = msg->angular_velocity.z; 58 | odom_pub_.publish(odom); 59 | } 60 | } 61 | 62 | 63 | void SalsaROS::odomCB(const nav_msgs::OdometryConstPtr& msg) 64 | { 65 | double t = (msg->header.stamp - start_time_).toSec(); 66 | Xformd z; 67 | z.arr() << msg->pose.pose.position.x, 68 | msg->pose.pose.position.y, 69 | msg->pose.pose.position.z, 70 | msg->pose.pose.orientation.w, 71 | msg->pose.pose.orientation.x, 72 | msg->pose.pose.orientation.y, 73 | msg->pose.pose.orientation.z; 74 | salsa_.mocapCallback(t, z, mocap_R_); 75 | } 76 | 77 | 78 | void SalsaROS::poseCB(const geometry_msgs::PoseStampedPtr& msg) 79 | { 80 | double t = (msg->header.stamp - start_time_).toSec(); 81 | Xformd z; 82 | z.arr() << msg->pose.position.x, 83 | msg->pose.position.y, 84 | msg->pose.position.z, 85 | msg->pose.orientation.w, 86 | msg->pose.orientation.x, 87 | msg->pose.orientation.y, 88 | msg->pose.orientation.z; 89 | salsa_.mocapCallback(t, z, mocap_R_); 90 | } 91 | 92 | 93 | void SalsaROS::tformCB(const geometry_msgs::TransformStampedPtr& msg) 94 | { 95 | double t = (msg->header.stamp - start_time_).toSec(); 96 | Xformd z; 97 | z.arr() << msg->transform.translation.x, 98 | msg->transform.translation.y, 99 | msg->transform.translation.z, 100 | msg->transform.rotation.w, 101 | msg->transform.rotation.x, 102 | msg->transform.rotation.y, 103 | msg->transform.rotation.z; 104 | salsa_.mocapCallback(t, z, mocap_R_); 105 | } 106 | 107 | } 108 | -------------------------------------------------------------------------------- /src/factors/feat.cpp: -------------------------------------------------------------------------------- 1 | #include "factors/feat.h" 2 | 3 | using namespace Eigen; 4 | using namespace xform; 5 | 6 | #define Tr transpose() 7 | 8 | namespace salsa 9 | { 10 | 11 | FeatFunctor::FeatFunctor(double t, const Matrix2d& cov, const xform::Xformd& x_b2c, const Vector3d &zetai, 12 | const Vector3d &zetaj, const Vector2d &pixj, int to_idx) : 13 | to_idx_(to_idx), 14 | zetai_(zetai), 15 | zetaj_(zetaj), 16 | x_b2c_(x_b2c), 17 | pixj_(pixj), 18 | t_(t) 19 | { 20 | Xi_ = cov.inverse().llt().matrixL().transpose(); 21 | Pz_.block<1,3>(0,0) = zetaj_.cross(e_x).transpose().normalized(); 22 | Pz_.block<1,3>(1,0) = zetaj_.cross(Pz_.block<1,3>(0,0).transpose()).transpose().normalized(); 23 | } 24 | 25 | template 26 | bool FeatFunctor::operator ()(const T* _xi, const T* _xj, const T* _rho, T* _res) const 27 | { 28 | typedef Matrix Vec2; 29 | typedef Matrix Vec3; 30 | typedef Matrix Mat3; 31 | Map res(_res); 32 | Xform xi(_xi); 33 | Xform xj(_xj); 34 | const T& rho(*_rho); 35 | Vec3 zi = 1.0/rho * zetai_; 36 | Vec3 zj_hat = x_b2c_.rotp(xj.rotp(xi.transforma(x_b2c_.transforma(zi)) - xj.transforma(x_b2c_.t_))); 37 | zj_hat.normalize(); 38 | res = Xi_ * Pz_ * (zj_hat - zetaj_); 39 | return true; 40 | } 41 | 42 | template bool FeatFunctor::operator ()(const double* _xi, const double* _xj, 43 | const double* _rho, double* _res) const; 44 | typedef ceres::Jet jactype; 45 | template bool FeatFunctor::operator ()(const jactype* _xi, const jactype* _xj, 46 | const jactype* _rho, jactype* _res) const; 47 | 48 | 49 | FeatFactor::FeatFactor(const FeatFunctor *functor) : 50 | ptr(functor) 51 | {} 52 | 53 | bool FeatFactor::Evaluate(const double * const *parameters, double *residuals, double **jacobians) const 54 | { 55 | Map res(residuals); 56 | Xformd xi(parameters[0]); 57 | Xformd xj(parameters[1]); 58 | const double& rho(*parameters[2]); 59 | Vector3d zi = 1.0/rho * ptr->zetai_; 60 | 61 | Vector3d zi_ci = ptr->x_b2c_.transforma(zi); 62 | Vector3d zj_hat = ptr->x_b2c_.rotp(xj.rotp(xi.transforma(zi_ci) - xj.transforma(ptr->x_b2c_.t()))); 63 | double zj_norm = zj_hat.norm(); 64 | 65 | res = ptr->Xi_ * ptr->Pz_ * (zj_hat/zj_hat.norm() - ptr->zetaj_); 66 | 67 | 68 | if (jacobians) 69 | { 70 | 71 | Matrix3d RI2i = xi.q().R(); 72 | Vector3d pI2i = xi.t(); 73 | Matrix3d RI2j = xj.q().R(); 74 | Vector3d pI2j = xj.t(); 75 | Matrix3d Rb2c = ptr->x_b2c_.q().R(); 76 | 77 | Matrix3d Z = (I_3x3*zj_norm - zj_hat*zj_hat.Tr/zj_norm)/(zj_norm * zj_norm); 78 | Matrix A = ptr->Xi_*ptr->Pz_*Z*Rb2c; 79 | Matrix AB = A*RI2j; 80 | 81 | if (jacobians[0]) 82 | { 83 | Map> dres_dxi(jacobians[0]); 84 | Matrix dqdd; 85 | quat::Quatd& qi(xi.q()); 86 | dqdd << -qi.x()*2.0, qi.w()*2.0, qi.z()*2.0, -qi.y()*2.0, 87 | -qi.y()*2.0, -qi.z()*2.0, qi.w()*2.0, qi.x()*2.0, 88 | -qi.z()*2.0, qi.y()*2.0, -qi.x()*2.0, qi.w()*2.0; 89 | dres_dxi.block<2,3>(0, 0) = AB; 90 | dres_dxi.block<2,4>(0, 3) = -AB*RI2i.Tr*skew(zi_ci)*dqdd; 91 | } 92 | if (jacobians[1]) 93 | { 94 | Map> dres_dxj(jacobians[1]); 95 | Matrix dqdd; 96 | quat::Quatd& qj(xj.q()); 97 | dqdd << -qj.x()*2.0, qj.w()*2.0, qj.z()*2.0, -qj.y()*2.0, 98 | -qj.y()*2.0, -qj.z()*2.0, qj.w()*2.0, qj.x()*2.0, 99 | -qj.z()*2.0, qj.y()*2.0, -qj.x()*2.0, qj.w()*2.0; 100 | dres_dxj.block<2,3>(0, 0) = -AB; 101 | dres_dxj.block<2,4>(0, 3) = A * skew(RI2j * (RI2i.Tr*zi_ci+pI2i-pI2j))*dqdd; 102 | } 103 | if (jacobians[2]) 104 | { 105 | Map> dres_drho(jacobians[2]); 106 | dres_drho = -AB*RI2i.Tr*Rb2c.Tr*1.0/rho*zi; 107 | } 108 | } 109 | return true; 110 | } 111 | } 112 | -------------------------------------------------------------------------------- /python/typedefs.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import yaml 3 | 4 | params = yaml.load(open("../params/salsa.yaml")) 5 | 6 | XType = np.dtype([ 7 | ('p', (np.float64, 3)), 8 | ('q', (np.float64, 4)) 9 | ]) 10 | 11 | CurrentStateType = np.dtype([ 12 | ('t', np.float64), 13 | ('xu', XType), 14 | ('euleru', (np.float64, 3)), 15 | ('vu', (np.float64, 3)), 16 | ('b', (np.float64, 6)), 17 | ('tau', (np.float64, 2)), 18 | ('x', XType), 19 | ('euler', (np.float64, 3)), 20 | ('v', (np.float64, 3)), 21 | ('lla', (np.float64, 3)) 22 | ]) 23 | 24 | SimStateType = np.dtype([ 25 | ('t', np.float64), 26 | ('x', XType), 27 | ('euler', (np.float64, 3)), 28 | ('v', (np.float64, 3)), 29 | ('b', (np.float64, 6)), 30 | ('tau', (np.float64, 2)), 31 | ('x_e2n', XType), 32 | ('x_b2c', XType), 33 | ('lla', (np.float64, 3)), 34 | ('multipath', np.int32), 35 | ('denied', np.int32), 36 | ('mp', (np.float64, params["num_sat"])) 37 | ]) 38 | 39 | StateType = np.dtype([ 40 | ('t', np.float64), 41 | ('xb', XType), 42 | ('vb', (np.float64, 3)), 43 | ('tau', (np.float64, 2)), 44 | ('x', XType), 45 | ('v', (np.float64, 3)), 46 | ('kf', np.int32), 47 | ('node', np.int32) 48 | ]) 49 | 50 | 51 | OptStateType = np.dtype([ 52 | ('node', np.int32), 53 | ('kf', np.int32), 54 | ('t', np.float64), 55 | ('p', (np.float64, 3)), 56 | ('q', (np.float64, 4)), 57 | ('v', (np.float64, 3)), 58 | ('tau', (np.float64, 2)), 59 | ('imu', (np.float64, 6)), 60 | ('lla', (np.float64, 3)) 61 | ]) 62 | 63 | OptType = np.dtype([ 64 | ('head', np.int32), 65 | ('tail', np.int32), 66 | ('x', (OptStateType, params["max_node_window"])), 67 | # ('s', (np.float64, 0)), 68 | ('x_b2c', XType), 69 | ('x_e2n', XType), 70 | ]) 71 | 72 | SwParamsType = np.dtype([ 73 | ('p', (np.dtype([ 74 | ('t', np.float64), 75 | ('s', np.float64)]), params["num_sat"]), params["max_node_window"]) 76 | ]) 77 | 78 | 79 | FtType = np.dtype([ 80 | ('id', np.int32), 81 | ('p', (np.float64, 3)), 82 | ('rho', np.float64), 83 | ('rho_true', np.float64), 84 | ('slide_count', np.int32) 85 | ]) 86 | 87 | FeatType = np.dtype([ 88 | ('t', np.float64), 89 | ('size', np.uint64), 90 | ('ft', (FtType, params["num_feat"])) 91 | ]) 92 | 93 | ResType2 = np.dtype([ 94 | ('to_node', np.int32), 95 | ('t', np.float64), 96 | ('res', (np.float64, 2)) 97 | ]) 98 | ResType1 = np.dtype([ 99 | ('id', np.int32), 100 | ('size', np.int32), 101 | ('from_node', np.int32), 102 | ('to', (ResType2, params["max_node_window"])) 103 | ]) 104 | FeatResType = np.dtype([ 105 | ('t', np.float64), 106 | ('size', np.int32), 107 | ('f', (ResType1, params["num_feat"])) 108 | ]) 109 | 110 | MocapResType = np.dtype([ 111 | ('t', np.float64), 112 | ('size', np.int32), 113 | ('r', (np.dtype([ 114 | ('t', np.float64), 115 | ('res', (np.float64, 6)) 116 | ]), params["max_node_window"])) 117 | ]) 118 | 119 | GnssResType = np.dtype([ 120 | ('t', np.float64), 121 | ('size', np.int32), 122 | ('r', (np.dtype([ 123 | ('size', np.int32), 124 | ('v', np.dtype([ 125 | ('t', np.float64), 126 | ('res', (np.float64, 2)) 127 | ]), params["num_sat"]) 128 | ]), params["max_node_window"])) 129 | ]) 130 | 131 | SatPosType = np.dtype([ 132 | ('t', np.float64), 133 | ('size', np.int32), 134 | ('sats', (np.dtype([ 135 | ('id', np.int32), 136 | ('p', (np.float64, 3)), 137 | ('v', (np.float64, 3)), 138 | ('tau', (np.float64, 2)), 139 | ('azel', (np.float64, 2)) 140 | ]), params["num_sat"])) 141 | ]) 142 | 143 | PRangeResType = np.dtype([ 144 | ('t', np.float64), 145 | ('size', np.int32), 146 | ('sats', (np.dtype([ 147 | ('id', np.int32), 148 | ('res', (np.float64, 3)), 149 | ('z', (np.float64, 3)), 150 | ('zhat', (np.float64, 3)) 151 | ]), params["num_sat"])) 152 | ]) 153 | 154 | ImuType = np.dtype([ 155 | ('t', np.float64), 156 | ('acc', (np.float64, 3)), 157 | ('omega', (np.float64, 3)) 158 | ]) 159 | 160 | LlaType = np.dtype([ 161 | ('t', np.float64), 162 | ('pp_lla', (np.float64, 3)), 163 | ('est_lla', (np.float64, 3)) 164 | ]) 165 | 166 | 167 | 168 | 169 | 170 | 171 | -------------------------------------------------------------------------------- /include/salsa/logger.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "salsa/salsa.h" 13 | 14 | #define LOGGER_BUFFER_SIZE 1024*1024 15 | 16 | namespace salsa 17 | { 18 | 19 | class Logger 20 | { 21 | public: 22 | Logger() {} 23 | 24 | Logger(const std::string filename) 25 | { 26 | open(filename); 27 | } 28 | 29 | void open(const std::string& filename) 30 | { 31 | file_.open(filename); 32 | } 33 | 34 | ~Logger() 35 | { 36 | file_.close(); 37 | } 38 | template 39 | void log(T... data) 40 | { 41 | int dummy[sizeof...(data)] = { (file_.write((char*)&data, sizeof(T)), 1)... }; 42 | } 43 | 44 | template 45 | void logVectors(T... data) 46 | { 47 | int dummy[sizeof...(data)] = { (file_.write((char*)data.data(), sizeof(typename T::Scalar)*data.rows()*data.cols()), 1)... }; 48 | } 49 | 50 | std::ofstream file_; 51 | }; 52 | 53 | 54 | class MTLogger 55 | { 56 | enum 57 | { 58 | N = LOGGER_BUFFER_SIZE 59 | }; 60 | 61 | public: 62 | MTLogger(std::string filename) 63 | { 64 | buffer_ = new char[N]; 65 | file_.open(filename); 66 | if (!file_.is_open()) 67 | std::cerr << "unable to open " << filename << std::endl; 68 | 69 | buf_head_ = buf_tail_ = 0; 70 | buf_free_ = N; 71 | shutdown_ = false; 72 | write_thread = new std::thread([this](){ this->writeThread(); }); 73 | } 74 | 75 | ~MTLogger() 76 | { 77 | close(); 78 | } 79 | 80 | void close() 81 | { 82 | closeWriteThread(); 83 | file_.close(); 84 | if (buffer_) 85 | { 86 | delete buffer_; 87 | buffer_ = nullptr; 88 | } 89 | } 90 | 91 | void closeWriteThread() 92 | { 93 | shutdown_ = true; 94 | if (write_thread) 95 | { 96 | write_thread->join(); 97 | delete write_thread; 98 | write_thread = nullptr; 99 | } 100 | } 101 | 102 | void bufferData(char* addr, size_t size) 103 | { 104 | if (size > buf_free_) 105 | { 106 | std::cerr << "Buffer Overflow" << std::endl; 107 | exit(2); 108 | } 109 | 110 | 111 | if (size < N - buf_head_) 112 | { 113 | // data will fit on end of buffer 114 | memcpy(&buffer_[buf_head_], addr, size); 115 | mtx_.lock(); 116 | buf_head_ += size; 117 | buf_free_ -= size; 118 | mtx_.unlock(); 119 | } 120 | else 121 | { 122 | // copy the first bit to the end of the buffer 123 | int n = N - buf_head_; 124 | memcpy(&buffer_[buf_head_], addr, n); 125 | mtx_.lock(); 126 | buf_head_ = 0; 127 | buf_free_ -= n; 128 | mtx_.unlock(); 129 | 130 | // copy the rest to the beginning 131 | n = size - n; 132 | memcpy(&buffer_[buf_head_], &addr[n], n); 133 | mtx_.lock(); 134 | buf_head_ = n; 135 | buf_free_ -= n; 136 | mtx_.unlock(); 137 | } 138 | } 139 | 140 | void writeThread() 141 | { 142 | while(1) 143 | { 144 | if (!writeData()) 145 | { 146 | if (shutdown_) 147 | break; 148 | usleep(1000); 149 | } 150 | } 151 | } 152 | 153 | int writeData() 154 | { 155 | if (buf_head_ > buf_tail_) 156 | { 157 | int n = buf_head_ - buf_tail_; 158 | file_.write(&buffer_[buf_tail_], n); 159 | mtx_.lock(); 160 | buf_tail_ = buf_head_; 161 | buf_free_ = N; 162 | mtx_.unlock(); 163 | return 1; 164 | } 165 | else if (buf_tail_ > buf_head_) 166 | { 167 | int n = N - buf_tail_; 168 | file_.write(&buffer_[buf_tail_], n); 169 | mtx_.lock(); 170 | buf_tail_ = 0; 171 | buf_free_ += n; 172 | mtx_.unlock(); 173 | 174 | n = buf_head_ - buf_tail_; 175 | file_.write(buffer_, n); 176 | mtx_.lock(); 177 | buf_tail_ = buf_head_; 178 | buf_free_ += n; 179 | mtx_.unlock(); 180 | return -1; 181 | } 182 | else 183 | { 184 | return 0; 185 | } 186 | } 187 | 188 | template 189 | void log(T... data) 190 | { 191 | int dummy[sizeof...(data)] = { (bufferData((char*)&data, sizeof(T)), 1)... }; 192 | } 193 | 194 | template 195 | void logVectors(T... data) 196 | { 197 | int dummy[sizeof...(data)] = { (bufferData((char*)data.data(), sizeof(typename T::Scalar)*data.rows()*data.cols()), 1)... }; 198 | } 199 | 200 | std::mutex mtx_; 201 | std::ofstream file_; 202 | bool shutdown_; 203 | char* buffer_; 204 | int buf_head_, buf_tail_; 205 | int buf_free_; 206 | std::thread* write_thread; 207 | }; 208 | } 209 | 210 | 211 | -------------------------------------------------------------------------------- /params/salsa.yaml: -------------------------------------------------------------------------------- 1 | log_prefix: "/tmp/Salsa/" 2 | 3 | ###################################### 4 | ### ROSbag Setup 5 | ###################################### 6 | imu_topic: "/imu" 7 | image_topic: "/left" 8 | mocap_topic: "/Ragnarok_ned" 9 | start_time: 10 10 | duration: 40 11 | bag_name: "/home/superjax/rosbag/mynt_outdoors_walk/walk1_uc.bag" 12 | mocap_rate: 10 13 | bias0: [0, 0, 0, 0, 0, 0] 14 | wait_key: false 15 | # bias0: [-0.04717741613721455, -0.07034896670337663, -0.03231176628010501, 16 | # -0.004936014393538678, 0.006832230548126756, 0.003299054277444827] 17 | 18 | ###################################### 19 | ### OPT SETUP 20 | ###################################### 21 | max_node_window: 50 22 | max_kf_window: 20 23 | state_buf_size: 1000 24 | num_feat: 100 25 | num_sat: 15 26 | disable_solver: false 27 | max_iter: 1000 28 | # max_solver_time: 0.07 29 | max_solver_time: 1.0 30 | num_threads: 12 31 | 32 | ##################################### 33 | ### Output Frame 34 | ##################################### 35 | label: "$\\hat{x}$" # Label of lines in plot 36 | x_b2o: [0.006228303245238449, 0.01936311380701126, -0.09925855920867407, 37 | 0.8870108331782217, 0.0, -0.4617486132350339, 0.0] 38 | 39 | 40 | ##################################### 41 | ### Static Start 42 | ##################################### 43 | enable_static_start: true 44 | static_start_freq: 0.02 # how often to drop static-start nodes 45 | static_start_imu_thresh: 15 # if imu mag exceeds this, then stop static start 46 | camera_start_delay: 2.0 # wait this long after static start to start fusing images 47 | static_start_Xi: [1e12, 1e12, 1e12, # pos 48 | 1e9, # yaw 49 | 1e9, 1e9, 1e9] # vel 50 | static_start_imu_bias_relaxation: 100 51 | 52 | ##################################### 53 | ### IMU 54 | ##################################### 55 | accel_noise_stdev: 3.0 56 | gyro_noise_stdev: 0.2 57 | imu_bias_xi: [5e2, 5e2, 5e2, # acc 58 | 1e4, 1e4, 1e4] # gyro 59 | 60 | ##################################### 61 | ### Camera 62 | ##################################### 63 | pix_err_stdev: 0.05 64 | disable_vision: false 65 | tracker_freq: 1000 66 | max_depth: 100.0 67 | use_measured_depth: false 68 | use_distort_mask: true 69 | 70 | 71 | # Small Camera 72 | # focal_len: [189.288014941235, 188.89300027962554] 73 | # cam_center: [189.57352223555952, 114.25998149460739] 74 | # image_size: [376, 240] 75 | # cam_skew: 0.0 76 | # distortion: [-0.28839265926889596, 0.06052865219979488, 0.003175682270413171, 0.0014326472335837869, 0] 77 | 78 | # Big Camera 79 | focal_len: [386.5056187959309, 386.9805107887256] 80 | cam_center: [387.88086531158535, 233.65220025666383] 81 | image_size: [752, 480] 82 | cam_skew: 0.0 83 | distortion: [-0.2816241410812369, 0.05758000227344342, 0.000913492516598625, 0.0011916159178065463, 0.0] 84 | 85 | 86 | kf_parallax_thresh: 10 87 | kf_feature_thresh: 0.90 88 | update_on_camera: true 89 | klt_quality: 0.3 90 | klt_block_size: 7 91 | klt_winsize: 21 92 | klt_max_levels: 7 93 | ransac_thresh: 0.001 94 | ransac_prob: 0.999 95 | 96 | show_matches: true 97 | make_video: true 98 | show_skip: 1 99 | get_feature_radius: 30 100 | track_feature_min_distance: 15 101 | feature_mask: "" 102 | simulate_klt: true 103 | tc: 0.000 # t_b = t_c + dt 104 | x_b2c: [0.01666044727690726, 0.06831908058722323, -0.3523408903053932, 105 | 0.7060646666507441, 0.004429725140391003, 0.001812721421084533, 0.7081311870576004] 106 | 107 | ##################################### 108 | ### GNSS 109 | ##################################### 110 | clk_bias_xi: [2, 2] # nsec 111 | prange_xi: [ 1.0, 10.0] 112 | min_satellite_elevation: 10 113 | use_point_positioning: true 114 | disable_gnss: false 115 | update_on_gnss: false 116 | static_start_fix_delay: 3.0 117 | x_e2n: [ -1798298, -4531280, 4098905, 118 | 0.2363030974, -0.7505535126, -0.509619223, -0.3480208592] 119 | p_b2g: [0, 0, 0] 120 | switch_xi: 12 121 | switchdot_xi: 40 122 | enable_switching_factors: true 123 | min_sats: 5 124 | dt_gnss: -0.03 # t_b = t_gnss + dt 125 | 126 | 127 | ##################################### 128 | ### Mocap 129 | ##################################### 130 | position_noise_stdev: 0.01 131 | attitude_noise_stdev: 0.01 132 | disable_mocap: true 133 | mocap_offset: 0.2 # t_b = t_m + dt 134 | x_b2m: [0.006228303245238449, 0.01936311380701126, -0.09925855920867407, 135 | 0.8870108331782217, 0.0, -0.4617486132350339, 0.0] 136 | update_on_mocap: true 137 | 138 | ##################################### 139 | ### Anchors 140 | ##################################### 141 | state_anchor_xi: [1e3, 1e3, 1e3, # p 142 | 300, 300, 300, # q 143 | 450, 450, 450, # v 144 | 30, 30, # clk_bias (nsec) 145 | 1e6, 1e6, 1e6, # acc bias 146 | 1e6, 1e6, 1e6] # gyro bias 147 | x_e2n_anchor_xi: [30, 30, 30, # p 148 | 1e3, 1e3, 1e3] # q -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(salsa) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) 6 | 7 | find_package(OpenCV REQUIRED EXACT 3.2) 8 | find_package(Ceres REQUIRED) 9 | find_package(Eigen3 REQUIRED) 10 | find_package(yaml-cpp REQUIRED) 11 | find_package(nanoflann_eigen REQUIRED) 12 | find_package(lin_alg_tools REQUIRED) 13 | find_package(geometry REQUIRED) 14 | find_package(gnss_utils REQUIRED) 15 | find_package(multirotor_sim REQUIRED) 16 | find_package(Threads REQUIRED) 17 | find_package(Boost REQUIRED) 18 | 19 | add_definitions(-DSALSA_DIR="${CMAKE_CURRENT_LIST_DIR}") 20 | 21 | add_library(factors 22 | src/factors/xform.cpp 23 | src/factors/imu.cpp 24 | src/factors/mocap.cpp 25 | src/factors/feat.cpp 26 | src/factors/anchor.cpp 27 | src/factors/clock_dynamics.cpp 28 | src/factors/switch_dynamics.cpp 29 | src/factors/zero_vel.cpp 30 | src/factors/pseudorange.cpp) 31 | target_link_libraries(factors 32 | ${geometry_LIBRARIES} 33 | ${gnss_utils_LIBRARIES} 34 | ${OpenCV_LIBRARIES} 35 | ${CERES_LIBRARIES} 36 | pthread) 37 | target_include_directories(factors PUBLIC 38 | include 39 | ${OpenCV_INCLUDE_DIRS} 40 | ${EIGEN3_INCLUDE_DIRS} 41 | ${CERES_INCLUDE_DIRS}) 42 | 43 | add_library(salsa 44 | src/state.cpp 45 | src/meas.cpp 46 | src/salsa.cpp 47 | src/salsa_mocap.cpp 48 | src/salsa_gnss.cpp 49 | src/salsa_vision.cpp 50 | src/salsa_log.cpp 51 | src/klt.cpp 52 | src/graph.cpp 53 | src/feat.cpp 54 | src/num_diff.cpp) 55 | target_link_libraries(salsa 56 | factors 57 | stdc++fs 58 | ${geometry_LIBRARIES} 59 | ${gnss_utils_LIBRARIES} 60 | ${YAML_CPP_LIBRARIES} 61 | ${OpenCV_LIBS} 62 | ${CERES_LIBRARIES} 63 | ${Boost_LIBRARIES} 64 | pthread) 65 | target_include_directories(salsa PUBLIC 66 | ${Boost_INCLUDE_DIRS}) 67 | 68 | 69 | option (BUILD_ROS "Build ROS Tools" OFF) 70 | if (BUILD_ROS) 71 | find_package(catkin REQUIRED COMPONENTS 72 | rosbag 73 | roscpp 74 | sensor_msgs 75 | inertial_sense 76 | class_loader 77 | cv_bridge 78 | nav_msgs) 79 | message(STATUS "Building salsa ROS library") 80 | add_library(salsa_ros src/salsa_ros.cpp) 81 | target_include_directories(salsa_ros PUBLIC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) 82 | target_link_libraries(salsa_ros salsa ${catkin_LIBRARIES}) 83 | 84 | add_executable(salsa_rosbag src/salsa_rosbag.cpp) 85 | target_include_directories(salsa_rosbag PUBLIC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) 86 | target_link_libraries(salsa_rosbag salsa_ros ${catkin_LIBRARIES}) 87 | 88 | add_executable(rosbag_decompressor src/rosbag_decompressor.cpp) 89 | target_include_directories(rosbag_decompressor PUBLIC include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 90 | target_link_libraries(rosbag_decompressor ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 91 | 92 | 93 | # add_executable(salsa_node src/salsa_node.cpp) 94 | # target_link_libraries(salsa_node salsa_ros ${catkin_LIBRARIES}) 95 | # target_include_directories(salsa_node PUBLIC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) 96 | endif() 97 | 98 | 99 | option (BUILD_TESTS "Build Tests" ON) 100 | if (BUILD_TESTS) 101 | find_package(GTest REQUIRED) 102 | message(STATUS "Building salsa tests") 103 | add_executable(test_salsa 104 | src/test/test_salsa.cpp 105 | src/test/test_mocap_factor.cpp 106 | src/test/test_imu_factor.cpp 107 | src/test/test_logger.cpp 108 | src/test/test_point_positioning.cpp 109 | src/test/test_feat_factor.cpp 110 | src/test/test_misc.cpp 111 | src/test/test_vision.cpp 112 | src/test/test_klt.cpp 113 | src/test/test_xform_local_param.cpp 114 | src/test/test_pseudorange_factor.cpp 115 | src/test/test_node_management.cpp 116 | ) 117 | target_link_libraries(test_salsa 118 | salsa 119 | gtest 120 | gtest_main 121 | ${geometry_LIBRARIES} 122 | ${gnss_utils_LIBRARIES} 123 | ${multirotor_sim_LIBRARIES} 124 | ${GTEST_LIBRARIES} 125 | ${OpenCV_LIBRARIES} 126 | ) 127 | endif() 128 | 129 | add_executable(featSim src/examples/feat_sim.cpp) 130 | target_link_libraries(featSim salsa geometry ${multirotor_sim_LIBRARIES}) 131 | add_executable(gnssSim src/examples/gnss_sim.cpp) 132 | target_link_libraries(gnssSim salsa geometry ${multirotor_sim_LIBRARIES}) 133 | add_executable(mocapSim src/examples/mocap_sim.cpp) 134 | target_link_libraries(mocapSim salsa geometry ${multirotor_sim_LIBRARIES}) 135 | add_executable(mixedSim src/examples/mixed_sim.cpp) 136 | target_link_libraries(mixedSim salsa geometry ${multirotor_sim_LIBRARIES}) 137 | add_executable(compareSim src/examples/compare_sim.cpp) 138 | target_link_libraries(compareSim salsa geometry ${multirotor_sim_LIBRARIES}) 139 | 140 | -------------------------------------------------------------------------------- /src/test/test_logger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "Eigen/Core" 4 | 5 | #include "salsa/logger.h" 6 | #include "salsa/test_common.h" 7 | 8 | using namespace Eigen; 9 | using namespace std; 10 | 11 | TEST (MTLogger, SingleThread) 12 | { 13 | std::string filename = "/tmp/Logger.dat"; 14 | salsa::MTLogger log(filename); 15 | const int N = 256; 16 | 17 | MatrixXd data1(N, N), data2(N, N), data3(N, N), data4(N,N), data5(N,N); 18 | data1.setRandom(); 19 | data2.setRandom(); 20 | data3.setRandom(); 21 | data4.setRandom(); 22 | data5.setRandom(); 23 | 24 | log.closeWriteThread(); 25 | 26 | log.logVectors(data1); 27 | EXPECT_EQ(log.buf_free_, LOGGER_BUFFER_SIZE - sizeof(double)*N*N); 28 | EXPECT_EQ(log.buf_head_, sizeof(double)*N*N); 29 | EXPECT_EQ(log.buf_tail_, 0); 30 | log.writeData(); 31 | EXPECT_EQ(log.buf_free_, LOGGER_BUFFER_SIZE); 32 | EXPECT_EQ(log.buf_head_, LOGGER_BUFFER_SIZE - sizeof(double)*N*N); 33 | EXPECT_EQ(log.buf_tail_, LOGGER_BUFFER_SIZE - sizeof(double)*N*N); 34 | log.logVectors(data2); 35 | EXPECT_EQ(log.buf_free_, LOGGER_BUFFER_SIZE - sizeof(double)*N*N); 36 | EXPECT_EQ(log.buf_head_, 2*sizeof(double)*N*N - LOGGER_BUFFER_SIZE); 37 | EXPECT_EQ(log.buf_tail_, LOGGER_BUFFER_SIZE - sizeof(double)*N*N); 38 | log.writeData(); 39 | EXPECT_EQ(log.buf_free_, LOGGER_BUFFER_SIZE); 40 | EXPECT_EQ(log.buf_head_, 2*sizeof(double)*N*N - LOGGER_BUFFER_SIZE); 41 | EXPECT_EQ(log.buf_tail_, 2*sizeof(double)*N*N - LOGGER_BUFFER_SIZE); 42 | log.logVectors(data3); 43 | log.writeData(); 44 | log.logVectors(data4); 45 | log.writeData(); 46 | log.logVectors(data5); 47 | log.writeData(); 48 | 49 | log.close(); 50 | 51 | ifstream file(filename, std::ios::in | std::ios::binary); 52 | MatrixXd data1_in(N,N), data2_in(N,N), data3_in(N,N), data4_in(N,N), data5_in(N,N); 53 | 54 | file.read((char*)data1_in.data(), sizeof(double)*N*N); 55 | file.read((char*)data2_in.data(), sizeof(double)*N*N); 56 | file.read((char*)data3_in.data(), sizeof(double)*N*N); 57 | file.read((char*)data4_in.data(), sizeof(double)*N*N); 58 | file.read((char*)data5_in.data(), sizeof(double)*N*N); 59 | 60 | EXPECT_NEAR((data1 - data1_in).array().abs().sum(), 0.0, 1e-14); 61 | EXPECT_NEAR((data2 - data2_in).array().abs().sum(), 0.0, 1e-14); 62 | EXPECT_NEAR((data3 - data3_in).array().abs().sum(), 0.0, 1e-14); 63 | EXPECT_NEAR((data4 - data4_in).array().abs().sum(), 0.0, 1e-14); 64 | EXPECT_NEAR((data5 - data5_in).array().abs().sum(), 0.0, 1e-14); 65 | } 66 | 67 | 68 | TEST (MTLogger, MultiThread) 69 | { 70 | std::string filename = "/tmp/Logger.dat"; 71 | salsa::MTLogger log(filename); 72 | const int N = 256; 73 | 74 | MatrixXd data1(N, N), data2(N, N), data3(N, N), data4(N,N), data5(N,N); 75 | data1.setRandom(); 76 | data2.setRandom(); 77 | data3.setRandom(); 78 | data4.setRandom(); 79 | data5.setRandom(); 80 | 81 | log.logVectors(data1); 82 | usleep(2000); 83 | log.logVectors(data2); 84 | usleep(2000); 85 | log.logVectors(data3); 86 | usleep(2000); 87 | log.logVectors(data4); 88 | usleep(2000); 89 | log.logVectors(data5); 90 | 91 | log.close(); 92 | 93 | ifstream file(filename, std::ios::in | std::ios::binary); 94 | MatrixXd data1_in(N,N), data2_in(N,N), data3_in(N,N), data4_in(N,N), data5_in(N,N); 95 | 96 | file.read((char*)data1_in.data(), sizeof(double)*N*N); 97 | file.read((char*)data2_in.data(), sizeof(double)*N*N); 98 | file.read((char*)data3_in.data(), sizeof(double)*N*N); 99 | file.read((char*)data4_in.data(), sizeof(double)*N*N); 100 | file.read((char*)data5_in.data(), sizeof(double)*N*N); 101 | 102 | EXPECT_NEAR((data1 - data1_in).array().abs().sum(), 0.0, 1e-14); 103 | EXPECT_NEAR((data2 - data2_in).array().abs().sum(), 0.0, 1e-14); 104 | EXPECT_NEAR((data3 - data3_in).array().abs().sum(), 0.0, 1e-14); 105 | EXPECT_NEAR((data4 - data4_in).array().abs().sum(), 0.0, 1e-14); 106 | EXPECT_NEAR((data5 - data5_in).array().abs().sum(), 0.0, 1e-14); 107 | } 108 | 109 | 110 | TEST (MTLogger, Different_Types) 111 | { 112 | std::string filename = "/tmp/Logger.dat"; 113 | salsa::MTLogger log(filename); 114 | 115 | int a = 7; 116 | double b = 28.02; 117 | float c = 3928.29; 118 | typedef struct 119 | { 120 | float d; 121 | uint32_t e; 122 | int16_t f; 123 | } thing_t; 124 | thing_t g; 125 | g.d = 1983.23; 126 | g.e = 239102930; 127 | g.f = 28395; 128 | 129 | log.log(a, b, c, g); 130 | 131 | log.close(); 132 | 133 | ifstream file(filename, std::ios::in | std::ios::binary); 134 | int a_in; 135 | double b_in; 136 | float c_in; 137 | thing_t g_in; 138 | 139 | file.read((char*)&a_in, sizeof(a_in)); 140 | file.read((char*)&b_in, sizeof(b_in)); 141 | file.read((char*)&c_in, sizeof(c_in)); 142 | file.read((char*)&g_in, sizeof(g_in)); 143 | 144 | EXPECT_EQ(a_in, a); 145 | EXPECT_EQ(b_in, b); 146 | EXPECT_EQ(c_in, c); 147 | EXPECT_EQ(g_in.d, g.d); 148 | EXPECT_EQ(g_in.e, g.e); 149 | EXPECT_EQ(g_in.f, g.f); 150 | } 151 | -------------------------------------------------------------------------------- /params/sim_params.yaml: -------------------------------------------------------------------------------- 1 | tmax: 60.0 # Simulation total time 2 | dt: 0.004 # Sim step rate 3 | log_filename: "" 4 | seed: 15 # 0 initializes seed with time 5 | 6 | # Path type: 7 | # 0 : waypoints 8 | # 1 : random waypoints 9 | # 2 : circular trajectory 10 | # 3 : constant velocity, randomly varying heading 11 | path_type: 2 12 | 13 | # User-defined waypoints 14 | waypoints: [ 15 | 10, 0, -5, .705, 16 | 10, 10, -4, 3.0, 17 | -10, 10, -5.5, -1.5, 18 | -10, -7, -5, .80, 19 | -8, -12, -4.5, -2.3, 20 | 8, -5, -4, 1.7 21 | ] 22 | 23 | # Random waypoints parameters 24 | num_random_waypoints: 30 25 | altitude: -5.0 26 | altitude_variance: 1.0 27 | heading_walk: 0.5 28 | waypoint_separation: 1.0 29 | waypoint_sep_variance: 2.0 # uniformly random number between zero and this to add to separation 30 | 31 | # Constant velocity trajectory parameters 32 | velocity_magnitude: 1.0 33 | traj_altitude: -5.0 34 | traj_heading_walk: 10.0 35 | traj_heading_straight_gain: 0.01 36 | 37 | # Follow the carrot trajectory parameters 38 | traj_delta_north: 10 39 | traj_delta_east: 10 40 | traj_delta_alt: -4 41 | traj_delta_yaw: 1.5 42 | traj_nom_north: 0 43 | traj_nom_east: 0 44 | traj_nom_alt: 4 45 | traj_nom_yaw: 0 46 | traj_north_period: 20 47 | traj_east_period: 20 48 | traj_alt_period: 20 49 | traj_yaw_period: 20 50 | 51 | 52 | # Controller Configuration 53 | takeoff_wait_time: 10 54 | throttle_eq: 0.5 55 | mass: 1.0 56 | max_thrust: 19.6133 57 | max_torque: [0.30625, 0.30625, 0.1] # N-m 58 | kp_w: [1.0, 1.0, 1.0] # Inner loop controller gains 59 | kd_w: [0.0, 0.0, 0.0] # Inner loop controller gains 60 | 61 | waypoint_threshold: 0.3 62 | waypoint_velocity_threshold: 0.7 63 | drag_constant: 0.1 64 | angular_drag_constant: 0.01 65 | 66 | Kp: [2, 2, 2] 67 | Kd: [0, 0, 0] 68 | Kv: [2, 2, 2] 69 | sh_kv: 50 # Hover throttle velocity gain 70 | sh_ks: 0.1 # Hover throttle gain 71 | 72 | lqr_max_pos_error: 0.5 73 | lqr_max_vel_error: 0.5 74 | lqr_max_yaw_error: 0.1 75 | lqr_Q: [1, 1, 10, 100, 100, 100] 76 | lqr_R: [10000, 1000, 1000, 1000] 77 | 78 | roll_kp: 5.0 79 | roll_ki: 0.0 80 | roll_kd: 1.0 81 | pitch_kp: 5.0 82 | pitch_ki: 0.0 83 | pitch_kd: 1.0 84 | yaw_rate_kp: 1.0 85 | yaw_rate_ki: 0.0 86 | yaw_rate_kd: 0.0 87 | max_roll: 0.5 88 | max_pitch: 0.5 89 | max_yaw_rate: 1.0 90 | max_throttle: 1.0 91 | max_vel: 2.0 92 | max_tau_x: 1.0 93 | max_tau_y: 1.0 94 | max_tau_z: 1.0 95 | 96 | inertia: [0.1, 0.1, 0.1] 97 | 98 | 99 | x0: [0, 0, -3, # POS 100 | 1, 0, 0, 0, # ATT 101 | 0, 0, 0, # VEL 102 | 0, 0, 0] # OMEGA 103 | 104 | # Control type 105 | # 0: Nonlinear controller 106 | # 1: LQR controller 107 | control_type: 0 108 | 109 | 110 | # Environment Setup 111 | wall_max_offset: 0.01 # Points are distributed normally about the wall 112 | 113 | 114 | # Wind Setup 115 | enable_wind: false # Turn wind on and off 116 | wind_init_stdev: 0.1 # Variation on the initial wind direction components 117 | wind_walk_stdev: 0.1 # Amount of random walk in wind components 118 | 119 | enable_dynamics_noise: false 120 | dyn_noise: [0, 0, 0, # POS 121 | 0, 0, 0, # ATT 122 | 0.02, 0.02, 0.02, # VEL 123 | 0.01, 0.01, 0.01] # OMEGA 124 | 125 | 126 | # Truth Configuration 127 | use_accel_truth: false 128 | use_gyro_truth: false 129 | use_camera_truth: false 130 | use_altimeter_truth: false 131 | use_baro_truth: false 132 | use_depth_truth: true 133 | use_vo_truth: false 134 | use_mocap_truth: false 135 | 136 | RK4: true 137 | 138 | # Sensor Configuration 139 | imu_enabled: true 140 | alt_enabled: true 141 | baro_enabled: true 142 | mocap_enabled: true 143 | vo_enabled: true 144 | camera_enabled: true 145 | gnss_enabled: true 146 | raw_gnss_enabled: true 147 | 148 | ## IMU 149 | imu_update_rate: 250 150 | accel_init_stdev: 0.0 151 | accel_noise_stdev: 0.1 152 | accel_bias_walk: 0.02 153 | gyro_init_stdev: 0.0 154 | gyro_noise_stdev: 0.05 155 | gyro_bias_walk: 0.01 156 | p_b_u: [0.0, 0.0, 0.0 ] 157 | q_b_u: [1.0, 0.0, 0.0, 0.0] 158 | 159 | ## Camera 160 | num_features: 200 161 | camera_time_offset: 0.0 162 | camera_transmission_time: 0.0 163 | camera_transmission_noise: 0.0 164 | camera_update_rate: 10 165 | pixel_noise_stdev: 0.1 166 | focal_len: [250.0, 250.0] 167 | cam_center: [320, 240] 168 | image_size: [640, 480] 169 | loop_closure: false 170 | feat_too_close_thresh: 25 171 | q_b_c: [1, 0, 0, 0] 172 | #q_b_c: [ 0.923879659447, 0.0, 0.382683125915, 0.0 ] # Pitch down 45 deg 173 | # q_b_c: [0.712301460669, -0.00770717975554, 0.0104993233706, 0.701752800292] 174 | # p_b_c: [1.0, 0.0, 0.0 ] 175 | p_b_c: [0.0, 0.0, 0.0 ] 176 | #p_b_c: [-0.0216401454975, -0.064676986768, 0.00981073058949 ] 177 | # 178 | # Visual Odometry 179 | vo_delta_position: 1.0 180 | vo_delta_attitude: 0.1 181 | vo_translation_noise_stdev: 0.1 182 | vo_rotation_noise_stdev: 0.01 183 | 184 | ## Altimeter 185 | altimeter_update_rate: 25 186 | altimeter_noise_stdev: 0.1 187 | 188 | ## Barometer 189 | baro_update_rate: 25 190 | baro_noise_stdev: 3.0 191 | baro_init_stdev: 300.0 192 | baro_bias_walk: 1.0 193 | 194 | ## Depth 195 | depth_update_rate: 30 196 | depth_noise_stdev: 0.1 197 | 198 | ## Motion Capture 199 | p_b_m: [0.0, 0.0, 0.0] 200 | q_b_m: [1.0, 0.0, 0.0, 0.0] 201 | mocap_update_rate: 10 202 | attitude_noise_stdev: 0.01 203 | position_noise_stdev: 0.01 204 | mocap_time_offset: 0 205 | mocap_transmission_noise: 0 206 | mocap_transmission_time: 0 207 | 208 | 209 | ## GNSS 210 | ref_LLA: [0.702443501891, -1.9486196478, 1387.998309] # (rad, rad, m) 211 | gnss_update_rate: 5 212 | use_gnss_truth: false 213 | gnss_horizontal_position_stdev: 1.0 214 | gnss_vertical_position_stdev: 3.0 215 | gnss_velocity_stdev: 0.1 216 | p_b2g: [0, 0, 0] 217 | 218 | ## Raw GNSS 219 | use_raw_gnss_truth: false 220 | pseudorange_stdev: 0.5 221 | pseudorange_rate_stdev: 0.05 222 | carrier_phase_stdev: 0.01 223 | ephemeris_filename: "../sample/eph.dat" 224 | start_time_week: 2026 225 | start_time_tow_sec: 165029 226 | clock_init_stdev: 1e-9 227 | clock_walk_stdev: 1e-9 228 | multipath_prob: 0.5 229 | multipath_error_range: 25 230 | cycle_slip_prob: 0.00 231 | multipath_area: [-2.5, 1, # x range [min, max] 232 | -100, 100] # y range [min, max] 233 | gps_denied_area: [-10, -2.5, 234 | -100, 100] -------------------------------------------------------------------------------- /src/test/test_salsa_mocap_indexing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "multirotor_sim/simulator.h" 5 | #include "multirotor_sim/controller.h" 6 | #include "geometry/support.h" 7 | 8 | #include "salsa/salsa.h" 9 | #include "salsa/test_common.h" 10 | 11 | TEST (MocapIndexing, CheckFirstNode) 12 | { 13 | Salsa salsa; 14 | salsa.init("../params/salsa.yaml"); 15 | 16 | Simulator sim; 17 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 18 | sim.gnss_enabled_ = false; 19 | sim.raw_gnss_enabled_ = false; 20 | sim.vo_enabled_ = false; 21 | sim.camera_enabled_ = false; 22 | 23 | sim.register_estimator(&salsa); 24 | 25 | while (salsa.current_node_ == -1) 26 | { 27 | sim.run(); 28 | } 29 | 30 | for (int i = 0; i < Salsa::N; i++) 31 | { 32 | EXPECT_FALSE(salsa.imu_[i].active_); 33 | if (i == 0) 34 | { 35 | EXPECT_TRUE(salsa.initialized_[i]); 36 | EXPECT_TRUE(salsa.mocap_[i].active_); 37 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 38 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 39 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 40 | } 41 | else 42 | { 43 | EXPECT_FALSE(salsa.initialized_[i]); 44 | EXPECT_FALSE(salsa.mocap_[i].active_); 45 | EXPECT_MAT_NAN(salsa.x_.col(i)); 46 | EXPECT_MAT_NAN(salsa.v_.col(i)); 47 | EXPECT_MAT_NAN(salsa.tau_.col(i)); 48 | } 49 | for (int j = 0; j < Salsa::N_SAT; j++) 50 | EXPECT_FALSE(salsa.prange_[i][j].active_); 51 | } 52 | EXPECT_MAT_FINITE(salsa.imu_bias_); 53 | 54 | EXPECT_EQ(salsa.current_node_, 0); 55 | EXPECT_FINITE(salsa.current_t_); 56 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 57 | EXPECT_MAT_FINITE(salsa.current_v_); 58 | 59 | EXPECT_EQ(salsa.x_idx_, 0); 60 | } 61 | 62 | TEST (MocapIndexing, CheckSecondNode) 63 | { 64 | Salsa salsa; 65 | salsa.init("../params/salsa.yaml"); 66 | 67 | Simulator sim; 68 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 69 | sim.gnss_enabled_ = false; 70 | sim.raw_gnss_enabled_ = false; 71 | sim.vo_enabled_ = false; 72 | sim.camera_enabled_ = false; 73 | sim.register_estimator(&salsa); 74 | 75 | while (salsa.current_node_ < 1) 76 | { 77 | sim.run(); 78 | } 79 | 80 | for (int i = 0; i < Salsa::N; i++) 81 | { 82 | if (i == 0) 83 | { 84 | EXPECT_TRUE(salsa.initialized_[i]); 85 | EXPECT_TRUE(salsa.imu_[i].active_); 86 | EXPECT_TRUE(salsa.mocap_[i].active_); 87 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 88 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 89 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 90 | } 91 | else if (i == 1) 92 | { 93 | EXPECT_TRUE(salsa.initialized_[i]); 94 | EXPECT_FALSE(salsa.imu_[i].active_); 95 | EXPECT_TRUE(salsa.mocap_[i].active_); 96 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 97 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 98 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 99 | } 100 | else 101 | { 102 | EXPECT_FALSE(salsa.initialized_[i]); 103 | EXPECT_FALSE(salsa.imu_[i].active_); 104 | EXPECT_FALSE(salsa.mocap_[i].active_); 105 | EXPECT_MAT_NAN(salsa.x_.col(i)); 106 | EXPECT_MAT_NAN(salsa.v_.col(i)); 107 | EXPECT_MAT_NAN(salsa.tau_.col(i)); 108 | } 109 | for (int j = 0; j < Salsa::N_SAT; j++) 110 | EXPECT_FALSE(salsa.prange_[i][j].active_); 111 | } 112 | 113 | EXPECT_MAT_FINITE(salsa.x_.col(1)); 114 | EXPECT_MAT_FINITE(salsa.v_.col(1)); 115 | EXPECT_MAT_FINITE(salsa.tau_.col(1)); 116 | EXPECT_MAT_FINITE(salsa.imu_bias_); 117 | 118 | EXPECT_EQ(salsa.current_node_, 1); 119 | EXPECT_FINITE(salsa.current_t_); 120 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 121 | EXPECT_MAT_FINITE(salsa.current_v_); 122 | 123 | EXPECT_EQ(salsa.current_node_, 1); 124 | EXPECT_EQ(salsa.x_idx_, 1); 125 | } 126 | 127 | TEST (MocapIndexing, CheckWindowWrap) 128 | { 129 | Salsa salsa; 130 | salsa.init("../params/salsa.yaml"); 131 | 132 | Simulator sim; 133 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 134 | sim.gnss_enabled_ = false; 135 | sim.raw_gnss_enabled_ = false; 136 | sim.vo_enabled_ = false; 137 | sim.camera_enabled_ = false; 138 | 139 | sim.register_estimator(&salsa); 140 | 141 | while (salsa.current_node_ < Salsa::N) 142 | { 143 | sim.run(); 144 | } 145 | 146 | EXPECT_EQ(salsa.current_node_, Salsa::N); 147 | EXPECT_EQ(salsa.x_idx_, 0); 148 | for (int i = 0; i < Salsa::N; i++) 149 | { 150 | if (i == 0) 151 | EXPECT_FALSE(salsa.imu_[i].active_); 152 | else 153 | EXPECT_TRUE(salsa.imu_[i].active_); 154 | 155 | EXPECT_TRUE(salsa.mocap_[i].active_); 156 | 157 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 158 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 159 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 160 | for (int j = 0; j < Salsa::N_SAT; j++) 161 | EXPECT_FALSE(salsa.prange_[i][j].active_); 162 | } 163 | EXPECT_FINITE(salsa.current_t_); 164 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 165 | EXPECT_MAT_FINITE(salsa.current_v_); 166 | } 167 | 168 | TEST (MocapIndexing, CheckWindowWrapPlus) 169 | { 170 | Salsa salsa; 171 | salsa.init("../params/salsa.yaml"); 172 | 173 | Simulator sim; 174 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 175 | sim.gnss_enabled_ = false; 176 | sim.raw_gnss_enabled_ = false; 177 | sim.vo_enabled_ = false; 178 | sim.camera_enabled_ = false; 179 | 180 | sim.register_estimator(&salsa); 181 | 182 | while (salsa.current_node_ < Salsa::N+3) 183 | { 184 | sim.run(); 185 | } 186 | 187 | EXPECT_EQ(salsa.current_node_, Salsa::N+3); 188 | EXPECT_EQ(salsa.x_idx_, 3); 189 | for (int i = 0; i < Salsa::N; i++) 190 | { 191 | if (i == 3) 192 | EXPECT_FALSE(salsa.imu_[i].active_); 193 | else 194 | EXPECT_TRUE(salsa.imu_[i].active_); 195 | 196 | EXPECT_TRUE(salsa.mocap_[i].active_); 197 | 198 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 199 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 200 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 201 | for (int j = 0; j < Salsa::N_SAT; j++) 202 | EXPECT_FALSE(salsa.prange_[i][j].active_); 203 | } 204 | EXPECT_FINITE(salsa.current_t_); 205 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 206 | EXPECT_MAT_FINITE(salsa.current_v_); 207 | } 208 | 209 | TEST (MocapIndexing, CurrentStateAlwaysValid) 210 | { 211 | Salsa salsa; 212 | salsa.init("../params/salsa.yaml"); 213 | 214 | Simulator sim; 215 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 216 | sim.gnss_enabled_ = false; 217 | sim.raw_gnss_enabled_ = false; 218 | sim.vo_enabled_ = false; 219 | sim.camera_enabled_ = false; 220 | 221 | sim.register_estimator(&salsa); 222 | 223 | while (salsa.current_node_ < Salsa::N+3) 224 | { 225 | sim.run(); 226 | if (salsa.current_node_ == -1) 227 | { 228 | EXPECT_TRUE(std::isnan(salsa.current_t_)); 229 | EXPECT_MAT_NAN(salsa.current_x_.arr()); 230 | EXPECT_MAT_NAN(salsa.current_v_); 231 | } 232 | else 233 | { 234 | EXPECT_FINITE(salsa.current_t_); 235 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 236 | EXPECT_MAT_FINITE(salsa.current_v_); 237 | } 238 | } 239 | } 240 | -------------------------------------------------------------------------------- /src/test/test_salsa_gps_indexing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "multirotor_sim/simulator.h" 5 | #include "multirotor_sim/controller.h" 6 | #include "geometry/support.h" 7 | 8 | #include "salsa/salsa.h" 9 | #include "salsa/test_common.h" 10 | 11 | TEST (GpsIndexing, CheckFirstNode) 12 | { 13 | Salsa salsa; 14 | salsa.init("../params/salsa.yaml"); 15 | 16 | Simulator sim; 17 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 18 | sim.gnss_enabled_ = false; 19 | sim.mocap_enabled_ = false; 20 | sim.raw_gnss_enabled_ = true; 21 | sim.vo_enabled_ = false; 22 | sim.camera_enabled_ = false; 23 | 24 | sim.register_estimator(&salsa); 25 | 26 | while (salsa.current_node_ == -1) 27 | { 28 | sim.run(); 29 | } 30 | 31 | for (int i = 0; i < Salsa::N; i++) 32 | { 33 | EXPECT_FALSE(salsa.imu_[i].active_); 34 | EXPECT_FALSE(salsa.clk_[i].active_); 35 | if (i == 0) 36 | { 37 | EXPECT_TRUE(salsa.initialized_[i]); 38 | EXPECT_FALSE(salsa.mocap_[i].active_); 39 | for (int j = 0; j < sim.satellites_.size(); j++) 40 | EXPECT_TRUE(salsa.prange_[i][j].active_); 41 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 42 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 43 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 44 | } 45 | else 46 | { 47 | for (int j = 0; j < sim.satellites_.size(); j++) 48 | EXPECT_FALSE(salsa.prange_[i][j].active_); 49 | EXPECT_FALSE(salsa.initialized_[i]); 50 | EXPECT_FALSE(salsa.mocap_[i].active_); 51 | EXPECT_MAT_NAN(salsa.x_.col(i)); 52 | EXPECT_MAT_NAN(salsa.v_.col(i)); 53 | EXPECT_MAT_NAN(salsa.tau_.col(i)); 54 | } 55 | } 56 | EXPECT_MAT_FINITE(salsa.imu_bias_); 57 | 58 | EXPECT_EQ(salsa.current_node_, 0); 59 | EXPECT_FINITE(salsa.current_t_); 60 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 61 | EXPECT_MAT_FINITE(salsa.current_v_); 62 | 63 | EXPECT_EQ(salsa.x_idx_, 0); 64 | } 65 | 66 | TEST (GpsIndexing, CheckSecondNode) 67 | { 68 | Salsa salsa; 69 | salsa.init("../params/salsa.yaml"); 70 | 71 | Simulator sim; 72 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 73 | sim.gnss_enabled_ = false; 74 | sim.raw_gnss_enabled_ = true; 75 | sim.mocap_enabled_ = false; 76 | sim.vo_enabled_ = false; 77 | sim.camera_enabled_ = false; 78 | sim.register_estimator(&salsa); 79 | 80 | while (salsa.current_node_ < 1) 81 | { 82 | sim.run(); 83 | } 84 | 85 | for (int i = 0; i < Salsa::N; i++) 86 | { 87 | if (i == 0) 88 | { 89 | EXPECT_TRUE(salsa.initialized_[i]); 90 | EXPECT_TRUE(salsa.imu_[i].active_); 91 | EXPECT_TRUE(salsa.clk_[i].active_); 92 | EXPECT_FALSE(salsa.mocap_[i].active_); 93 | for (int j = 0; j < sim.satellites_.size(); j++) 94 | EXPECT_TRUE(salsa.prange_[i][j].active_); 95 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 96 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 97 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 98 | } 99 | else if (i == 1) 100 | { 101 | EXPECT_TRUE(salsa.initialized_[i]); 102 | for (int j = 0; j < sim.satellites_.size(); j++) 103 | EXPECT_TRUE(salsa.prange_[i][j].active_); 104 | EXPECT_FALSE(salsa.imu_[i].active_); 105 | EXPECT_FALSE(salsa.clk_[i].active_); 106 | EXPECT_FALSE(salsa.mocap_[i].active_); 107 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 108 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 109 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 110 | } 111 | else 112 | { 113 | for (int j = 0; j < sim.satellites_.size(); j++) 114 | EXPECT_FALSE(salsa.prange_[i][j].active_); 115 | EXPECT_FALSE(salsa.initialized_[i]); 116 | EXPECT_FALSE(salsa.imu_[i].active_); 117 | EXPECT_FALSE(salsa.clk_[i].active_); 118 | EXPECT_FALSE(salsa.mocap_[i].active_); 119 | EXPECT_MAT_NAN(salsa.x_.col(i)); 120 | EXPECT_MAT_NAN(salsa.v_.col(i)); 121 | EXPECT_MAT_NAN(salsa.tau_.col(i)); 122 | } 123 | } 124 | 125 | EXPECT_MAT_FINITE(salsa.x_.col(1)); 126 | EXPECT_MAT_FINITE(salsa.v_.col(1)); 127 | EXPECT_MAT_FINITE(salsa.tau_.col(1)); 128 | EXPECT_MAT_FINITE(salsa.imu_bias_); 129 | 130 | EXPECT_EQ(salsa.current_node_, 1); 131 | EXPECT_FINITE(salsa.current_t_); 132 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 133 | EXPECT_MAT_FINITE(salsa.current_v_); 134 | 135 | EXPECT_EQ(salsa.current_node_, 1); 136 | EXPECT_EQ(salsa.x_idx_, 1); 137 | } 138 | 139 | TEST (GpsIndexing, CheckWindowWrap) 140 | { 141 | Salsa salsa; 142 | salsa.init("../params/salsa.yaml"); 143 | 144 | Simulator sim; 145 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 146 | sim.gnss_enabled_ = false; 147 | sim.mocap_enabled_ = false; 148 | sim.raw_gnss_enabled_ = true; 149 | sim.vo_enabled_ = false; 150 | sim.camera_enabled_ = false; 151 | 152 | sim.register_estimator(&salsa); 153 | 154 | while (salsa.current_node_ < Salsa::N) 155 | { 156 | sim.run(); 157 | } 158 | 159 | EXPECT_EQ(salsa.current_node_, Salsa::N); 160 | EXPECT_EQ(salsa.x_idx_, 0); 161 | for (int i = 0; i < Salsa::N; i++) 162 | { 163 | if (i == 0) 164 | { 165 | EXPECT_FALSE(salsa.imu_[i].active_); 166 | EXPECT_FALSE(salsa.clk_[i].active_); 167 | } 168 | else 169 | { 170 | EXPECT_TRUE(salsa.imu_[i].active_); 171 | EXPECT_TRUE(salsa.clk_[i].active_); 172 | } 173 | 174 | for (int j = 0; j < sim.satellites_.size(); j++) 175 | EXPECT_TRUE(salsa.prange_[i][j].active_); 176 | EXPECT_FALSE(salsa.mocap_[i].active_); 177 | 178 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 179 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 180 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 181 | } 182 | EXPECT_FINITE(salsa.current_t_); 183 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 184 | EXPECT_MAT_FINITE(salsa.current_v_); 185 | } 186 | 187 | TEST (GpsIndexing, CheckWindowWrapPlus) 188 | { 189 | Salsa salsa; 190 | salsa.init("../params/salsa.yaml"); 191 | 192 | Simulator sim; 193 | sim.load("../lib/multirotor_sim/params/sim_params.yaml"); 194 | sim.gnss_enabled_ = false; 195 | sim.mocap_enabled_ = false; 196 | sim.raw_gnss_enabled_ = true; 197 | sim.vo_enabled_ = false; 198 | sim.camera_enabled_ = false; 199 | 200 | sim.register_estimator(&salsa); 201 | 202 | while (salsa.current_node_ < Salsa::N+3) 203 | { 204 | sim.run(); 205 | } 206 | 207 | EXPECT_EQ(salsa.current_node_, Salsa::N+3); 208 | EXPECT_EQ(salsa.x_idx_, 3); 209 | for (int i = 0; i < Salsa::N; i++) 210 | { 211 | if (i == 3) 212 | { 213 | EXPECT_FALSE(salsa.imu_[i].active_); 214 | EXPECT_FALSE(salsa.clk_[i].active_); 215 | } 216 | else 217 | { 218 | EXPECT_TRUE(salsa.imu_[i].active_); 219 | EXPECT_TRUE(salsa.clk_[i].active_); 220 | } 221 | 222 | for (int j = 0; j < sim.satellites_.size(); j++) 223 | EXPECT_TRUE(salsa.prange_[i][j].active_); 224 | EXPECT_FALSE(salsa.mocap_[i].active_); 225 | 226 | EXPECT_MAT_FINITE(salsa.x_.col(i)); 227 | EXPECT_MAT_FINITE(salsa.v_.col(i)); 228 | EXPECT_MAT_FINITE(salsa.tau_.col(i)); 229 | } 230 | EXPECT_FINITE(salsa.current_t_); 231 | EXPECT_MAT_FINITE(salsa.current_x_.arr()); 232 | EXPECT_MAT_FINITE(salsa.current_v_); 233 | } 234 | 235 | -------------------------------------------------------------------------------- /src/salsa_gnss.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa.h" 2 | 3 | using namespace std; 4 | using namespace Eigen; 5 | using namespace xform; 6 | using namespace gnss_utils; 7 | namespace fs = std::experimental::filesystem; 8 | 9 | namespace salsa 10 | { 11 | 12 | void Salsa::initGNSS(const std::string& filename) 13 | { 14 | get_yaml_eigen("x_e2n", filename, x_e2n_.arr_); 15 | get_yaml_node("min_satellite_elevation", filename, min_satellite_elevation_); 16 | min_satellite_elevation_ = deg2rad(min_satellite_elevation_); 17 | get_yaml_node("use_point_positioning", filename, use_point_positioning_); 18 | get_yaml_node("disable_gnss", filename, disable_gnss_); 19 | get_yaml_node("min_sats", filename, min_sats_); 20 | get_yaml_node("dt_gnss", filename, dt_gnss_); 21 | } 22 | 23 | void Salsa::ephCallback(const GTime& t, const eph_t &eph) 24 | { 25 | if (disable_gnss_) 26 | return; 27 | 28 | if (eph.sat > 90) 29 | return; 30 | 31 | auto s = sats_.begin(); 32 | while (s != sats_.end()) 33 | { 34 | if (s->id_ == eph.sat) 35 | break; 36 | s++; 37 | } 38 | bool new_sat = (s == sats_.end()); 39 | 40 | if (start_time_.tow_sec < 0) 41 | return; 42 | 43 | GTime now = t; 44 | Vector3d rec_pos_ecef = WGS84::ned2ecef(x_e2n_, current_state_.x.t()); 45 | Satellite sat(eph, sats_.size()); 46 | bool high_enough = sat.azimuthElevation(now, rec_pos_ecef)(1) > min_satellite_elevation_; 47 | if (new_sat) 48 | { 49 | if (high_enough) 50 | sats_.push_back(sat); 51 | } 52 | else 53 | { 54 | if (high_enough) 55 | s->addEphemeris(eph); 56 | else 57 | sats_.erase(s); 58 | } 59 | refreshSatIdx(); 60 | } 61 | 62 | void Salsa::refreshSatIdx() 63 | { 64 | for (int i = 0; i < sats_.size(); i++) 65 | { 66 | sats_[i].idx_ = i; 67 | } 68 | } 69 | 70 | void Salsa::filterObs(const ObsVec &obs) 71 | { 72 | filtered_obs_.clear(); 73 | GTime time; 74 | time.tow_sec = 0.0; 75 | 76 | // Only add observations we have the satellite for 77 | for (auto& o : obs) 78 | { 79 | assert(time.tow_sec == 0.0 || time.tow_sec == o.t.tow_sec); 80 | time = o.t; 81 | int idx = getSatIdx(o.sat); 82 | if (idx >= 0) 83 | { 84 | filtered_obs_.push_back(o); 85 | filtered_obs_.back().sat_idx = idx; 86 | } 87 | } 88 | } 89 | 90 | int Salsa::getSatIdx(int sat_id) const 91 | { 92 | for (auto& s : sats_) 93 | { 94 | if (s.id_ == sat_id) 95 | return s.idx_; 96 | } 97 | return -1; 98 | } 99 | 100 | 101 | 102 | void Salsa::rawGnssCallback(const GTime &t, const VecVec3 &z, const VecMat3 &R, 103 | SatVec &sats, const std::vector& slip) 104 | { 105 | if (disable_gnss_) 106 | return; 107 | 108 | sats_ = sats; 109 | ObsVec obsvec; 110 | for (int i = 0; i < z.size(); i++) 111 | { 112 | Obs new_obs; 113 | new_obs.z = z[i]; 114 | new_obs.LLI = slip[i]; 115 | new_obs.t = t; 116 | new_obs.qualL = R[i](2,2); 117 | new_obs.qualP = R[i](0,0); 118 | new_obs.sat = sats[i].id_; 119 | new_obs.sat_idx = sats_[i].idx_ = i; 120 | obsvec.push_back(new_obs); 121 | } 122 | obsCallback(obsvec); 123 | } 124 | 125 | void Salsa::initializeNodeWithGnss(const meas::Gnss& m, int idx) 126 | { 127 | if (filtered_obs_.size() >= min_sats_ && use_point_positioning_) 128 | { 129 | // Use Iterated Least-Squares to estimate x_e2n and time offset 130 | Vector8d pp_sol = Vector8d::Zero(); 131 | pp_sol.topRows<3>() = x_e2n_.t(); 132 | pointPositioning(m.obs[0].t, m.obs, sats_, pp_sol); 133 | auto phat = pp_sol.segment<3>(0); 134 | auto vhat = pp_sol.segment<3>(3); 135 | auto that = pp_sol.segment<2>(6); 136 | pp_lla_ = phat; 137 | logPointPosLla(); 138 | xbuf_[idx].tau = that; 139 | } 140 | } 141 | 142 | void Salsa::gnssUpdate(const meas::Gnss &m, int idx) 143 | { 144 | SD(2, "Gnss Update on node %d, t=%.3f", xbuf_[idx].node, m.t); 145 | 146 | // Sanity Checks 147 | SALSA_ASSERT((xbuf_[idx].type & State::Gnss) == 0, "Cannot double-up with Gnss nodes"); 148 | initializeNodeWithGnss(m, idx); 149 | 150 | Vector3d rec_pos_ecef = WGS84::ned2ecef(x_e2n_, xbuf_[idx].p); 151 | prange_.emplace_back(m.obs.size()); 152 | int i = 0; 153 | for (auto& ob : m.obs) 154 | { 155 | Matrix2d xi = prange_Xi_; 156 | xi(0,0) *= std::sqrt(1.0/ob.qualP); 157 | xi(1,1) *= std::sqrt(1.0/ob.qualP); 158 | prange_.back()[i++].init(m.obs[0].t, ob.z.topRows<2>(), sats_[ob.sat_idx], rec_pos_ecef, prange_Xi_, 159 | switch_Xi_, p_b2g_, xbuf_[idx].node, idx); 160 | } 161 | xbuf_[idx].type |= State::Gnss; 162 | } 163 | 164 | bool Salsa::initializeStateGnss(const meas::Gnss &m) 165 | { 166 | if (sats_.size() < min_sats_) 167 | { 168 | SD(5, "Waiting for Ephemeris, got %lu sats\n", sats_.size()); 169 | return false; 170 | } 171 | 172 | if (use_point_positioning_) 173 | { 174 | Vector8d pp_sol = Vector8d::Zero(); 175 | pp_sol.topRows<3>() = x_e2n_.t(); 176 | pointPositioning(m.obs[0].t, m.obs, sats_, pp_sol); 177 | auto phat = pp_sol.segment<3>(0); 178 | auto vhat = pp_sol.segment<3>(3); 179 | auto that = pp_sol.segment<2>(6); 180 | pp_lla_ = phat; 181 | logPointPosLla(); 182 | 183 | Xformd x_e2bn = gnss_utils::WGS84::x_ecef2ned(phat); 184 | Xformd x_bn2b(Vector3d::Zero(), x0_.q()); 185 | x_e2n_ = x_e2bn * x_bn2b * x0_.inverse(); 186 | } 187 | initialize(m.t, x0_, Vector3d::Zero(), Vector2d::Zero()); 188 | return true; 189 | } 190 | 191 | void Salsa::obsCallback(const ObsVec &obs) 192 | { 193 | if (!std::isfinite(current_state_.t)) 194 | return; 195 | 196 | if (start_time_.tow_sec < 0) 197 | start_time_ = obs[0].t - current_state_.t - dt_gnss_; 198 | 199 | filterObs(obs); 200 | if (filtered_obs_.size() > 0) 201 | { 202 | GTime& t(filtered_obs_[0].t); 203 | addMeas(meas::Gnss((t - start_time_).toSec(), filtered_obs_)); 204 | } 205 | } 206 | 207 | void Salsa::pointPositioning(const GTime &t, const ObsVec &obs, SatVec &sats, Vector8d &xhat) const 208 | { 209 | const int nobs = obs.size(); 210 | MatrixXd A, b; 211 | A.setZero(nobs*2, 8); 212 | b.setZero(nobs*2, 1); 213 | Vector8d dx; 214 | ColPivHouseholderQR solver; 215 | 216 | int iter = 0; 217 | do 218 | { 219 | int i = 0; 220 | for (auto&& o : obs) 221 | { 222 | Satellite& sat(sats[o.sat_idx]); 223 | Vector3d sat_pos, sat_vel; 224 | Vector2d sat_clk_bias; 225 | auto phat = xhat.segment<3>(0); 226 | auto vhat = xhat.segment<3>(3); 227 | auto that = xhat.segment<2>(6); 228 | GTime tnew = t + that(0); 229 | sat.computePositionVelocityClock(tnew, sat_pos, sat_vel, sat_clk_bias); 230 | 231 | Vector3d zhat ; 232 | sat.computeMeasurement(tnew, phat, vhat, that, zhat); 233 | assert ((zhat.array() == zhat.array()).all()); 234 | b(2*i) = o.z(0) - zhat(0); 235 | b(2*i + 1) = o.z(1) - zhat(1); 236 | 237 | Vector3d e_i = (sat_pos - phat).normalized(); 238 | A.block<1,3>(2*i,0) = -e_i.transpose(); 239 | A(2*i,6) = Satellite::C_LIGHT; 240 | A.block<1,3>(2*i+1,3) = -e_i.transpose(); 241 | A(2*i+1,7) = Satellite::C_LIGHT; 242 | 243 | i++; 244 | } 245 | 246 | solver.compute(A); 247 | dx = solver.solve(b); 248 | 249 | xhat += dx; 250 | } while (dx.norm() > 1e-4 && ++iter < 10); 251 | assert ((xhat.array() == xhat.array()).all()); 252 | } 253 | 254 | } 255 | -------------------------------------------------------------------------------- /src/factors/pseudorange.cpp: -------------------------------------------------------------------------------- 1 | #include "factors/pseudorange.h" 2 | 3 | using namespace Eigen; 4 | using namespace xform; 5 | using namespace gnss_utils; 6 | 7 | #define Tr transpose() 8 | 9 | namespace salsa 10 | { 11 | 12 | PseudorangeFunctor::PseudorangeFunctor() 13 | { 14 | active_ = false; 15 | sw = 1.0; 16 | } 17 | 18 | void PseudorangeFunctor::init(const GTime& _t, const Vector2d& _rho, Satellite& sat, 19 | const Vector3d& _rec_pos_ecef, const Matrix2d& xi, double sw_xi, 20 | const Vector3d &_p_b2g, int node, int idx) 21 | { 22 | sat_id_ = sat.id_; 23 | node_ = node; 24 | idx_ = idx; 25 | p_b2g = _p_b2g; 26 | sw_xi_ = sw_xi; 27 | 28 | // We don't have ephemeris for this satellite, we can't do anything with it yet 29 | if (sat.eph_.A == 0) 30 | return; 31 | 32 | t = _t; 33 | rho = _rho; 34 | rec_pos = _rec_pos_ecef; 35 | sat.computePositionVelocityClock(t, sat_pos, sat_vel, sat_clk); 36 | 37 | Vector3d los_to_sat = sat_pos - rec_pos; 38 | double range = (sat_pos - rec_pos).norm(); 39 | sagnac_comp = Satellite::OMEGA_EARTH * (sat_pos.x()*rec_pos.y() - sat_pos.y()*rec_pos.x())/Satellite::C_LIGHT; 40 | range += sagnac_comp; 41 | double tau = range / Satellite::C_LIGHT; 42 | sat_pos -= sat_vel * tau; 43 | 44 | // Earth rotation correction. The change in velocity can be neglected. 45 | Vector3d earth_rot = sat_pos.cross(e_z*Satellite::OMEGA_EARTH * tau); 46 | sat_pos += earth_rot; 47 | 48 | // pre-calculate the (basically) constant adjustments to pseudorange we will have to make 49 | los_to_sat = sat_pos - rec_pos; 50 | sagnac_comp = Satellite::OMEGA_EARTH * (sat_pos.x()*rec_pos.y() - sat_pos.y()*rec_pos.x())/Satellite::C_LIGHT; 51 | Vector2d az_el = sat.los2azimuthElevation(rec_pos, los_to_sat); 52 | Vector3d lla = WGS84::ecef2lla(rec_pos); 53 | ion_delay = sat.ionosphericDelay(t, lla, az_el); 54 | trop_delay = sat.troposphericDelay(t, lla, az_el); 55 | sat_clk = sat.clk; 56 | Xi_ = xi; 57 | active_ = true; 58 | } 59 | 60 | //#include 61 | //#define DBG(x) printf(#x": %6.6f\n", x); std::cout << std::flush 62 | template 63 | bool PseudorangeFunctor::operator()(const T* _x, const T* _v, const T* _clk, 64 | const T* _x_e2n, const T* _sw, T* _res) const 65 | { 66 | typedef Matrix Vec3; 67 | typedef Matrix Vec2; 68 | 69 | 70 | Xform x(_x); 71 | Map v_b(_v); 72 | Map clk(_clk); 73 | Xform x_e2n(_x_e2n); 74 | const T& k(*_sw); 75 | Map res(_res); 76 | 77 | 78 | Vec3 v_ECEF = x_e2n.rota(x.rota(v_b)); 79 | Vec3 p_ECEF = x_e2n.transforma(x.t()+x.rota(p_b2g)); 80 | Vec3 los_to_sat = sat_pos - p_ECEF; 81 | 82 | Vec2 rho_hat; 83 | T range = los_to_sat.norm() + sagnac_comp; 84 | rho_hat(0) = range + ion_delay + trop_delay + (T)Satellite::C_LIGHT*(clk(0)- sat_clk(0)); 85 | rho_hat(1) = (sat_vel - v_ECEF).dot(los_to_sat/range) 86 | + Satellite::OMEGA_EARTH / Satellite::C_LIGHT * (sat_vel[1]*p_ECEF[0] + sat_pos[1]*v_ECEF[0] - sat_vel[0]*p_ECEF[1] - sat_pos[0]*v_ECEF[1]) 87 | + (T)Satellite::C_LIGHT*(clk(1) - sat_clk(1)); 88 | 89 | res << Xi_ * k * (rho_hat - rho), 90 | sw_xi_ * (1.0 - k); 91 | 92 | 93 | /// TODO: Check if time or rec_pos have deviated too much 94 | /// and re-calculate ion_delay and earth rotation effect 95 | 96 | return true; 97 | } 98 | 99 | template bool PseudorangeFunctor::operator()(const double* _x, const double* _v, const double* _clk, const double* _x_e2n, const double* _s, double* _res) const; 100 | typedef ceres::Jet jactype; 101 | template bool PseudorangeFunctor::operator()(const jactype* _x, const jactype* _v, const jactype* _clk, const jactype* _x_e2n, const jactype* _s, jactype* _res) const; 102 | 103 | PseudorangeFactor::PseudorangeFactor(const PseudorangeFunctor *functor) : 104 | ptr(functor) 105 | {} 106 | 107 | bool PseudorangeFactor::Evaluate(const double * const *parameters, double *residuals, double **jacobians) const 108 | { 109 | Xformd x(parameters[0]); 110 | Map v_b(parameters[1]); 111 | Map clk(parameters[2]); 112 | Xformd x_e2n(parameters[3]); 113 | const double& k(*parameters[4]); 114 | Map res(residuals); 115 | const double& C(Satellite::C_LIGHT); 116 | const double& W(Satellite::OMEGA_EARTH); 117 | 118 | Vector3d v_ECEF = x_e2n.rota(x.rota(v_b)); 119 | Vector3d p_ECEF = x_e2n.transforma(x.t()+x.rota(ptr->p_b2g)); 120 | Vector3d los = ptr->sat_pos - p_ECEF; 121 | double los_norm = los.stableNorm(); 122 | 123 | Vector2d rho_hat; 124 | double range = los_norm + ptr->sagnac_comp; 125 | rho_hat(0) = range 126 | + ptr->ion_delay 127 | + ptr->trop_delay 128 | + C*(clk(0)- ptr->sat_clk(0)); 129 | rho_hat(1) = (ptr->sat_vel - v_ECEF).dot(los/range) 130 | + W / C * (ptr->sat_vel[1]*ptr->rec_pos[0] + ptr->sat_pos[1]*v_ECEF[0] 131 | - ptr->sat_vel[0]*ptr->rec_pos[1] - ptr->sat_pos[0]*v_ECEF[1]) 132 | + C*(clk(1) - ptr->sat_clk(1)); 133 | 134 | res << ptr->Xi_ * k * (rho_hat - ptr->rho), 135 | ptr->sw_xi_ * (1.0 - k); 136 | 137 | if (jacobians) 138 | { 139 | Vector3d e = los.Tr/los_norm; 140 | Matrix3d RE2I = x_e2n.q().R(); 141 | Matrix3d RI2b = x.q().R(); 142 | double xi0 = ptr->Xi_(0,0) * k; 143 | double xi1 = ptr->Xi_(1,1) * k; 144 | // Matrix3d Z = (los*e.Tr - I_3x3*norm)/(norm*norm); 145 | 146 | if (jacobians[0]) 147 | { 148 | Map> dres_dx(jacobians[0]); 149 | Matrix dqdd; 150 | quat::Quatd& q(x.q()); 151 | dqdd << -q.x()*2.0, q.w()*2.0, q.z()*2.0, -q.y()*2.0, 152 | -q.y()*2.0, -q.z()*2.0, q.w()*2.0, q.x()*2.0, 153 | -q.z()*2.0, q.y()*2.0, -q.x()*2.0, q.w()*2.0; 154 | 155 | dres_dx.block<1,3>(0,0) = xi0 * (-e.Tr * RE2I.Tr); 156 | dres_dx.block<1,4>(0,3) = xi0 * (e.Tr * RE2I.Tr * RI2b.Tr * skew(ptr->p_b2g) * dqdd); 157 | dres_dx.block<1,3>(1,0).setZero(); //approx 158 | dres_dx.block<1,4>(1,3) = xi1 * (e.Tr*RE2I.Tr*RI2b.Tr*skew(v_b) * dqdd); // approx 159 | dres_dx.bottomRows<1>().setZero(); 160 | } 161 | 162 | if (jacobians[1]) 163 | { 164 | Map> dres_dv(jacobians[1]); 165 | dres_dv.topRows<1>().setZero(); 166 | dres_dv.row(1) = xi1*(-e.Tr*RE2I.Tr*RI2b.Tr); 167 | dres_dv.bottomRows<1>().setZero(); 168 | } 169 | 170 | if (jacobians[2]) 171 | { 172 | Map> dres_dclk(jacobians[2]); 173 | dres_dclk << xi0*C, 0, 174 | 0, xi1*C, 175 | 0, 0; 176 | 177 | } 178 | 179 | if (jacobians[3]) 180 | { 181 | Map> dres_dx2en(jacobians[3]); 182 | Matrix dqdd; 183 | quat::Quatd& q(x_e2n.q()); 184 | dqdd << -q.x()*2.0, q.w()*2.0, q.z()*2.0, -q.y()*2.0, 185 | -q.y()*2.0, -q.z()*2.0, q.w()*2.0, q.x()*2.0, 186 | -q.z()*2.0, q.y()*2.0, -q.x()*2.0, q.w()*2.0; 187 | 188 | dres_dx2en.block<1,3>(0,0) = xi0 * (-e.Tr); 189 | dres_dx2en.block<1,4>(0,3) = xi0 * (e.Tr*RE2I.Tr*skew(x.t() + RI2b.Tr*ptr->p_b2g) * dqdd); 190 | dres_dx2en.block<1,3>(1,0).setZero(); //approx 191 | dres_dx2en.block<1,4>(1,3) = xi1 * (e.Tr*RE2I.Tr*skew(RI2b.Tr*v_b)*dqdd); //approx 192 | dres_dx2en.bottomRows<1>().setZero(); 193 | } 194 | 195 | if (jacobians[4]) 196 | { 197 | Map> dres_ds(jacobians[4]); 198 | dres_ds << ptr->Xi_ * (rho_hat - ptr->rho), 199 | -ptr->sw_xi_; 200 | } 201 | } 202 | 203 | return true; 204 | } 205 | 206 | 207 | } 208 | -------------------------------------------------------------------------------- /src/salsa_vision.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa.h" 2 | 3 | using namespace std; 4 | using namespace Eigen; 5 | using namespace xform; 6 | using namespace quat; 7 | 8 | namespace salsa 9 | { 10 | 11 | bool Salsa::isTrackedFeature(int id) const 12 | { 13 | FeatMap::const_iterator it = xfeat_.begin(); 14 | while (it != xfeat_.end() && id != it->first) 15 | ++it; 16 | return it != xfeat_.end(); 17 | } 18 | 19 | void Salsa::imageCallback(const double& tc, const ImageFeat& z, 20 | const Matrix2d& R_pix, const Matrix1d& R_depth) 21 | { 22 | SD(3, "IMG CB t%.3f", tc); 23 | if (disable_vision_) 24 | return; 25 | 26 | if (enable_static_start_ && (lt(tc, static_start_end_ + camera_start_delay_))) 27 | { 28 | SD(2, "Waiting for Camera delay after static start"); 29 | return; 30 | } 31 | 32 | 33 | if (sim_KLT_) 34 | { 35 | SD(1, "Simulating KLT"); 36 | if (current_img_.empty()) 37 | { 38 | current_img_.create(cv::Size(cam_.image_size_(0), cam_.image_size_(1)), 0); 39 | } 40 | current_img_ = 0; 41 | for (auto pix : z.pixs) 42 | { 43 | int d = 2; 44 | cv::Point2d xp(pix(0)+d, pix(1)); 45 | cv::Point2d xm(pix(0)-d, pix(1)); 46 | cv::Point2d yp(pix(0), pix(1)+d); 47 | cv::Point2d ym(pix(0), pix(1)-d); 48 | cv::line(current_img_, xp, xm, 255); 49 | cv::line(current_img_, yp, ym, 255); 50 | } 51 | imageCallback(tc, current_img_, R_pix); 52 | } 53 | else 54 | { 55 | Features zfeat; 56 | zfeat.id = z.id; 57 | zfeat.t = z.t; 58 | zfeat.feat_ids = z.feat_ids; 59 | zfeat.depths = z.depths; 60 | zfeat.zetas.reserve(z.pixs.size()); 61 | zfeat.pix.reserve(z.pixs.size()); 62 | for (auto pix : z.pixs) 63 | { 64 | zfeat.zetas.emplace_back(cam_.invProj(pix, 1.0)); 65 | zfeat.pix.emplace_back(pix.x(), pix.y()); 66 | } 67 | bool new_keyframe = calcNewKeyframeCondition(zfeat); 68 | if (!disable_vision_) 69 | addMeas(meas::Img(tc, zfeat, R_pix, new_keyframe)); 70 | } 71 | } 72 | 73 | void Salsa::initializeStateImage(const meas::Img &m) 74 | { 75 | initialize(m.t, x0_, v0_, Vector2d::Zero()); 76 | } 77 | 78 | void Salsa::imageUpdate(const meas::Img &m, int idx) 79 | { 80 | // Sanity Checks 81 | SALSA_ASSERT(xbuf_[idx].kf < 0, "Cannot add measuremnts to keyframe"); 82 | SALSA_ASSERT(lastKfId() > 0 ? (xbuf_[idx].node > xbuf_[lastKfId()].node) : true, 83 | "Cannot add image measurements before keyframe"); 84 | 85 | 86 | for (auto& ft : xfeat_) 87 | ft.second.updated_in_last_image_ = false; 88 | 89 | SD(2, "Image Update, t=%.3f", m.t); 90 | for (int i = 0; i < m.z.zetas.size(); i++) 91 | { 92 | if (isTrackedFeature(m.z.feat_ids[i])) 93 | { 94 | Feat& ft(xfeat_.at(m.z.feat_ids[i])); 95 | if (ft.funcs.size() == 0 || (xbuf_[ft.funcs.back().to_idx_].kf >= 0)) 96 | { 97 | SD(1, "Adding new measurement to feature %d", m.z.feat_ids[i]); 98 | ft.addMeas(m.t, idx, m.R_pix, x_b2c_, m.z.zetas[i], Vector2d(m.z.pix[i].x, m.z.pix[i].y)); 99 | } 100 | else 101 | { 102 | SD(1, "Moving feature measurement %d", m.z.feat_ids[i]); 103 | ft.moveMeas(m.t, idx, m.z.zetas[i]); 104 | } 105 | ft.updated_in_last_image_ = true; 106 | ft.funcs.back().rho_true_ = 1.0/m.z.depths[i]; 107 | } 108 | else if (m.new_keyframe) 109 | { 110 | double rho0 = 1.0/max_depth_; 111 | if (use_measured_depth_ && !std::isnan(m.z.depths[i])) 112 | rho0 = 1.0/m.z.depths[i]; 113 | SD(1, "Adding new feature %d", m.z.feat_ids[i]); 114 | xfeat_.insert({m.z.feat_ids[i], Feat(m.t, xbuf_head_, current_kf_+1, m.z.zetas[i], Vector2d(m.z.pix[i].x, m.z.pix[i].y), rho0, 1.0/m.z.depths[i])}); 115 | } 116 | } 117 | xbuf_[idx].type |= State::Camera; 118 | xbuf_[idx].n_cam++;; 119 | 120 | if (m.new_keyframe) 121 | { 122 | setNewKeyframe(idx); 123 | SD(2, "New Keyframe %d", current_kf_); 124 | } 125 | rmLostFeatFromKf(); 126 | } 127 | 128 | bool Salsa::calcNewKeyframeCondition(const Features &z) 129 | { 130 | if (current_kf_ == -1) 131 | { 132 | kf_condition_ = FIRST_KEYFRAME; 133 | kf_feat_ = z; 134 | kf_num_feat_ = z.feat_ids.size(); 135 | SD(2, "new keyframe, first image"); 136 | return true; 137 | } 138 | 139 | kf_parallax_ = 0; 140 | kf_Nmatch_feat_ = 0; 141 | Quatd q_I2i(lastKfState().x.q()); 142 | Quatd q_I2j(current_state_.x.q()); 143 | Quatd q_b2c(x_b2c_.q()); 144 | Quatd q_cj2ci = q_b2c.inverse() * q_I2j.inverse() * q_I2i * q_b2c; 145 | Matrix3d R_cj2ci = q_cj2ci.R(); 146 | int ni = 0; 147 | int nj = 0; 148 | while (ni < kf_feat_.zetas.size() && nj < z.zetas.size()) 149 | { 150 | int idi = kf_feat_.feat_ids[ni]; 151 | int idj = z.feat_ids[nj]; 152 | if (idi == idj) 153 | { 154 | Vector3d zihat = R_cj2ci * z.zetas[nj]; 155 | Vector2d lihat = cam_.proj(zihat); 156 | Vector2d li(kf_feat_.pix[ni].x, kf_feat_.pix[ni].y); 157 | double err = (lihat - li).norm(); 158 | SD(1, "pix error %.2f. lihat = (%f,%f), li = (%f,%f)", err, lihat.x(), lihat.y(), li.x(), li.y()); 159 | kf_parallax_ += err; 160 | ni++; 161 | nj++; 162 | kf_Nmatch_feat_++; 163 | } 164 | else if (idi < idj) 165 | ni++; 166 | else if (idi > idj) 167 | nj++; 168 | } 169 | kf_parallax_ /= kf_Nmatch_feat_; 170 | 171 | if (kf_parallax_ > kf_parallax_thresh_) 172 | { 173 | kf_condition_ = TOO_MUCH_PARALLAX; 174 | kf_feat_ = z; 175 | kf_num_feat_ = z.feat_ids.size(); 176 | SD(2, "new keyframe, too much parallax: = %f, nmatch = %d", kf_parallax_, kf_Nmatch_feat_); 177 | return true; 178 | } 179 | else if(kf_Nmatch_feat_ <= std::round(kf_feature_thresh_ * kf_num_feat_) + 0.001) 180 | { 181 | kf_condition_ = INSUFFICIENT_MATCHES; 182 | kf_feat_ = z; 183 | kf_num_feat_ = z.feat_ids.size(); 184 | SD(2, "new keyframe, not enough matches: = %d/%f", 185 | kf_Nmatch_feat_, std::round(kf_feature_thresh_ * kf_num_feat_)); 186 | return true; 187 | } 188 | else 189 | return false; 190 | } 191 | 192 | void Salsa::cleanUpFeatureTracking(int oldest_kf_idx) 193 | { 194 | FeatMap::iterator fit = xfeat_.begin(); 195 | while (fit != xfeat_.end()) 196 | { 197 | SD(1, "Attempting to Slide anchor for Feature %d, %d->%d", fit->first, fit->second.idx0, oldest_kf_idx); 198 | if (!fit->second.slideAnchor(oldest_kf_idx, xbuf_, x_b2c_)) 199 | { 200 | SD(1, "Unable to slide, removing feature %d", fit->first); 201 | fit = xfeat_.erase(fit); 202 | } 203 | else 204 | fit++; 205 | } 206 | SALSA_ASSERT(checkFeatures(), "Features Lost Wiring"); 207 | } 208 | 209 | void Salsa::createNewKeyframe() 210 | { 211 | filterFeaturesTooClose(track_feature_min_distance_); 212 | collectNewfeatures(); 213 | kf_feat_ = current_feat_; 214 | kf_num_feat_ = kf_feat_.size(); 215 | SD(2, "Creating new Keyframe with %d features", kf_feat_.size()); 216 | current_img_.copyTo(kf_img_); 217 | } 218 | 219 | void Salsa::rmLostFeatFromKf() 220 | { 221 | FeatMap::iterator ftpair = xfeat_.begin(); 222 | while (ftpair != xfeat_.end()) 223 | { 224 | Feat& ft(ftpair->second); 225 | if (ft.funcs.size() == 0) 226 | { 227 | ftpair++; 228 | continue; 229 | } 230 | else if (!ft.updated_in_last_image_ 231 | && (ft.funcs.back().to_idx_ == xbuf_head_ || xbuf_[ft.funcs.back().to_idx_].kf < 0)) 232 | { 233 | SD(1, "Feature %d not tracked", ftpair->first); 234 | ft.funcs.pop_back(); 235 | if (ft.funcs.size() == 0) 236 | { 237 | SD(2, "removing feature %d", ftpair->first); 238 | ftpair = xfeat_.erase(ftpair); 239 | continue; 240 | } 241 | } 242 | ftpair++; 243 | } 244 | } 245 | 246 | int Salsa::numTotalFeat() const 247 | { 248 | int n_feat_meas = 0; 249 | for (auto& ft : xfeat_) 250 | { 251 | n_feat_meas += 1 + ft.second.funcs.size(); 252 | } 253 | return n_feat_meas; 254 | } 255 | 256 | void Salsa::fixDepth() 257 | { 258 | for (auto& ft : xfeat_) 259 | { 260 | if (ft.second.rho < 1.0/max_depth_) 261 | { 262 | ft.second.rho = 1.0/(2.0 * max_depth_); 263 | } 264 | } 265 | } 266 | 267 | 268 | 269 | } 270 | -------------------------------------------------------------------------------- /include/salsa/test_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define DEG2RAD (M_PI / 180.0) 9 | #define RAD2DEG (180.0 / M_PI) 10 | 11 | #define ASSERT_MAT_EQ(v1, v2) \ 12 | { \ 13 | ASSERT_EQ((v1).rows(), (v2).rows()); \ 14 | ASSERT_EQ((v1).cols(), (v2).cols()); \ 15 | for (int row = 0; row < (v1).rows(); row++) {\ 16 | for (int col = 0; col < (v2).cols(); col++) {\ 17 | ASSERT_FLOAT_EQ((v1)(row, col), (v2)(row,col));\ 18 | }\ 19 | }\ 20 | } 21 | 22 | #define ASSERT_MAT_NEAR(v1, v2, tol) \ 23 | { \ 24 | ASSERT_EQ((v1).rows(), (v2).rows()); \ 25 | ASSERT_EQ((v1).cols(), (v2).cols()); \ 26 | for (int row = 0; row < (v1).rows(); row++) {\ 27 | for (int col = 0; col < (v2).cols(); col++) {\ 28 | if (std::abs((v1)(row, col) - (v2)(row,col)) > (tol)) \ 29 | cout << "[ " << row << ", " << col << " ] ";\ 30 | ASSERT_NEAR((v1)(row, col), (v2)(row,col), (tol));\ 31 | }\ 32 | }\ 33 | } 34 | 35 | #define EXPECT_MAT_NEAR(v1, v2, tol) \ 36 | { \ 37 | EXPECT_EQ((v1).rows(), (v2).rows()); \ 38 | EXPECT_EQ((v1).cols(), (v2).cols()); \ 39 | for (int row = 0; row < (v1).rows(); row++) {\ 40 | for (int col = 0; col < (v2).cols(); col++) {\ 41 | if (std::abs((v1)(row, col) - (v2)(row,col)) > tol) \ 42 | std::cout << "[ " << row << ", " << col << " ] ";\ 43 | EXPECT_NEAR((v1)(row, col), (v2)(row,col), (tol));\ 44 | }\ 45 | }\ 46 | } 47 | 48 | #define ASSERT_XFORM_NEAR(x1, x2, tol) \ 49 | { \ 50 | ASSERT_NEAR((x1).t()(0), (x2).t()(0), tol);\ 51 | ASSERT_NEAR((x1).t()(1), (x2).t()(1), tol);\ 52 | ASSERT_NEAR((x1).t()(2), (x2).t()(2), tol);\ 53 | ASSERT_NEAR((x1).q().w(), (x2).q().w(), tol);\ 54 | ASSERT_NEAR((x1).q().x(), (x2).q().x(), tol);\ 55 | ASSERT_NEAR((x1).q().y(), (x2).q().y(), tol);\ 56 | ASSERT_NEAR((x1).q().z(), (x2).q().z(), tol);\ 57 | } 58 | 59 | #define ASSERT_QUAT_NEAR(q1, q2, tol) \ 60 | do { \ 61 | Vector3d qt = (q1) - (q2);\ 62 | ASSERT_LE(std::abs(qt(0)), tol);\ 63 | ASSERT_LE(std::abs(qt(1)), tol);\ 64 | ASSERT_LE(std::abs(qt(2)), tol);\ 65 | } while(0) 66 | 67 | #define EXPECT_QUAT_NEAR(q1, q2, tol) \ 68 | do { \ 69 | Vector3d qt = (q1) - (q2);\ 70 | EXPECT_LE(std::abs(qt(0)), tol);\ 71 | EXPECT_LE(std::abs(qt(1)), tol);\ 72 | EXPECT_LE(std::abs(qt(2)), tol);\ 73 | } while(0) 74 | 75 | 76 | #define EXPECT_MAT_FINITE(mat) \ 77 | do {\ 78 | for (int c = 0; c < (mat).cols(); c++) \ 79 | { \ 80 | for (int r = 0; r < (mat).rows(); r++) \ 81 | { \ 82 | EXPECT_TRUE(std::isfinite((mat)(r,c))); \ 83 | } \ 84 | }\ 85 | } while(0) 86 | 87 | #define EXPECT_MAT_NAN(mat) \ 88 | do {\ 89 | for (int c = 0; c < (mat).cols(); c++) \ 90 | { \ 91 | for (int r = 0; r < (mat).rows(); r++) \ 92 | { \ 93 | EXPECT_TRUE(std::isnan((mat)(r,c))); \ 94 | } \ 95 | }\ 96 | } while(0) 97 | 98 | 99 | #define EXPECT_FINITE(val) EXPECT_TRUE(std::isfinite(val)) 100 | 101 | 102 | inline std::string default_params(const std::string& prefix, std::string label="test") 103 | { 104 | std::string filename = "/tmp/Salsa.default.yaml"; 105 | std::ofstream tmp(filename); 106 | YAML::Node node = YAML::LoadFile(SALSA_DIR"/params/salsa.yaml"); 107 | node["x_b2m"] = std::vector{0, 0, 0, 1, 0, 0, 0}; 108 | node["x_b2o"] = std::vector{0, 0, 0, 1, 0, 0, 0}; 109 | node["x_b2c"] = std::vector{0, 0, 0, 1, 0, 0, 0}; 110 | node["tm"] = 0.0; 111 | node["tc"] = 0.0; 112 | node["log_prefix"] = prefix; 113 | node["enable_out_of_order"] = false; 114 | node["label"] = label; 115 | node["simulate_klt"] = false; 116 | node["enable_static_start"] = false; 117 | node["max_kf_window"] = 10; 118 | tmp << node; 119 | tmp.close(); 120 | return filename; 121 | } 122 | 123 | inline std::string small_feat_test(const std::string& prefix, bool init_depth=true) 124 | { 125 | std::string filename = "/tmp/Salsa.smallfeat.yaml"; 126 | std::ofstream tmp(filename); 127 | YAML::Node node = YAML::LoadFile(SALSA_DIR"/params/salsa.yaml"); 128 | node["x_b2m"] = std::vector{0, 0, 0, 1, 0, 0, 0}; 129 | node["x_b2c"] = std::vector{0, 0, 0, 1, 0, 0, 0}; 130 | node["x_b2o"] = std::vector{0, 0, 0, 1, 0, 0, 0}; 131 | node["tm"] = 0.0; 132 | node["tc"] = 0.0; 133 | node["log_prefix"] = prefix; 134 | node["N"] = 4; 135 | node["kf_feature_thresh"] = 0.80; 136 | node["kf_parallax_thresh"] = 500; 137 | node["num_feat"] = 4; 138 | node["use_measured_depth"] = init_depth; 139 | node["enable_out_of_order"] = false; 140 | node["enable_static_start"] = false; 141 | tmp << node; 142 | tmp.close(); 143 | return filename; 144 | } 145 | 146 | inline std::string imu_only() 147 | { 148 | std::string filename = "/tmp/Salsa.imu_only.yaml"; 149 | std::ofstream tmp(filename); 150 | YAML::Node node = YAML::LoadFile(SALSA_DIR"/params/sim_params.yaml"); 151 | node["imu_enabled"] = true; 152 | node["alt_enabled"] = false; 153 | node["baro_enabled"] = false; 154 | node["mocap_enabled"] = false; 155 | node["vo_enabled"] = false; 156 | node["camera_enabled"] = false; 157 | node["gnss_enabled"] = false; 158 | node["raw_gnss_enabled"] = false; 159 | node["enable_out_of_order"] = false; 160 | 161 | tmp << node; 162 | tmp.close(); 163 | return filename; 164 | } 165 | 166 | inline std::string imu_mocap(bool noise=true) 167 | { 168 | std::string filename = "/tmp/Salsa.imu_only.yaml"; 169 | std::ofstream tmp(filename); 170 | YAML::Node node = YAML::LoadFile(SALSA_DIR"/params/sim_params.yaml"); 171 | node["imu_enabled"] = true; 172 | node["alt_enabled"] = false; 173 | node["baro_enabled"] = false; 174 | node["mocap_enabled"] = true; 175 | node["vo_enabled"] = false; 176 | node["camera_enabled"] = false; 177 | node["gnss_enabled"] = false; 178 | node["raw_gnss_enabled"] = false; 179 | node["enable_out_of_order"] = false; 180 | 181 | if (!noise) 182 | { 183 | node["use_accel_truth"] = !noise; 184 | node["use_mocap_truth"] = !noise; 185 | node["use_gyro_truth"] = !noise; 186 | node["use_camera_truth"] = !noise; 187 | node["use_depth_truth"] = !noise; 188 | node["use_gnss_truth"] = !noise; 189 | node["use_raw_gnss_truth"] = !noise; 190 | } 191 | 192 | 193 | tmp << node; 194 | tmp.close(); 195 | return filename; 196 | } 197 | 198 | inline std::string imu_feat(bool noise=true, double tmax=-1.0) 199 | { 200 | std::string filename = "/tmp/Salsa.imu_only.yaml"; 201 | std::ofstream tmp(filename); 202 | YAML::Node node = YAML::LoadFile(SALSA_DIR"/params/sim_params.yaml"); 203 | if (tmax > 0) 204 | node["tmax"] = tmax; 205 | node["imu_enabled"] = true; 206 | node["alt_enabled"] = false; 207 | node["baro_enabled"] = false; 208 | node["mocap_enabled"] = false; 209 | node["vo_enabled"] = false; 210 | node["camera_enabled"] = true; 211 | node["gnss_enabled"] = false; 212 | node["raw_gnss_enabled"] = false; 213 | node["enable_out_of_order"] = false; 214 | 215 | if (!noise) 216 | { 217 | node["use_accel_truth"] = !noise; 218 | node["use_gyro_truth"] = !noise; 219 | node["use_camera_truth"] = !noise; 220 | node["use_depth_truth"] = !noise; 221 | node["use_gnss_truth"] = !noise; 222 | node["use_raw_gnss_truth"] = !noise; 223 | } 224 | 225 | tmp << node; 226 | tmp.close(); 227 | return filename; 228 | } 229 | 230 | inline std::string imu_raw_gnss(bool noise=true, double tmax=-1.0) 231 | { 232 | std::string filename = "/tmp/Salsa.imu_raw_gnss.yaml"; 233 | std::ofstream tmp(filename); 234 | YAML::Node node = YAML::LoadFile(SALSA_DIR"/params/sim_params.yaml"); 235 | if (tmax > 0) 236 | node["tmax"] = tmax; 237 | node["imu_enabled"] = true; 238 | node["alt_enabled"] = false; 239 | node["baro_enabled"] = false; 240 | node["mocap_enabled"] = false; 241 | node["vo_enabled"] = false; 242 | node["camera_enabled"] = false; 243 | node["gnss_enabled"] = false; 244 | node["raw_gnss_enabled"] = true; 245 | node["ephemeris_filename"] = SALSA_DIR"/sample/eph.dat"; 246 | node["enable_out_of_order"] = false; 247 | 248 | if (!noise) 249 | { 250 | node["use_accel_truth"] = !noise; 251 | node["use_gyro_truth"] = !noise; 252 | node["use_camera_truth"] = !noise; 253 | node["use_depth_truth"] = !noise; 254 | node["use_gnss_truth"] = !noise; 255 | node["use_raw_gnss_truth"] = !noise; 256 | } 257 | 258 | tmp << node; 259 | tmp.close(); 260 | return filename; 261 | } 262 | 263 | inline std::string imu_feat_gnss(bool noise=true, double tmax=-1.0) 264 | { 265 | std::string filename = "/tmp/Salsa.imu_only.yaml"; 266 | std::ofstream tmp(filename); 267 | YAML::Node node = YAML::LoadFile(SALSA_DIR"/params/sim_params.yaml"); 268 | if (tmax > 0) 269 | node["tmax"] = tmax; 270 | node["imu_enabled"] = true; 271 | node["alt_enabled"] = false; 272 | node["baro_enabled"] = false; 273 | node["mocap_enabled"] = false; 274 | node["vo_enabled"] = false; 275 | node["camera_enabled"] = true; 276 | node["gnss_enabled"] = false; 277 | node["raw_gnss_enabled"] = true; 278 | node["ephemeris_filename"] = SALSA_DIR"/sample/eph.dat"; 279 | node["enable_out_of_order"] = false; 280 | 281 | if (!noise) 282 | { 283 | node["use_accel_truth"] = !noise; 284 | node["use_gyro_truth"] = !noise; 285 | node["use_camera_truth"] = !noise; 286 | node["use_depth_truth"] = !noise; 287 | node["use_gnss_truth"] = !noise; 288 | node["use_raw_gnss_truth"] = !noise; 289 | } 290 | 291 | tmp << node; 292 | tmp.close(); 293 | return filename; 294 | } 295 | -------------------------------------------------------------------------------- /src/graph.cpp: -------------------------------------------------------------------------------- 1 | #include "salsa/salsa.h" 2 | 3 | using namespace std; 4 | using namespace Eigen; 5 | using namespace xform; 6 | 7 | namespace salsa 8 | { 9 | 10 | void Salsa::initFactors() 11 | { 12 | state_anchor_ = new StateAnchor(state_anchor_xi_); 13 | x_e2n_anchor_ = new XformAnchor(x_e2n_anchor_xi_); 14 | } 15 | 16 | void Salsa::addParameterBlocks(ceres::Problem &problem) 17 | { 18 | problem.AddParameterBlock(x_e2n_.data(), 7, new XformParam()); 19 | if (!enable_static_start_ || xhead().t > static_start_end_ + static_start_fix_delay_) 20 | problem.SetParameterBlockConstant(x_e2n_.data()); 21 | 22 | int idx = xbuf_tail_; 23 | int prev_idx = -1; 24 | while (prev_idx != xbuf_head_) 25 | { 26 | SALSA_ASSERT(std::abs(1.0 - xbuf_[idx].x.q().arr_.norm()) < 1e-8, "Quat Left Manifold"); 27 | problem.AddParameterBlock(xbuf_[idx].x.data(), 7, new XformParam()); 28 | problem.AddParameterBlock(xbuf_[idx].v.data(), 3); 29 | problem.AddParameterBlock(xbuf_[idx].tau.data(), 2); 30 | prev_idx = idx; 31 | idx = (idx+1) % STATE_BUF_SIZE; 32 | } 33 | 34 | 35 | for (auto& feat : xfeat_) 36 | { 37 | Feat& ft(feat.second); 38 | if (ft.funcs.size() > 0) 39 | { 40 | problem.AddParameterBlock(&ft.rho, 1); 41 | problem.SetParameterLowerBound(&ft.rho, 0, 0.01); 42 | } 43 | } 44 | 45 | for (auto& pvec : prange_) 46 | { 47 | for (auto& p : pvec) 48 | { 49 | problem.AddParameterBlock(&p.sw, 1); 50 | problem.SetParameterLowerBound(&p.sw, 0, 0); 51 | problem.SetParameterUpperBound(&p.sw, 0, 1); 52 | if (!enable_switching_factors_) 53 | problem.SetParameterBlockConstant(&p.sw); 54 | } 55 | } 56 | } 57 | 58 | void Salsa::setAnchors(ceres::Problem &problem) 59 | { 60 | if (xbuf_tail_ == xbuf_head_) 61 | return; 62 | 63 | x_e2n_anchor_->set(x_e2n_); 64 | FunctorShield* xe2n_ptr = new FunctorShield(x_e2n_anchor_); 65 | problem.AddResidualBlock(new XformAnchorFactorAD(xe2n_ptr), NULL, x_e2n_.data()); 66 | 67 | // if (!enable_static_start_ || current_state_.t > static_start_end_) 68 | // { 69 | state_anchor_->set(xtail()); 70 | FunctorShield* state_ptr = new FunctorShield(state_anchor_); 71 | problem.AddResidualBlock(new StateAnchorFactorAD(state_ptr), NULL, xbuf_[xbuf_tail_].x.data(), 72 | xbuf_[xbuf_tail_].v.data(), xbuf_[xbuf_tail_].tau.data(), 73 | xbuf_[xbuf_tail_].bias.data()); 74 | // } 75 | } 76 | 77 | void Salsa::addImuFactors(ceres::Problem &problem) 78 | { 79 | int prev_idx = xbuf_tail_; 80 | for (auto it = imu_.begin(); it != imu_.end(); it++) 81 | { 82 | // ignore unfinished IMU factors 83 | if (it->to_idx_ < 0) 84 | continue; 85 | 86 | 87 | SALSA_ASSERT(it->from_idx_ == prev_idx, "Out-of-order IMU intervals, expected: %d, got %d, tail: %d, head: %d", 88 | prev_idx, it->from_idx_, xbuf_tail_, xbuf_head_); 89 | SALSA_ASSERT(it->to_idx_ == (it->from_idx_ + 1) % STATE_BUF_SIZE, "Skipping States! from %d to %d ", it->from_idx_, it->to_idx_); 90 | prev_idx = it->to_idx_; 91 | SALSA_ASSERT(inWindow(it->to_idx_), "Trying to add IMU factor to node outside of window"); 92 | SALSA_ASSERT(inWindow(it->from_idx_), "Trying to add IMU factor to node outside of window"); 93 | SALSA_ASSERT(std::abs(1.0 - xbuf_[it->to_idx_].x.q().arr_.norm()) < 1e-8, "Quat Left Manifold"); 94 | FunctorShield* ptr = new FunctorShield(&*it); 95 | problem.AddResidualBlock(new ImuFactorAD(ptr), 96 | NULL, 97 | xbuf_[it->from_idx_].x.data(), 98 | xbuf_[it->to_idx_].x.data(), 99 | xbuf_[it->from_idx_].v.data(), 100 | xbuf_[it->to_idx_].v.data(), 101 | xbuf_[it->from_idx_].bias.data(), 102 | xbuf_[it->to_idx_].bias.data()); 103 | } 104 | SALSA_ASSERT(prev_idx == xbuf_head_, "not enough intervals"); 105 | } 106 | 107 | void Salsa::addMocapFactors(ceres::Problem &problem) 108 | { 109 | for (auto it = mocap_.begin(); it != mocap_.end(); it++) 110 | { 111 | SALSA_ASSERT(inWindow(it->idx_), "Trying to add Mocap factor to node outside of window"); 112 | FunctorShield* ptr = new FunctorShield(&*it); 113 | problem.AddResidualBlock(new MocapFactorAD(ptr), 114 | NULL, 115 | xbuf_[it->idx_].x.data()); 116 | } 117 | } 118 | 119 | void Salsa::addRawGnssFactors(ceres::Problem &problem) 120 | { 121 | if (disable_gnss_) 122 | return; 123 | 124 | std::unordered_map prev_sw; 125 | for (auto pvec = prange_.begin(); pvec != prange_.end(); pvec++) 126 | { 127 | for (int i = 0; i < pvec->size(); i++) 128 | { 129 | PseudorangeFunctor &p((*pvec)[i]); 130 | if(!inWindow(p.idx_)) 131 | { 132 | SD(5, "Trying to add GNSS factor to node outside of window. idx=%d, tail=%d, head=%d", p.idx_, xbuf_tail_, xbuf_head_); 133 | continue; 134 | } 135 | problem.AddResidualBlock(new PseudorangeFactor(&p), 136 | NULL, 137 | xbuf_[p.idx_].x.data(), 138 | xbuf_[p.idx_].v.data(), 139 | xbuf_[p.idx_].tau.data(), 140 | x_e2n_.data(), 141 | &(p.sw)); 142 | 143 | // If we had a previous switching factor on this satellite, 144 | // then add the switching factor dynamics 145 | auto prev_sw_it = prev_sw.find(p.sat_id_); 146 | if (prev_sw_it != prev_sw.end()) 147 | { 148 | problem.AddResidualBlock(new SwitchFactor(switchdot_Xi_), 149 | NULL, 150 | &(p.sw), 151 | prev_sw_it->second); 152 | } 153 | prev_sw[p.sat_id_] = &(p.sw); 154 | } 155 | 156 | } 157 | for (auto it = clk_.begin(); it != clk_.end(); it++) 158 | { 159 | if (it->to_idx_ < 0) 160 | continue; 161 | FunctorShield* ptr = new FunctorShield(&*it); 162 | problem.AddResidualBlock(new ClockBiasFactorAD(ptr), 163 | NULL, 164 | xbuf_[it->from_idx_].tau.data(), 165 | xbuf_[it->to_idx_].tau.data()); 166 | } 167 | 168 | } 169 | 170 | void Salsa::addFeatFactors(ceres::Problem &problem) 171 | { 172 | if (disable_vision_) 173 | return; 174 | 175 | FeatMap::iterator ft = xfeat_.begin(); 176 | while (ft != xfeat_.end()) 177 | { 178 | if (ft->second.funcs.size() < 2) 179 | { 180 | ft++; 181 | continue; 182 | } 183 | FeatDeque::iterator func = ft->second.funcs.begin(); 184 | while (func != ft->second.funcs.end()) 185 | { 186 | SALSA_ASSERT(inWindow(ft->second.idx0), "Trying to add factor to node outside of window: %d", ft->second.idx0); 187 | SALSA_ASSERT(inWindow(func->to_idx_), "Trying to add factor to node outside of window: %d", func->to_idx_); 188 | FeatFactor* ptr = new FeatFactor(&*func); 189 | problem.AddResidualBlock(ptr, 190 | new ceres::CauchyLoss(1.0), 191 | // NULL, 192 | xbuf_[ft->second.idx0].x.data(), 193 | xbuf_[func->to_idx_].x.data(), 194 | &ft->second.rho); 195 | func++; 196 | } 197 | ft++; 198 | } 199 | } 200 | 201 | void Salsa::addZeroVelFactors(ceres::Problem &problem) 202 | { 203 | if (!enable_static_start_) 204 | return; 205 | 206 | int end = (xbuf_head_ + 1) % STATE_BUF_SIZE; 207 | for (int idx = xbuf_tail_; idx != end; idx = (idx + 1) % STATE_BUF_SIZE) 208 | { 209 | if (xbuf_[idx].t > static_start_end_) 210 | break; 211 | else 212 | { 213 | ZeroVelFunctor* ptr = new ZeroVelFunctor(x0_, v0_, zero_vel_Xi_); 214 | // problem.AddResidualBlock(new ZeroVelFactorAD(ptr), 215 | // NULL, 216 | // xbuf_[idx].x.data(), 217 | // xbuf_[idx].v.data()); 218 | } 219 | } 220 | 221 | } 222 | 223 | void Salsa::initSolverOptions() 224 | { 225 | options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 226 | options_.minimizer_progress_to_stdout = false ; 227 | } 228 | 229 | void Salsa::solve() 230 | { 231 | ceres::Problem* problem = new ceres::Problem(); 232 | 233 | addParameterBlocks(*problem); 234 | setAnchors(*problem); 235 | addImuFactors(*problem); 236 | addFeatFactors(*problem); 237 | addMocapFactors(*problem); 238 | addRawGnssFactors(*problem); 239 | addZeroVelFactors(*problem); 240 | 241 | if (!disable_solver_) 242 | ceres::Solve(options_, problem, &summary_); 243 | 244 | fixDepth(); 245 | 246 | if (summary_.IsSolutionUsable()) 247 | // std::cout << summary_.FullReport() << std::endl; 248 | 249 | delete problem; 250 | 251 | SD_S(3, "Finished Solve Iteration t: " << xhead().t << " p: [" << xhead().x.t().transpose() 252 | << "] att: [" << xhead().x.q().euler().transpose() << "]"); 253 | 254 | logState(); 255 | logOptimizedWindow(); 256 | logRawGNSSRes(); 257 | logFeatRes(); 258 | logMocapRes(); 259 | logFeatures(); 260 | logSatPos(); 261 | logPrangeRes(); 262 | } 263 | 264 | } 265 | -------------------------------------------------------------------------------- /src/test/test_imu_factor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "factors/shield.h" 9 | #include "factors/xform.h" 10 | #include "factors/imu.h" 11 | #include "factors/mocap.h" 12 | 13 | #include "geometry/xform.h" 14 | #include "geometry/support.h" 15 | #include "multirotor_sim/simulator.h" 16 | #include "multirotor_sim/controller.h" 17 | #include "multirotor_sim/estimator_wrapper.h" 18 | #include "salsa/num_diff.h" 19 | #include "salsa/logger.h" 20 | #include "salsa/test_common.h" 21 | 22 | using namespace ceres; 23 | using namespace Eigen; 24 | using namespace std; 25 | using namespace xform; 26 | using namespace quat; 27 | using namespace multirotor_sim; 28 | using namespace salsa; 29 | 30 | TEST(ImuFactor, compile) 31 | { 32 | ImuFunctor imu(0.0, Vector6d::Zero(), Matrix6d::Identity(), 0, 0); 33 | } 34 | 35 | TEST (ImuFactor, splitInMiddle) 36 | { 37 | Vector6d z; 38 | z << 0, 0, -9.80665, 0, 0, 0; 39 | Matrix6d R = Matrix6d::Identity(); 40 | ImuFunctor imu(0.0, Vector6d::Zero(), Matrix6d::Identity(), 0, 0); 41 | imu.integrate(0.01, z, R); 42 | imu.integrate(0.02, z, R); 43 | imu.integrate(0.03, z, R); 44 | imu.integrate(0.04, z, R); 45 | imu.integrate(0.05, z, R); 46 | ImuFunctor f0 = imu.split(0.025); 47 | 48 | EXPECT_FLOAT_EQ(f0.t0_, 0.0); 49 | EXPECT_FLOAT_EQ(f0.t, 0.025); 50 | EXPECT_FLOAT_EQ(imu.t0_, 0.025); 51 | EXPECT_FLOAT_EQ(imu.t, 0.05); 52 | EXPECT_EQ(f0.meas_hist_.size(), 3); 53 | EXPECT_EQ(imu.meas_hist_.size(), 3); 54 | } 55 | 56 | TEST (ImuFactor, splitInOnTopOfMeas) 57 | { 58 | Vector6d z; 59 | z << 0, 0, -9.80665, 0, 0, 0; 60 | Matrix6d R = Matrix6d::Identity(); 61 | ImuFunctor imu(0.0, Vector6d::Zero(), Matrix6d::Identity(), 0, 0); 62 | imu.integrate(0.01, z, R); 63 | imu.integrate(0.02, z, R); 64 | imu.integrate(0.03, z, R); 65 | imu.integrate(0.04, z, R); 66 | imu.integrate(0.05, z, R); 67 | ImuFunctor f0 = imu.split(0.03); 68 | 69 | EXPECT_FLOAT_EQ(f0.t0_, 0.0); 70 | EXPECT_FLOAT_EQ(f0.t, 0.03); 71 | EXPECT_FLOAT_EQ(imu.t0_, 0.03); 72 | EXPECT_FLOAT_EQ(imu.t, 0.05); 73 | EXPECT_EQ(f0.meas_hist_.size(), 3); 74 | EXPECT_EQ(imu.meas_hist_.size(), 2); 75 | } 76 | 77 | TEST (ImuFactor, splitJustAfterStart) 78 | { 79 | Vector6d z; 80 | z << 0, 0, -9.80665, 0, 0, 0; 81 | Matrix6d R = Matrix6d::Identity(); 82 | ImuFunctor imu(0.0, Vector6d::Zero(), Matrix6d::Identity(), 0, 0); 83 | imu.integrate(0.01, z, R); 84 | imu.integrate(0.02, z, R); 85 | imu.integrate(0.03, z, R); 86 | imu.integrate(0.04, z, R); 87 | imu.integrate(0.05, z, R); 88 | ImuFunctor f0 = imu.split(0.005); 89 | 90 | f0.finished(1); 91 | imu.finished(1); 92 | 93 | EXPECT_FLOAT_EQ(f0.t0_, 0.0); 94 | EXPECT_FLOAT_EQ(f0.t, 0.005); 95 | EXPECT_FLOAT_EQ(imu.t0_, 0.005); 96 | EXPECT_FLOAT_EQ(imu.t, 0.05); 97 | EXPECT_EQ(f0.meas_hist_.size(), 2); 98 | EXPECT_EQ(imu.meas_hist_.size(), 5); 99 | } 100 | 101 | TEST (ImuFactor, splitJustBeforeEnd) 102 | { 103 | Vector6d z; 104 | z << 0, 0, -9.80665, 0, 0, 0; 105 | Matrix6d R = Matrix6d::Identity(); 106 | ImuFunctor imu(0.0, Vector6d::Zero(), Matrix6d::Identity(), 0, 0); 107 | imu.integrate(0.01, z, R); 108 | imu.integrate(0.02, z, R); 109 | imu.integrate(0.03, z, R); 110 | imu.integrate(0.04, z, R); 111 | imu.integrate(0.05, z, R); 112 | ImuFunctor f0 = imu.split(0.045); 113 | 114 | f0.finished(1); 115 | imu.finished(1); 116 | 117 | EXPECT_FLOAT_EQ(f0.t0_, 0.0); 118 | EXPECT_FLOAT_EQ(f0.t, 0.045); 119 | EXPECT_FLOAT_EQ(imu.t0_, 0.045); 120 | EXPECT_FLOAT_EQ(imu.t, 0.05); 121 | EXPECT_EQ(f0.meas_hist_.size(), 5); 122 | EXPECT_EQ(imu.meas_hist_.size(), 2); 123 | } 124 | 125 | TEST(ImuFactor, Propagation) 126 | { 127 | Simulator multirotor(false, 1); 128 | multirotor.load(imu_only()); 129 | 130 | 131 | typedef ImuFunctor IMU; 132 | Vector6d b0; 133 | b0.setZero(); 134 | Matrix6d cov = Matrix6d::Identity() * 1e-3; 135 | 136 | multirotor.run(); 137 | IMU* imu = new IMU(multirotor.t_, b0, Matrix6d::Identity(), 0, 0); 138 | Xformd x0 = multirotor.state().X; 139 | Vector3d v0 = multirotor.state().v; 140 | 141 | salsa::Logger log("/tmp/ImuFactor.CheckPropagation.log"); 142 | 143 | Xformd xhat = multirotor.state().X; 144 | Vector3d vhat = multirotor.state().v; 145 | log.log(multirotor.t_); 146 | log.logVectors(xhat.elements(), vhat, multirotor.state().X.elements(), 147 | multirotor.state().v, multirotor.imu()); 148 | 149 | double next_reset = 1.0; 150 | multirotor.tmax_ = 10.0; 151 | while (multirotor.run()) 152 | { 153 | imu->integrate(multirotor.t_, multirotor.imu(), cov); 154 | 155 | if (std::abs(multirotor.t_ - next_reset) <= multirotor.dt_ /2.0) 156 | { 157 | delete imu; 158 | imu = new IMU(multirotor.t_, b0, Matrix6d::Identity(), 0, 0); 159 | x0 = multirotor.state().X; 160 | v0 = multirotor.state().v; 161 | next_reset += 1.0; 162 | } 163 | 164 | imu->estimateXj(x0, v0, xhat, vhat); 165 | log.log(multirotor.t_); 166 | log.logVectors(xhat.elements(), vhat, multirotor.state().X.elements(), multirotor.state().v, multirotor.imu()); 167 | EXPECT_MAT_NEAR(xhat.t(), multirotor.state().X.t(), 0.076); 168 | EXPECT_QUAT_NEAR(xhat.q(), multirotor.state().X.q(), 0.01); 169 | EXPECT_MAT_NEAR(vhat, multirotor.state().v, 0.05); 170 | } 171 | delete imu; 172 | } 173 | 174 | 175 | TEST(ImuFactor, ErrorStateDynamics) 176 | { 177 | typedef ImuFunctor IMU; 178 | Vector9d dy; 179 | double t = 0; 180 | const double Tmax = 10.0; 181 | static const double dt = 0.001; 182 | 183 | Vector6d bias; 184 | bias.setZero(); 185 | IMU y(t, bias, Matrix6d::Identity(), 0, 0); 186 | IMU yhat(t, bias, Matrix6d::Identity(), 0, 0); 187 | IMU::boxplus(y.y_, Vector9d::Constant(0.01), yhat.y_); 188 | IMU::boxminus(y.y_, yhat.y_, dy); 189 | 190 | Vector10d y_check; 191 | IMU::boxplus(yhat.y_, dy, y_check); 192 | ASSERT_MAT_NEAR(y.y_, y_check, 1e-8); 193 | 194 | Vector6d u, eta; 195 | u.setZero(); 196 | eta.setZero(); 197 | 198 | std::default_random_engine gen; 199 | std::normal_distribution normal; 200 | 201 | salsa::Logger log("/tmp/ImuFactor.CheckDynamics.log"); 202 | 203 | 204 | Matrix6d cov = Matrix6d::Identity() * 1e-3; 205 | Vector9d dydot; 206 | log.log(t); 207 | log.logVectors(y.y_, yhat.y_, dy, y_check, u); 208 | for (int i = 0; i < Tmax/dt; i++) 209 | { 210 | u += dt * randomNormal(normal, gen); 211 | t += dt; 212 | y.errorStateDynamics(y.y_, dy, u, eta, dydot); 213 | dy += dydot * dt; 214 | 215 | y.integrate(t, u, cov); 216 | yhat.integrate(t, u, cov); 217 | IMU::boxplus(yhat.y_, dy, y_check); 218 | log.log(t); 219 | log.logVectors(y.y_, yhat.y_, dy, y_check, u); 220 | ASSERT_MAT_NEAR(y.y_, y_check, t > 0.3 ? 5e-6*t*t : 2e-7); 221 | } 222 | } 223 | 224 | 225 | TEST(ImuFactor, DynamicsJacobians) 226 | { 227 | Matrix6d cov = Matrix6d::Identity()*1e-3; 228 | 229 | Vector6d b0; 230 | Vector10d y0; 231 | Vector6d u0; 232 | Vector6d eta0; 233 | Vector9d ydot; 234 | Vector9d dy0; 235 | 236 | Matrix9d A; 237 | Eigen::Matrix B; 238 | 239 | for (int i = 0; i < 100; i++) 240 | { 241 | b0.setRandom(); 242 | y0.setRandom(); 243 | y0.segment<4>(6) = Quatd::Random().elements(); 244 | u0.setRandom(); 245 | 246 | eta0.setZero(); 247 | dy0.setZero(); 248 | 249 | ImuFunctor f(0, b0, Matrix6d::Identity(), 0, 0); 250 | f.dynamics(y0, u0, ydot, A, B); 251 | Vector9d dy0 = Vector9d::Zero(); 252 | 253 | auto yfun = [&y0, &cov, &b0, &u0, &eta0](const Vector9d& dy) 254 | { 255 | ImuFunctor f(0, b0, Matrix6d::Identity(), 0, 0); 256 | Vector9d dydot; 257 | f.errorStateDynamics(y0, dy, u0, eta0, dydot); 258 | return dydot; 259 | }; 260 | auto etafun = [&y0, &cov, &b0, &dy0, &u0](const Vector6d& eta) 261 | { 262 | ImuFunctor f(0, b0, Matrix6d::Identity(), 0, 0); 263 | Vector9d dydot; 264 | f.errorStateDynamics(y0, dy0, u0, eta, dydot); 265 | return dydot; 266 | }; 267 | 268 | Matrix9d AFD; 269 | AFD.setZero(); 270 | AFD = calc_jac(yfun, dy0); 271 | Eigen::Matrix BFD = calc_jac(etafun, u0); 272 | 273 | // cout << "A\n" << A << "\nAFD\n" << AFD << "\n\n"; 274 | // cout << "B\n" << B << "\nBFD\n" << BFD << "\n\n"; 275 | 276 | ASSERT_MAT_NEAR(AFD, A, 1e-7); 277 | ASSERT_MAT_NEAR(BFD, B, 1e-7); 278 | } 279 | } 280 | 281 | TEST(ImuFactor, BiasJacobians) 282 | { 283 | Simulator multirotor(false, 2); 284 | multirotor.load(imu_only()); 285 | std::vector> meas; 286 | std::vector t; 287 | multirotor.dt_ = 0.001; 288 | 289 | while (multirotor.t_ < 0.1) 290 | { 291 | multirotor.run(); 292 | meas.push_back(multirotor.imu()); 293 | t.push_back(multirotor.t_); 294 | } 295 | 296 | Matrix6d cov = Matrix6d::Identity()*1e-3; 297 | Vector6d b0; 298 | Eigen::Matrix J, JFD; 299 | 300 | b0.setZero(); 301 | ImuFunctor f(0, b0, Matrix6d::Identity(), 0, 0); 302 | Vector10d y0 = f.y_; 303 | for (int i = 0; i < meas.size(); i++) 304 | { 305 | f.integrate(t[i], meas[i], cov); 306 | } 307 | J = f.J_; 308 | 309 | auto fun = [&cov, &meas, &t, &y0](const Vector6d& b0) 310 | { 311 | ImuFunctor f(0, b0, Matrix6d::Identity(), 0, 0); 312 | for (int i = 0; i < meas.size(); i++) 313 | { 314 | f.integrate(t[i], meas[i], cov); 315 | } 316 | return f.y_; 317 | }; 318 | auto bm = [](const MatrixXd& x1, const MatrixXd& x2) 319 | { 320 | Vector9d dx; 321 | ImuFunctor::boxminus(x1, x2, dx); 322 | return dx; 323 | }; 324 | 325 | JFD = calc_jac(fun, b0, nullptr, nullptr, bm, 1e-5); 326 | // std::cout << "FD:\n" << JFD << "\nA:\n" << J < beta = y.segment<3>(BETA); 35 | Quatd gamma(y.data()+GAMMA); 36 | VectorBlock ba = b_.segment<3>(ACC); 37 | VectorBlock bw = b_.segment<3>(OMEGA); 38 | Vector3d a = u.segment<3>(ACC) - ba; 39 | Vector3d w = u.segment<3>(OMEGA)- bw; 40 | 41 | ydot.segment<3>(ALPHA) = beta; 42 | ydot.segment<3>(BETA) = gamma.rota(a); 43 | ydot.segment<3>(GAMMA) = w; 44 | } 45 | 46 | void ImuIntegrator::boxplus(const Vector10d& y, const Vector9d& dy, Vector10d& yp) 47 | { 48 | yp.segment<3>(P) = y.segment<3>(P) + dy.segment<3>(P); 49 | yp.segment<3>(V) = y.segment<3>(V) + dy.segment<3>(V); 50 | yp.segment<4>(Q) = (Quatd(y.segment<4>(Q)) + dy.segment<3>(Q)).elements(); 51 | } 52 | 53 | 54 | void ImuIntegrator::boxminus(const Vector10d& y1, const Vector10d& y2, Vector9d& d) 55 | { 56 | d.segment<3>(P) = y1.segment<3>(P) - y2.segment<3>(P); 57 | d.segment<3>(V) = y1.segment<3>(V) - y2.segment<3>(V); 58 | d.segment<3>(Q) = Quatd(y1.segment<4>(Q)) - Quatd(y2.segment<4>(Q)); 59 | } 60 | 61 | void ImuIntegrator::integrateStateOnly(const double& _t, const Vector6d& u) 62 | { 63 | double dt = _t - t; 64 | t = _t; 65 | delta_t_ = t - t0_; 66 | Vector9d ydot; 67 | Vector10d yp; 68 | dynamics(y_, u, ydot); 69 | boxplus(y_, ydot * dt, yp); 70 | y_ = yp; 71 | } 72 | 73 | void ImuIntegrator::estimateXj(const Xformd &xi, const Vector3d &vi, Xformd &xj, const Ref& _vj) const 74 | { 75 | VectorBlock alpha = y_.segment<3>(ALPHA); 76 | VectorBlock beta = y_.segment<3>(BETA); 77 | Quatd gamma(y_.data()+GAMMA); 78 | Ref vj = const_cast &>(_vj); 79 | 80 | SALSA_ASSERT(std::abs(1.0 - gamma.arr_.norm()) < 1e-8, "Quat left manifold"); 81 | SALSA_ASSERT(std::abs(1.0 - xi.q_.arr_.norm()) < 1e-8, "Quat left manifold"); 82 | 83 | xj.t_ = xi.t_ + xi.q_.rota(vi*delta_t_) + 1/2.0 * gravity_*delta_t_*delta_t_ + xi.q_.rota(alpha); 84 | xj.t_ = xi.t_ + xi.q_.rota(vi*delta_t_) + 1/2.0 * gravity_*delta_t_*delta_t_ + xi.q_.rota(alpha); 85 | vj = gamma.rotp(vi + xi.q_.rotp(gravity_)*delta_t_ + beta); 86 | xj.q_ = xi.q_ * gamma; 87 | SALSA_ASSERT(std::abs(1.0 - xj.q_.arr_.norm()) < 1e-8, "Quat left manifold"); 88 | } 89 | 90 | ImuFunctor::ImuFunctor(const double &_t0, const Vector6d &b0, const Matrix6d& bias_Xi, int from_idx, int from_node) 91 | { 92 | delta_t_ = 0.0; 93 | t0_ = _t0; 94 | t = _t0; 95 | b_ = b0; 96 | 97 | n_updates_ = 0; 98 | y_.setZero(); 99 | y_(Q) = 1.0; 100 | P_.setZero(); 101 | J_.setZero(); 102 | 103 | from_idx_ = from_idx; 104 | to_idx_ = -1; 105 | from_node_ = from_node; 106 | bias_Xi_ = bias_Xi; 107 | } 108 | 109 | void ImuFunctor::errorStateDynamics(const Vector10d& y, const Vector9d& dy, const Vector6d& u, 110 | const Vector6d& eta, Vector9d& dydot) 111 | { 112 | VectorBlock dbeta = dy.segment<3>(BETA); 113 | VectorBlock dgamma = dy.segment<3>(GAMMA); 114 | 115 | Quatd gamma(y.data()+GAMMA); 116 | VectorBlock a = u.segment<3>(ACC); 117 | VectorBlock w = u.segment<3>(OMEGA); 118 | VectorBlock ba = b_.segment<3>(ACC); 119 | VectorBlock bw = b_.segment<3>(OMEGA); 120 | 121 | VectorBlock eta_a = eta.segment<3>(ACC); 122 | VectorBlock eta_w = eta.segment<3>(OMEGA); 123 | 124 | dydot.segment<3>(ALPHA) = dbeta; 125 | dydot.segment<3>(BETA) = -gamma.rota((a - ba).cross(dgamma) + eta_a); 126 | dydot.segment<3>(GAMMA) = -skew(w - bw)*dgamma - eta_w; 127 | } 128 | 129 | // ydot = f(y, u) <-- nonlinear dynamics (reference state) 130 | // A = d(dydot)/d(dy) <-- error state 131 | // B = d(dydot)/d(eta) <-- error state 132 | // Because of the error state, ydot != Ay+Bu 133 | void ImuFunctor::dynamics(const Vector10d& y, const Vector6d& u, 134 | Vector9d& ydot, Matrix9d& A, Matrix96&B) 135 | { 136 | VectorBlock alpha = y.segment<3>(ALPHA); 137 | VectorBlock beta = y.segment<3>(BETA); 138 | Quatd gamma(y.data()+GAMMA); 139 | VectorBlock ba = b_.segment<3>(ACC); 140 | VectorBlock bw = b_.segment<3>(OMEGA); 141 | Vector3d a = u.segment<3>(ACC) - ba; 142 | Vector3d w = u.segment<3>(OMEGA)- bw; 143 | 144 | ydot.segment<3>(ALPHA) = beta; 145 | ydot.segment<3>(BETA) = gamma.rota(a); 146 | ydot.segment<3>(GAMMA) = w; 147 | 148 | A.setZero(); 149 | A.block<3,3>(ALPHA, BETA) = I_3x3; 150 | A.block<3,3>(BETA, GAMMA) = -gamma.R().transpose() * skew(a); 151 | A.block<3,3>(GAMMA, GAMMA) = -skew(w); 152 | 153 | B.setZero(); 154 | B.block<3,3>(BETA, ACC) = -gamma.R().transpose(); 155 | B.block<3,3>(GAMMA, OMEGA) = -I_3x3; 156 | } 157 | 158 | 159 | void ImuFunctor::integrate(const meas::Imu& z, bool save) 160 | { 161 | integrate(z.t, z.z, z.R, save); 162 | } 163 | 164 | void ImuFunctor::integrate(const double& _t, const Vector6d& u, const Matrix6d& cov, bool save) 165 | { 166 | SALSA_ASSERT((cov.array() == cov.array()).all(), "NaN detected in covariance on propagation"); 167 | SALSA_ASSERT((u.array() == u.array()).all(), "NaN detected in covariance on propagation"); 168 | SALSA_ASSERT(ge(_t, t), "Trying to integrate backwards"); 169 | // if(le(_t, t)) 170 | // { 171 | // SD(5, "Trying to integrate backwards z.t%.3f, y.t%.3f", _t, t); 172 | // return; 173 | // } 174 | 175 | double dt = _t - t; 176 | n_updates_++; 177 | t = _t; 178 | delta_t_ = t - t0_; 179 | Vector9d ydot; 180 | Matrix9d A; 181 | Matrix96 B; 182 | Vector10d yp; 183 | u_ = u; 184 | cov_ = cov; 185 | dynamics(y_, u, ydot, A, B); 186 | boxplus(y_, ydot * dt, yp); 187 | y_ = yp; 188 | 189 | A = Matrix9d::Identity() + A*dt + 1/2.0 * A*A*dt*dt; 190 | B = B*dt; 191 | 192 | P_ = A*P_*A.transpose() + B*cov*B.transpose(); 193 | J_ = A*J_ + B; 194 | 195 | if (save) 196 | meas_hist_.push_back(meas::Imu(_t, u, cov)); 197 | 198 | SALSA_ASSERT((A.array() == A.array()).all(), "NaN detected in covariance on propagation"); 199 | SALSA_ASSERT((P_.array() == P_.array()).all(), "NaN detected in covariance on propagation"); 200 | } 201 | 202 | void ImuFunctor::finished(int to_idx) 203 | { 204 | to_idx_ = to_idx; 205 | if (n_updates_ <= 2) 206 | { 207 | P_ = P_ + Matrix9d::Identity() * 1e-15; 208 | } 209 | Xi_ = P_.inverse().llt().matrixL().transpose(); 210 | SALSA_ASSERT((Xi_.array() == Xi_.array()).all(), "NaN detected in IMU information matrix"); 211 | } 212 | 213 | ImuFunctor ImuFunctor::split(double _t) 214 | { 215 | SALSA_ASSERT(t > t0_ && _t < t, "Trying to split the wrong IMU functor"); 216 | 217 | // Figure out if we will run into single IMU message problems for the first functor 218 | bool single_imu0 = le(_t, meas_hist_.front().t); // _t <= z.t 219 | 220 | // Create new ImuFunctor from the beginning to the split 221 | ImuFunctor f0(t0_, b_, bias_Xi_, from_idx_, from_node_); 222 | while (lt(f0.t, _t)) // f0.t < _t 223 | { 224 | if (le(meas_hist_.front().t, _t)) // z.t <= _t 225 | { 226 | f0.integrate(meas_hist_.front()); 227 | meas_hist_.pop_front(); 228 | } 229 | else 230 | { 231 | // interpolate 232 | if (single_imu0) 233 | { 234 | // Handle single IMU case 235 | double dt = _t - f0.t; 236 | f0.integrate(f0.t+dt/2.0, meas_hist_.front().z, meas_hist_.front().R); 237 | f0.integrate(_t, meas_hist_.front().z, meas_hist_.front().R); 238 | 239 | } 240 | else 241 | { 242 | f0.integrate(_t, meas_hist_.front().z, meas_hist_.front().R); 243 | } 244 | } 245 | } 246 | 247 | // Adjust this functor 248 | reset(_t); 249 | n_updates_ = 0; 250 | P_.setZero(); 251 | bool single_imu1 = meas_hist_.size() == 1; 252 | if (single_imu1) 253 | { 254 | meas::Imu z = meas_hist_.front(); 255 | meas_hist_.clear(); 256 | double dt = z.t - t; 257 | integrate(t + dt/2.0, z.z, z.R, true); 258 | integrate(z, true); 259 | } 260 | else 261 | { 262 | for (auto z : meas_hist_) 263 | { 264 | integrate(z, false); 265 | } 266 | 267 | } 268 | return f0; 269 | } 270 | 271 | Vector6d ImuFunctor::avgImuOverInterval() 272 | { 273 | Vector6d sum = Vector6d::Zero(); 274 | for (auto& u : meas_hist_) 275 | { 276 | sum += u.z; 277 | } 278 | return sum / meas_hist_.size(); 279 | } 280 | 281 | template 282 | bool ImuFunctor::operator()(const T* _xi, const T* _xj, const T* _vi, const T* _vj, 283 | const T* _bi, const T* _bj, T* residuals) const 284 | { 285 | typedef Matrix VecT3; 286 | typedef Matrix VecT6; 287 | typedef Matrix VecT9; 288 | typedef Matrix VecT10; 289 | typedef Matrix VecT15; 290 | 291 | Xform xi(_xi); 292 | Xform xj(_xj); 293 | Map vi(_vi); 294 | Map vj(_vj); 295 | Map bi(_bi); 296 | Map bj(_bj); 297 | 298 | VecT9 dy = J_ * (bi - b_); 299 | VecT10 y; 300 | y.template segment<6>(0) = y_.template segment<6>(0) + dy.template segment<6>(0); 301 | Quat q_dy; 302 | q_dy.arr_(0) = (T)1.0; 303 | q_dy.arr_.template segment<3>(1) = 0.5 * dy.template segment<3>(GAMMA); 304 | y.template segment<4>(6) = (Quatd(y_.template segment<4>(6)).otimes(q_dy)).elements(); 305 | // y.template segment<4>(6) = (Quatd(y_.template segment<4>(6)).otimes(Quat::exp(dy.template segment<3>(6)))).elements(); 306 | 307 | Map alpha(y.data()+ALPHA); 308 | Map beta(y.data()+BETA); 309 | Quat gamma(y.data()+GAMMA); 310 | Map r(residuals); 311 | 312 | r.template block<3,1>(ALPHA, 0) = xi.q_.rotp(xj.t_ - xi.t_ - 1/2.0*gravity_*delta_t_*delta_t_) - vi*delta_t_ - alpha; 313 | r.template block<3,1>(BETA, 0) = gamma.rota(vj) - vi - xi.q_.rotp(gravity_)*delta_t_ - beta; 314 | r.template block<3,1>(GAMMA, 0) = (xi.q_.inverse() * xj.q_) - gamma; 315 | r.template tail<6>() = bi - bj; 316 | 317 | 318 | r.template head<9>() = Xi_ * r.template head<9>(); 319 | r.template tail<6>() = bias_Xi_ * (bi - bj); 320 | 321 | return true; 322 | } 323 | template bool ImuFunctor::operator()(const double* _xi, const double* _xj, const double* _vi, const double* _vj, const double* _bi, const double* _bj, double* residuals) const; 324 | typedef ceres::Jet jactype; 325 | template bool ImuFunctor::operator()(const jactype* _xi, const jactype* _xj, const jactype* _vi, const jactype* _vj, const jactype* _bi, const jactype* _bj, jactype* residuals) const; 326 | 327 | 328 | } 329 | --------------------------------------------------------------------------------