///////////////////////////////////////////////////////////////////////////////
// tail_mean.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_TAIL_MEAN_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_TAIL_MEAN_HPP_DE_01_01_2006

#include <numeric>
#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{

    ///////////////////////////////////////////////////////////////////////////////
    // coherent_tail_mean_impl
    //
    /**
        @brief Estimation of the coherent tail mean based on order statistics (for both left and right tails)

        The coherent tail mean \f$\widehat{CTM}_{n,\alpha}(X)\f$ is equal to the non-coherent tail mean \f$\widehat{NCTM}_{n,\alpha}(X)\f$
        plus a correction term that ensures coherence in case of non-continuous distributions.

        \f[
            \widehat{CTM}_{n,\alpha}^{\mathrm{right}}(X) = \widehat{NCTM}_{n,\alpha}^{\mathrm{right}}(X) +
            \frac{1}{\lceil n(1-\alpha)\rceil}\hat{q}_{n,\alpha}(X)\left(1 - \alpha - \frac{1}{n}\lceil n(1-\alpha)\rceil \right)
        \f]

        \f[
            \widehat{CTM}_{n,\alpha}^{\mathrm{left}}(X) = \widehat{NCTM}_{n,\alpha}^{\mathrm{left}}(X) +
            \frac{1}{\lceil n\alpha\rceil}\hat{q}_{n,\alpha}(X)\left(\alpha - \frac{1}{n}\lceil n\alpha\rceil \right)
        \f]
    */
    template<typename Sample, typename LeftRight>
    struct coherent_tail_mean_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef float_type result_type;

        coherent_tail_mean_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            std::size_t cnt = count(args);

            std::size_t n = static_cast<std::size_t>(
                std::ceil(
                    cnt * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] )
                )
            );

            extractor<tag::non_coherent_tail_mean<LeftRight> > const some_non_coherent_tail_mean = {};

            return some_non_coherent_tail_mean(args)
                 + numeric::fdiv(quantile(args), n)
                 * (
                     ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability]
                     - numeric::fdiv(n, count(args))
                   );
        }

        // serialization is done by accumulators it depends on
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version) {}
    };

    ///////////////////////////////////////////////////////////////////////////////
    // non_coherent_tail_mean_impl
    //
    /**
        @brief Estimation of the (non-coherent) tail mean based on order statistics (for both left and right tails)

        An estimation of the non-coherent tail mean \f$\widehat{NCTM}_{n,\alpha}(X)\f$ is given by the mean of the
        \f$\lceil n\alpha\rceil\f$ smallest samples (left tail) or the mean of the  \f$\lceil n(1-\alpha)\rceil\f$
        largest samples (right tail), \f$n\f$ being the total number of samples and \f$\alpha\f$ the quantile level:

        \f[
            \widehat{NCTM}_{n,\alpha}^{\mathrm{right}}(X) = \frac{1}{\lceil n(1-\alpha)\rceil} \sum_{i=\lceil \alpha n \rceil}^n X_{i:n}
        \f]

        \f[
            \widehat{NCTM}_{n,\alpha}^{\mathrm{left}}(X) = \frac{1}{\lceil n\alpha\rceil} \sum_{i=1}^{\lceil \alpha n \rceil} X_{i:n}
        \f]

        It thus requires the caching of at least the \f$\lceil n\alpha\rceil\f$ smallest or the \f$\lceil n(1-\alpha)\rceil\f$
        largest samples.

        @param quantile_probability
    */
    template<typename Sample, typename LeftRight>
    struct non_coherent_tail_mean_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef float_type result_type;

        non_coherent_tail_mean_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            std::size_t cnt = count(args);

            std::size_t n = static_cast<std::size_t>(
                std::ceil(
                    cnt * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] )
                )
            );

            // If n is in a valid range, return result, otherwise return NaN or throw exception
            if (n <= static_cast<std::size_t>(tail(args).size()))
                return numeric::fdiv(
                    std::accumulate(
                        tail(args).begin()
                      , tail(args).begin() + n
                      , Sample(0)
                    )
                  , n
                );
            else
            {
                if (std::numeric_limits<result_type>::has_quiet_NaN)
                {
                    return std::numeric_limits<result_type>::quiet_NaN();
                }
                else
                {
                    std::ostringstream msg;
                    msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                    boost::throw_exception(std::runtime_error(msg.str()));
                    return Sample(0);
                }
            }
        }

        // serialization is done by accumulators it depends on
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version) {}
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::coherent_tail_mean<>
// tag::non_coherent_tail_mean<>
//
namespace tag
{
    template<typename LeftRight>
    struct coherent_tail_mean
      : depends_on<count, quantile, non_coherent_tail_mean<LeftRight> >
    {
        typedef accumulators::impl::coherent_tail_mean_impl<mpl::_1, LeftRight> impl;
    };

    template<typename LeftRight>
    struct non_coherent_tail_mean
      : depends_on<count, tail<LeftRight> >
    {
        typedef accumulators::impl::non_coherent_tail_mean_impl<mpl::_1, LeftRight> impl;
    };

