├── doc
    ├── IMUOrientationEstimator.mp4
    └── EKF_Based_IMU_Orientation_Estimation.pdf
├── include
    ├── Utils.h
    ├── Updater.h
    ├── Initializer.h
    ├── Propagator.h
    └── Estimator.h
├── README.md
├── CMakeLists.txt
├── src
    ├── Initializer.cpp
    ├── Updater.cpp
    ├── Propagator.cpp
    └── Estimator.cpp
└── example
    ├── RunRosbag.cpp
    └── RunMyntEye.cpp


/doc/IMUOrientationEstimator.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ydsf16/IMUOrientationEstimator/HEAD/doc/IMUOrientationEstimator.mp4


--------------------------------------------------------------------------------
/doc/EKF_Based_IMU_Orientation_Estimation.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ydsf16/IMUOrientationEstimator/HEAD/doc/EKF_Based_IMU_Orientation_Estimation.pdf


--------------------------------------------------------------------------------
/include/Utils.h:
--------------------------------------------------------------------------------
 1 | #pragma once 
 2 | 
 3 | #include <Eigen/Core>
 4 | 
 5 | namespace OriEst {
 6 | 
 7 | constexpr double kDeg2Rad = M_PI / 180.;
 8 | constexpr double kRad2Deg = 180. / M_PI;
 9 | constexpr double kGravity = 9.8;
10 | 
11 | inline Eigen::Matrix3d SkewMat(const Eigen::Vector3d& v) {
12 |     Eigen::Matrix3d w;
13 |     w <<  0.,   -v(2),  v(1),
14 |           v(2),  0.,   -v(0),
15 |          -v(1),  v(0),  0.;
16 |     return w;
17 | }
18 | 
19 | 
20 | }  // namespace OriEst


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | 
 2 | # IMUOrientationEstimator
 3 | Estimate 3DoFs orientation using IMU measurement.
 4 | 
 5 | ## Method drivation and video
 6 | A detailed derivation of the algorithm and a video are in the **doc** folder of this repository. 
 7 | 
 8 | **Doc**
 9 | [EKF-Based IMU Orientation Estimation](https://github.com/ydsf16/IMUOrientationEstimator/blob/master/doc/EKF_Based_IMU_Orientation_Estimation.pdf).
10 | 
11 | 知乎：https://zhuanlan.zhihu.com/p/166100648
12 | 
13 | **Video**
14 | <https://github.com/ydsf16/IMUOrientationEstimator/blob/master/doc/IMUOrientationEstimator.mp4>
15 | 


--------------------------------------------------------------------------------
/include/Updater.h:
--------------------------------------------------------------------------------
 1 | #pragma once 
 2 | 
 3 | #include <Eigen/Dense>
 4 | 
 5 | #include <Utils.h>
 6 | 
 7 | namespace OriEst {
 8 | 
 9 | void Update(const Eigen::Matrix3d& prior_G_R_I, 
10 |             const Eigen::Vector3d& prior_bg, 
11 |             const Eigen::Matrix<double, 6, 6>& prior_cov,
12 |             const Eigen::Vector3d& acc, 
13 |             const Eigen::Matrix3d& acc_noise,
14 |             Eigen::Matrix3d* posterior_G_R_I,
15 |             Eigen::Vector3d* posterior_bg,
16 |             Eigen::Matrix<double, 6, 6>* posterior_cov);
17 | 
18 | }  // namespace OriEst


--------------------------------------------------------------------------------
/include/Initializer.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <deque>
 4 | 
 5 | #include <Eigen/Dense>
 6 | 
 7 | namespace OriEst {
 8 | 
 9 | class Initializer {
10 | public:
11 |     struct Config {
12 |         size_t acc_buffer_size = 10;
13 |         double max_acc_std = 0.5;
14 |     };
15 | 
16 |     Initializer() = default;
17 | 
18 |     bool Initialize(const Eigen::Vector3d& acc, Eigen::Matrix3d* G_R_I);
19 | 
20 | private:
21 |     const Config config_;
22 | 
23 |     std::deque<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> acc_buffer_;
24 | };
25 | 
26 | } // namespace OriEst


--------------------------------------------------------------------------------
/include/Propagator.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <Eigen/Dense>
 4 | 
 5 | namespace OriEst {
 6 | 
 7 | class Propagator {
 8 | public:
 9 |     Propagator(const double& gyro_noise, const double& gyro_bias_noise);
10 | 
11 |     void PropagateMeanAndCov(const Eigen::Matrix3d& begin_G_R_I, 
12 |                              const Eigen::Vector3d& begin_bg, 
13 |                              const Eigen::Matrix<double, 6, 6>& begin_cov,
14 |                              const Eigen::Vector3d& gyro, 
15 |                              const double delta_t,
16 |                              Eigen::Matrix3d* end_G_R_I,
17 |                              Eigen::Vector3d* end_bg,
18 |                              Eigen::Matrix<double, 6, 6>* end_cov);
19 | 
20 | private:
21 |     double gyro_noise_;
22 |     double gyro_bias_noise_;
23 | };
24 | 
25 | }  // namespace OriEst


--------------------------------------------------------------------------------
/include/Estimator.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <deque>
 4 | #include <memory>
 5 | 
 6 | #include <Eigen/Core>
 7 | 
 8 | #include <Initializer.h>
 9 | #include <Propagator.h>
10 | #include <Updater.h>
11 | 
12 | namespace OriEst {
13 | 
14 | enum class Status {
15 |     kValid,
16 |     kInvalid
17 | };
18 | 
19 | class Estimator {
20 | public:
21 |     struct Config {
22 |         size_t acc_buffer_size = 1;
23 |     };
24 | 
25 |     Estimator(const double gyro_noise, const double gyro_bias_noise, const double acc_noise);
26 | 
27 |     Status Estimate(double timestamp, const Eigen::Vector3d& gyro, const Eigen::Vector3d& acc, Eigen::Matrix3d* G_R_I);
28 | 
29 | private:
30 |     // State.
31 |     Eigen::Matrix3d G_R_I_;
32 |     Eigen::Vector3d bg_;
33 |     Eigen::Matrix<double, 6, 6> cov_;
34 | 
35 |     Status status_;
36 |     double last_timestamp_;
37 |     Eigen::Matrix3d acc_noise_mat_;
38 | 
39 |     std::unique_ptr<Initializer> initializer_;
40 |     std::unique_ptr<Propagator> propagator_;
41 | 
42 |     const Config config_;
43 | 
44 |     std::deque<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> acc_buffer_;
45 | };
46 | 
47 | }  // namespace OriEst


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(IMUOrientationEstimator)
 3 | 
 4 | # Set C++14
 5 | add_compile_options(-std=c++14)
 6 | 
 7 | # Release mode.
 8 | set(CMAKE_BUILD_TYPE Release)
 9 | 
10 | find_package(Eigen3 REQUIRED)
11 | include_directories(${EIGEN3_INCLUDE_DIR})
12 | 
13 | include_directories(include)
14 | add_library(${PROJECT_NAME} SHARED 
15 |     src/Initializer.cpp
16 |     src/Propagator.cpp
17 |     src/Updater.cpp
18 |     src/Estimator.cpp
19 | )
20 | target_link_libraries(${PROJECT_NAME} ${EIGEN3_LIBS})
21 | 
22 | option(BUILD_EXAMPLE "Build example code" ON)
23 | 
24 | if (BUILD_EXAMPLE) 
25 |     find_package(OpenCV 4 REQUIRED)
26 |     include_directories(${OpenCV_INCLUDE_DIRS})
27 | 
28 |     # ROSBAG
29 |     find_package(catkin REQUIRED COMPONENTS
30 |         sensor_msgs
31 |         rosbag
32 |     )
33 |     include_directories(${catkin_INCLUDE_DIRS})
34 | 
35 |     add_executable(RunRosbag example/RunRosbag.cpp)
36 |     target_link_libraries(RunRosbag
37 |         ${catkin_LIBRARIES}
38 |         ${PROJECT_NAME}
39 |         ${OpenCV_LIBS}
40 |     )
41 | 
42 |     # MYNTEYE
43 |     find_package(mynteye REQUIRED)
44 |     message(STATUS "Found mynteye: ${mynteye_VERSION}")
45 |     add_executable(RunMyntEye example/RunMyntEye.cpp)
46 |     target_link_libraries(RunMyntEye ${PROJECT_NAME} mynteye ${OpenCV_LIBS})
47 | 
48 | endif()


--------------------------------------------------------------------------------
/src/Initializer.cpp:
--------------------------------------------------------------------------------
 1 | #include <Initializer.h>
 2 | 
 3 | #include <iostream>
 4 | 
 5 | namespace OriEst {
 6 | 
 7 | bool Initializer::Initialize(const Eigen::Vector3d& acc, Eigen::Matrix3d* G_R_I) {
 8 |     // Feed new data.
 9 |     acc_buffer_.push_back(acc);
10 |     if (acc_buffer_.size() <= config_.acc_buffer_size) {
11 |         return false;
12 |     }
13 |     acc_buffer_.pop_front();
14 | 
15 |     // Compute mean acc.
16 |     Eigen::Vector3d mean_acc(0., 0., 0.);
17 |     for (const Eigen::Vector3d& one_acc : acc_buffer_) {
18 |         mean_acc += one_acc;
19 |     }
20 |     mean_acc = mean_acc / static_cast<double>(acc_buffer_.size());
21 | 
22 |     // Compute std acc.
23 |     Eigen::Vector3d std_acc(0., 0., 0.);
24 |     for (const Eigen::Vector3d& one_acc : acc_buffer_) {
25 |         std_acc += (one_acc - mean_acc).cwiseAbs2();
26 |     }
27 |     std_acc = (std_acc / static_cast<double>(acc_buffer_.size())).cwiseSqrt();
28 |     if (std_acc.norm() > config_.max_acc_std) {
29 |         std::cout << "[Initialize]: Initializaion failed. Too big acc std: " << std::fixed << std_acc.transpose(); 
30 |         return false;
31 |     }
32 | 
33 |     // Get initial orientaion.
34 |     const Eigen::Vector3d z_axis = mean_acc.normalized();
35 |     const Eigen::Vector3d x_axis = 
36 |         (Eigen::Vector3d::UnitX() - z_axis * z_axis.transpose() * Eigen::Vector3d::UnitX()).normalized();
37 |     const Eigen::Vector3d y_axis = (z_axis.cross(x_axis)).normalized();
38 |     
39 |     Eigen::Matrix3d I_R_G;
40 |     I_R_G.block<3, 1>(0, 0) = x_axis;
41 |     I_R_G.block<3, 1>(0, 1) = y_axis;
42 |     I_R_G.block<3, 1>(0, 2) = z_axis;
43 | 
44 |     *G_R_I = I_R_G.transpose();
45 | 
46 |     return true;
47 | }
48 | 
49 | }  // namespace OriEst


--------------------------------------------------------------------------------
/src/Updater.cpp:
--------------------------------------------------------------------------------
 1 | #include <Updater.h>
 2 | 
 3 | namespace OriEst {
 4 | 
 5 | void Update(const Eigen::Matrix3d& prior_G_R_I, 
 6 |             const Eigen::Vector3d& prior_bg, 
 7 |             const Eigen::Matrix<double, 6, 6>& prior_cov,
 8 |             const Eigen::Vector3d& acc, 
 9 |             const Eigen::Matrix3d& acc_noise,
10 |             Eigen::Matrix3d* posterior_G_R_I,
11 |             Eigen::Vector3d* posterior_bg,
12 |             Eigen::Matrix<double, 6, 6>* posterior_cov) {
13 |     // Residual
14 |     Eigen::Vector3d gravity_vec(0., 0., 1.);
15 |     Eigen::Vector3d residual = acc.normalized() - prior_G_R_I.transpose() * gravity_vec;
16 |     
17 |     // Jacobian.
18 |     Eigen::Matrix<double, 3, 6> H;
19 |     H.setZero();
20 |     H.block<3, 3>(0, 0) = SkewMat(prior_G_R_I.transpose() * gravity_vec);
21 | 
22 |     // Kalman gain.
23 |     Eigen::Matrix<double, 6, 3> K = prior_cov * H.transpose() * (H * prior_cov * H.transpose() + acc_noise).inverse();
24 | 
25 |     // Delta x.
26 |     Eigen::Matrix<double, 6, 1> delta_x = K * residual;
27 | 
28 |     // Update state.
29 |     Eigen::Matrix3d delta_Rot;
30 |     if (delta_x.topRows<3>().norm() < 1e-12) {
31 |         delta_Rot = 
32 |             Eigen::Quaterniond(
33 |                 Eigen::Matrix3d::Identity() + SkewMat(delta_x.topRows<3>())).normalized().toRotationMatrix();
34 |     } else {
35 |         const double angle = delta_x.topRows<3>().norm();
36 |         const Eigen::Vector3d axis = delta_x.topRows<3>() / angle;
37 |         delta_Rot = Eigen::AngleAxisd(angle, axis).toRotationMatrix();
38 |     }
39 |     *posterior_G_R_I = prior_G_R_I * delta_Rot;
40 |     *posterior_bg = prior_bg + delta_x.bottomRows<3>();
41 | 
42 |     // Update covariance.
43 |     const Eigen::Matrix<double, 6, 6> I_mins_KH = Eigen::Matrix<double, 6, 6>::Identity() - K * H;
44 |     *posterior_cov = I_mins_KH * prior_cov * I_mins_KH.transpose() + K * acc_noise * K.transpose();
45 | }
46 | } // namesapce OriEst


--------------------------------------------------------------------------------
/src/Propagator.cpp:
--------------------------------------------------------------------------------
 1 | #include <Propagator.h>
 2 | 
 3 | #include <Utils.h>
 4 | 
 5 | namespace OriEst {
 6 | 
 7 | Propagator::Propagator(const double& gyro_noise, const double& gyro_bias_noise)
 8 |     : gyro_noise_(gyro_noise), gyro_bias_noise_(gyro_bias_noise) { }
 9 | 
10 | void Propagator::PropagateMeanAndCov(const Eigen::Matrix3d& begin_G_R_I, 
11 |                                      const Eigen::Vector3d& begin_bg, 
12 |                                      const Eigen::Matrix<double, 6, 6>& begin_cov,
13 |                                      const Eigen::Vector3d& gyro, 
14 |                                      const double delta_t,
15 |                                      Eigen::Matrix3d* end_G_R_I,
16 |                                      Eigen::Vector3d* end_bg,
17 |                                      Eigen::Matrix<double, 6, 6>* end_cov) {
18 |     // Mean propagation.
19 |     const Eigen::Vector3d unbiased_gyro = gyro - begin_bg;
20 |     const Eigen::Vector3d angle_vec = unbiased_gyro * delta_t;
21 |     Eigen::Matrix3d delta_rot;
22 |     if (angle_vec.norm() < 1e-12) {
23 |         delta_rot = Eigen::Quaterniond(Eigen::Matrix3d::Identity() + SkewMat(angle_vec)).normalized().toRotationMatrix();
24 |     } else {
25 |         const double angle = angle_vec.norm();
26 |         const Eigen::Vector3d axis = angle_vec / angle;
27 |         delta_rot = Eigen::AngleAxisd(angle, axis).toRotationMatrix();
28 |     }
29 |     *end_G_R_I = begin_G_R_I * delta_rot;
30 |     *end_bg = begin_bg;
31 | 
32 |     // Jacobian.
33 |     Eigen::Matrix<double, 6, 6> Fx;
34 |     Fx.topLeftCorner<3, 3>() = delta_rot.transpose();
35 |     Fx.topRightCorner<3,3>() = -Eigen::Matrix3d::Identity() * delta_t;
36 |     Fx.bottomLeftCorner<3, 3>() = Eigen::Matrix3d::Zero();
37 |     Fx.bottomRightCorner<3, 3>() = Eigen::Matrix3d::Identity();
38 | 
39 |     Eigen::Matrix<double, 6, 6> Q = Eigen::Matrix<double, 6, 6>::Zero();
40 |     Q.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity() * gyro_noise_ * delta_t * delta_t;
41 |     Q.bottomRightCorner<3, 3>() = Eigen::Matrix3d::Identity() * gyro_bias_noise_ * delta_t;
42 | 
43 |     *end_cov = Fx * begin_cov * Fx.transpose() + Q;
44 | }
45 | 
46 | }  // namespac OriEst


--------------------------------------------------------------------------------
/src/Estimator.cpp:
--------------------------------------------------------------------------------
 1 | #include <Estimator.h>
 2 | 
 3 | #include <iostream>
 4 | 
 5 | namespace OriEst {
 6 | 
 7 | Estimator::Estimator(const double gyro_noise, const double gyro_bias_noise, const double acc_noise)
 8 |     : status_(Status::kInvalid), 
 9 |       last_timestamp_(-1.),
10 |       initializer_(std::make_unique<Initializer>()),
11 |       propagator_(std::make_unique<Propagator>(gyro_noise, gyro_bias_noise)) { 
12 |     acc_noise_mat_ = Eigen::Matrix3d::Identity() * acc_noise;
13 | }
14 | 
15 | Status Estimator::Estimate(double timestamp, const Eigen::Vector3d& gyro, const Eigen::Vector3d& acc, Eigen::Matrix3d* G_R_I) {
16 |     if (status_ == Status::kInvalid) {
17 |         if (!initializer_->Initialize(acc, &G_R_I_)) {
18 |             G_R_I->setIdentity();
19 |             return Status::kInvalid;
20 |         }
21 | 
22 |         bg_.setZero();
23 |         cov_.setZero();
24 |         cov_.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity() * 0.5 * 0.5 * kDeg2Rad * kDeg2Rad;
25 |         cov_.bottomRightCorner<3, 3>() = Eigen::Matrix3d::Identity() * 0.1 * 0.1 * kDeg2Rad * kDeg2Rad;
26 |         last_timestamp_ = timestamp;
27 | 
28 |         // Send out.
29 |         *G_R_I = G_R_I_;
30 | 
31 |         status_ = Status::kValid;
32 |         return Status::kValid;
33 |     }
34 | 
35 |     double delta_t = timestamp - last_timestamp_;
36 |     last_timestamp_ = timestamp;
37 | 
38 |     // Propagation.
39 |     Eigen::Matrix3d prior_G_R_I;
40 |     Eigen::Vector3d prior_bg;
41 |     Eigen::Matrix<double, 6, 6> prior_cov;
42 |     propagator_->PropagateMeanAndCov(G_R_I_, bg_, cov_, gyro, delta_t, &prior_G_R_I, &prior_bg, &prior_cov);
43 | 
44 |     acc_buffer_.push_back(acc);
45 |     if (acc_buffer_.size() > config_.acc_buffer_size) {
46 |         acc_buffer_.pop_front();
47 |     }
48 |     // compute mean.
49 |     Eigen::Vector3d mean_acc(0., 0., 0.);
50 |     for (const Eigen::Vector3d& one_acc : acc_buffer_) {
51 |         mean_acc += one_acc;
52 |     }
53 |     mean_acc = mean_acc / static_cast<double>(acc_buffer_.size());
54 | 
55 |     // Update
56 |     Update(prior_G_R_I, prior_bg, prior_cov, mean_acc, acc_noise_mat_, &G_R_I_, &bg_, &cov_);
57 | 
58 |     std::cout << "GyroBias: " << std::fixed << bg_.transpose() << std::endl;
59 | 
60 |     // Send out.
61 |     *G_R_I = G_R_I_;
62 | 
63 |     return Status::kValid;
64 | }
65 | 
66 | }  // namespace OriEst


--------------------------------------------------------------------------------
/example/RunRosbag.cpp:
--------------------------------------------------------------------------------
 1 | #include <Estimator.h>
 2 | 
 3 | #include <iostream>
 4 | 
 5 | #include <rosbag/view.h>
 6 | #include <sensor_msgs/Imu.h>
 7 | 
 8 | #include <opencv2/viz.hpp>
 9 | 
10 | int main(int argc, char** argv) {
11 |     if (argc != 2) {
12 |         std::cout << "Please input: Rosbag file!\n";
13 |         return EXIT_FAILURE;
14 |     }
15 | 
16 |     // Set viz.
17 |     cv::viz::Viz3d viz_windows("IMU Orientation");
18 |     viz_windows.showWidget("ENU Frame", cv::viz::WCoordinateSystem(0.8));
19 |     viz_windows.showWidget("IMU Frame", cv::viz::WCoordinateSystem(0.5));
20 | 
21 |     // Init OriEst.
22 |     const double gyro_noise = 1e-6;
23 |     const double gyro_bias_noise = 1e-8;
24 |     const double acc_noise = 1e-6;
25 |     OriEst::Estimator orientation_estimatro(gyro_noise, gyro_bias_noise, acc_noise);
26 | 
27 |     // Play Data
28 |     const std::string rosbag_path = argv[1];
29 |     const std::string kImuTopic = "/imu0";
30 |     rosbag::Bag bag;
31 |     bag.open(rosbag_path);  // BagMode is Read by default
32 |     for (rosbag::MessageInstance const m : rosbag::View(bag)) {
33 |         if (m.getTopic() == kImuTopic) {
34 |             const auto& msg = m.instantiate<sensor_msgs::Imu>();
35 |             const double timestamp = msg->header.stamp.toSec();
36 |             Eigen::Vector3d acc(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
37 |             Eigen::Vector3d gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
38 | 
39 |             Eigen::Matrix3d G_R_I;
40 |             OriEst::Status status = orientation_estimatro.Estimate(timestamp, gyro, acc, &G_R_I);  
41 | 
42 |             std::cout << "status " << static_cast<int>(status) << ", Rot\n" << G_R_I << "\n\n";
43 |             
44 |              // Show result.
45 |             cv::Mat cv_R =(cv::Mat_<float>(3, 3) << G_R_I(0, 0), G_R_I(0, 1), G_R_I(0, 2), 
46 |                                                     G_R_I(1, 0), G_R_I(1, 1), G_R_I(1, 2), 
47 |                                                     G_R_I(2, 0), G_R_I(2, 1), G_R_I(2, 2));
48 |             cv::Affine3d pose;
49 |             pose.linear(cv_R);
50 |             pose.translate(cv::Vec3d(0., 0., 0.));
51 |             viz_windows.setWidgetPose("IMU Frame", pose);
52 |             viz_windows.spinOnce(1, true);      
53 |         } 
54 |     }
55 | 
56 |     return EXIT_SUCCESS;
57 | }


--------------------------------------------------------------------------------
/example/RunMyntEye.cpp:
--------------------------------------------------------------------------------
 1 | #include <mynteye/logger.h>
 2 | #include <mynteye/device/device.h>
 3 | #include <mynteye/device/utils.h>
 4 | #include <mynteye/util/times.h>
 5 | 
 6 | #include <Estimator.h>
 7 | #include <opencv2/viz.hpp>
 8 | 
 9 | MYNTEYE_USE_NAMESPACE
10 | 
11 | int main(int argc, char **argv) {
12 |     // Init out orientation estimator.
13 |     const double gyro_noise = 1e-6;
14 |     const double gyro_bias_noise = 1e-8;
15 |     const double acc_noise = 1e-3;
16 |     OriEst::Estimator orientation_estimatro(gyro_noise, gyro_bias_noise, acc_noise);
17 | 
18 |     // Set viz.
19 |     cv::viz::Viz3d viz_windows("IMU Orientation");
20 |     viz_windows.showWidget("ENU Frame", cv::viz::WCoordinateSystem(0.3));
21 |     viz_windows.showWidget("IMU Frame", cv::viz::WCoordinateSystem(0.8));
22 | 
23 |     // Set up our camera.
24 |     auto &&device = device::select();
25 |     if (!device) return EXIT_FAILURE;
26 | 
27 |     bool ok;
28 |     auto &&request = device::select_request(device, &ok);
29 |     if (!ok) return EXIT_FAILURE;
30 |     device->ConfigStreamRequest(request);
31 | 
32 |     std::size_t imu_count = 0;
33 |     device->SetMotionCallback([&imu_count, &orientation_estimatro, &viz_windows](const device::MotionData &data) {
34 |         CHECK_NOTNULL(data.imu);
35 |         ++imu_count;
36 |         std::cout << "Imu count: " << imu_count << std::endl;
37 |         std::cout << ", timestamp: " << data.imu->timestamp
38 |                   << ", accel_x: " << data.imu->accel[0]
39 |                   << ", accel_y: " << data.imu->accel[1]
40 |                   << ", accel_z: " << data.imu->accel[2]
41 |                   << ", gyro_x: " << data.imu->gyro[0]
42 |                   << ", gyro_y: " << data.imu->gyro[1]
43 |                   << ", gyro_z: " << data.imu->gyro[2]
44 |                   << ", temperature: " << data.imu->temperature << std::endl;
45 | 
46 |         Eigen::Vector3d acc(data.imu->accel[0] * OriEst::kGravity, 
47 |                             data.imu->accel[1] * OriEst::kGravity, 
48 |                             data.imu->accel[2] * OriEst::kGravity);
49 |         Eigen::Vector3d gyro(data.imu->gyro[0] * OriEst::kDeg2Rad, 
50 |                                 data.imu->gyro[1] * OriEst::kDeg2Rad, 
51 |                                 data.imu->gyro[2] * OriEst::kDeg2Rad);
52 | 
53 |         double timestamp = data.imu->timestamp * 1e-6;
54 |         Eigen::Matrix3d G_R_I;
55 |         OriEst::Status status = orientation_estimatro.Estimate(timestamp, gyro, acc, &G_R_I);  
56 | 
57 |         // Show result.
58 |         cv::Mat cv_R =(cv::Mat_<float>(3, 3) << G_R_I(0, 0), G_R_I(0, 1), G_R_I(0, 2), 
59 |                                                 G_R_I(1, 0), G_R_I(1, 1), G_R_I(1, 2), 
60 |                                                 G_R_I(2, 0), G_R_I(2, 1), G_R_I(2, 2));
61 |         cv::Affine3d pose;
62 |         pose.linear(cv_R);
63 |         pose.translate(cv::Vec3d(0., 0., 0.));
64 |         viz_windows.setWidgetPose("IMU Frame", pose); 
65 | 
66 |         if (imu_count % 10 == 0) {
67 |             viz_windows.spinOnce(1);
68 |         }
69 |     });
70 | 
71 |     // Enable this will cache the motion datas until you get them.
72 |     device->EnableMotionDatas();
73 |     device->Start(Source::ALL);
74 |     
75 |     // Wait key to stop.
76 |     std::cin.ignore();
77 | 
78 |     device->Stop(Source::ALL);
79 | 
80 |     return EXIT_SUCCESS;
81 | }
82 | 


--------------------------------------------------------------------------------