414 lines
12 KiB
C++
414 lines
12 KiB
C++
/*
|
|
* COPYRIGHT AND PERMISSION NOTICE
|
|
* Penn Software MSCKF_VIO
|
|
* Copyright (C) 2017 The Trustees of the University of Pennsylvania
|
|
* All rights reserved.
|
|
*/
|
|
|
|
#ifndef MSCKF_VIO_H
|
|
#define MSCKF_VIO_H
|
|
|
|
#include <map>
|
|
#include <set>
|
|
#include <vector>
|
|
#include <string>
|
|
#include <Eigen/Dense>
|
|
#include <Eigen/Geometry>
|
|
#include <math.h>
|
|
#include <boost/shared_ptr.hpp>
|
|
#include <opencv2/opencv.hpp>
|
|
#include <opencv2/video.hpp>
|
|
|
|
#include <ros/ros.h>
|
|
#include <sensor_msgs/Imu.h>
|
|
#include <sensor_msgs/Image.h>
|
|
#include <sensor_msgs/PointCloud.h>
|
|
#include <nav_msgs/Odometry.h>
|
|
#include <std_msgs/Float64.h>
|
|
#include <tf/transform_broadcaster.h>
|
|
#include <std_srvs/Trigger.h>
|
|
|
|
#include "imu_state.h"
|
|
#include "cam_state.h"
|
|
#include "feature.hpp"
|
|
#include <msckf_vio/CameraMeasurement.h>
|
|
|
|
#include <cv_bridge/cv_bridge.h>
|
|
#include <image_transport/image_transport.h>
|
|
#include <message_filters/subscriber.h>
|
|
#include <message_filters/time_synchronizer.h>
|
|
|
|
#define PI 3.14159265
|
|
|
|
namespace msckf_vio {
|
|
/*
|
|
* @brief MsckfVio Implements the algorithm in
|
|
* Anatasios I. Mourikis, and Stergios I. Roumeliotis,
|
|
* "A Multi-State Constraint Kalman Filter for Vision-aided
|
|
* Inertial Navigation",
|
|
* http://www.ee.ucr.edu/~mourikis/tech_reports/TR_MSCKF.pdf
|
|
*/
|
|
class MsckfVio {
|
|
public:
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
// Constructor
|
|
MsckfVio(ros::NodeHandle& pnh);
|
|
// Disable copy and assign constructor
|
|
MsckfVio(const MsckfVio&) = delete;
|
|
MsckfVio operator=(const MsckfVio&) = delete;
|
|
|
|
// Destructor
|
|
~MsckfVio() {}
|
|
|
|
/*
|
|
* @brief initialize Initialize the VIO.
|
|
*/
|
|
bool initialize();
|
|
|
|
/*
|
|
* @brief reset Resets the VIO to initial status.
|
|
*/
|
|
void reset();
|
|
|
|
typedef boost::shared_ptr<MsckfVio> Ptr;
|
|
typedef boost::shared_ptr<const MsckfVio> ConstPtr;
|
|
|
|
private:
|
|
/*
|
|
* @brief StateServer Store one IMU states and several
|
|
* camera states for constructing measurement
|
|
* model.
|
|
*/
|
|
struct StateServer {
|
|
IMUState imu_state;
|
|
CamStateServer cam_states;
|
|
|
|
// State covariance matrix
|
|
Eigen::MatrixXd state_cov;
|
|
Eigen::Matrix<double, 12, 12> continuous_noise_cov;
|
|
};
|
|
|
|
|
|
/*
|
|
* @brief loadParameters
|
|
* Load parameters from the parameter server.
|
|
*/
|
|
bool loadParameters();
|
|
|
|
/*
|
|
* @brief createRosIO
|
|
* Create ros publisher and subscirbers.
|
|
*/
|
|
bool createRosIO();
|
|
|
|
/*
|
|
* @brief imuCallback
|
|
* Callback function for the imu message.
|
|
* @param msg IMU msg.
|
|
*/
|
|
void imuCallback(const sensor_msgs::ImuConstPtr& msg);
|
|
|
|
/*
|
|
* @brief truthCallback
|
|
* Callback function for ground truth navigation information
|
|
* @param TransformStamped msg
|
|
*/
|
|
void truthCallback(
|
|
const geometry_msgs::TransformStampedPtr& msg);
|
|
|
|
|
|
/*
|
|
* @brief imageCallback
|
|
* Callback function for feature measurements
|
|
* Triggers measurement update
|
|
* @param msg
|
|
* Camera 0 Image
|
|
* Camera 1 Image
|
|
* Stereo feature measurements.
|
|
*/
|
|
void imageCallback (
|
|
const sensor_msgs::ImageConstPtr& cam0_img,
|
|
const sensor_msgs::ImageConstPtr& cam1_img,
|
|
const CameraMeasurementConstPtr& feature_msg);
|
|
|
|
/*
|
|
* @brief publish Publish the results of VIO.
|
|
* @param time The time stamp of output msgs.
|
|
*/
|
|
void publish(const ros::Time& time);
|
|
|
|
/*
|
|
* @brief initializegravityAndBias
|
|
* Initialize the IMU bias and initial orientation
|
|
* based on the first few IMU readings.
|
|
*/
|
|
void initializeGravityAndBias();
|
|
|
|
/*
|
|
* @biref resetCallback
|
|
* Callback function for the reset service.
|
|
* Note that this is NOT anytime-reset. This function should
|
|
* only be called before the sensor suite starts moving.
|
|
* e.g. while the robot is still on the ground.
|
|
*/
|
|
bool resetCallback(std_srvs::Trigger::Request& req,
|
|
std_srvs::Trigger::Response& res);
|
|
|
|
// Saves the exposure time and the camera measurementes
|
|
void manageMovingWindow(
|
|
const sensor_msgs::ImageConstPtr& cam0_img,
|
|
const sensor_msgs::ImageConstPtr& cam1_img,
|
|
const CameraMeasurementConstPtr& feature_msg);
|
|
|
|
|
|
void calcErrorState();
|
|
|
|
// Debug related Functions
|
|
// Propagate the true state
|
|
void batchTruthProcessing(
|
|
const double& time_bound);
|
|
|
|
|
|
void processTruthtoIMU(const double& time,
|
|
const Eigen::Vector4d& m_rot,
|
|
const Eigen::Vector3d& m_trans);
|
|
|
|
|
|
// Filter related functions
|
|
// Propogate the state
|
|
void batchImuProcessing(
|
|
const double& time_bound);
|
|
void processModel(const double& time,
|
|
const Eigen::Vector3d& m_gyro,
|
|
const Eigen::Vector3d& m_acc);
|
|
void predictNewState(const double& dt,
|
|
const Eigen::Vector3d& gyro,
|
|
const Eigen::Vector3d& acc);
|
|
|
|
// groundtruth state augmentation
|
|
void truePhotometricStateAugmentation(const double& time);
|
|
|
|
// Measurement update
|
|
void stateAugmentation(const double& time);
|
|
void PhotometricStateAugmentation(const double& time);
|
|
void addFeatureObservations(const CameraMeasurementConstPtr& msg);
|
|
// This function is used to compute the measurement Jacobian
|
|
// for a single feature observed at a single camera frame.
|
|
void measurementJacobian(const StateIDType& cam_state_id,
|
|
const FeatureIDType& feature_id,
|
|
Eigen::Matrix<double, 4, 6>& H_x,
|
|
Eigen::Matrix<double, 4, 3>& H_f,
|
|
Eigen::Vector4d& r);
|
|
// This function computes the Jacobian of all measurements viewed
|
|
// in the given camera states of this feature.
|
|
void featureJacobian(
|
|
const FeatureIDType& feature_id,
|
|
const std::vector<StateIDType>& cam_state_ids,
|
|
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
|
|
|
|
|
void twodotMeasurementJacobian(
|
|
const StateIDType& cam_state_id,
|
|
const FeatureIDType& feature_id,
|
|
Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
|
|
|
|
bool ConstructJacobians(
|
|
Eigen::MatrixXd& H_rho,
|
|
Eigen::MatrixXd& H_pl,
|
|
Eigen::MatrixXd& H_pA,
|
|
const Feature& feature,
|
|
const StateIDType& cam_state_id,
|
|
Eigen::MatrixXd& H_xl,
|
|
Eigen::MatrixXd& H_yl);
|
|
|
|
bool PhotometricPatchPointResidual(
|
|
const StateIDType& cam_state_id,
|
|
const Feature& feature,
|
|
Eigen::VectorXd& r);
|
|
|
|
bool PhotometricPatchPointJacobian(
|
|
const CAMState& cam_state,
|
|
const StateIDType& cam_state_id,
|
|
const Feature& feature,
|
|
Eigen::Vector3d point,
|
|
int count,
|
|
Eigen::Matrix<double, 1, 1>& H_rhoj,
|
|
Eigen::Matrix<double, 1, 6>& H_plj,
|
|
Eigen::Matrix<double, 1, 6>& H_pAj,
|
|
Eigen::Matrix<double, 1, 2>& dI_dhj);
|
|
|
|
bool PhotometricMeasurementJacobian(
|
|
const StateIDType& cam_state_id,
|
|
const FeatureIDType& feature_id,
|
|
Eigen::MatrixXd& H_x,
|
|
Eigen::MatrixXd& H_y,
|
|
Eigen::VectorXd& r);
|
|
|
|
void twodotFeatureJacobian(
|
|
const FeatureIDType& feature_id,
|
|
const std::vector<StateIDType>& cam_state_ids,
|
|
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
|
|
|
bool PhotometricFeatureJacobian(
|
|
const FeatureIDType& feature_id,
|
|
const std::vector<StateIDType>& cam_state_ids,
|
|
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
|
|
|
void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
|
|
void measurementUpdate(const Eigen::MatrixXd& H,
|
|
const Eigen::VectorXd& r);
|
|
void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
|
|
|
|
bool gatingTest(const Eigen::MatrixXd& H,
|
|
const Eigen::VectorXd&r, const int& dof);
|
|
void removeLostFeatures();
|
|
void findRedundantCamStates(
|
|
std::vector<StateIDType>& rm_cam_state_ids);
|
|
|
|
void pruneLastCamStateBuffer();
|
|
void pruneCamStateBuffer();
|
|
// Reset the system online if the uncertainty is too large.
|
|
void onlineReset();
|
|
|
|
// Photometry flag
|
|
int FILTER;
|
|
|
|
// debug flag
|
|
bool STREAMPAUSE;
|
|
bool PRINTIMAGES;
|
|
bool GROUNDTRUTH;
|
|
|
|
bool nan_flag;
|
|
bool play;
|
|
double last_time_bound;
|
|
|
|
// Patch size for Photometry
|
|
int N;
|
|
|
|
// Chi squared test table.
|
|
static std::map<int, double> chi_squared_test_table;
|
|
|
|
double eval_time;
|
|
|
|
IMUState timed_old_imu_state;
|
|
IMUState timed_old_true_state;
|
|
|
|
IMUState old_imu_state;
|
|
IMUState old_true_state;
|
|
|
|
// change in position
|
|
Eigen::Vector3d delta_position;
|
|
Eigen::Vector3d delta_orientation;
|
|
|
|
// State vector
|
|
StateServer state_server;
|
|
StateServer photometric_state_server;
|
|
|
|
// Ground truth state vector
|
|
StateServer true_state_server;
|
|
|
|
// error state based on ground truth
|
|
StateServer err_state_server;
|
|
|
|
// Maximum number of camera states
|
|
int max_cam_state_size;
|
|
|
|
// Features used
|
|
MapServer map_server;
|
|
|
|
// IMU data buffer
|
|
// This is buffer is used to handle the unsynchronization or
|
|
// transfer delay between IMU and Image messages.
|
|
std::vector<sensor_msgs::Imu> imu_msg_buffer;
|
|
|
|
// Ground Truth message data
|
|
std::vector<geometry_msgs::TransformStamped> truth_msg_buffer;
|
|
// Moving Window buffer
|
|
movingWindow cam0_moving_window;
|
|
movingWindow cam1_moving_window;
|
|
|
|
// Camera calibration parameters
|
|
CameraCalibration cam0;
|
|
CameraCalibration cam1;
|
|
|
|
// covariance data
|
|
double irradiance_frame_bias;
|
|
|
|
|
|
ros::Time image_save_time;
|
|
|
|
// Indicate if the gravity vector is set.
|
|
bool is_gravity_set;
|
|
|
|
// Indicate if the received image is the first one. The
|
|
// system will start after receiving the first image.
|
|
bool is_first_img;
|
|
|
|
// The position uncertainty threshold is used to determine
|
|
// when to reset the system online. Otherwise, the ever-
|
|
// increaseing uncertainty will make the estimation unstable.
|
|
// Note this online reset will be some dead-reckoning.
|
|
// Set this threshold to nonpositive to disable online reset.
|
|
double position_std_threshold;
|
|
|
|
// Tracking rate
|
|
double tracking_rate;
|
|
|
|
// Threshold for determine keyframes
|
|
double translation_threshold;
|
|
double rotation_threshold;
|
|
double tracking_rate_threshold;
|
|
|
|
// Ros node handle
|
|
ros::NodeHandle nh;
|
|
|
|
// Subscribers and publishers
|
|
ros::Subscriber imu_sub;
|
|
ros::Subscriber truth_sub;
|
|
ros::Publisher truth_odom_pub;
|
|
ros::Publisher odom_pub;
|
|
ros::Publisher marker_pub;
|
|
ros::Publisher feature_pub;
|
|
tf::TransformBroadcaster tf_pub;
|
|
ros::ServiceServer reset_srv;
|
|
|
|
|
|
message_filters::Subscriber<sensor_msgs::Image> cam0_img_sub;
|
|
message_filters::Subscriber<sensor_msgs::Image> cam1_img_sub;
|
|
message_filters::Subscriber<CameraMeasurement> feature_sub;
|
|
|
|
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, CameraMeasurement> image_sub;
|
|
|
|
// Frame id
|
|
std::string fixed_frame_id;
|
|
std::string child_frame_id;
|
|
|
|
// Whether to publish tf or not.
|
|
bool publish_tf;
|
|
|
|
// Framte rate of the stereo images. This variable is
|
|
// only used to determine the timing threshold of
|
|
// each iteration of the filter.
|
|
double frame_rate;
|
|
|
|
// Debugging variables and functions
|
|
void mocapOdomCallback(
|
|
const nav_msgs::OdometryConstPtr& msg);
|
|
|
|
ros::Subscriber mocap_odom_sub;
|
|
ros::Publisher mocap_odom_pub;
|
|
geometry_msgs::TransformStamped raw_mocap_odom_msg;
|
|
Eigen::Isometry3d mocap_initial_frame;
|
|
|
|
Eigen::Vector4d mocap_initial_orientation;
|
|
Eigen::Vector3d mocap_initial_position;
|
|
};
|
|
|
|
typedef MsckfVio::Ptr MsckfVioPtr;
|
|
typedef MsckfVio::ConstPtr MsckfVioConstPtr;
|
|
|
|
} // namespace msckf_vio
|
|
|
|
#endif
|