    struct abstract_non_coherent_tail_mean
      : depends_on<>
    {
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::non_coherent_tail_mean;
// extract::coherent_tail_mean;
//
namespace extract
{
    extractor<tag::abstract_non_coherent_tail_mean> const non_coherent_tail_mean = {};
    extractor<tag::tail_mean> const coherent_tail_mean = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(non_coherent_tail_mean)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(coherent_tail_mean)
}

using extract::non_coherent_tail_mean;
using extract::coherent_tail_mean;

// for the purposes of feature-based dependency resolution,
// coherent_tail_mean<LeftRight> provides the same feature as tail_mean
template<typename LeftRight>
struct feature_of<tag::coherent_tail_mean<LeftRight> >
  : feature_of<tag::tail_mean>
{
};

template<typename LeftRight>
struct feature_of<tag::non_coherent_tail_mean<LeftRight> >
  : feature_of<tag::abstract_non_coherent_tail_mean>
{
};

// So that non_coherent_tail_mean can be automatically substituted
// with weighted_non_coherent_tail_mean when the weight parameter is non-void.
template<typename LeftRight>
struct as_weighted_feature<tag::non_coherent_tail_mean<LeftRight> >
{
    typedef tag::non_coherent_weighted_tail_mean<LeftRight> type;
};

template<typename LeftRight>
struct feature_of<tag::non_coherent_weighted_tail_mean<LeftRight> >
  : feature_of<tag::non_coherent_tail_mean<LeftRight> >
{};

// NOTE that non_coherent_tail_mean cannot be feature-grouped with tail_mean,
// which is the base feature for coherent tail means, since (at least for
// non-continuous distributions) non_coherent_tail_mean is a different measure!

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


#ifndef PARSETYPE_HPP
#define PARSETYPE_HPP

#include <string>
#include <vector>
#include <iostream>
#include <sstream>
#include <boost/variant.hpp>
#include <gmpxx.h>

struct parse_t;

typedef std::string val_t;
typedef std::vector<parse_t> args_t;

typedef parse_t command;
typedef parse_t expr;
typedef parse_t test;

namespace checktestdata {

struct none_t {};

std::ostream& operator<<(std::ostream&, const none_t&);

struct value_t {
	boost::variant<none_t, mpz_class, mpf_class, std::string> val;

	value_t(): val(none_t()) {}
	explicit value_t(mpz_class x): val(x) {}
	explicit value_t(mpf_class x): val(x) {}
	explicit value_t(std::string x): val(x) {}

	operator mpz_class() const;
	operator mpf_class() const;

	// This is a member function instead of a casting operator, since
	// otherwise the string could be used in other implicit casts.
	std::string getstr() const;

	// This converts any value type to a string representation.
	std::string tostr() const;
};

const int value_none   = 0;
const int value_int    = 1;
const int value_float  = 2;
const int value_string = 3;

} // namespace checktestdata

std::ostream &operator<<(std::ostream &, const parse_t &);

struct parse_t {
	val_t val;
	args_t args;
	char op;
	/*
	  Operator/type of this node, can be any of the following:

	  +-*%/^ standard binary arithmetic operations
	  n      (unary) negation

	  ?      a comparison operator stored in 'val'
	  |&!    logical AND,OR,NOT
	  EMUA   EOF,MATCH,UNIQUE,INARRAY keywords used within test expressions

	  I      integer constant
	  F      float constant
	  S      string constant

	  a      variable assigment with two arguments: variable name, expression.
	  f      function returning a value
	  l      list of expressions (e.g. for array indices or argument list)
	  v      variable with array indices in second argument
	  @      command with list of arguments provided in second argument,
	  ' '    command
	  ~      uninitialized object, to detect unset default arguments
	*/

	mutable checktestdata::value_t cache;

	parse_t(): val(), args(), op('~') {}
	parse_t(args_t _args): val(), args(_args), op(' ') {}
	parse_t(val_t _val, args_t _args): val(_val), args(_args), op(' ') {}

	// Parsing command with optional arguments
	explicit
	parse_t(val_t _val, parse_t arg1 = parse_t(),
	                    parse_t arg2 = parse_t(),
	                    parse_t arg3 = parse_t(),
	                    parse_t arg4 = parse_t(),
	                    parse_t arg5 = parse_t(),
	                    parse_t arg6 = parse_t())
	: val(_val), args(), op(' ')
	{
		if ( arg1.op!='~' ) args.push_back(arg1);
		if ( arg2.op!='~' ) args.push_back(arg2);
		if ( arg3.op!='~' ) args.push_back(arg3);
		if ( arg4.op!='~' ) args.push_back(arg4);
		if ( arg5.op!='~' ) args.push_back(arg5);
		if ( arg6.op!='~' ) args.push_back(arg6);
	}

	// Parsing arithmetic/logical/compare operator and some other
	// special cases
	explicit
	parse_t(char _op, parse_t arg1 = parse_t(),
	                  parse_t arg2 = parse_t(),
	                  parse_t arg3 = parse_t(),
	                  parse_t arg4 = parse_t(),
	                  parse_t arg5 = parse_t(),
	                  parse_t arg6 = parse_t())
	: val(), args(), op(_op)
	{
		switch ( op ) {
		case 'l': // list: create new or append one argument
			if ( arg2.op=='~' ) {
				if ( arg1.op!='~' ) args.push_back(arg1);
			} else {
				args = arg1.args;
				args.push_back(arg2);
			}
			break;

		case 'I': // integer, float, string literal values
		case 'F':
		case 'S':
			val = arg1.val;
			break;

		case 'a': // variable assignment
			args.push_back(arg1);
			args.push_back(arg2);
			break;

		case 'v': // variable, read index from arg2 if present
			val = arg1.val;
			if ( arg2.op=='l' ) args = arg2.args;
			break;

		case '@': // Command with argument list in arg2
			op = ' ';
			val = arg1.val;
			args = arg2.args;
			break;

		case 'U': // UNIQUE test, has argument list in arg1
			args = arg1.args;
			break;

		case '?': // comparison operator as arg1
			val = arg1.val;
			args.push_back(arg2);
			args.push_back(arg3);
			break;

		default:
			if ( arg1.op!='~' ) args.push_back(arg1);
			if ( arg2.op!='~' ) args.push_back(arg2);
			if ( arg3.op!='~' ) args.push_back(arg3);
			if ( arg4.op!='~' ) args.push_back(arg4);
			if ( arg5.op!='~' ) args.push_back(arg5);
			if ( arg6.op!='~' ) args.push_back(arg6);
		}
	}

	const val_t& name()  const { return val; }
	size_t       nargs() const { return args.size(); }
	const char*  c_str() const { return val.c_str(); }

	operator const std::string& () const { return val; }
};

#endif /* PARSETYPE_HPP */


/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

#include <iostream>
#include <iomanip>
#include <cmath>
#include <iterator>
#include <algorithm>

#include <Eigen/SVD>
#include <Eigen/QR>
#include <Eigen/SparseCore>
#include <Eigen/SPQRSupport>
#include <boost/math/distributions/chi_squared.hpp>

#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

#include <msckf_vio/msckf_vio.h>
#include <msckf_vio/math_utils.hpp>
#include <msckf_vio/utils.h>

using namespace std;
using namespace Eigen;

namespace msckf_vio{
// Static member variables in IMUState class.
StateIDType IMUState::next_id = 0;
double IMUState::gyro_noise = 0.001;
double IMUState::acc_noise = 0.01;
double IMUState::gyro_bias_noise = 0.001;
double IMUState::acc_bias_noise = 0.01;
Vector3d IMUState::gravity = Vector3d(0, 0, -GRAVITY_ACCELERATION);
Isometry3d IMUState::T_imu_body = Isometry3d::Identity();

// Static member variables in CAMState class.
Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();

// Static member variables in Feature class.
FeatureIDType Feature::next_id = 0;
double Feature::observation_noise = 0.01;
Feature::OptimizationConfig Feature::optimization_config;

map<int, double> MsckfVio::chi_squared_test_table;

MsckfVio::MsckfVio(ros::NodeHandle& pnh):
  is_gravity_set(false),
  is_first_img(true),
  nh(pnh) {
  return;
}

// 导入各种参数，包括阈值、传感器误差标准差等
bool MsckfVio::loadParameters() {
  // Frame id
  nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
  nh.param<string>("child_frame_id", child_frame_id, "robot");
  nh.param<bool>("publish_tf", publish_tf, true);
  nh.param<double>("frame_rate", frame_rate, 40.0);
  nh.param<double>("position_std_threshold", position_std_threshold, 8.0);

  nh.param<double>("rotation_threshold", rotation_threshold, 0.2618);	// QXC：大约为15°
  nh.param<double>("translation_threshold", translation_threshold, 0.4);
  nh.param<double>("tracking_rate_threshold", tracking_rate_threshold, 0.5);

  // Feature optimization parameters
  nh.param<double>("feature/config/translation_threshold",
      Feature::optimization_config.translation_threshold, 0.2);

  // Noise related parameters
  nh.param<double>("noise/gyro", IMUState::gyro_noise, 0.001);
  nh.param<double>("noise/acc", IMUState::acc_noise, 0.01);
  nh.param<double>("noise/gyro_bias", IMUState::gyro_bias_noise, 0.001);
  nh.param<double>("noise/acc_bias", IMUState::acc_bias_noise, 0.01);
  nh.param<double>("noise/feature", Feature::observation_noise, 0.01);

  // Use variance instead of standard deviation.
  IMUState::gyro_noise *= IMUState::gyro_noise;
  IMUState::acc_noise *= IMUState::acc_noise;
  IMUState::gyro_bias_noise *= IMUState::gyro_bias_noise;
  IMUState::acc_bias_noise *= IMUState::acc_bias_noise;
  Feature::observation_noise *= Feature::observation_noise;

  // Set the initial IMU state.
  // The intial orientation and position will be set to the origin
  // implicitly. But the initial velocity and bias can be
  // set by parameters.
  // TODO: is it reasonable to set the initial bias to 0?
  nh.param<double>("initial_state/velocity/x",
      state_server.imu_state.velocity(0), 0.0);
  nh.param<double>("initial_state/velocity/y",
      state_server.imu_state.velocity(1), 0.0);
  nh.param<double>("initial_state/velocity/z",
      state_server.imu_state.velocity(2), 0.0);

  // The initial covariance of orientation and position can be
  // set to 0. But for velocity, bias and extrinsic parameters,
  // there should be nontrivial uncertainty.
  double gyro_bias_cov, acc_bias_cov, velocity_cov;
  nh.param<double>("initial_covariance/velocity",
      velocity_cov, 0.25);
  nh.param<double>("initial_covariance/gyro_bias",
      gyro_bias_cov, 1e-4);
  nh.param<double>("initial_covariance/acc_bias",
      acc_bias_cov, 1e-2);

  double extrinsic_rotation_cov, extrinsic_translation_cov;
  nh.param<double>("initial_covariance/extrinsic_rotation_cov",
      extrinsic_rotation_cov, 3.0462e-4);	// QXC：大约为1°的平方
  nh.param<double>("initial_covariance/extrinsic_translation_cov",
      extrinsic_translation_cov, 1e-4);

  state_server.state_cov = MatrixXd::Zero(21, 21);
  for (int i = 3; i < 6; ++i)
    state_server.state_cov(i, i) = gyro_bias_cov;
  for (int i = 6; i < 9; ++i)
    state_server.state_cov(i, i) = velocity_cov;
  for (int i = 9; i < 12; ++i)
    state_server.state_cov(i, i) = acc_bias_cov;
  for (int i = 15; i < 18; ++i)
    state_server.state_cov(i, i) = extrinsic_rotation_cov;
  for (int i = 18; i < 21; ++i)
    state_server.state_cov(i, i) = extrinsic_translation_cov;

  // Transformation offsets between the frames involved.
  Isometry3d T_imu_cam0 = utils::getTransformEigen(nh, "cam0/T_cam_imu");
  Isometry3d T_cam0_imu = T_imu_cam0.inverse();

  state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
  state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
  CAMState::T_cam0_cam1 =
    utils::getTransformEigen(nh, "cam1/T_cn_cnm1");
  IMUState::T_imu_body =
    utils::getTransformEigen(nh, "T_imu_body").inverse();

  // Maximum number of camera states to be stored
  nh.param<int>("max_cam_state_size", max_cam_state_size, 30);

  ROS_INFO("===========================================");
  ROS_INFO("fixed frame id: %s", fixed_frame_id.c_str());
  ROS_INFO("child frame id: %s", child_frame_id.c_str());
  ROS_INFO("publish tf: %d", publish_tf);
  ROS_INFO("frame rate: %f", frame_rate);
  ROS_INFO("position std threshold: %f", position_std_threshold);
  ROS_INFO("Keyframe rotation threshold: %f", rotation_threshold);
  ROS_INFO("Keyframe translation threshold: %f", translation_threshold);
  ROS_INFO("Keyframe tracking rate threshold: %f", tracking_rate_threshold);
  ROS_INFO("gyro noise: %.10f", IMUState::gyro_noise);
  ROS_INFO("gyro bias noise: %.10f", IMUState::gyro_bias_noise);
  ROS_INFO("acc noise: %.10f", IMUState::acc_noise);
  ROS_INFO("acc bias noise: %.10f", IMUState::acc_bias_noise);
  ROS_INFO("observation noise: %.10f", Feature::observation_noise);
  ROS_INFO("initial velocity: %f, %f, %f",
      state_server.imu_state.velocity(0),
      state_server.imu_state.velocity(1),
      state_server.imu_state.velocity(2));
  ROS_INFO("initial gyro bias cov: %f", gyro_bias_cov);
  ROS_INFO("initial acc bias cov: %f", acc_bias_cov);
  ROS_INFO("initial velocity cov: %f", velocity_cov);
  ROS_INFO("initial extrinsic rotation cov: %f",
      extrinsic_rotation_cov);
  ROS_INFO("initial extrinsic translation cov: %f",
      extrinsic_translation_cov);

  cout << T_imu_cam0.linear() << endl;
  cout << T_imu_cam0.translation().transpose() << endl;

  ROS_INFO("max camera state #: %d", max_cam_state_size);
  ROS_INFO("===========================================");
  return true;
}

// 声明本节点开始发布和订阅的各种topic
bool MsckfVio::createRosIO() {
  odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);	// QXC：开始发布名为odom的topic，其消息类型为nav_msgs::Odometry，最大缓存为10
  feature_pub = nh.advertise<sensor_msgs::PointCloud2>(		// QXC：开始发布名为feature_point_cloud的topic，其消息类型为sensor_msgs::PointCloud2，
      "feature_point_cloud", 10);				            //	    最大缓存为10

  reset_srv = nh.advertiseService("reset",
      &MsckfVio::resetCallback, this);

  imu_sub = nh.subscribe("imu", 100,				// QXC：开始订阅名为imu的topic，最大缓存为100
      &MsckfVio::imuCallback, this);
  feature_sub = nh.subscribe("features", 40,			// QXC：开始订阅名为features的topic，最大缓存为40
      &MsckfVio::featureCallback, this);

  mocap_odom_sub = nh.subscribe("mocap_odom", 10,		// QXC：开始订阅名为mocap_odom的topic，最大缓存为10
      &MsckfVio::mocapOdomCallback, this);
  mocap_odom_pub = nh.advertise<nav_msgs::Odometry>("gt_odom", 1);	// QXC：开始发布名为gt_odom的topic，其消息类型为nav_msgs::Odometry，最大缓存为1

  return true;
}

// 调用loadParameters()函数导入各种参数，调用createRosIO()函数发布和订阅各种topic
bool MsckfVio::initialize() {
  if (!loadParameters()) return false;
  ROS_INFO("Finish loading ROS parameters...");

  // Initialize state server
  state_server.continuous_noise_cov =
    Matrix<double, 12, 12>::Zero();
  state_server.continuous_noise_cov.block<3, 3>(0, 0) =
    Matrix3d::Identity()*IMUState::gyro_noise;
  state_server.continuous_noise_cov.block<3, 3>(3, 3) =
    Matrix3d::Identity()*IMUState::gyro_bias_noise;
  state_server.continuous_noise_cov.block<3, 3>(6, 6) =
    Matrix3d::Identity()*IMUState::acc_noise;
  state_server.continuous_noise_cov.block<3, 3>(9, 9) =
    Matrix3d::Identity()*IMUState::acc_bias_noise;

  // Initialize the chi squared test table with confidence
  // level 0.95.
  for (int i = 1; i < 100; ++i) {
    boost::math::chi_squared chi_squared_dist(i);
    chi_squared_test_table[i] =
      boost::math::quantile(chi_squared_dist, 0.05);
  }

  if (!createRosIO()) return false;
  ROS_INFO("Finish creating ROS IO...");

  return true;
}

// 当接收到topic：imu时调用的函数，静止时的前200条进行初始化（陀螺bias、重力、初始姿态）工作，其他时间只管将imu数据压入容器中
void MsckfVio::imuCallback(
    const sensor_msgs::ImuConstPtr& msg) {

  // IMU msgs are pushed backed into a buffer instead of
  // being processed immediately. The IMU msgs are processed
  // when the next image is available, in which way, we can
  // easily handle the transfer delay.
  imu_msg_buffer.push_back(*msg);

  if (!is_gravity_set) {
    if (imu_msg_buffer.size() < 200) return;	// QXC：IMU数据不足200条时不对重力和bias进行初始化
    //if (imu_msg_buffer.size() < 10) return;
    initializeGravityAndBias();		// QXC：进行初始化的IMU数据必须是静止时采集的
    is_gravity_set = true;
  }

  return;
}

// 初始化陀螺仪bias、重力以及初始姿态，可以看出这里需要利用静止时IMU采集的数据
// QXC：这个初始化方法不对加计零偏做补偿，可能会出问题
void MsckfVio::initializeGravityAndBias() {

  // Initialize gravity and gyro bias.
  Vector3d sum_angular_vel = Vector3d::Zero();
  Vector3d sum_linear_acc = Vector3d::Zero();

  for (const auto& imu_msg : imu_msg_buffer) {
    Vector3d angular_vel = Vector3d::Zero();
    Vector3d linear_acc = Vector3d::Zero();

    tf::vectorMsgToEigen(imu_msg.angular_velocity, angular_vel);	// QXC：该函数来自<eigen_conversions/eigen_msg.h>
    tf::vectorMsgToEigen(imu_msg.linear_acceleration, linear_acc);

    sum_angular_vel += angular_vel;
    sum_linear_acc += linear_acc;
  }

  state_server.imu_state.gyro_bias =
    sum_angular_vel / imu_msg_buffer.size();
  //IMUState::gravity =
  //  -sum_linear_acc / imu_msg_buffer.size();
  // This is the gravity in the IMU frame.
  Vector3d gravity_imu =
    sum_linear_acc / imu_msg_buffer.size();

  // Initialize the initial orientation, so that the estimation
  // is consistent with the inertial frame.
  double gravity_norm = gravity_imu.norm();
  IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);

  Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
    gravity_imu, -IMUState::gravity);	// QXC：Quaterniond及其相关方法来自EIgen，返回的是C_b2w对应的四元数，但注意Eigen的四元数采用HN，且与旋转矩阵的转换采用罗德里格斯公式
  state_server.imu_state.orientation =
    rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());    // QXC：该函数为本程序自带，此处传入的参数是C_w2b

  return;
}

