Cartographer源码阅读(8)：imu_tracker

合集 - SLAM(55)

1.个人资源索引2018-03-032.SLAM学习笔记（1）基本概念2015-08-063.SLAM学习笔记（2）SLAM算法2015-08-214.[ROS]2 尝试编译OrbSLAM2016-01-185.[ROS]1 小乌龟2016-01-186.SLAM最近的工作2015-11-057.SLAM学习笔记（3）相关概念2015-09-168.[SLAM]2D激光线特征提取2016-07-169.[SLAM] GMapping SLAM源码阅读（草稿）2016-07-0110.[QGLViewer]First Demo2016-06-2911.SLAM数据集2016-06-2312.[SLAM]2D激光扫描匹配方法2016-06-1513.[g2o]一个备忘2016-04-2114.[ROS] slam_gmapping2016-01-2415.[ROS]3 Linux编程练习2016-01-2016.Linux学习和ROS安装（1）2017-12-1917.小豆包的学习之旅：机器人定位2017-05-0218.小豆包的学习之旅：传感器观测模型2017-04-2119.Kinect2.0相机标定2017-04-1020.小豆包的学习之旅：里程计运动模型2017-04-0321.小豆包的学习之旅：入门篇2017-04-0222.Kinect2.0点云数据获取2017-03-3023.[硬件]Robot运动控制2017-03-2224.[硬件]Urg_viewer数据读取2017-03-1425.[硬件]三维点云数据获取2017-03-0426.Kinect2.0获取数据2017-02-1527.ndt histogram_direction2016-09-0828.rplidar & hector slam without odometry2016-08-2629.rplidar测试2016-08-1230.[原创]NDT方法在SLAM中的应用2016-08-0831.[SLAM]Karto SLAM算法学习（草稿）2016-07-2232.ROS学习备忘2018-07-1833.[ROS]激光驱动安装2018-07-1634.OrbSLAM2采集点云数据2018-03-26

35.Cartographer源码阅读(8)：imu_tracker2018-03-21

36.[硬件]点云数据采集22018-03-1937.Cartographer源码阅读(9)：图优化的前端——闭环检测2018-03-0838.Cartographer源码阅读(7)：轨迹推算和位姿推算的原理2018-03-0839.Ethzasl MSF源码阅读(3)：MSF_Core和PoseMeasurement2018-03-0740.Ethzasl MSF源码阅读(2)：百川汇海2018-03-0641.Ethzasl MSF源码阅读(1)：程序入口和主题订阅2018-03-0442.Cartographer源码阅读(6)：LocalTrajectoryBuilder和PoseExtrapolator2018-02-2543.Cartographer源码阅读(5)：PoseGraph位姿图2018-02-2444.Cartographer源码阅读(4)：Node和MapBuilder对象22018-01-1045.Cartographer源码阅读(3)：程序逻辑结构2017-12-2946.Cartographer源码阅读(2)：Node和MapBuilder对象2017-12-2847.Cartographer源码阅读(1)：程序入口2017-12-2848.实时Cartographer测试(1) - rplidar2017-12-2149.Cartographer安装2017-12-2050.ROS安装（2）2017-12-2051.[ROS]一些传感器数据读取融合问题的思考2017-05-0652.小豆包的学习之旅：占用概率栅格地图和cost-map2017-05-0253.小豆包的学习之旅：开发记录2017-04-2954.[概述]移动机器人自主探索2017-04-1855.MRPT编译2017-04-14

收起

 IMU的输入为imu_linear_acceleration 和  imu_angular_velocity 线加速和角速度。最终作为属性输出的是方位四元数。

 Eigen::Quaterniond orientation() const { return orientation_; }

/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/mapping/imu_tracker.h"

#include <cmath>
#include <limits>

#include "cartographer/common/math.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer {
namespace mapping {

ImuTracker::ImuTracker(const double imu_gravity_time_constant,
                       const common::Time time)
    : imu_gravity_time_constant_(imu_gravity_time_constant),
      time_(time),
      last_linear_acceleration_time_(common::Time::min()),
      orientation_(Eigen::Quaterniond::Identity()),
      gravity_vector_(Eigen::Vector3d::UnitZ()),
      imu_angular_velocity_(Eigen::Vector3d::Zero()) {}

void ImuTracker::Advance(const common::Time time) {
  CHECK_LE(time_, time);
  const double delta_t = common::ToSeconds(time - time_);
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  orientation_ = (orientation_ * rotation).normalized();
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) {
  // Update the 'gravity_vector_' with an exponential moving average using the
  // 'imu_gravity_time_constant'.
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();
  last_linear_acceleration_time_ = time_;
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
  // Change the 'orientation_' so that it agrees with the current
  // 'gravity_vector_'.
  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
  orientation_ = (orientation_ * rotation).normalized();
  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}

void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity) {
  imu_angular_velocity_ = imu_angular_velocity;
}

}  // namespace mapping
}  // namespace cartographer