// 响应重置消息的回调函数，重置整个vio
bool MsckfVio::resetCallback(
    std_srvs::Trigger::Request& req,
    std_srvs::Trigger::Response& res) {

  ROS_WARN("Start resetting msckf vio...");
  // Temporarily shutdown the subscribers to prevent the
  // state from updating.
  feature_sub.shutdown();
  imu_sub.shutdown();

  // Reset the IMU state.
  IMUState& imu_state = state_server.imu_state;
  imu_state.time = 0.0;
  imu_state.orientation = Vector4d(0.0, 0.0, 0.0, 1.0);
  imu_state.position = Vector3d::Zero();
  imu_state.velocity = Vector3d::Zero();
  imu_state.gyro_bias = Vector3d::Zero();
  imu_state.acc_bias = Vector3d::Zero();
  imu_state.orientation_null = Vector4d(0.0, 0.0, 0.0, 1.0);
  imu_state.position_null = Vector3d::Zero();
  imu_state.velocity_null = Vector3d::Zero();

  // Remove all existing camera states.
  state_server.cam_states.clear();

  // Reset the state covariance.
  double gyro_bias_cov, acc_bias_cov, velocity_cov;
  nh.param<double>("initial_covariance/velocity",
      velocity_cov, 0.25);
  nh.param<double>("initial_covariance/gyro_bias",
      gyro_bias_cov, 1e-4);
  nh.param<double>("initial_covariance/acc_bias",
      acc_bias_cov, 1e-2);

  double extrinsic_rotation_cov, extrinsic_translation_cov;
  nh.param<double>("initial_covariance/extrinsic_rotation_cov",
      extrinsic_rotation_cov, 3.0462e-4);
  nh.param<double>("initial_covariance/extrinsic_translation_cov",
      extrinsic_translation_cov, 1e-4);

  state_server.state_cov = MatrixXd::Zero(21, 21);
  for (int i = 3; i < 6; ++i)
    state_server.state_cov(i, i) = gyro_bias_cov;
  for (int i = 6; i < 9; ++i)
    state_server.state_cov(i, i) = velocity_cov;
  for (int i = 9; i < 12; ++i)
    state_server.state_cov(i, i) = acc_bias_cov;
  for (int i = 15; i < 18; ++i)
    state_server.state_cov(i, i) = extrinsic_rotation_cov;
  for (int i = 18; i < 21; ++i)
    state_server.state_cov(i, i) = extrinsic_translation_cov;

  // Clear all exsiting features in the map.
  map_server.clear();

  // Clear the IMU msg buffer.
  imu_msg_buffer.clear();

  // Reset the starting flags.
  is_gravity_set = false;
  is_first_img = true;

  // Restart the subscribers.
  imu_sub = nh.subscribe("imu", 100,
      &MsckfVio::imuCallback, this);            // QXC：重新开始订阅imu消息
  feature_sub = nh.subscribe("features", 40,
      &MsckfVio::featureCallback, this);        // QXC：重新开始订阅feature消息

  // TODO: When can the reset fail?
  res.success = true;
  ROS_WARN("Resetting msckf vio completed...");
  return true;
}

// 根据到来的新一帧的features，进行处理，包括：积分上帧到本帧之间的IMU数据；计算并将当前帧状态扩维到系统状态中；更新feature观测信息；
// 依据不再跟踪的feature的测量进行MSCKF的测量更新；当扩维的cam达到最大值时剔除部分cam状态，并依据与这些cam状态相关联的一些feature进行MSCKF测量更新；
// 发布本节点应当发布的一些消息；根据IMU状态位置协方差判断是否需要重置整个系统。
void MsckfVio::featureCallback(
    const CameraMeasurementConstPtr& msg) {

  // Return if the gravity vector has not been set.
  if (!is_gravity_set) return;		// QXC：IMU姿态初始化之前不处理feature消息

  // Start the system if the first image is received.
  // The frame where the first image is received will be
  // the origin.
  if (is_first_img) {
    is_first_img = false;
    state_server.imu_state.time = msg->header.stamp.toSec();
  }

  static double max_processing_time = 0.0;
  static int critical_time_cntr = 0;
  double processing_start_time = ros::Time::now().toSec();

  // Propogate the IMU state.
  // that are received before the image msg.
  ros::Time start_time = ros::Time::now();
  batchImuProcessing(msg->header.stamp.toSec());	// QXC：对上一帧时刻之后、当前帧时刻之前的IMU数据进行积分
  double imu_processing_time = (
      ros::Time::now()-start_time).toSec();     // QXC：获取处理时间

  // Augment the state vector.      // QXC：featureCallback每次都调用这个函数，可见每一帧的相机状态是一直都被增广的
  start_time = ros::Time::now();
  stateAugmentation(msg->header.stamp.toSec());		// QXC：根据当前IMU状态计算相机状态，同时更新増广协方差矩阵
  double state_augmentation_time = (
      ros::Time::now()-start_time).toSec();

  // Add new observations for existing features or new
  // features in the map server.
  start_time = ros::Time::now();
  addFeatureObservations(msg);                      // QXC：为map_server添加新的特征点观测（可能是旧特征点的新观测，也可能是新特征点首次观测到）
  double add_observations_time = (
      ros::Time::now()-start_time).toSec();

  // Perform measurement update if necessary.
  start_time = ros::Time::now();
  removeLostFeatures();                             // QXC：利用当前帧不再能跟踪到的feature进行MSCKF的测量更新
  double remove_lost_features_time = (
      ros::Time::now()-start_time).toSec();

  start_time = ros::Time::now();
  pruneCamStateBuffer();        // QXC：当cam状态数达到最大值时，挑出若干cam状态待删除，并基于能被2帧以上这些cam观测到的feature进行MSCKF测量更新
  double prune_cam_states_time = (
      ros::Time::now()-start_time).toSec();

  // Publish the odometry.
  start_time = ros::Time::now();
  publish(msg->header.stamp);                       // QXC：开始发布tf、odometry和特征点云等消息
  double publish_time = (
      ros::Time::now()-start_time).toSec();

  // Reset the system if necessary.
  onlineReset();                                    // QXC：根据IMU状态位置协方差判断是否重置整个系统

  double processing_end_time = ros::Time::now().toSec();
  double processing_time =
    processing_end_time - processing_start_time;
  if (processing_time > 1.0/frame_rate) {
    ++critical_time_cntr;
    ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
        processing_time, critical_time_cntr);
    //printf("IMU processing time: %f/%f\n",
    //    imu_processing_time, imu_processing_time/processing_time);
    //printf("State augmentation time: %f/%f\n",
    //    state_augmentation_time, state_augmentation_time/processing_time);
    //printf("Add observations time: %f/%f\n",
    //    add_observations_time, add_observations_time/processing_time);
    printf("Remove lost features time: %f/%f\n",
        remove_lost_features_time, remove_lost_features_time/processing_time);
    printf("Remove camera states time: %f/%f\n",
        prune_cam_states_time, prune_cam_states_time/processing_time);
    //printf("Publish time: %f/%f\n",
    //    publish_time, publish_time/processing_time);
  }

  return;
}

// 与odometry真值相关的方法
void MsckfVio::mocapOdomCallback(
    const nav_msgs::OdometryConstPtr& msg) {
  static bool first_mocap_odom_msg = true;

  // If this is the first mocap odometry messsage, set
  // the initial frame.
  if (first_mocap_odom_msg) {
    Quaterniond orientation;
    Vector3d translation;
    tf::pointMsgToEigen(
        msg->pose.pose.position, translation);
    tf::quaternionMsgToEigen(
        msg->pose.pose.orientation, orientation);
    //tf::vectorMsgToEigen(
    //    msg->transform.translation, translation);
    //tf::quaternionMsgToEigen(
    //    msg->transform.rotation, orientation);
    mocap_initial_frame.linear() = orientation.toRotationMatrix();
    mocap_initial_frame.translation() = translation;
    first_mocap_odom_msg = false;
  }

  // Transform the ground truth.
  Quaterniond orientation;
  Vector3d translation;
  //tf::vectorMsgToEigen(
  //    msg->transform.translation, translation);
  //tf::quaternionMsgToEigen(
  //    msg->transform.rotation, orientation);
  tf::pointMsgToEigen(
      msg->pose.pose.position, translation);
  tf::quaternionMsgToEigen(
      msg->pose.pose.orientation, orientation);

  Eigen::Isometry3d T_b_v_gt;
  T_b_v_gt.linear() = orientation.toRotationMatrix();
  T_b_v_gt.translation() = translation;
  Eigen::Isometry3d T_b_w_gt = mocap_initial_frame.inverse() * T_b_v_gt;

  //Eigen::Vector3d body_velocity_gt;
  //tf::vectorMsgToEigen(msg->twist.twist.linear, body_velocity_gt);
  //body_velocity_gt = mocap_initial_frame.linear().transpose() *
  //  body_velocity_gt;

  // Ground truth tf.
  if (publish_tf) {
    tf::Transform T_b_w_gt_tf;
    tf::transformEigenToTF(T_b_w_gt, T_b_w_gt_tf);
    tf_pub.sendTransform(tf::StampedTransform(
          T_b_w_gt_tf, msg->header.stamp, fixed_frame_id, child_frame_id+"_mocap"));
  }

  // Ground truth odometry.
  nav_msgs::Odometry mocap_odom_msg;
  mocap_odom_msg.header.stamp = msg->header.stamp;
  mocap_odom_msg.header.frame_id = fixed_frame_id;
  mocap_odom_msg.child_frame_id = child_frame_id+"_mocap";

  tf::poseEigenToMsg(T_b_w_gt, mocap_odom_msg.pose.pose);
  //tf::vectorEigenToMsg(body_velocity_gt,
  //    mocap_odom_msg.twist.twist.linear);

  mocap_odom_pub.publish(mocap_odom_msg);
  return;
}

// 对上一帧图像之前最后一条IMU数据之后的，当前帧图像时刻之前的IMU数据进行惯性递推解算，同时更新状态协方差矩阵
void MsckfVio::batchImuProcessing(const double& time_bound) {
  // Counter how many IMU msgs in the buffer are used.
  int used_imu_msg_cntr = 0;

  for (const auto& imu_msg : imu_msg_buffer) {
    double imu_time = imu_msg.header.stamp.toSec();
    if (imu_time < state_server.imu_state.time) {
      ++used_imu_msg_cntr;
      continue;
    }
    if (imu_time > time_bound) break;

    // QXC：注意，在首次处理feature而调用本函数时，time_bound与state_server.imu_state.time相等，
      //    因此上述两个if将会漏掉一种极端情况，即某条IMU数据时间戳正好和feature时间戳相等，但这种情况下，根据processModel的分析，将不会进行任何递推

    // Convert the msgs.
    Vector3d m_gyro, m_acc;
    tf::vectorMsgToEigen(imu_msg.angular_velocity, m_gyro);         // QXC：tf系列函数来自ros自身
    tf::vectorMsgToEigen(imu_msg.linear_acceleration, m_acc);

    // Execute process model.
    processModel(imu_time, m_gyro, m_acc);      // QXC：注意这个函数是递推一条IMI数据，而不是递推一系列（整个for循环实现递推一系列）
    ++used_imu_msg_cntr;
  }

  // Set the state ID for the new IMU state.
  state_server.imu_state.id = IMUState::next_id++;

  // Remove all used IMU msgs.
  imu_msg_buffer.erase(imu_msg_buffer.begin(),
      imu_msg_buffer.begin()+used_imu_msg_cntr);

  return;
}

// 进行惯性递推和状态协方差阵递推
// 首次调用该函数时，可能遇到”time==imu_state.time“的情形（存在和第一个被处理的feature时间戳相同的IMU数据），这时会使得dtime为0，从而并不发生什么递推
void MsckfVio::processModel(const double& time,
    const Vector3d& m_gyro,
    const Vector3d& m_acc) {

  // Remove the bias from the measured gyro and acceleration
  IMUState& imu_state = state_server.imu_state;
  Vector3d gyro = m_gyro - imu_state.gyro_bias;
  Vector3d acc = m_acc - imu_state.acc_bias;		// QXC：在没开始滤波之前，acc_bias始终是0
  double dtime = time - imu_state.time;		// QXC：两个IMU测量之间的时间间隔（除了在第一帧之后的第一条IMU数据，此时算出的是和第一帧之间的时间差）

  // Compute discrete transition and noise covariance matrix
  Matrix<double, 21, 21> F = Matrix<double, 21, 21>::Zero();
  Matrix<double, 21, 12> G = Matrix<double, 21, 12>::Zero();

  F.block<3, 3>(0, 0) = -skewSymmetric(gyro);
  F.block<3, 3>(0, 3) = -Matrix3d::Identity();
  F.block<3, 3>(6, 0) = -quaternionToRotation(
      imu_state.orientation).transpose()*skewSymmetric(acc);
  F.block<3, 3>(6, 9) = -quaternionToRotation(		// QXC：注意！！这里的F阵中没有考虑地球自转！！！
      imu_state.orientation).transpose();
  F.block<3, 3>(12, 6) = Matrix3d::Identity();

  G.block<3, 3>(0, 0) = -Matrix3d::Identity();
  G.block<3, 3>(3, 3) = Matrix3d::Identity();
  G.block<3, 3>(6, 6) = -quaternionToRotation(
      imu_state.orientation).transpose();
  G.block<3, 3>(9, 9) = Matrix3d::Identity();

  // Approximate matrix exponential to the 3rd order,
  // which can be considered to be accurate enough assuming
  // dtime is within 0.01s.
  Matrix<double, 21, 21> Fdt = F * dtime;
  Matrix<double, 21, 21> Fdt_square = Fdt * Fdt;
  Matrix<double, 21, 21> Fdt_cube = Fdt_square * Fdt;
  Matrix<double, 21, 21> Phi = Matrix<double, 21, 21>::Identity() +
    Fdt + 0.5*Fdt_square + (1.0/6.0)*Fdt_cube;

  // Propogate the state using 4th order Runge-Kutta
  predictNewState(dtime, gyro, acc);

  // Modify the transition matrix	// QXC：这部分完全没看懂
  Matrix3d R_kk_1 = quaternionToRotation(imu_state.orientation_null);
  Phi.block<3, 3>(0, 0) =
    quaternionToRotation(imu_state.orientation) * R_kk_1.transpose();	// QXC：这么一改，相当于F矩阵对角线上首个3*3矩阵压根没用了

  Vector3d u = R_kk_1 * IMUState::gravity;
  RowVector3d s = (u.transpose()*u).inverse() * u.transpose();

  Matrix3d A1 = Phi.block<3, 3>(6, 0);
  Vector3d w1 = skewSymmetric(
      imu_state.velocity_null-imu_state.velocity) * IMUState::gravity;
  Phi.block<3, 3>(6, 0) = A1 - (A1*u-w1)*s;	// QXC：看不懂，但知道对于Phi阵中的这一部分，F矩阵对角线上首个3*3矩阵也没有贡献了

  Matrix3d A2 = Phi.block<3, 3>(12, 0);
  Vector3d w2 = skewSymmetric(
      dtime*imu_state.velocity_null+imu_state.position_null-
      imu_state.position) * IMUState::gravity;
  Phi.block<3, 3>(12, 0) = A2 - (A2*u-w2)*s;	// QXC：同上

  // Propogate the state covariance matrix.
  Matrix<double, 21, 21> Q = Phi*G*state_server.continuous_noise_cov*
    G.transpose()*Phi.transpose()*dtime;        // QXC：用常值矩阵模型估算Q阵，Q=Phi*G*q*
  state_server.state_cov.block<21, 21>(0, 0) =
    Phi*state_server.state_cov.block<21, 21>(0, 0)*Phi.transpose() + Q;

  if (state_server.cam_states.size() > 0) {	// QXC：当状态量扩维了相机状态时，还需要更新扩维的P阵
    state_server.state_cov.block(	// QXC：block(i,j,p,q)函数表示从矩阵的第i行第j列元素开始，取p行，q列
        0, 21, 21, state_server.state_cov.cols()-21) =
      Phi * state_server.state_cov.block(
        0, 21, 21, state_server.state_cov.cols()-21);
    state_server.state_cov.block(
        21, 0, state_server.state_cov.rows()-21, 21) =
      state_server.state_cov.block(
        21, 0, state_server.state_cov.rows()-21, 21) * Phi.transpose();
  }

  MatrixXd state_cov_fixed = (state_server.state_cov +		// QXC：保持对称性
      state_server.state_cov.transpose()) / 2.0;
  state_server.state_cov = state_cov_fixed;

  // Update the state correspondes to null space.	// QXC：这样看来，这个所谓的null space不过就是保存上一时刻的状态罢了。。
  imu_state.orientation_null = imu_state.orientation;
  imu_state.position_null = imu_state.position;
  imu_state.velocity_null = imu_state.velocity;

  // Update the state info
  state_server.imu_state.time = time;       // QXC：imu状态时间更新到递推完的这条IMU数据时间
  return;
}

// 进行惯性递推，其中姿态四元数采用毕卡法更新，速度和位置采用匀加速度假设的4阶LK法
void MsckfVio::predictNewState(const double& dt,
    const Vector3d& gyro,
    const Vector3d& acc) {

  // TODO: Will performing the forward integration using
  //    the inverse of the quaternion give better accuracy?
  double gyro_norm = gyro.norm();
  Matrix4d Omega = Matrix4d::Zero();
  Omega.block<3, 3>(0, 0) = -skewSymmetric(gyro);
  Omega.block<3, 1>(0, 3) = gyro;
  Omega.block<1, 3>(3, 0) = -gyro;

  Vector4d& q = state_server.imu_state.orientation;
  Vector3d& v = state_server.imu_state.velocity;
  Vector3d& p = state_server.imu_state.position;

  // Some pre-calculation
  Vector4d dq_dt, dq_dt2;
  if (gyro_norm > 1e-5) {
    dq_dt = (cos(gyro_norm*dt*0.5)*Matrix4d::Identity() +
      1/gyro_norm*sin(gyro_norm*dt*0.5)*Omega) * q;	// QXC：四元数的毕卡法更新，参见秦永元《惯性导航》第一版P301式(9.2.52)，注意角增量的计算
    dq_dt2 = (cos(gyro_norm*dt*0.25)*Matrix4d::Identity() +
      1/gyro_norm*sin(gyro_norm*dt*0.25)*Omega) * q;	// QXC：dt/2时的四元数毕卡法更新
  }
  else {	// QXC：当角增量很小时的近似，实部项没有做近似，虚部项使用了洛必达法则！
    dq_dt = (Matrix4d::Identity()+0.5*dt*Omega) *
      cos(gyro_norm*dt*0.5) * q;
    dq_dt2 = (Matrix4d::Identity()+0.25*dt*Omega) *
      cos(gyro_norm*dt*0.25) * q;
  }
  Matrix3d dR_dt_transpose = quaternionToRotation(dq_dt).transpose();
  Matrix3d dR_dt2_transpose = quaternionToRotation(dq_dt2).transpose();

  // k1 = f(tn, yn)
  Vector3d k1_v_dot = quaternionToRotation(q).transpose()*acc +
    IMUState::gravity;
  Vector3d k1_p_dot = v;
					// QXC：这里的4阶LK法用了匀加速度假设，即认为前一时刻的加速度和当前时刻相等！
  // k2 = f(tn+dt/2, yn+k1*dt/2)
  Vector3d k1_v = v + k1_v_dot*dt/2;
  Vector3d k2_v_dot = dR_dt2_transpose*acc +
    IMUState::gravity;
  Vector3d k2_p_dot = k1_v;

  // k3 = f(tn+dt/2, yn+k2*dt/2)
  Vector3d k2_v = v + k2_v_dot*dt/2;
  Vector3d k3_v_dot = dR_dt2_transpose*acc +
    IMUState::gravity;
  Vector3d k3_p_dot = k2_v;

  // k4 = f(tn+dt, yn+k3*dt)
  Vector3d k3_v = v + k3_v_dot*dt;
  Vector3d k4_v_dot = dR_dt_transpose*acc +
    IMUState::gravity;
  Vector3d k4_p_dot = k3_v;

  // yn+1 = yn + dt/6*(k1+2*k2+2*k3+k4)
  q = dq_dt;				// QXC：所谓的4阶LK法只针对速度和位置使用，并未针对姿态做，姿态只用了前面的毕卡法更新
  quaternionNormalize(q);	// QXC：来自本工程中的math_utils.hpp
  v = v + dt/6*(k1_v_dot+2*k2_v_dot+2*k3_v_dot+k4_v_dot);
  p = p + dt/6*(k1_p_dot+2*k2_p_dot+2*k3_p_dot+k4_p_dot);

  return;
}

// 根据IMU的状态计算相机状态，同时更新增广协方差矩阵
void MsckfVio::stateAugmentation(const double& time) {

  const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0;	// QXC：本程序中的R_a_b理解为a系到b系的旋转矩阵
  const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu;	// QXC：本程序中的t_a_b理解为b系下a系原点的坐标（与orbslam中位姿R-t中的t不同）

  // Add a new camera state to the state server.	// QXC：可以看出，本程序不做任何时间对准，直接用每帧图像前最近的一个IMU状态换算CAM状态
  Matrix3d R_w_i = quaternionToRotation(
      state_server.imu_state.orientation);      // QXC：quaternionToRotation方法为本程序自带，此处返回的参数是C_w2b
  Matrix3d R_w_c = R_i_c * R_w_i;
  Vector3d t_c_w = state_server.imu_state.position +
    R_w_i.transpose()*t_c_i;

  state_server.cam_states[state_server.imu_state.id] =
    CAMState(state_server.imu_state.id);
  CAMState& cam_state = state_server.cam_states[
    state_server.imu_state.id];

  cam_state.time = time;
  cam_state.orientation = rotationToQuaternion(R_w_c);
  cam_state.position = t_c_w;

  cam_state.orientation_null = cam_state.orientation;
  cam_state.position_null = cam_state.position;

  // Update the covariance matrix of the state.
  // To simplify computation, the matrix J below is the nontrivial block
  // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision
  // -aided Inertial Navigation".
  Matrix<double, 6, 21> J = Matrix<double, 6, 21>::Zero();
  J.block<3, 3>(0, 0) = R_i_c;
  J.block<3, 3>(0, 15) = Matrix3d::Identity();		// QXC：这里是关于q_c_i的Jacobian，恐怕不对，应当是。。
  J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose()*t_c_i);
  //J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i);
  J.block<3, 3>(3, 12) = Matrix3d::Identity();
  J.block<3, 3>(3, 18) = Matrix3d::Identity();		// QXC：这里是关于p_c_i的Jacobian，恐怕不对，应当是R_w_i.transpose()

  // Resize the state covariance matrix.
  size_t old_rows = state_server.state_cov.rows();
  size_t old_cols = state_server.state_cov.cols();
  state_server.state_cov.conservativeResize(old_rows+6, old_cols+6);

  // Rename some matrix blocks for convenience.
  const Matrix<double, 21, 21>& P11 =
    state_server.state_cov.block<21, 21>(0, 0);
  const MatrixXd& P12 =
    state_server.state_cov.block(0, 21, 21, old_cols-21);

  // Fill in the augmented state covariance.
  state_server.state_cov.block(old_rows, 0, 6, old_cols) << J*P11, J*P12;	// QXC：PIC（的转置）最初是由J*PIIkk计算得到的。类似测量值与状态的协方差
  state_server.state_cov.block(0, old_cols, old_rows, 6) =
    state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose();
  state_server.state_cov.block<6, 6>(old_rows, old_cols) =
    J * P11 * J.transpose();

  // Fix the covariance to be symmetric
  MatrixXd state_cov_fixed = (state_server.state_cov +
      state_server.state_cov.transpose()) / 2.0;
  state_server.state_cov = state_cov_fixed;

  return;
}

// 依据视觉前端发来的feature（特征点已经进行了相关处理，不同的特征点有不同ID）消息，为map_server添加新的观测（某ID特征点在某ID状态下的像素坐标）
void MsckfVio::addFeatureObservations(
    const CameraMeasurementConstPtr& msg) {

  StateIDType state_id = state_server.imu_state.id;
  int curr_feature_num = map_server.size();
  int tracked_feature_num = 0;

  // Add new observations for existing features or new
  // features in the map server.
  for (const auto& feature : msg->features) {
    if (map_server.find(feature.id) == map_server.end()) {	// QXC：视觉前端处理应当是将所有的特征都编号了，如果没有和原来的匹配结果，就新增一个编号
      // This is a new feature.
      map_server[feature.id] = Feature(feature.id);
      map_server[feature.id].observations[state_id] =
        Vector4d(feature.u0, feature.v0,
            feature.u1, feature.v1);    // QXC：本工程为双目方式
    } else {
      // This is an old feature.
      map_server[feature.id].observations[state_id] =
        Vector4d(feature.u0, feature.v0,
            feature.u1, feature.v1);
      ++tracked_feature_num;
    }
  }

  tracking_rate =					// QXC：跟踪率，表示旧的特征点被跟踪上的比例
    static_cast<double>(tracked_feature_num) /
    static_cast<double>(curr_feature_num);

  return;
}

// 求观测关于cam误差状态（姿态误差角、位置）和feature位置的Jacobian（根据文献TR_MSCKF），并对获得的Jacobian进行修改以满足可观性要求（根据文献OC-VINS）
void MsckfVio::measurementJacobian(
    const StateIDType& cam_state_id,
    const FeatureIDType& feature_id,
    Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {

  // Prepare all the required data.
  const CAMState& cam_state = state_server.cam_states[cam_state_id];
  const Feature& feature = map_server[feature_id];

  // Cam0 pose.
  Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
  const Vector3d& t_c0_w = cam_state.position;

  // Cam1 pose.
  Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
  Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
  Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();  // QXC：T_cam0_cam1表示c1系下c0的位置

  // 3d feature position in the world frame.
  // And its observation with the stereo cameras.
  const Vector3d& p_w = feature.position;   // QXC：feature.position由Mour07的Appendix给出的方法计算出
  const Vector4d& z = feature.observations.find(cam_state_id)->second;

  // Convert the feature position from the world frame to
  // the cam0 and cam1 frame.
  Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
  Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);

  // Compute the Jacobians.
  Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
  dz_dpc0(0, 0) = 1 / p_c0(2);
  dz_dpc0(1, 1) = 1 / p_c0(2);
  dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
  dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));

  Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
  dz_dpc1(2, 0) = 1 / p_c1(2);
  dz_dpc1(3, 1) = 1 / p_c1(2);
  dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
  dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));

  Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
  dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
  dpc0_dxc.rightCols(3) = -R_w_c0;

  Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
  dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0);
  dpc1_dxc.rightCols(3) = -R_w_c1;

  Matrix3d dpc0_dpg = R_w_c0;
  Matrix3d dpc1_dpg = R_w_c1;

  H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
  H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;

  // Modifty the measurement Jacobian to ensure
  // observability constrain.
  Matrix<double, 4, 6> A = H_x;
  Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
  u.block<3, 1>(0, 0) = quaternionToRotation(
      cam_state.orientation_null) * IMUState::gravity;
  u.block<3, 1>(3, 0) = skewSymmetric(
      p_w-cam_state.position_null) * IMUState::gravity;
  H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
  H_f = -H_x.block<4, 3>(0, 3);

  // Compute the residual.
  r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
      p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));

  return;
}

// 求指定的feature的观测关于能观测到它的所有cam的状态及feature位置的Jacobian，并进行null space投影使得feature位置被marginalize掉
void MsckfVio::featureJacobian(
    const FeatureIDType& feature_id,
    const std::vector<StateIDType>& cam_state_ids,
    MatrixXd& H_x, VectorXd& r) {

  const auto& feature = map_server[feature_id];

  // Check how many camera states in the provided camera
  // id camera has actually seen this feature.
  vector<StateIDType> valid_cam_state_ids(0);
  for (const auto& cam_id : cam_state_ids) {    // QXC：输入参数cam_state_ids实际上已经是观测到当前feature的camid了，但这里又确认了一次！是否有必要？？
    if (feature.observations.find(cam_id) ==
        feature.observations.end()) continue;

    valid_cam_state_ids.push_back(cam_id);
  }

  int jacobian_row_size = 0;
  jacobian_row_size = 4 * valid_cam_state_ids.size();   // QXC：这是文献TR_MSCKF中式22的行（双目情形），还未进行null space marginalization

  MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size,
      21+state_server.cam_states.size()*6);
  MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3);
  VectorXd r_j = VectorXd::Zero(jacobian_row_size);
  int stack_cntr = 0;

  for (const auto& cam_id : valid_cam_state_ids) {    // QXC：对所有可观测到当前feature的cam，求feature观测关于cam状态和feature位置的Jacobian

    Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
    Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
    Vector4d r_i = Vector4d::Zero();
    measurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);   // QXC：计算测量值关于cam状态和feature位置的Jacobian，同时做关于可观性的修正

    auto cam_state_iter = state_server.cam_states.find(cam_id);
    int cam_state_cntr = std::distance(
        state_server.cam_states.begin(), cam_state_iter);     // QXC：可见TR_MSCKF式22中的cam位姿包括了所有被增广的cam

    // Stack the Jacobians.
    H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi;
    H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
    r_j.segment<4>(stack_cntr) = r_i;
    stack_cntr += 4;
  }

  // Project the residual and Jacobians onto the nullspace
  // of H_fj.
  JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
  MatrixXd A = svd_helper.matrixU().rightCols(
      jacobian_row_size - 3);

  H_x = A.transpose() * H_xj;     // QXC：本句和下一句参照Mour07式23和24
  r = A.transpose() * r_j;

  return;
}

// 根据Mour07中III-E部分进行MSCKF的测量更新，首先利用QR分解将观测残差方程进一步降维，然后根据新残差方程计算卡尔曼增益，进行IMU状态、cam状态以及P阵更新
void MsckfVio::measurementUpdate(
    const MatrixXd& H, const VectorXd& r) {

  if (H.rows() == 0 || r.rows() == 0) return;

  // Decompose the final Jacobian matrix to reduce computational
  // complexity as in Equation (28), (29).
  MatrixXd H_thin;
  VectorXd r_thin;

  if (H.rows() > H.cols()) {    // QXC：H阵行数超过列数时，才通过H阵的QR分解降维（H的列数是受IMU状态数和cam状态上限限制的，不会过多）
    // Convert H to a sparse matrix.
    SparseMatrix<double> H_sparse = H.sparseView();

    // Perform QR decompostion on H_sparse.
    SPQR<SparseMatrix<double> > spqr_helper;
    spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
    spqr_helper.compute(H_sparse);

    MatrixXd H_temp;
    VectorXd r_temp;
    (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
    (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);

    H_thin = H_temp.topRows(21+state_server.cam_states.size()*6);       // QXC：为什么只取前(21+6N)行呢？
    r_thin = r_temp.head(21+state_server.cam_states.size()*6);

    //HouseholderQR<MatrixXd> qr_helper(H);
    //MatrixXd Q = qr_helper.householderQ();
    //MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6);

    //H_thin = Q1.transpose() * H;
    //r_thin = Q1.transpose() * r;
  } else {
    H_thin = H;
    r_thin = r;
  }

  // Compute the Kalman gain.
  const MatrixXd& P = state_server.state_cov;
  MatrixXd S = H_thin*P*H_thin.transpose() +
      Feature::observation_noise*MatrixXd::Identity(
        H_thin.rows(), H_thin.rows());
  //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P);
  MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
  MatrixXd K = K_transpose.transpose();

  // Compute the error of the state.
  VectorXd delta_x = K * r_thin;

  // Update the IMU state.
  const VectorXd& delta_x_imu = delta_x.head<21>();

  if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
      //delta_x_imu.segment<3>(3).norm() > 0.15 ||
      delta_x_imu.segment<3>(6).norm() > 0.5 ||
      //delta_x_imu.segment<3>(9).norm() > 0.5 ||
      delta_x_imu.segment<3>(12).norm() > 1.0) {
    printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm());
    printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm());
    ROS_WARN("Update change is too large.");
    //return;
  }

  const Vector4d dq_imu =
    smallAngleQuaternion(delta_x_imu.head<3>());
  state_server.imu_state.orientation = quaternionMultiplication(
      dq_imu, state_server.imu_state.orientation);
  state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3);
  state_server.imu_state.velocity += delta_x_imu.segment<3>(6);
  state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9);
  state_server.imu_state.position += delta_x_imu.segment<3>(12);

  const Vector4d dq_extrinsic =
    smallAngleQuaternion(delta_x_imu.segment<3>(15));
  state_server.imu_state.R_imu_cam0 = quaternionToRotation(
      dq_extrinsic) * state_server.imu_state.R_imu_cam0;
  state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18);

  // Update the camera states.
  auto cam_state_iter = state_server.cam_states.begin();
  for (int i = 0; i < state_server.cam_states.size();
      ++i, ++cam_state_iter) {
    const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*6);
    const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
    cam_state_iter->second.orientation = quaternionMultiplication(
        dq_cam, cam_state_iter->second.orientation);
    cam_state_iter->second.position += delta_x_cam.tail<3>();
  }

  // Update state covariance.
  MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
  //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
  //  K*K.transpose()*Feature::observation_noise;       // QXC：有点小错误，应是K*Feature::observation_noise*K.transpose()
  state_server.state_cov = I_KH*state_server.state_cov;     // QXC：参考秦永元《卡尔曼滤波与组合导航》第三版P34式(2.2.4e')

  // Fix the covariance to be symmetric
  MatrixXd state_cov_fixed = (state_server.state_cov +
      state_server.state_cov.transpose()) / 2.0;
  state_server.state_cov = state_cov_fixed;

  return;
}

// 没太看懂具体原理，但应该是用来检测基于H阵的测量预测协方差和残差r是否契合的方法
bool MsckfVio::gatingTest(
    const MatrixXd& H, const VectorXd& r, const int& dof) {

  MatrixXd P1 = H * state_server.state_cov * H.transpose();
  MatrixXd P2 = Feature::observation_noise *
    MatrixXd::Identity(H.rows(), H.rows());
  double gamma = r.transpose() * (P1+P2).ldlt().solve(r);   // QXC：相当于是在算 rT*[(P1+P2)^-1]*r

  //cout << dof << " " << gamma << " " <<
  //  chi_squared_test_table[dof] << " ";

  if (gamma < chi_squared_test_table[dof]) {
    //cout << "passed" << endl;
    return true;
  } else {
    //cout << "failed" << endl;
    return false;
  }
}

// 选出当前帧不再能跟踪到的feature，筛选掉质量不佳者，利用通过筛选的feature计算测量Jacobian，继而进行MSCKF测量更新，最后将这些不再被跟踪的feature删掉。
// 注意，最后被删除的feature包括：失效feature以及进行了测量更新的feature，首先它们得满足不被当前帧观测到。
// 还有一个细节是，所有通过筛选的feature，在计算完Jacobian之后都应当进行门限筛选（基于Jacobian和测量残差），进一步剔除一些质量不好的feature。
void MsckfVio::removeLostFeatures() {

  // Remove the features that lost track.
  // BTW, find the size the final Jacobian matrix and residual vector.
  int jacobian_row_size = 0;
  vector<FeatureIDType> invalid_feature_ids(0);
  vector<FeatureIDType> processed_feature_ids(0);

  for (auto iter = map_server.begin();    // QXC：根据Mour07中的III-E，筛选出的是不再能跟踪到的，且能够初始化成功的feature
      iter != map_server.end(); ++iter) {
    // Rename the feature to be checked.
    auto& feature = iter->second;

    // Pass the features that are still being tracked.
    if (feature.observations.find(state_server.imu_state.id) !=     // QXC：当某feature在当前状态中有观测时，跳过（continue）
        feature.observations.end()) continue;
    if (feature.observations.size() < 3) {        // QXC：对于当前未观测到的feature，如果它在其他帧被观测到的总次数小于3（即只在两个采样时刻被观测到过），则认为它是失效的
      invalid_feature_ids.push_back(feature.id);
      continue;
    }

    // Check if the feature can be initialized if it
    // has not been.
    if (!feature.is_initialized) {      // QXC：feature未被初始化时才会进行下列检查
      if (!feature.checkMotion(state_server.cam_states)) {
        invalid_feature_ids.push_back(feature.id);      // QXC：当feature在观测到其的首末两帧中反映的视差较小时，认为它是失效的
        continue;
      } else {
        if(!feature.initializePosition(state_server.cam_states)) {      // QXC：尝试对feature的位置进行计算（利用Mour07中Appendix给出的方法）
          invalid_feature_ids.push_back(feature.id);    // QXC：feature初始化失败时也认为它是失效的
          continue;
        }
      }
    }

    // QXC：来到这里说明这个feature是当前未观测到，但在其他时刻观测次数超过2次，且成功初始化的

    jacobian_row_size += 4*feature.observations.size() - 3;     // QXC：双目所以乘以4，但为什么要减3呢？这里应该是计算进行了nullspace边缘化后的H尺寸，因此要减去3
    processed_feature_ids.push_back(feature.id);
  }

  //cout << "invalid/processed feature #: " <<
  //  invalid_feature_ids.size() << "/" <<
  //  processed_feature_ids.size() << endl;
  //cout << "jacobian row #: " << jacobian_row_size << endl;

  // Remove the features that do not have enough measurements.
  for (const auto& feature_id : invalid_feature_ids)    // QXC：移除失效特征，包括只观测到2次的、视差过小的以及初始化位置失败的
    map_server.erase(feature_id);

  // Return if there is no lost feature to be processed.
  if (processed_feature_ids.size() == 0) return;    // QXC：根据Mour07中III-E，要处理的是不再能跟踪到的feature

  MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
      21+6*state_server.cam_states.size());
  VectorXd r = VectorXd::Zero(jacobian_row_size);
  int stack_cntr = 0;

  // Process the features which lose track.
  for (const auto& feature_id : processed_feature_ids) {    // QXC：求取所有选出的feature对应的Jacobian，将它们堆叠起来
    auto& feature = map_server[feature_id];

    vector<StateIDType> cam_state_ids(0);
    for (const auto& measurement : feature.observations)
      cam_state_ids.push_back(measurement.first);

    MatrixXd H_xj;
    VectorXd r_j;
    featureJacobian(feature.id, cam_state_ids, H_xj, r_j);    // QXC：求某个feature观测相关的Jacobian，进行null space marginalization

    if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {    // QXC：用门限测试检测基于H_xj的测量预测协方差和残差的关系是否合理
      H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
      r.segment(stack_cntr, r_j.rows()) = r_j;
      stack_cntr += H_xj.rows();
    }

    // Put an upper bound on the row size of measurement Jacobian,
    // which helps guarantee the executation time.
    if (stack_cntr > 1500) break;
  }

  H_x.conservativeResize(stack_cntr, H_x.cols());   // QXC：重新定义矩阵维数，因为有的feature没通过gatingtest
  r.conservativeResize(stack_cntr);                 //      或者有的feature在featureJacobian中二次确认观测cam可能会减少cam数量

  // Perform the measurement update step.
  measurementUpdate(H_x, r);        // QXC：根据Mour07中III-E部分进行MSCKF的测量更新

  // Remove all processed features from the map.
  for (const auto& feature_id : processed_feature_ids)      // QXC：这些测量不再被观测到，将它们移除
    map_server.erase(feature_id);

  return;
}

// 挑选出两条冗余cam状态，规则与Mour07的III-E部分不符，在本工程对应文献的III-D中进行了说明
void MsckfVio::findRedundantCamStates(
    vector<StateIDType>& rm_cam_state_ids) {

  // Move the iterator to the key position.
  auto key_cam_state_iter = state_server.cam_states.end();
  for (int i = 0; i < 4; ++i)
    --key_cam_state_iter;
  auto cam_state_iter = key_cam_state_iter;
  ++cam_state_iter;
  auto first_cam_state_iter = state_server.cam_states.begin();

  // Pose of the key camera state.
  const Vector3d key_position =
    key_cam_state_iter->second.position;
  const Matrix3d key_rotation = quaternionToRotation(
      key_cam_state_iter->second.orientation);

  // Mark the camera states to be removed based on the
  // motion between states.
  for (int i = 0; i < 2; ++i) {
    const Vector3d position =
      cam_state_iter->second.position;
    const Matrix3d rotation = quaternionToRotation(
        cam_state_iter->second.orientation);

    double distance = (position-key_position).norm();
    double angle = AngleAxisd(
        rotation*key_rotation.transpose()).angle();

    //if (angle < 0.1745 && distance < 0.2 && tracking_rate > 0.5) {
    if (angle < 0.2618 && distance < 0.4 && tracking_rate > 0.5) {  // QXC：如果这个条件第一次就不满足，则会选择最早的两帧状态
      rm_cam_state_ids.push_back(cam_state_iter->first);
      ++cam_state_iter;
    } else {
      rm_cam_state_ids.push_back(first_cam_state_iter->first);
      ++first_cam_state_iter;
    }
  }

  // Sort the elements in the output vector.
  sort(rm_cam_state_ids.begin(), rm_cam_state_ids.end());

  return;
}

// 当扩维的cam状态数量达到最大尺寸时，选取若干（目前为2）条cam状态来删除，以清理buffer，在所有feature中找到至少能被其中2帧观测到，且成功初始化的feature，
// 基于这些feature关于要被删除的帧中相应的观测进行MSCKF测量更新。最后在协方差矩阵中去除与这些帧相关的维度。
// 注意，所有feature关于要被剔除的帧的观测都将被删除，相当于完全消除要被剔除帧的残余影响。
void MsckfVio::pruneCamStateBuffer() {

  if (state_server.cam_states.size() < max_cam_state_size)      // QXC：当扩维的cam状态超过最大数量时才继续
    return;

  // Find two camera states to be removed.
  vector<StateIDType> rm_cam_state_ids(0);
  findRedundantCamStates(rm_cam_state_ids);     // QXC：挑选出冗余的cam状态（两条）

  // Find the size of the Jacobian matrix.
  int jacobian_row_size = 0;
  for (auto& item : map_server) {
    auto& feature = item.second;
    // Check how many camera states to be removed are associated
    // with this feature.
    vector<StateIDType> involved_cam_state_ids(0);
    for (const auto& cam_id : rm_cam_state_ids) {       // QXC：挑选出当前feature对应的要剔除的cam
      if (feature.observations.find(cam_id) !=
          feature.observations.end())
        involved_cam_state_ids.push_back(cam_id);
    }

    if (involved_cam_state_ids.size() == 0) continue;
    if (involved_cam_state_ids.size() == 1) {
      feature.observations.erase(involved_cam_state_ids[0]);    // QXC：如果feature只被要被删除的cam中其中之一观测到，则只是在feature中剔除该观测
      continue;
    }

    if (!feature.is_initialized) {      // QXC：feature未被初始化时才会进行下列检查
      // Check if the feature can be initialize.
      if (!feature.checkMotion(state_server.cam_states)) {
        // If the feature cannot be initialized, just remove
        // the observations associated with the camera states
        // to be removed.       // QXC：当feature在观测到其的首末两帧中反映的视差较小时，删除其关于要剔除的cam的观测
        for (const auto& cam_id : involved_cam_state_ids)
          feature.observations.erase(cam_id);
        continue;
      } else {
        if(!feature.initializePosition(state_server.cam_states)) {      // QXC：初始化失败时，删除其关于要剔除的cam的观测
          for (const auto& cam_id : involved_cam_state_ids)
            feature.observations.erase(cam_id);
          continue;
        }
      }
    }

    // QXC：代码来到这里说明某个feature能被要剔除的1个以上的can观测到，且该feature位置成功初始化（过）。

    jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
  }

  //cout << "jacobian row #: " << jacobian_row_size << endl;

  // Compute the Jacobian and residual.
  MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
      21+6*state_server.cam_states.size());
  VectorXd r = VectorXd::Zero(jacobian_row_size);
  int stack_cntr = 0;

  for (auto& item : map_server) {
    auto& feature = item.second;
    // Check how many camera states to be removed are associated
    // with this feature.
    vector<StateIDType> involved_cam_state_ids(0);
    for (const auto& cam_id : rm_cam_state_ids) {
      if (feature.observations.find(cam_id) !=
          feature.observations.end())
        involved_cam_state_ids.push_back(cam_id);
    }

    // QXC：经过前面的一个for循环处理，所有的feature要么完全不被要剔除的cam观测到，要么至少被两个要剔除的cam观测到

    if (involved_cam_state_ids.size() == 0) continue;

    MatrixXd H_xj;
    VectorXd r_j;
    featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);   // QXC：求某个feature观测相关的Jacobian

    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {     // QXC：用门限测试检测基于H_xj的测量预测协方差和残差的关系是否合理
      H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
      r.segment(stack_cntr, r_j.rows()) = r_j;
      stack_cntr += H_xj.rows();
    }

    for (const auto& cam_id : involved_cam_state_ids)   // QXC：处理过后将feature中关于要剔除的cam的观测剔除
      feature.observations.erase(cam_id);
  }

  H_x.conservativeResize(stack_cntr, H_x.cols());   // QXC：重新定义矩阵维数，因为有的feature没通过gatingtest，
  r.conservativeResize(stack_cntr);                 //      或者有的feature在featureJacobian中二次确认观测cam可能会减少cam数量

  // Perform measurement update.
  measurementUpdate(H_x, r);        // QXC：进行MSCKF的测量更新，参照Mour07的III-E

  for (const auto& cam_id : rm_cam_state_ids) {
    int cam_sequence = std::distance(state_server.cam_states.begin(),
        state_server.cam_states.find(cam_id));
    int cam_state_start = 21 + 6*cam_sequence;
    int cam_state_end = cam_state_start + 6;

    // Remove the corresponding rows and columns in the state
    // covariance matrix.
    if (cam_state_end < state_server.state_cov.rows()) {
      state_server.state_cov.block(cam_state_start, 0,
          state_server.state_cov.rows()-cam_state_end,
          state_server.state_cov.cols()) =
        state_server.state_cov.block(cam_state_end, 0,
            state_server.state_cov.rows()-cam_state_end,
            state_server.state_cov.cols());

      state_server.state_cov.block(0, cam_state_start,
          state_server.state_cov.rows(),
          state_server.state_cov.cols()-cam_state_end) =
        state_server.state_cov.block(0, cam_state_end,
            state_server.state_cov.rows(),
            state_server.state_cov.cols()-cam_state_end);

      state_server.state_cov.conservativeResize(
          state_server.state_cov.rows()-6, state_server.state_cov.cols()-6);
    } else {    // QXC：暂时没想明白为什么会出现else的情形（即要删除的cam状态的维度已经超出了协方差维度）
      state_server.state_cov.conservativeResize(
          state_server.state_cov.rows()-6, state_server.state_cov.cols()-6);
    }

    // Remove this camera state in the state vector.
    state_server.cam_states.erase(cam_id);
  }

  return;
}

// 当IMU状态的位置协方差（的根）超出阈值时（说明滤波发散了），进行整个系统重置
void MsckfVio::onlineReset() {

  // Never perform online reset if position std threshold
  // is non-positive.
  if (position_std_threshold <= 0) return;
  static long long int online_reset_counter = 0;

  // Check the uncertainty of positions to determine if
  // the system can be reset.
  double position_x_std = std::sqrt(state_server.state_cov(12, 12));
  double position_y_std = std::sqrt(state_server.state_cov(13, 13));
  double position_z_std = std::sqrt(state_server.state_cov(14, 14));

  if (position_x_std < position_std_threshold &&
      position_y_std < position_std_threshold &&
      position_z_std < position_std_threshold) return;

  ROS_WARN("Start %lld online reset procedure...",
      ++online_reset_counter);
  ROS_INFO("Stardard deviation in xyz: %f, %f, %f",
      position_x_std, position_y_std, position_z_std);

  // Remove all existing camera states.
  state_server.cam_states.clear();

  // Clear all exsiting features in the map.
  map_server.clear();

  // Reset the state covariance.
  double gyro_bias_cov, acc_bias_cov, velocity_cov;
  nh.param<double>("initial_covariance/velocity",
      velocity_cov, 0.25);
  nh.param<double>("initial_covariance/gyro_bias",
      gyro_bias_cov, 1e-4);
  nh.param<double>("initial_covariance/acc_bias",
      acc_bias_cov, 1e-2);

  double extrinsic_rotation_cov, extrinsic_translation_cov;
  nh.param<double>("initial_covariance/extrinsic_rotation_cov",
      extrinsic_rotation_cov, 3.0462e-4);
  nh.param<double>("initial_covariance/extrinsic_translation_cov",
      extrinsic_translation_cov, 1e-4);

  state_server.state_cov = MatrixXd::Zero(21, 21);
  for (int i = 3; i < 6; ++i)
    state_server.state_cov(i, i) = gyro_bias_cov;
  for (int i = 6; i < 9; ++i)
    state_server.state_cov(i, i) = velocity_cov;
  for (int i = 9; i < 12; ++i)
    state_server.state_cov(i, i) = acc_bias_cov;
  for (int i = 15; i < 18; ++i)
    state_server.state_cov(i, i) = extrinsic_rotation_cov;
  for (int i = 18; i < 21; ++i)
    state_server.state_cov(i, i) = extrinsic_translation_cov;

  ROS_WARN("%lld online reset complete...", online_reset_counter);
  return;
}

// 发布tf、odometry、特征点云等消息
void MsckfVio::publish(const ros::Time& time) {

  // Convert the IMU frame to the body frame.
  const IMUState& imu_state = state_server.imu_state;
  Eigen::Isometry3d T_i_w = Eigen::Isometry3d::Identity();
  T_i_w.linear() = quaternionToRotation(
      imu_state.orientation).transpose();
  T_i_w.translation() = imu_state.position;

  Eigen::Isometry3d T_b_w = IMUState::T_imu_body * T_i_w *
    IMUState::T_imu_body.inverse();     // QXC：没看懂为什么还要乘个T_imu_body的逆
  Eigen::Vector3d body_velocity =
    IMUState::T_imu_body.linear() * imu_state.velocity;

  // Publish tf     // QXC：tf是用于维护一个机器人上所有坐标系间相对变换关系的东西
  if (publish_tf) {
    tf::Transform T_b_w_tf;
    tf::transformEigenToTF(T_b_w, T_b_w_tf);
    tf_pub.sendTransform(tf::StampedTransform(
          T_b_w_tf, time, fixed_frame_id, child_frame_id));
  }

  // Publish the odometry
  nav_msgs::Odometry odom_msg;
  odom_msg.header.stamp = time;
  odom_msg.header.frame_id = fixed_frame_id;
  odom_msg.child_frame_id = child_frame_id;

  tf::poseEigenToMsg(T_b_w, odom_msg.pose.pose);
  tf::vectorEigenToMsg(body_velocity, odom_msg.twist.twist.linear);

  // Convert the covariance.
  Matrix3d P_oo = state_server.state_cov.block<3, 3>(0, 0);
  Matrix3d P_op = state_server.state_cov.block<3, 3>(0, 12);
  Matrix3d P_po = state_server.state_cov.block<3, 3>(12, 0);
  Matrix3d P_pp = state_server.state_cov.block<3, 3>(12, 12);
  Matrix<double, 6, 6> P_imu_pose = Matrix<double, 6, 6>::Zero();
  P_imu_pose << P_pp, P_po, P_op, P_oo;

  Matrix<double, 6, 6> H_pose = Matrix<double, 6, 6>::Zero();
  H_pose.block<3, 3>(0, 0) = IMUState::T_imu_body.linear();
  H_pose.block<3, 3>(3, 3) = IMUState::T_imu_body.linear();
  Matrix<double, 6, 6> P_body_pose = H_pose *
    P_imu_pose * H_pose.transpose();

  for (int i = 0; i < 6; ++i)
    for (int j = 0; j < 6; ++j)
      odom_msg.pose.covariance[6*i+j] = P_body_pose(i, j);

  // Construct the covariance for the velocity.
  Matrix3d P_imu_vel = state_server.state_cov.block<3, 3>(6, 6);
  Matrix3d H_vel = IMUState::T_imu_body.linear();
  Matrix3d P_body_vel = H_vel * P_imu_vel * H_vel.transpose();
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
      odom_msg.twist.covariance[i*6+j] = P_body_vel(i, j);

  odom_pub.publish(odom_msg);

  // Publish the 3D positions of the features that
  // has been initialized.
  pcl::PointCloud<pcl::PointXYZ>::Ptr feature_msg_ptr(
      new pcl::PointCloud<pcl::PointXYZ>());
  feature_msg_ptr->header.frame_id = fixed_frame_id;
  feature_msg_ptr->height = 1;
  for (const auto& item : map_server) {
    const auto& feature = item.second;
    if (feature.is_initialized) {
      Vector3d feature_position =
        IMUState::T_imu_body.linear() * feature.position;
      feature_msg_ptr->points.push_back(pcl::PointXYZ(
            feature_position(0), feature_position(1), feature_position(2)));
    }
  }
  feature_msg_ptr->width = feature_msg_ptr->points.size();

  feature_pub.publish(feature_msg_ptr);

  return;
}

} // namespace msckf_vio


/*
 * cg.cpp
 *
 *  Created on: Feb 22, 2013
 *      Author: Joao Cunha <joao.cunha@ua.pt>
 */

#include "cg.h"

#include <iostream>

#include <Eigen/Core>

using namespace std;

namespace libgp
{

CG::CG()
{
}

CG::~CG()
{
}

void CG::maximize(GaussianProcess* gp, size_t n, bool verbose)
{
	const double INT = 0.1; // don't reevaluate within 0.1 of the limit of the current bracket
	const double EXT = 3.0; // extrapolate maximum 3 times the current step-size
	const int MAX = 20;		// max 20 function evaluations per line search
	const double RATIO = 10;	// maximum allowed slope ratio
	const double SIG = 0.1, RHO = SIG/2;
	/* SIG and RHO are the constants controlling the Wolfe-
	   Powell conditions. SIG is the maximum allowed absolute ratio between
	   previous and new slopes (derivatives in the search direction), thus setting
	   SIG to low (positive) values forces higher precision in the line-searches.
	   RHO is the minimum allowed fraction of the expected (from the slope at the
	   initial point in the linesearch). Constants must satisfy 0 < RHO < SIG < 1.
	   Tuning of SIG (depending on the nature of the function to be optimized) may
	  speed up the minimization; it is probably not worth playing much with RHO.
	*/

	/* The code falls naturally into 3 parts, after the initial line search is
	   started in the direction of steepest descent. 1) we first enter a while loop
	   which uses point 1 (p1) and (p2) to compute an extrapolation (p3), until we
	   have extrapolated far enough (Wolfe-Powell conditions). 2) if necessary, we
	   enter the second loop which takes p2, p3 and p4 chooses the subinterval
	   containing a (local) minimum, and interpolates it, unil an acceptable point
	   is found (Wolfe-Powell conditions). Note, that points are always maintained
	   in order p0 <= p1 <= p2 < p3 < p4. 3) compute a new search direction using
	   conjugate gradients (Polack-Ribiere flavour), or revert to steepest if there
	   was a problem in the previous line-search. Return the best value so far, if
	   two consecutive line-searches fail, or whenever we run out of function
	   evaluations or line-searches. During extrapolation, the "f" function may fail
	   either with an error or returning Nan or Inf, and maxmize should handle this
	   gracefully.
	*/

	bool ls_failed = false;									//prev line-search failed
	double f0 = -gp->log_likelihood();						//initial negative marginal log likelihood
	Eigen::VectorXd df0 = -gp->log_likelihood_gradient();	//initial gradient
	Eigen::VectorXd X = gp->covf().get_loghyper();			//hyper parameters

	if(verbose) cout << f0 << endl;

	Eigen::VectorXd s = -df0;								//initial search direction
	double d0 = -s.dot(s);									//initial slope
	double x3 = 1/(1-d0);

	double f3 = 0;
	double d3 = 0;
	Eigen::VectorXd df3 = df0;

	double x2 = 0, x4 = 0;
	double f2 = 0, f4 = 0;
	double d2 = 0, d4 = 0;

	for (unsigned int i = 0; i < n; ++i)
	{
		//copy current values
		Eigen::VectorXd X0 = X;
		double F0 = f0;
		Eigen::VectorXd dF0 = df0;
		unsigned int M = min(MAX, (int)(n-i));

		while(1)											//keep extrapolating until necessary
		{
			x2 = 0;
			f2 = f0;
			d2 = d0;
			f3 = f0;
			df3 = df0;
			double success = false;

			while( !success && M>0)
			{
				M --;
				i++;
				gp->covf().set_loghyper(X+s*x3);
				f3 = -gp->log_likelihood();
				df3 = -gp->log_likelihood_gradient();

				if(verbose) cout << f3 << endl;

				bool nanFound = false;
				//test NaN and Inf's
				for (int j = 0; j < df3.rows(); ++j)
				{
					if(isnan(df3(j)))
					{
						nanFound = true;
						break;
					}
				}
				if(!isnan(f3) && !isinf(f3) && !nanFound)
					success = true;
				else
				{
					x3 = (x2+x3)/2; 						// if fail, bissect and try again
				}
			}
			//keep best values
			if(f3 < F0)
			{
				X0 = X+s*x3;
				F0 = f3;
				dF0 = df3;
			}

			d3 = df3.dot(s);								// new slope

			if( (d3 > SIG*d0) || (f3 >  f0+x3*RHO*d0) || M == 0) // are we done extrapolating?
			{
				break;
			}

			double x1 = x2; double f1 = f2; double d1 = d2;	// move point 2 to point 1
			x2 = x3; f2 = f3; d2 = d3;						// move point 3 to point 2
			double A = 6*(f1-f2) + 3*(d2+d1)*(x2-x1);				// make cubic extrapolation
			double B = 3*(f2-f1) - (2*d1+d2)*(x2-x1);
			x3 = x1-d1*(x2-x1)*(x2-x1)/(B+sqrt(B*B -A*d1*(x2-x1)));
			if(isnan(x3) || x3 < 0 || x3 > x2*EXT)			// num prob | wrong sign | beyond extrapolation limit
				x3 = EXT*x2;
			else if(x3 < x2+INT*(x2-x1))					// too close to previous point
				x3 = x2+INT*(x2-x1);
		}

		while( ( (abs(d3) > -SIG*d0) || (f3 > f0+x3*RHO*d0) ) && (M > 0))	// keep interpolating
		{
			if( (d3 > 0) || (f3 > f0+x3*RHO*d0) )			// choose subinterval
			{												// move point 3 to point 4
				x4 = x3;
				f4 = f3;
				d4 = d3;
			}
			else
			{
				x2 = x3;									//move point 3 to point 2
				f2 = f3;
				d2 = d3;
			}

			if(f4 > f0)
				x3 = x2 - (0.5*d2*(x4-x2)*(x4-x2))/(f4-f2-d2*(x4-x2));	// quadratic interpolation
			else
			{
				double A = 6*(f2-f4)/(x4-x2)+3*(d4+d2);
				double B = 3*(f4-f2)-(2*d2+d4)*(x4-x2);
				x3 = x2+sqrt(B*B-A*d2*(x4-x2)*(x4-x2) -B)/A;
			}

			if(isnan(x3) || isinf(x3))
				x3 = (x2+x4)/2;

			x3 = std::max(std::min(x3, x4-INT*(x4-x2)), x2+INT*(x4-x2));

			gp->covf().set_loghyper(X+s*x3);
			f3 = -gp->log_likelihood();
			df3 = -gp->log_likelihood_gradient();

			if(f3 < F0)												// keep best values
			{
				X0 = X+s*x3;
				F0 = f3;
				dF0 = df3;
			}

			if(verbose) cout << F0 << endl;

			M--;
			i++;
			d3 = df3.dot(s);										// new slope
		}

		if( (abs(d3) < -SIG*d0) && (f3 < f0+x3*RHO*d0))
		{
			X = X+s*x3;
			f0 = f3;
			s = (df3.dot(df3)-df0.dot(df3)) / (df0.dot(df0))*s - df3;	// Polack-Ribiere CG direction
			df0 = df3;													// swap derivatives
			d3 = d0; d0 = df0.dot(s);
			if(verbose) cout << f0 << endl;
			if(d0 > 0)													// new slope must be negative
			{															// otherwise use steepest direction
				s = -df0;
				d0 = -s.dot(s);
			}

			x3 = x3 * std::min(RATIO, d3/(d0-std::numeric_limits< double >::min()));	// slope ratio but max RATIO
			ls_failed = false;																// this line search did not fail
		}
		else
		{														// restore best point so far
			X = X0;
			f0 = F0;
			df0 = dF0;

			if(verbose) cout << f0 << endl;

			if(ls_failed || i >= n)								// line search failed twice in a row
				break;											// or we ran out of time, so we give up

			s = -df0;
			d0 = -s.dot(s);										// try steepest
			x3 = 1/(1-d0);
			ls_failed = true;									// this line search failed
		}

	}
	gp->covf().set_loghyper(X);
}

}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/test/unit_test.hpp>
#include <qle/math/deltagammavar.hpp>

using namespace QuantLib;
using namespace QuantExt;

using namespace boost::unit_test_framework;
using std::vector;

namespace {
void test(const Size dim, const bool nonzeroDelta, const bool nonzeroGamma, const Size seedParam, const Size seedMc,
          const Size paths) {

    BOOST_TEST_MESSAGE("################ Testing delta gamma VaR, dim=" << dim << ", delta=" << nonzeroDelta
                                                                        << ", gamma=" << nonzeroGamma
                                                                        << ", paths=" << paths << "\n");

    MersenneTwisterUniformRng mt(seedParam);

    // generate random covariance matrix

    BOOST_TEST_MESSAGE("Generate transformation matrix L");
    Matrix L(dim, dim, 0.0);
    Real det = 0.0;
    do {
        for (Size i = 0; i < dim; ++i) {
            for (Size j = 0; j < dim; ++j) {
                L[i][j] = mt.nextReal();
            }
        }
        det = determinant(L);
        BOOST_TEST_MESSAGE("... done, determinant is " << det);
    } while (close_enough(det, 0.0));

    Matrix omega = transpose(L) * L;

    // scale entries such that they have order of magnitude 0.1
    Real num = QuantExt::detail::absMax(omega);
    omega /= num * 10.0;

    // generate random delta vector

    BOOST_TEST_MESSAGE("Generate delta");
    Array delta(dim, 0.0);
    if (nonzeroDelta) {
        for (Size i = 0; i < dim; ++i) {
            delta[i] = mt.nextReal() * 1000.0 - 500.0;
        }
    }

    // generate random gamma matrix, TODO negative gammas?

    BOOST_TEST_MESSAGE("Generate gamma");
    Matrix gamma(dim, dim, 0.0), shift(dim, dim, 0.0);
    if (nonzeroGamma) {
        for (Size i = 0; i < dim; ++i) {
            for (Size j = 0; j < i; ++j) {
                gamma[i][j] = gamma[j][i] = mt.nextReal() * 1000.0; // - 500.0;
            }
            gamma[i][i] = mt.nextReal() * 1000.0; // - 500.0;
        }
    }

    BOOST_TEST_MESSAGE("delta=" << delta);
    if (gamma.rows() <= 20) {
        BOOST_TEST_MESSAGE("\ngamma=\n" << gamma);
        BOOST_TEST_MESSAGE("omega=\n" << omega);
    } else {
        BOOST_TEST_MESSAGE("\ngamma= too big to display (" << gamma.rows() << "x" << gamma.columns() << ")");
        BOOST_TEST_MESSAGE("omega= too big to display (" << omega.rows() << "x" << omega.columns() << ")\n");
    }

    // check results against MC simulation

    std::vector<Real> quantiles;
    quantiles.push_back(0.9);
    quantiles.push_back(0.95);
    quantiles.push_back(0.99);
    quantiles.push_back(0.999);
    quantiles.push_back(0.9999);

    BOOST_TEST_MESSAGE("Run MC simulation...");
    Matrix nullGamma(dim, dim, 0.0);
    std::vector<Real> mc1All = deltaGammaVarMc<PseudoRandom>(omega, delta, nullGamma, quantiles, paths, seedMc);
    std::vector<Real> mc2All = deltaGammaVarMc<PseudoRandom>(omega, delta, gamma, quantiles, paths, seedMc);
    BOOST_TEST_MESSAGE("MC simulation Done.");

    BOOST_TEST_MESSAGE("      Quantile      dVaR(MC)      dgVaR(MC)    dVaR(Mdl)");
    BOOST_TEST_MESSAGE("========================================================");

    Size i = 0;
    BOOST_FOREACH (Real q, quantiles) {

        Real mc1 = mc1All[i];
        Real mc2 = mc2All[i++];

        Real dVar = deltaVar(omega, delta, q);

        BOOST_TEST_MESSAGE(std::right << std::setw(14) << q << std::setw(14) << mc1 << std::setw(14) << mc2
                                      << std::setw(14) << dVar);

        BOOST_CHECK_CLOSE(dVar, mc1, 5.0);
    }

    BOOST_TEST_MESSAGE("========================================================\n\n");

} // test
} // anonymous namespace

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(DeltaGammaVarTest)

BOOST_AUTO_TEST_CASE(testDeltaGammaVar) {

    // TODO add more test cases (negative gammas, higher dimensions)

    Size n = (Size)1E7;

    test(1, true, false, 42, 42, n);
    test(1, true, true, 42, 42, n);

    test(2, true, false, 42, 42, n);
    test(2, true, true, 42, 42, n);

    test(10, true, false, 42, 42, n);
    test(10, true, true, 42, 42, n);

    // fewer paths here
    test(100, true, false, 42, 42, (Size)1E6);
    test(100, true, true, 42, 42, (Size)1E6);
}

BOOST_AUTO_TEST_CASE(testNegativeGamma) {

    BOOST_TEST_MESSAGE("Testing delta gamma var for pl = -u^2, u standard normal...");

    // choose n=1, gamma=-10k, omega = 1, then the pl is -0.5*u^2 with
    // u standard normal, in other words  -2pl is chi-squared
    // distributed with one degree of freedom

    boost::math::chi_squared_distribution<Real> chisq(1.0);

    Real gamma = -10000.0;

    Array delta(1, 0.0);
    Matrix gamma_m(1, 1, gamma);
    Matrix omega(1, 1, 1.0);

    Real p = 0.99;

    Real var_mc = deltaGammaVarMc<PseudoRandom>(omega, delta, gamma_m, p, 1000000, 142);

    Real refVal = 0.5 * gamma * boost::math::quantile(chisq, 1.0 - p);

    BOOST_TEST_MESSAGE("mc  = " << var_mc);
    BOOST_TEST_MESSAGE("ref = " << refVal);

    BOOST_CHECK_SMALL(std::abs(refVal - var_mc), 0.5);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_MAXMAG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_MAXMAG_HPP_INCLUDED

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( maxmag_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      A0 aa0 = simd::abs(a0);
      A0 aa1 = simd::abs(a1);
      return aa0 < aa1 ? a1 : aa1 <  aa0 ? a0 : simd::max(a0, a1);
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_EXPONENT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_EXPONENT_HPP_INCLUDED

#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/exponentbits.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/detail/math.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( exponent_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE result_t operator() ( A0) const BOOST_NOEXCEPT
    {
      return Zero<result_t>();
    }
  };

#ifdef BOOST_SIMD_HAS_ILOGB
  BOOST_DISPATCH_OVERLOAD ( exponent_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      if (is_invalid(a0) || is_eqz(a0)) return Zero<result_t>();
      return ::ilogb(a0);
    }
  };
#endif

#ifdef BOOST_SIMD_HAS_ILOGBF
  BOOST_DISPATCH_OVERLOAD ( exponent_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      if (is_invalid(a0) || is_eqz(a0)) return Zero<result_t>();
      return ::ilogbf(a0);
    }
  };
#endif

  BOOST_DISPATCH_OVERLOAD ( exponent_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      if (is_invalid(a0) || is_eqz(a0)) return Zero<result_t>();
      const int nmb = int(Nbmantissabits<A0>());
      const result_t x = shr(exponentbits(a0), nmb);
      return x-if_else_zero(a0, Maxexponent<A0>());
    }
  };
} } }

#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MINEXPONENT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MINEXPONENT_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_minexponent Minexponent
 *
 * \par Description
 * Constant Minexponent
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/minexponent.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::minexponent_(A0)>::type
 *     Minexponent();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Minexponent
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Minexponent of functor Minexponent
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    struct Minexponent : ext::pure_constant_<Minexponent>
    {
      template<class Target, class Dummy=void>
      struct  apply : meta::int_c<typename Target::type,0> {};
    };

  template<class T, class Dummy>
  struct  Minexponent::apply<boost::dispatch::meta::single_<T>,Dummy>
        : meta::int_c<boost::simd::int32_t,-126> {};

  template<class T, class Dummy>
  struct  Minexponent::apply<boost::dispatch::meta::double_<T>,Dummy>
        : meta::int_c<boost::simd::int64_t,-1022> {};
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Minexponent, Minexponent)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


//  (C) Copyright 2006 Eric Niebler, Olivier Gygi
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/random.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // two random number generators
    double mu = 1.;
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma(mu,1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);

    accumulator_set<double, stats<tag::median(with_p_square_quantile) > > acc;
    accumulator_set<double, stats<tag::median(with_density) > >
        acc_dens( density_cache_size = 10000, density_num_bins = 1000 );
    accumulator_set<double, stats<tag::median(with_p_square_cumulative_distribution) > >
        acc_cdist( p_square_cumulative_distribution_num_cells = 100 );

    for (std::size_t i=0; i<100000; ++i)
    {
        double sample = normal();
        acc(sample);
        acc_dens(sample);
        acc_cdist(sample);
    }

    BOOST_CHECK_CLOSE(1., median(acc), 1.);
    BOOST_CHECK_CLOSE(1., median(acc_dens), 1.);
    BOOST_CHECK_CLOSE(1., median(acc_cdist), 3.);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("median test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2015.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/metaparse/string.hpp>
#include <boost/metaparse/sequence_apply.hpp>
#include <boost/metaparse/last_of.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/lit_c.hpp>
#include <boost/metaparse/one_of.hpp>
#include <boost/metaparse/empty.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/build_parser.hpp>

#include <boost/rational.hpp>

#include <boost/config.hpp>

#include <boost/mpl/int.hpp>

#include <iostream>

using boost::metaparse::sequence_apply2;
using boost::metaparse::last_of;
using boost::metaparse::int_;
using boost::metaparse::token;
using boost::metaparse::lit_c;
using boost::metaparse::one_of;
using boost::metaparse::empty;
using boost::metaparse::entire_input;
using boost::metaparse::build_parser;

template <class Num, class Denom>
struct rational
{
  typedef rational type;

  static boost::rational<int> run()
  {
    return boost::rational<int>(Num::type::value, Denom::type::value);
  }
};

typedef
  sequence_apply2<
    rational,

    token<int_>,
    one_of<
      last_of<lit_c<'/'>, token<int_> >,
      empty<boost::mpl::int_<1> >
    >
  >
  rational_grammar;

typedef build_parser<entire_input<rational_grammar> > rational_parser;

#ifdef RATIONAL
#  error RATIONAL already defined
#endif
#define RATIONAL(s) \
  (::rational_parser::apply<BOOST_METAPARSE_STRING(s)>::type::run())

#if BOOST_METAPARSE_STD < 2011

int main()
{
  std::cout << "Please use a compiler that supports constexpr" << std::endl;
}

#else

int main()
{
  const boost::rational<int> r1 = RATIONAL("1/3");
  const boost::rational<int> r2 = RATIONAL("4/4");
  const boost::rational<int> r3 = RATIONAL("1");
  const boost::rational<int> r4 = RATIONAL("13/11");

  // Uncommenting the following line generates a compilation error. On a
  // number of platforms the error report contains the following (or something
  // similar):
  // x__________________PARSING_FAILED__________________x<1, 3, digit_expected>
  // where 1, 3 is the location of the error inside the string literal.
//  const boost::rational<int> r5 = RATIONAL("7/");

  std::cout
    << r1 << std::endl
    << r2 << std::endl
    << r3 << std::endl
    << r4 << std::endl;
}

#endif


#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <iostream>
#include "setup_rotation_cluster.h"
#include "kmeans_clustering.h"

using namespace Eigen;
using namespace std;
typedef Eigen::Triplet<double> Trip;

void setup_rotation_cluster(int nrc, bool reduced, const MatrixXi& mT,
    const MatrixXd& mV, std::vector<VectorXi>& ibones, std::vector<VectorXi>& imuscle,
    VectorXd& mred_x, VectorXd& mred_r, VectorXd& mred_w,
	SparseMatrix<double>& mC, SparseMatrix<double>& mA, MatrixXd& mG, VectorXd& mx0,
	std::vector<SparseMatrix<double>>& mRotationBLOCK, std::map<int, std::vector<int>>& mr_cluster_elem_map, VectorXi& mr_elem_cluster_map){
    std::cout<<"+ Rotation Clusters"<<std::endl;
    if(nrc==0){
        //unreduced
        nrc = mT.rows();
    }

    mr_elem_cluster_map.resize(mT.rows());
    if(nrc==mT.rows() && reduced==false){
        //unreduced
        for(int i=0; i<mT.rows(); i++){
            mr_elem_cluster_map[i] = i;
        }
    }else{

        if(3*mV.rows()==mred_x.size() && reduced==false){
            std::cout<<"Continuous mesh is unreduced. Kmeans won't work."<<std::endl;
            exit(0);
        }else{
            if(nrc==mT.rows()){
                for(int i=0; i<mT.rows(); i++){
                   mr_elem_cluster_map[i] = i;
                }
            }else{
                kmeans_clustering(mr_elem_cluster_map, nrc, ibones, imuscle, mG, mC, mA, mx0);
            }
        }

    }

    for(int i=0; i<mT.rows(); i++){
        mr_cluster_elem_map[mr_elem_cluster_map[i]].push_back(i);
    }

    mred_r.resize(9*nrc);
    for(int i=0; i<nrc; i++){
        mred_r[9*i+0] = 1;
        mred_r[9*i+1] = 0;
        mred_r[9*i+2] = 0;
        mred_r[9*i+3] = 0;
        mred_r[9*i+4] = 1;
        mred_r[9*i+5] = 0;
        mred_r[9*i+6] = 0;
        mred_r[9*i+7] = 0;
        mred_r[9*i+8] = 1;
    }
    mred_w.resize(3*nrc);
    mred_w.setZero();

    if(reduced){
        for(int c=0; c<nrc; c++){
            std::vector<int> notfix = mr_cluster_elem_map[c];
            // SparseMatrix<double> bo(mT.rows(), notfix.size());
            // bo.setZero();
            std::vector<Trip> bo_trip;
            bo_trip.reserve(notfix.size());

            int i = 0;
            int f = 0;
            for(int j =0; j<notfix.size(); j++){
                if (i==notfix[f]){
                    bo_trip.push_back(Trip(i, j, 1));
                    f++;
                    i++;
                    continue;
                }
                j--;
                i++;
            }

            std::vector<Trip> b_trip;
            b_trip.reserve(bo_trip.size());
            for(int k =0; k<bo_trip.size(); k++){
                b_trip.push_back(Trip(12*bo_trip[k].row()+0, 12*bo_trip[k].col()+0, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+1, 12*bo_trip[k].col()+1, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+2, 12*bo_trip[k].col()+2, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+3, 12*bo_trip[k].col()+3, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+4, 12*bo_trip[k].col()+4, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+5, 12*bo_trip[k].col()+5, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+6, 12*bo_trip[k].col()+6, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+7, 12*bo_trip[k].col()+7, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+8, 12*bo_trip[k].col()+8, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+9, 12*bo_trip[k].col()+9, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+10, 12*bo_trip[k].col()+10, bo_trip[k].value()));
                b_trip.push_back(Trip(12*bo_trip[k].row()+11, 12*bo_trip[k].col()+11, bo_trip[k].value()));
            }

            SparseMatrix<double> b(12*mT.rows(), 12*notfix.size());
            b.setFromTriplets(b_trip.begin(), b_trip.end());
            mRotationBLOCK.push_back(b);
        }
    }
    std::cout<<"- Rotation Clusters"<<std::endl;
}