├── README.md └── VIEKFSLAM2 ├── CMakeLists.txt ├── aruco_markers ├── marker.docx ├── marker.pdf ├── marker0.png ├── marker1.png ├── marker10.png ├── marker11.png ├── marker12.png ├── marker13.png ├── marker14.png ├── marker15.png ├── marker16.png ├── marker17.png ├── marker18.png ├── marker19.png ├── marker2.png ├── marker3.png ├── marker4.png ├── marker5.png ├── marker6.png ├── marker7.png ├── marker8.png └── marker9.png ├── config └── mynteye.yaml ├── demo └── viekfslam.png ├── include └── VIEKFSLAM2 │ ├── converter.hpp │ ├── math_utils.hpp │ ├── state.hpp │ ├── types.hpp │ └── vi_ekfslam.hpp ├── launch └── vi_ekfslam.launch ├── package.xml ├── rviz └── rviz_config.rviz └── src ├── create_aruco_markers.cpp ├── vi_ekfslam.cpp └── vi_ekfslam_node.cpp /README.md: -------------------------------------------------------------------------------- 1 | # Visual-Inerital-EKF-SLAM 2 | An implementation of visual-inertial EKF SLAM, more specific, the known correspondence EKF SLAM. 3 | The visual features are markers.It uses IMU measurements to predict system states and 4 | visual markers measurements to update system states. 5 | The system simultaneously estimates the 15-dims imu states and 6-dims marker states 6 | 7 | The detail description can be found in https://zhuanlan.zhihu.com/p/98472384 8 | 9 | ### If you have any issues, feel free to contact me through email(daojun@mail.ustc.edu.cn) 10 | 11 | ### click the image to watch the demo video 12 | 13 | [![image](https://github.com/DaojunZhu/Visual-Inerital-EKF-SLAM/blob/master/VIEKFSLAM2/demo/viekfslam.png)](https://www.youtube.com/watch?v=G7p0Oaroe9g) 14 | -------------------------------------------------------------------------------- /VIEKFSLAM2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(VIEKFSLAM2) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | tf 15 | geometry_msgs 16 | sensor_msgs 17 | nav_msgs 18 | std_msgs 19 | cv_bridge 20 | tf_conversions 21 | eigen_conversions 22 | ) 23 | 24 | find_package(Eigen3 REQUIRED) 25 | find_package(OpenCV REQUIRED) 26 | 27 | 28 | catkin_package( 29 | ) 30 | 31 | ## Specify additional locations of header files 32 | ## Your package locations should be listed before other locations 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ${EIGEN3_INCLUDE_DIR} 37 | ${OpenCV_INCLUDE_DIRS} 38 | ) 39 | 40 | add_executable(vi_ekfslam_node src/vi_ekfslam_node.cpp src/vi_ekfslam.cpp) 41 | target_link_libraries(vi_ekfslam_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 42 | 43 | add_executable(create_aruco_markers src/create_aruco_markers.cpp) 44 | target_link_libraries(create_aruco_markers ${OpenCV_LIBRARIES}) -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker.docx -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker.pdf -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker0.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker1.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker10.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker11.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker12.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker13.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker13.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker14.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker14.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker15.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker16.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker16.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker17.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker17.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker18.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker18.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker19.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker19.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker2.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker3.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker4.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker5.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker6.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker7.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker8.png -------------------------------------------------------------------------------- /VIEKFSLAM2/aruco_markers/marker9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/aruco_markers/marker9.png -------------------------------------------------------------------------------- /VIEKFSLAM2/config/mynteye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | R_imu_cam: !!opencv-matrix 4 | rows: 3 5 | cols: 3 6 | dt: d 7 | data: [0.010309024186486937, -0.9999260102291188,-0.006457405639991585, 8 | 0.9998300378494687, 0.010208895415655062, 0.015351673144095974, 9 | -0.01528461429848226, -0.006614568895187682, 0.9998613044038049] 10 | 11 | t_imu_cam: !!opencv-matrix 12 | rows: 3 13 | cols: 1 14 | dt: d 15 | data: [-0.0019196,-0.0392691, 0.0072418] 16 | 17 | intrinsics: !!opencv-matrix 18 | rows: 4 19 | cols: 1 20 | dt: d 21 | data: [377.37396383958304,377.33612468606094,385.5019347556897,231.1887530656648] 22 | 23 | #distortion_coeffs: !!opencv-matrix 24 | # rows: 4 25 | # cols: 1 26 | # dt: d 27 | # data: [-0.31460157366941205,0.09123078956376497,-0.0009452600710251895,-0.0018935763084249112] 28 | 29 | distortion_coeffs: !!opencv-matrix 30 | rows: 4 31 | cols: 1 32 | dt: d 33 | data: [0.,0.,0.,0.] -------------------------------------------------------------------------------- /VIEKFSLAM2/demo/viekfslam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaojunZhu/Visual-Inerital-EKF-SLAM/39cd16ade41e0ed8c51e5706054b6e519d423e5b/VIEKFSLAM2/demo/viekfslam.png -------------------------------------------------------------------------------- /VIEKFSLAM2/include/VIEKFSLAM2/converter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CONVERTER_H 2 | #define CONVERTER_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace vi_ekfslam{ 10 | 11 | 12 | inline Eigen::Vector3d vectorMsg2Eigen(const geometry_msgs::Vector3& msg){ 13 | Eigen::Vector3d v; 14 | v(0) = msg.x; 15 | v(1) = msg.y; 16 | v(2) = msg.z; 17 | return v; 18 | } 19 | 20 | inline void vectorCv2Eigen(const cv::Mat& v_c, Eigen::Vector3d& v_e){ 21 | v_e(0) = v_c.at(0,0); 22 | v_e(1) = v_c.at(1,0); 23 | v_e(2) = v_c.at(2,0); 24 | } 25 | 26 | inline void vectorCv2Eigen(const cv::Vec3d& v_c, Eigen::Vector3d& v_e){ 27 | v_e(0) = v_c(0); 28 | v_e(1) = v_c(1); 29 | v_e(2) = v_c(2); 30 | } 31 | 32 | inline void matrixCv2Eigen(const cv::Mat& m_c,Eigen::Matrix3d& m_e){ 33 | m_e << m_c.at(0,0),m_c.at(0,1),m_c.at(0,2), 34 | m_c.at(1,0),m_c.at(1,1),m_c.at(1,2), 35 | m_c.at(2,0),m_c.at(2,1),m_c.at(2,2); 36 | } 37 | 38 | } 39 | 40 | #endif -------------------------------------------------------------------------------- /VIEKFSLAM2/include/VIEKFSLAM2/math_utils.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace vi_ekfslam{ 6 | 7 | inline Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& v){ 8 | Eigen::Matrix3d m = Eigen::Matrix3d::Zero(); 9 | m(0,1) = -v(2); 10 | m(0,2) = v(1); 11 | m(1,0) = v(2); 12 | m(1,2) = -v(0); 13 | m(2,0) = -v(1); 14 | m(2,1) = v(0); 15 | return m; 16 | } 17 | 18 | //ref Kinematics (115) 19 | inline Eigen::Matrix3d quaterion2Rotation(const Eigen::Vector4d& q){ 20 | Eigen::Matrix3d R; 21 | double qw = q(0); 22 | double qx = q(1); 23 | double qy = q(2); 24 | double qz = q(3); 25 | R << qw*qw+qx*qx-qy*qy-qz*qz, 2*(qx*qy-qw*qz),2*(qx*qz+qw*qy), 26 | 2*(qx*qy+qw*qz), qw*qw-qx*qx+qy*qy-qz*qz, 2*(qy*qz-qw*qx), 27 | 2*(qx*qz-qw*qy), 2*(qy*qz+qw*qx), qw*qw-qx*qx-qy*qy+qz*qz; 28 | return R; 29 | } 30 | 31 | inline Eigen::Vector4d delta_q(const Eigen::Vector3d& w_dt){ 32 | Eigen::Vector4d d_q; 33 | d_q(0) = 1; 34 | d_q.tail(3) = 0.5 * w_dt; 35 | return d_q; 36 | } 37 | 38 | } 39 | 40 | -------------------------------------------------------------------------------- /VIEKFSLAM2/include/VIEKFSLAM2/state.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "types.hpp" 11 | #include "converter.hpp" 12 | 13 | namespace vi_ekfslam{ 14 | 15 | //IMU state 16 | struct IMUState{ 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | //Time when the state is recorded 20 | double time; 21 | 22 | //Orientation 23 | //use hamilton quaternion representation(qw,qv) 24 | //Take a vector from the IMU(body) frame 25 | //to the world frame 26 | Eigen::Quaterniond orientation; 27 | 28 | //Position of the IMU frame in the world frame 29 | Eigen::Vector3d position; 30 | 31 | //Velocity of the IMU frame in the world frame 32 | Eigen::Vector3d velocity; 33 | 34 | //gyroscope and acclerometer bias to be estimated 35 | Eigen::Vector3d gyro_bias; 36 | Eigen::Vector3d acc_bias; 37 | 38 | //constructor 39 | IMUState(): time(0), 40 | orientation(Eigen::Quaterniond::Identity()) , 41 | position(Eigen::Vector3d::Zero()), 42 | velocity(Eigen::Vector3d::Zero()), 43 | gyro_bias(Eigen::Vector3d::Zero()), 44 | acc_bias(Eigen::Vector3d::Zero()) {} 45 | 46 | }; 47 | 48 | 49 | struct FeatureState{ 50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 51 | 52 | //The marker feature id 53 | int id; 54 | 55 | FeatureState(int _id) : id(_id){} 56 | 57 | //Rotation 58 | //Transform a vector from the marker frame to the world frame 59 | Eigen::Quaterniond orientation; 60 | 61 | //The marker position in the world frame 62 | Eigen::Vector3d position; 63 | 64 | //initialize the pose of the marker 65 | static inline void initializePose(const IMUState& imu_state, 66 | const MarkerMeasData& meas, 67 | Eigen::Quaterniond& rotation, 68 | Eigen::Vector3d& position); 69 | }; 70 | 71 | typedef std::vector FeatureMap; 72 | 73 | void FeatureState::initializePose(const IMUState& imu_state, 74 | const MarkerMeasData& meas, 75 | Eigen::Quaterniond& rotation, 76 | Eigen::Vector3d& position){ 77 | 78 | //opencv function 79 | cv::Vec3d rvec,tvec; 80 | //The corner point have been undistorted to nomalized plan 81 | std::vector> corners_in; 82 | std::vector rvecs,tvecs; 83 | corners_in.push_back(meas.corners); 84 | double fx = camera_imu_parameters.camera_intrinsics.at(0,0); 85 | double fy = camera_imu_parameters.camera_intrinsics.at(1,0); 86 | double cx = camera_imu_parameters.camera_intrinsics.at(2,0); 87 | double cy = camera_imu_parameters.camera_intrinsics.at(3,0); 88 | // cv::Mat K = (cv::Mat_(3,3) << fx,0,cx,0,fy,cy,0,0,1 ); 89 | cv::Mat K = cv::Mat::eye(3,3,CV_64F); 90 | cv::Mat distcoeffs = (cv::Mat_(4,1) << 0.0,0.0,0.0,0.0,0.0); 91 | cv::aruco::estimatePoseSingleMarkers(corners_in,marker_parameters.size, 92 | K ,distcoeffs,rvecs,tvecs); 93 | 94 | rvec = rvecs[0]; 95 | tvec = tvecs[0]; 96 | Eigen::Vector3d t_cam_marker; 97 | vectorCv2Eigen(tvec,t_cam_marker); 98 | cv::Mat R; 99 | cv::Rodrigues(rvec,R); 100 | Eigen::Matrix3d R_cam_marker; 101 | matrixCv2Eigen(R,R_cam_marker); 102 | rotation = Eigen::Quaterniond( imu_state.orientation.toRotationMatrix() * 103 | camera_imu_parameters.R_imu_cam * R_cam_marker ); 104 | rotation.normalize(); 105 | position = imu_state.orientation.toRotationMatrix()*(camera_imu_parameters.R_imu_cam* 106 | t_cam_marker+camera_imu_parameters.t_imu_cam) + 107 | imu_state.position; 108 | 109 | } 110 | 111 | } 112 | -------------------------------------------------------------------------------- /VIEKFSLAM2/include/VIEKFSLAM2/types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace vi_ekfslam{ 8 | 9 | struct MarkerMeasData 10 | { 11 | int id; 12 | std::vector corners; 13 | }; 14 | 15 | struct CameraIMUParameters{ 16 | //camera intrinsics and distortion parameters 17 | cv::Mat camera_intrinsics; 18 | cv::Mat distortion_coeffs; 19 | //the transform from camera frame to imu frame 20 | Eigen::Vector3d t_imu_cam; 21 | Eigen::Matrix3d R_imu_cam; 22 | 23 | //Process noise 24 | double gyro_noise; // gn 25 | double acc_noise; // an 26 | double gyro_bias_noise; // gw 27 | double acc_bias_noise; // aw 28 | 29 | //Gravity vector in the world frame(NED) 30 | Eigen::Vector3d gravity; 31 | }; 32 | 33 | extern CameraIMUParameters camera_imu_parameters; 34 | 35 | struct MarkerParemeters{ 36 | //The aruco marker size in metric unit 37 | double size; 38 | 39 | //the four coordinates of marker's four corners expressed in marker frame 40 | //started by top-left corner ,then top right... 41 | Eigen::Vector3d M_c0; 42 | Eigen::Vector3d M_c1; 43 | Eigen::Vector3d M_c2; 44 | Eigen::Vector3d M_c3; 45 | 46 | //the observation noise in nominal image plane 47 | double observation_noise; 48 | 49 | double initial_position_noise; 50 | double initla_orientation_noise; 51 | }; 52 | 53 | extern MarkerParemeters marker_parameters; 54 | 55 | } 56 | 57 | 58 | -------------------------------------------------------------------------------- /VIEKFSLAM2/include/VIEKFSLAM2/vi_ekfslam.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "state.hpp" 17 | #include "types.hpp" 18 | 19 | namespace vi_ekfslam{ 20 | 21 | 22 | class VIEkfSlam{ 23 | public: 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | 26 | VIEkfSlam(::ros::NodeHandle& nh); 27 | VIEkfSlam(const VIEkfSlam&) = delete; 28 | VIEkfSlam operator=(const VIEkfSlam&) = delete; 29 | 30 | ~VIEkfSlam(){} 31 | 32 | //initialize the vi-ekfslam 33 | bool initialize(); 34 | 35 | typedef std::shared_ptr Ptr; 36 | typedef std::shared_ptr ConstPtr; 37 | 38 | private: 39 | 40 | struct StateServer 41 | { 42 | IMUState imu_state; 43 | FeatureMap fature_states; 44 | 45 | //State covariance matrix 46 | Eigen::MatrixXd state_cov; 47 | Eigen::Matrix continuous_noise_cov; 48 | }; 49 | 50 | // Load parameters from the parameter server 51 | bool loadParameters(); 52 | 53 | //Create ros publisher and subscribers 54 | bool createRosIO(); 55 | 56 | //Callback function for the imu message 57 | void imuCallback(const sensor_msgs::ImuConstPtr& imu_msg); 58 | 59 | //Callback function for camera message 60 | void camCallback(const sensor_msgs::ImageConstPtr& img_msg); 61 | 62 | //Publish the result of VIEKFSLAM 63 | void publish(const ros::Time& time); 64 | 65 | //Initialize the IMU bias and initial orientation 66 | // based on the first few IMU readings. 67 | // static initialization 68 | void initializeGravityAndBias(); 69 | 70 | //process IMU measurement 71 | void processIMUData(const double& time, 72 | const Eigen::Vector3d& m_gyro,const Eigen::Vector3d& m_acc); 73 | 74 | //predict the nominal IMU state 75 | void predictNominalState(const double& dt, 76 | const Eigen::Vector3d& gyro,const Eigen::Vector3d& acc); 77 | 78 | //augment a new detected marker to state 79 | void stateAugmentation(const MarkerMeasData& marker_data); 80 | 81 | //process image measurement 82 | void processMarkerData(const std::vector& marker_meas); 83 | 84 | // compute the marker feature measuement jacobian 85 | void measurementJacobian(int index, const MarkerMeasData& meas, 86 | Eigen::MatrixXd& H_x_i, Eigen::VectorXd& r_i); 87 | 88 | //measurement update 89 | void measurementUpdate(const Eigen::MatrixXd& H_x, const Eigen::VectorXd& r); 90 | 91 | //update state 92 | void updateState(const Eigen::VectorXd& delta); 93 | 94 | //detect aruco marker 95 | void detectArucoMarker(const cv::Mat& image,std::vector& markers); 96 | 97 | //undistort points 98 | void undistortPoints(const std::vector& pts_in, 99 | const cv::Vec4d& intrinsics, 100 | const cv::Vec4d& distortion_coeffs, 101 | std::vector& pts_out); 102 | 103 | //draw aruco marker pose and covariance in the rviz 104 | void drawRosMarker(int id, const ros::Time& time, 105 | const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, 106 | const Eigen::Matrix3d& cov_pp, 107 | visualization_msgs::Marker& marker); 108 | 109 | 110 | private: 111 | 112 | //State vector 113 | StateServer state_server; 114 | 115 | // Indicate if the gravity vector is set. 116 | bool is_gravity_set; 117 | 118 | //beginning IMU msgs for initialization 119 | std::vector imu_buffer; 120 | 121 | //opencv aruco marker dictionary for detection 122 | cv::Ptr dictionary; 123 | 124 | //Ros node handle 125 | ros::NodeHandle nh; 126 | 127 | //subscribers and publishers 128 | ros::Subscriber imu_sub; 129 | ros::Subscriber img_sub; 130 | ros::Publisher odom_pub; 131 | ros::Publisher debug_img_pub; 132 | ros::Publisher feature_map_pub; 133 | ros::Publisher features_pose_pub; 134 | ros::Publisher trajectory_pub; 135 | tf::TransformBroadcaster tf_pub; 136 | 137 | //Frame id 138 | std::string world_frame_id; 139 | std::string body_frame_id; 140 | 141 | //Topic name 142 | std::string imu_topic; 143 | std::string img_topic; 144 | 145 | //debug image for visualization 146 | cv::Mat debug_image; 147 | 148 | 149 | }; 150 | 151 | 152 | } 153 | 154 | -------------------------------------------------------------------------------- /VIEKFSLAM2/launch/vi_ekfslam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /VIEKFSLAM2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | VIEKFSLAM2 4 | 0.0.0 5 | The VIEKFSLAM2 package 6 | 7 | 8 | 9 | 10 | daojun 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /VIEKFSLAM2/rviz/rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 539 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Image 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | robot: 61 | Value: true 62 | world: 63 | Value: true 64 | Marker Scale: 1 65 | Name: TF 66 | Show Arrows: true 67 | Show Axes: true 68 | Show Names: true 69 | Tree: 70 | world: 71 | robot: 72 | {} 73 | Update Interval: 0 74 | Value: true 75 | - Class: rviz/Image 76 | Enabled: true 77 | Image Topic: /debug_img 78 | Max Value: 1 79 | Median window: 5 80 | Min Value: 0 81 | Name: Image 82 | Normalize Range: true 83 | Queue Size: 2 84 | Transport Hint: raw 85 | Unreliable: false 86 | Value: true 87 | - Class: rviz/MarkerArray 88 | Enabled: true 89 | Marker Topic: /features 90 | Name: MarkerArray 91 | Namespaces: 92 | ArucoMarker: true 93 | Queue Size: 100 94 | Value: true 95 | - Alpha: 1 96 | Arrow Length: 0.30000001192092896 97 | Axes Length: 0.30000001192092896 98 | Axes Radius: 0.009999999776482582 99 | Class: rviz/PoseArray 100 | Color: 255; 25; 0 101 | Enabled: true 102 | Head Length: 0.07000000029802322 103 | Head Radius: 0.029999999329447746 104 | Name: PoseArray 105 | Shaft Length: 0.23000000417232513 106 | Shaft Radius: 0.009999999776482582 107 | Shape: Arrow (Flat) 108 | Topic: /features_pose 109 | Unreliable: false 110 | Value: true 111 | - Alpha: 1 112 | Buffer Length: 1 113 | Class: rviz/Path 114 | Color: 25; 255; 0 115 | Enabled: true 116 | Head Diameter: 0.30000001192092896 117 | Head Length: 0.20000000298023224 118 | Length: 0.30000001192092896 119 | Line Style: Lines 120 | Line Width: 0.029999999329447746 121 | Name: Path 122 | Offset: 123 | X: 0 124 | Y: 0 125 | Z: 0 126 | Pose Color: 255; 85; 255 127 | Pose Style: None 128 | Radius: 0.029999999329447746 129 | Shaft Diameter: 0.10000000149011612 130 | Shaft Length: 0.10000000149011612 131 | Topic: /traj 132 | Unreliable: false 133 | Value: true 134 | Enabled: true 135 | Global Options: 136 | Background Color: 48; 48; 48 137 | Default Light: true 138 | Fixed Frame: world 139 | Frame Rate: 30 140 | Name: root 141 | Tools: 142 | - Class: rviz/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz/MoveCamera 145 | - Class: rviz/Select 146 | - Class: rviz/FocusCamera 147 | - Class: rviz/Measure 148 | - Class: rviz/SetInitialPose 149 | Theta std deviation: 0.2617993950843811 150 | Topic: /initialpose 151 | X std deviation: 0.5 152 | Y std deviation: 0.5 153 | - Class: rviz/SetGoal 154 | Topic: /move_base_simple/goal 155 | - Class: rviz/PublishPoint 156 | Single click: true 157 | Topic: /clicked_point 158 | Value: true 159 | Views: 160 | Current: 161 | Class: rviz/Orbit 162 | Distance: 8.899293899536133 163 | Enable Stereo Rendering: 164 | Stereo Eye Separation: 0.05999999865889549 165 | Stereo Focal Distance: 1 166 | Swap Stereo Eyes: false 167 | Value: false 168 | Focal Point: 169 | X: 0 170 | Y: 0 171 | Z: 0 172 | Focal Shape Fixed Size: true 173 | Focal Shape Size: 0.05000000074505806 174 | Invert Z Axis: false 175 | Name: Current View 176 | Near Clip Distance: 0.009999999776482582 177 | Pitch: 0.7603981494903564 178 | Target Frame: 179 | Value: Orbit (rviz) 180 | Yaw: 0.930392324924469 181 | Saved: ~ 182 | Window Geometry: 183 | Displays: 184 | collapsed: true 185 | Height: 846 186 | Hide Left Dock: true 187 | Hide Right Dock: true 188 | Image: 189 | collapsed: false 190 | QMainWindow State: 000000ff00000000fd00000004000000000000017e000002adfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e000002ad000000da00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006503000008960000024b000001ba00000121000000010000010f000002adfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002ad000000b100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054f0000003efc0100000002fb0000000800540069006d006501000000000000054f000002c400fffffffb0000000800540069006d006501000000000000045000000000000000000000054f000002ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 191 | Selection: 192 | collapsed: false 193 | Time: 194 | collapsed: false 195 | Tool Properties: 196 | collapsed: false 197 | Views: 198 | collapsed: true 199 | Width: 1359 200 | X: 2168 201 | Y: 106 202 | -------------------------------------------------------------------------------- /VIEKFSLAM2/src/create_aruco_markers.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | using namespace cv; 8 | 9 | int main(int argc,char** argv) 10 | { 11 | string save_path = 12 | "/home/daojun/myrosprojects_ws/src/VIEKFSLAM2/aruco_markers"; 13 | 14 | int marker_num = 15; 15 | 16 | Ptr dictionary = 17 | aruco::getPredefinedDictionary(aruco::DICT_6X6_250); 18 | 19 | for(int i = 15; i < 20; ++i){ 20 | Mat markerImage; 21 | aruco::drawMarker(dictionary,i,400,markerImage,1); 22 | string marker_name = save_path + "/marker" + to_string(i) + ".png"; 23 | imwrite(marker_name,markerImage); 24 | } 25 | 26 | return 0; 27 | } -------------------------------------------------------------------------------- /VIEKFSLAM2/src/vi_ekfslam.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "VIEKFSLAM2/vi_ekfslam.hpp" 3 | #include "VIEKFSLAM2/converter.hpp" 4 | #include "VIEKFSLAM2/math_utils.hpp" 5 | 6 | #include 7 | //conversion between eigen and tf class 8 | #include 9 | //conversion between eigen and ros msg 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | using namespace Eigen; 17 | using namespace cv; 18 | 19 | namespace vi_ekfslam{ 20 | 21 | 22 | CameraIMUParameters camera_imu_parameters; 23 | MarkerParemeters marker_parameters; 24 | 25 | 26 | VIEkfSlam::VIEkfSlam(ros::NodeHandle& pnh) 27 | : is_gravity_set(false), 28 | nh(pnh){ 29 | dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); 30 | 31 | 32 | } 33 | 34 | bool VIEkfSlam::loadParameters(){ 35 | nh.param("world_frame_id",world_frame_id,"world"); 36 | nh.param("body_frame_id",body_frame_id,"robot"); 37 | nh.param("imu_topic",imu_topic,"imu"); 38 | nh.param("img_topic",img_topic,"image"); 39 | 40 | nh.param("noise/gyro",camera_imu_parameters.gyro_noise,0.001); 41 | nh.param("noise/acc",camera_imu_parameters.acc_noise,0.01); 42 | nh.param("noise/gyro_bias",camera_imu_parameters.gyro_bias_noise,0.001); 43 | nh.param("noise/acc_bias",camera_imu_parameters.acc_bias_noise,0.01); 44 | 45 | camera_imu_parameters.gyro_noise *= camera_imu_parameters.gyro_noise; 46 | camera_imu_parameters.acc_noise *= camera_imu_parameters.acc_noise; 47 | camera_imu_parameters.gyro_bias_noise *= camera_imu_parameters.gyro_bias_noise; 48 | camera_imu_parameters.acc_bias_noise *= camera_imu_parameters.acc_bias_noise; 49 | 50 | nh.param("initial_state/velocity/x", 51 | state_server.imu_state.velocity(0), 0.0); 52 | nh.param("initial_state/velocity/y", 53 | state_server.imu_state.velocity(1), 0.0); 54 | nh.param("initial_state/velocity/z", 55 | state_server.imu_state.velocity(2), 0.0); 56 | 57 | double gyro_bias_cov, acc_bias_cov, velocity_cov; 58 | nh.param("initial_covariance/velocity", 59 | velocity_cov, 100); 60 | nh.param("initial_covariance/gyro_bias", 61 | gyro_bias_cov, 1e-4); 62 | nh.param("initial_covariance/acc_bias", 63 | acc_bias_cov, 1e-2); 64 | 65 | nh.param("marker_size",marker_parameters.size,0.101); 66 | nh.param("observation_noise",marker_parameters.observation_noise,0.0001); 67 | double s = 0.5 * marker_parameters.size; 68 | marker_parameters.M_c0 = Vector3d(-s,s,0); 69 | marker_parameters.M_c1 = Vector3d(s,s,0); 70 | marker_parameters.M_c2 = Vector3d(s,-s,0); 71 | marker_parameters.M_c3 = Vector3d(-s,-s,0); 72 | 73 | state_server.state_cov = MatrixXd::Zero(15,15); 74 | for(int i = 6; i < 9 ; ++i) 75 | state_server.state_cov(i,i) = velocity_cov; 76 | for(int i = 9; i < 12; ++i) 77 | state_server.state_cov(i,i) = acc_bias_cov; 78 | for(int i = 12; i < 15; ++i) 79 | state_server.state_cov(i,i) = gyro_bias_cov; 80 | 81 | //read parameters in the config file 82 | string config_file; 83 | if(!nh.getParam("config_file",config_file)){ 84 | ROS_ERROR("The config_file parameter not set."); 85 | return false; 86 | } 87 | FileStorage fs; 88 | fs.open(config_file,FileStorage::READ); 89 | if(!fs.isOpened()){ 90 | ROS_ERROR("Failed to open config file."); 91 | return false; 92 | } 93 | cv::Mat R,t; 94 | fs["R_imu_cam"] >> R; 95 | matrixCv2Eigen(R,camera_imu_parameters.R_imu_cam); 96 | fs["t_imu_cam"] >> t; 97 | vectorCv2Eigen(t,camera_imu_parameters.t_imu_cam); 98 | fs["intrinsics"] >> camera_imu_parameters.camera_intrinsics; 99 | fs["distortion_coeffs"] >> camera_imu_parameters.distortion_coeffs; 100 | 101 | ROS_INFO("Finish reading config file..."); 102 | cout << "R_imu_cam: " << endl << camera_imu_parameters.R_imu_cam << endl; 103 | cout << "t_imu_cam: " << endl << camera_imu_parameters.t_imu_cam << endl; 104 | cout << "camera intrinsics: " << endl << camera_imu_parameters.camera_intrinsics << endl; 105 | cout << "distortion_coeffs: " << endl << camera_imu_parameters.distortion_coeffs << endl; 106 | 107 | fs.release(); 108 | 109 | return true; 110 | } 111 | 112 | bool VIEkfSlam::createRosIO(){ 113 | odom_pub = nh.advertise("odom",10); 114 | debug_img_pub = nh.advertise("debug_img",10); 115 | feature_map_pub = nh.advertise("features",10); 116 | features_pose_pub = nh.advertise("features_pose",10); 117 | trajectory_pub = nh.advertise("traj",10); 118 | imu_sub = nh.subscribe(imu_topic,100,&VIEkfSlam::imuCallback,this); 119 | img_sub = nh.subscribe(img_topic,40,&VIEkfSlam::camCallback,this); 120 | return true; 121 | } 122 | 123 | bool VIEkfSlam::initialize(){ 124 | if(!loadParameters()) return false; 125 | ROS_INFO("Finishing loading ROS parameters...."); 126 | 127 | //Initialize state server 128 | state_server.continuous_noise_cov = Matrix::Zero(); 129 | state_server.continuous_noise_cov.block<3,3>(0,0) = 130 | camera_imu_parameters.gyro_noise * Matrix3d::Identity(); 131 | state_server.continuous_noise_cov.block<3,3>(3,3) = 132 | camera_imu_parameters.acc_bias_noise * Matrix3d::Identity(); 133 | state_server.continuous_noise_cov.block<3,3>(6,6) = 134 | camera_imu_parameters.gyro_bias_noise * Matrix3d::Identity(); 135 | state_server.continuous_noise_cov.block<3,3>(6,6) = 136 | camera_imu_parameters.acc_bias_noise * Matrix3d::Identity(); 137 | 138 | //imu initial position 139 | state_server.imu_state.position(0) = 0.0; 140 | state_server.imu_state.position(1) = 0.0; 141 | state_server.imu_state.position(2) = 0.0; 142 | 143 | // state_server.state_cov.block<3,3>(0,0) = 100.0 * Matrix3d::Identity(); 144 | // state_server.state_cov.block<2,2>(3,3) = 10. * M_PI/180. * 10. * M_PI/180. * Matrix2d::Identity(); 145 | // state_server.state_cov(5,5) = 100. * M_PI/180. * 100. * M_PI/180. ; 146 | 147 | 148 | if(!createRosIO()) return false; 149 | ROS_INFO("Finish creating ROS IO..."); 150 | 151 | return true; 152 | } 153 | 154 | void VIEkfSlam::initializeGravityAndBias(){ 155 | 156 | Vector3d sum_angular_vel = Vector3d::Zero(); 157 | Vector3d sum_linear_acc = Vector3d::Zero(); 158 | 159 | for(const auto& imu_msg : imu_buffer){ 160 | Vector3d angular_vel = 161 | vectorMsg2Eigen(imu_msg.angular_velocity); 162 | Vector3d linear_acc = 163 | vectorMsg2Eigen(imu_msg.linear_acceleration); 164 | 165 | sum_angular_vel += angular_vel; 166 | sum_linear_acc += linear_acc; 167 | } 168 | 169 | state_server.imu_state.gyro_bias = sum_angular_vel / imu_buffer.size(); 170 | //this is gravity in the imu frame 171 | Vector3d gravity_imu = sum_linear_acc / imu_buffer.size(); 172 | 173 | double gravity_norm = gravity_imu.norm(); 174 | camera_imu_parameters.gravity = Vector3d(0.0,0.0,-gravity_norm); 175 | 176 | //q_w_i0 convert a vector from IMU frame to world frame(NED) 177 | // initial IMU orientation estimation 178 | Quaterniond q_w_i0 = Quaterniond::FromTwoVectors(gravity_imu,-camera_imu_parameters.gravity); 179 | state_server.imu_state.orientation = q_w_i0; 180 | } 181 | 182 | void VIEkfSlam::imuCallback(const sensor_msgs::ImuConstPtr& imu_msg){ 183 | //wait for the enough imu msg to initialize 184 | //gravity and bias 185 | static int imu_msg_cntr = 0; 186 | if(!is_gravity_set){ 187 | imu_buffer.push_back(*imu_msg); 188 | if(imu_msg_cntr++ < 200) return; 189 | initializeGravityAndBias(); 190 | 191 | cout << "initial acc bias: " << endl << " " << 192 | state_server.imu_state.acc_bias.transpose() << endl; 193 | cout << "initial gyro bias: " << endl << " " << 194 | state_server.imu_state.gyro_bias.transpose() << endl; 195 | cout << "initla gravity in world frame: " << endl << " " << 196 | camera_imu_parameters.gravity.transpose() << endl; 197 | cout << "initial imu orientation: " << endl << " " << 198 | state_server.imu_state.orientation.toRotationMatrix() << endl; 199 | 200 | is_gravity_set = true; 201 | ROS_INFO("[imuCallback] initialized done."); 202 | state_server.imu_state.time = imu_msg->header.stamp.toSec(); 203 | } 204 | else{ 205 | // process imu data 206 | double time = imu_msg->header.stamp.toSec(); 207 | Vector3d m_gyro = vectorMsg2Eigen(imu_msg->angular_velocity); 208 | Vector3d m_acc = vectorMsg2Eigen(imu_msg->linear_acceleration); 209 | // cout << "process IMU measurement ..." << endl; 210 | processIMUData(time,m_gyro,m_acc); 211 | } 212 | 213 | } 214 | 215 | void VIEkfSlam::camCallback(const sensor_msgs::ImageConstPtr& img_msg){ 216 | 217 | if(!is_gravity_set){ 218 | ROS_INFO("[camCallback] gravity and bias not initialized..."); 219 | return; 220 | } 221 | 222 | //get the current image 223 | cv_bridge::CvImageConstPtr image_ptr = 224 | cv_bridge::toCvShare(img_msg,sensor_msgs::image_encodings::MONO8); 225 | 226 | vector marker_meas; 227 | 228 | ros::Time start_time = ros::Time::now(); 229 | 230 | //detect aruco marker 231 | detectArucoMarker(image_ptr->image,marker_meas); 232 | 233 | double marker_detection_time = (ros::Time::now()-start_time).toSec(); 234 | ROS_INFO("marker detection time: %f" ,marker_detection_time); 235 | 236 | //process marker measurement update 237 | processMarkerData(marker_meas); 238 | 239 | //publish ros topics and tf 240 | publish(img_msg->header.stamp); 241 | } 242 | 243 | 244 | void VIEkfSlam::processIMUData(const double& time, 245 | const Eigen::Vector3d& m_gyro, 246 | const Eigen::Vector3d& m_acc){ 247 | 248 | //nomial measurement 249 | IMUState& imu_state = state_server.imu_state; 250 | Vector3d gyro = m_gyro - imu_state.gyro_bias; 251 | Vector3d acc = m_acc - imu_state.acc_bias; 252 | double dtime = time - imu_state.time; 253 | double dtime2 = dtime * dtime; 254 | 255 | //ref: Kinematics P88 256 | //x_dot = F * x + G * n; 257 | //Phi = exp(F*dt) , approximate it using block wise, 2 order 258 | Matrix Phi = Matrix::Zero(); 259 | Matrix G = Matrix::Zero(); 260 | 261 | Matrix3d R = state_server.imu_state.orientation.toRotationMatrix(); 262 | Phi.block<3,3>(0,0) = Matrix3d::Identity(); 263 | Phi.block<3,3>(0,3) = -0.5 * R * skewSymmetric(acc) * dtime2; 264 | Phi.block<3,3>(0,6) = Matrix3d::Identity() * dtime; 265 | Phi.block<3,3>(0,9) = -0.5 * R * dtime2; 266 | Phi.block<3,3>(3,3) = quaterion2Rotation(delta_q(gyro*dtime)).transpose(); 267 | Phi.block<3,3>(3,12) = -Matrix3d::Identity() * dtime; 268 | Phi.block<3,3>(6,3) = -R * skewSymmetric(acc) * dtime; 269 | Phi.block<3,3>(6,6) = Matrix3d::Identity(); 270 | Phi.block<3,3>(6,9) = -R * dtime; 271 | Phi.block<3,3>(6,12) = 0.5 * R * skewSymmetric(acc) * dtime2; 272 | Phi.block<3,3>(9,9) = Matrix3d::Identity(); 273 | Phi.block<3,3>(12,12) = Matrix3d::Identity(); 274 | 275 | G.block<3,3>(3,0) = -Matrix3d::Identity(); 276 | G.block<3,3>(6,3) = -R; 277 | G.block<3,3>(9,9) = Matrix3d::Identity(); 278 | G.block<3,3>(12,6) = Matrix3d::Identity(); 279 | 280 | //predict nominal state 281 | predictNominalState(dtime,gyro,acc); 282 | 283 | //Propagate the state covariance matrix 284 | Matrix Q = Phi*G*state_server.continuous_noise_cov* 285 | G.transpose()*Phi.transpose()*dtime; 286 | state_server.state_cov.block<15,15>(0,0) = Phi* 287 | state_server.state_cov.block<15,15>(0,0)*Phi.transpose() + Q; 288 | 289 | if(state_server.fature_states.size() > 0){ 290 | state_server.state_cov.topRightCorner( 291 | 15,state_server.state_cov.cols()-15) = Phi * 292 | state_server.state_cov.topRightCorner(15,state_server.state_cov.cols()-15); 293 | state_server.state_cov.bottomLeftCorner( 294 | state_server.state_cov.rows()-15,15) = state_server.state_cov.bottomLeftCorner( 295 | state_server.state_cov.rows()-15,15)*Phi.transpose(); 296 | } 297 | 298 | //ensure the state covariance matrix to be symmetric 299 | MatrixXd state_cov_fixed = (state_server.state_cov + 300 | state_server.state_cov.transpose()) / 2.0; 301 | state_server.state_cov = state_cov_fixed; 302 | 303 | state_server.imu_state.time = time; 304 | 305 | } 306 | 307 | 308 | void VIEkfSlam::predictNominalState(const double& dt, 309 | const Eigen::Vector3d& gyro, 310 | const Eigen::Vector3d& acc){ 311 | 312 | double gyro_norm = gyro.norm(); 313 | 314 | //the imu nominal state in time point tn 315 | Quaterniond& q = state_server.imu_state.orientation; 316 | Vector3d& p = state_server.imu_state.position; 317 | Vector3d& v = state_server.imu_state.velocity; 318 | 319 | //The propagate orientation in tn+dt(tn+1) , tn+dt/2 320 | Quaterniond dq_dt , dq_dt2; 321 | if(gyro_norm > 1e-5){ 322 | Vector3d dqv = gyro / gyro_norm * sin(0.5*dt*gyro_norm); 323 | dq_dt = q * Quaterniond(cos(gyro_norm*0.5*dt),dqv(0),dqv(1),dqv(2)); 324 | Vector3d dqv2 = gyro / gyro_norm * sin(0.25*dt*gyro_norm); 325 | dq_dt2 = q * Quaterniond(cos(gyro_norm*0.25*dt),dqv2(0),dqv2(1),dqv2(2)); 326 | }else{ 327 | Vector3d dqv = 0.5 * gyro * dt; 328 | dq_dt = q * Quaterniond(1,dqv(0),dqv(1),dqv(2)); 329 | Vector3d dqv2 = 0.25 * gyro * dt; 330 | dq_dt2 = q * Quaterniond(1,dqv2(0),dqv2(1),dqv2(2)); 331 | } 332 | 333 | //Convert the quaternion to rotation matrix 334 | Matrix3d dR_dt = dq_dt.toRotationMatrix(); 335 | Matrix3d dR_dt2 = dq_dt.toRotationMatrix(); 336 | 337 | //k1 = f(tn,xn) 338 | Vector3d k1_v_dot = q*acc + camera_imu_parameters.gravity; 339 | Vector3d k1_p_dot = v; 340 | 341 | //k2 = f(tn+dt/2,xn+k1*dt/2) 342 | Vector3d k1_v = v + k1_v_dot * dt * 0.5; 343 | Vector3d k2_v_dot = dR_dt2 * acc + camera_imu_parameters.gravity; 344 | Vector3d k2_p_dot = k1_v; 345 | 346 | //k3 = f(tn+dt/2,xn+k2*dt/2) 347 | Vector3d k2_v = v + k2_v_dot * dt * 0.5; 348 | Vector3d k3_v_dot = dR_dt2 * acc + camera_imu_parameters.gravity; 349 | Vector3d k3_p_dot = k2_v; 350 | 351 | //k4 = f(tn+dt,xn+k3*dt) 352 | Vector3d k3_v = v + k3_v_dot * dt; 353 | Vector3d k4_v_dot = dR_dt * acc + camera_imu_parameters.gravity; 354 | Vector3d k4_p_dot = k3_v; 355 | 356 | q = dq_dt; 357 | q.normalize(); 358 | v = v + dt/6*(k1_v_dot+2*k2_v_dot+2*k3_v_dot+k4_v_dot); 359 | p = p + dt/6*(k1_p_dot+2*k2_p_dot+2*k3_p_dot+k4_p_dot); 360 | } 361 | 362 | void VIEkfSlam::detectArucoMarker(const cv::Mat& image, 363 | std::vector& markers){ 364 | 365 | markers.clear(); 366 | // The number of aruco marker detected 367 | int detect_marker_num = 0; 368 | 369 | //store detected marker id map to observation times 370 | //a marker whos observation times greater than 5 times 371 | // will be considered a valid marker measurement 372 | static map map_id2obstimes; 373 | 374 | vector marker_ids; 375 | vector> marker_corners; 376 | 377 | image.copyTo(debug_image); 378 | 379 | //detection 380 | cv::aruco::detectMarkers(image,dictionary,marker_corners,marker_ids); 381 | detect_marker_num = marker_ids.size(); 382 | cv::aruco::drawDetectedMarkers(debug_image,marker_corners,marker_ids); 383 | 384 | //Make marker measurement data 385 | for(int i = 0; i < detect_marker_num; ++i){ 386 | 387 | if(map_id2obstimes.find(marker_ids[i]) == map_id2obstimes.end()){ 388 | map_id2obstimes[marker_ids[i]] = 1; 389 | }else{ 390 | // a valid marker measurement 391 | if(map_id2obstimes[marker_ids[i]] >= 5 ){ 392 | MarkerMeasData marker_data; 393 | marker_data.id = marker_ids[i]; 394 | // undistort points to normalized plane 395 | undistortPoints(marker_corners[i],camera_imu_parameters.camera_intrinsics, 396 | camera_imu_parameters.distortion_coeffs,marker_data.corners); 397 | markers.push_back(marker_data); 398 | 399 | }else{ 400 | ++map_id2obstimes[marker_ids[i]]; 401 | } 402 | } 403 | 404 | 405 | } 406 | } 407 | 408 | void VIEkfSlam::processMarkerData( 409 | const std::vector& marker_meas){ 410 | 411 | //return if empty marker detected 412 | if(marker_meas.empty()) return; 413 | //the marker counter that detected 414 | int marker_meas_cntr = marker_meas.size(); 415 | 416 | //the marker measurement jacobian 417 | //and residual for old markers 418 | vector v_H_x; 419 | vector v_r; 420 | 421 | vector new_marker_datas; 422 | 423 | for(auto marker_data : marker_meas){ 424 | 425 | // auto find_iter = state_server.fature_states.find(marker_data.id); 426 | auto find_iter = std::find_if(state_server.fature_states.begin(), 427 | state_server.fature_states.end(),[marker_data](const FeatureState& f){ 428 | return f.id == marker_data.id;}); 429 | //This is a new marker 430 | if(find_iter == state_server.fature_states.end()){ 431 | cout << "new marker detected. marker id: " << marker_data.id << endl; 432 | //add the new marker to state 433 | stateAugmentation(marker_data); 434 | // new_marker_datas.push_back(marker_data); 435 | } 436 | //This is an old marker 437 | else{ 438 | cout << "old marker detected. marker id: " << marker_data.id << endl; 439 | //The ordered slam index of this marker 440 | int slam_index = std::distance( 441 | state_server.fature_states.begin(),find_iter); 442 | // cout << "slam_index : " << slam_index << endl; 443 | MatrixXd H_x_i = MatrixXd::Zero(8, 444 | 15+6*state_server.fature_states.size()); 445 | VectorXd r_i = VectorXd::Zero(8); 446 | //compute jacobian 447 | measurementJacobian(slam_index,marker_data,H_x_i,r_i); 448 | v_H_x.push_back(H_x_i); 449 | v_r.push_back(r_i); 450 | 451 | // cout << "measurement jacobian : " << endl << 452 | // H_x_i << endl; 453 | // cout << "measurement error: " << endl << 454 | // r_i << endl; 455 | ros::Time start_time = ros::Time::now(); 456 | 457 | measurementUpdate(H_x_i,r_i); 458 | 459 | double measurement_update_time = (ros::Time::now()-start_time).toSec(); 460 | ROS_INFO("measurement update time: %f",measurement_update_time); 461 | 462 | // //debug 463 | // cout << "imu cov: " << endl; 464 | // cout << state_server.state_cov.block<15,15>(0,0) << endl; 465 | // for(int i = 0; i < state_server.fature_states.size(); ++i){ 466 | // cout << "marker " << state_server.fature_states[i].id << " cov: " << endl; 467 | // cout << state_server.state_cov.block<6,6>(15+6*i,15+6*i) << endl; 468 | 469 | // } 470 | 471 | } 472 | } 473 | 474 | // for(auto marker_data : new_marker_datas){ 475 | // stateAugmentation(marker_data); 476 | // } 477 | 478 | // if(v_H_x.size() > 0){ 479 | // int cntr = v_H_x.size(); 480 | // cout << "v_H_x size : " << cntr << endl; 481 | // MatrixXd H = MatrixXd::Zero(8*cntr, 482 | // 15+6*state_server.fature_states.size()); 483 | // VectorXd r = VectorXd::Zero(8*cntr); 484 | // for(int i = 0; i < cntr; ++i){ 485 | // H.block(8*i,0,8, 486 | // 15+6*state_server.fature_states.size()) = v_H_x[i]; 487 | // r.segment(8*i,8) = v_r[i]; 488 | // } 489 | // cout << "process measurement update..." << endl; 490 | // measurementUpdate(H,r); 491 | // cout << "measurement update done. " << endl; 492 | // } 493 | 494 | 495 | 496 | // //debug 497 | // cout << "imu position cov: " << endl; 498 | // cout << state_server.state_cov.block<3,3>(0,0) << endl; 499 | // for(int i = 0; i < state_server.fature_states.size(); ++i){ 500 | // cout << "marker " << state_server.fature_states[i].id << " cov: " << endl; 501 | // cout << state_server.state_cov.block<3,3>(15+6*i,15+6*i) << endl; 502 | // } 503 | 504 | } 505 | 506 | void VIEkfSlam::stateAugmentation( 507 | const MarkerMeasData& marker_data){ 508 | 509 | //add new marker to state server 510 | FeatureState marker_feature(marker_data.id); 511 | FeatureState::initializePose(state_server.imu_state,marker_data, 512 | marker_feature.orientation,marker_feature.position); 513 | 514 | state_server.fature_states.push_back(marker_feature); 515 | 516 | //resize the covariance matrix 517 | size_t old_rows = state_server.state_cov.rows(); 518 | size_t old_cols = state_server.state_cov.cols(); 519 | state_server.state_cov.conservativeResize(old_rows+6,old_cols+6); 520 | state_server.state_cov.block<6,6>(old_rows,old_cols).setZero(); 521 | //initialize the marker postion covariance 522 | state_server.state_cov.block<3,3>(old_rows,old_cols) = 523 | 100*100*Matrix3d::Identity(); 524 | // initialize the marker orientation covariance 525 | state_server.state_cov.bottomRightCorner(3,3) = 526 | 10*10*Matrix3d::Identity(); 527 | state_server.state_cov.block(old_rows,0,6,old_cols).setZero(); 528 | state_server.state_cov.block(0,old_cols,old_rows,6).setZero(); 529 | 530 | 531 | } 532 | 533 | void VIEkfSlam::measurementJacobian(int index,const MarkerMeasData& meas, 534 | MatrixXd& H_x_i, VectorXd& r_i){ 535 | 536 | const IMUState& imu_state = state_server.imu_state; 537 | const FeatureState& feature_state = state_server.fature_states[index]; 538 | 539 | //the four corners expressed in marker frame; 540 | vector corners_in_marker(4); 541 | 542 | corners_in_marker[0] = marker_parameters.M_c0; 543 | corners_in_marker[1] = marker_parameters.M_c1; 544 | corners_in_marker[2] = marker_parameters.M_c2; 545 | corners_in_marker[3] = marker_parameters.M_c3; 546 | 547 | //current IMU pose and Marker pose estimation 548 | Matrix3d R_w_i = imu_state.orientation.toRotationMatrix(); 549 | Vector3d t_w_i = imu_state.position; 550 | Matrix3d R_w_m = feature_state.orientation.toRotationMatrix(); 551 | Vector3d t_w_m = feature_state.position; 552 | 553 | int total_state_size = 15 + 6 * state_server.fature_states.size(); 554 | 555 | const Matrix3d& R_i_c = camera_imu_parameters.R_imu_cam; 556 | const Vector3d& t_i_c = camera_imu_parameters.t_imu_cam; 557 | 558 | for(int i =0; i < 4; ++i){ 559 | 560 | //Corner in imu frame 561 | Vector3d p_i = R_w_i.transpose()*( 562 | R_w_m*corners_in_marker[i]+t_w_m-t_w_i); 563 | //Corner in camera frame 564 | Vector3d p_c = R_i_c.transpose()*(p_i-t_i_c); 565 | 566 | Matrix jacobian_uv_xyz; 567 | // const double& fx = camera_imu_parameters.camera_intrinsics.at(0,0); 568 | // const double& fy = camera_imu_parameters.camera_intrinsics.at(1,0); 569 | // jacobian_uv_xyz << fx/p_c(2), 0, -p_c(0)*fx/(p_c(2)*p_c(2)), 570 | // 0, fy/p_c(2), -p_c(1)*fy/(p_c(2)*p_c(2)); 571 | jacobian_uv_xyz << 1.0/p_c(2), 0, -p_c(0)/(p_c(2)*p_c(2)), 572 | 0, 1.0/p_c(2), -p_c(1)/(p_c(2)*p_c(2)); 573 | MatrixXd jacobian_xyz_state= MatrixXd::Zero(3,total_state_size); 574 | jacobian_xyz_state.block<3,3>(0,0) = -R_i_c.transpose() * R_w_i.transpose(); 575 | jacobian_xyz_state.block<3,3>(0,3) = R_i_c.transpose() * skewSymmetric(p_i); 576 | jacobian_xyz_state.block<3,3>(0,15+6*index) = R_i_c.transpose() * R_w_i.transpose(); 577 | jacobian_xyz_state.block<3,3>(0,15+6*index+3) = -R_i_c.transpose() * R_w_i.transpose() * 578 | R_w_m * skewSymmetric(corners_in_marker[i]); 579 | MatrixXd tmp_H = MatrixXd::Zero(2,total_state_size); 580 | tmp_H = jacobian_uv_xyz * jacobian_xyz_state; 581 | Vector2d tmp_r; 582 | tmp_r << meas.corners[i].x - p_c(0) / p_c(2), 583 | meas.corners[i].y - p_c(1) / p_c(2); 584 | H_x_i.block(2*i,0,2,total_state_size) = tmp_H; 585 | r_i.segment(2*i,2) = tmp_r; 586 | } 587 | 588 | } 589 | 590 | void VIEkfSlam::measurementUpdate(const MatrixXd& H, const VectorXd& r){ 591 | if(H.rows() == 0 || r.rows() == 0) return; 592 | 593 | const MatrixXd& P = state_server.state_cov; 594 | MatrixXd V = marker_parameters.observation_noise * 595 | MatrixXd::Identity(H.rows(),H.rows()); 596 | MatrixXd S = H * P * H.transpose() + V; 597 | MatrixXd S_inv = S.inverse(); 598 | //kalman gain 599 | MatrixXd K = P * H.transpose() * S_inv; 600 | // cout << "S: " << endl << S << endl; 601 | // cout << "S_inv : " << endl << S_inv << endl; 602 | 603 | VectorXd delta_x = K * r; 604 | 605 | //update state 606 | updateState(delta_x); 607 | //update state covariance 608 | int total_state_size = 15 + 6 * state_server.fature_states.size(); 609 | MatrixXd I_KH = MatrixXd::Identity( 610 | total_state_size,total_state_size) - K * H; 611 | // This formular is not stable here , why?? 612 | // state_server.state_cov = I_KH * P * I_KH.transpose() + K * V * K.transpose(); 613 | state_server.state_cov = P - K * S * K.transpose(); 614 | 615 | MatrixXd state_cov_fixed = (state_server.state_cov + 616 | state_server.state_cov.transpose()) / 2.0; 617 | state_server.state_cov = state_cov_fixed; 618 | } 619 | 620 | void VIEkfSlam::updateState(const VectorXd& delta){ 621 | 622 | IMUState& imu_state = state_server.imu_state; 623 | imu_state.position += delta.segment(0,3); 624 | imu_state.orientation *= Quaterniond(AngleAxisd(delta.segment(3,3).norm(), 625 | delta.segment(3,3).normalized())); 626 | imu_state.orientation.normalize(); 627 | imu_state.velocity += delta.segment(6,3); 628 | imu_state.acc_bias += delta.segment(9,3); 629 | imu_state.gyro_bias += delta.segment(12,3); 630 | 631 | FeatureMap& features = state_server.fature_states; 632 | int marker_state_size = delta.size() - 15; 633 | int marker_cntr = marker_state_size / 6; 634 | for(int i = 0; i < marker_cntr; ++i){ 635 | features[i].position += delta.segment(15+6*i,3); 636 | features[i].orientation *= Quaterniond(AngleAxisd(delta.segment(15+6*i+3,3).norm(), 637 | delta.segment(15+6*i+3,3).normalized())); 638 | features[i].orientation.normalize(); 639 | } 640 | } 641 | 642 | void VIEkfSlam::publish(const ros::Time& time){ 643 | const IMUState& imu_state = state_server.imu_state; 644 | 645 | //publish tf 646 | tf::Vector3 tf_imu_position; 647 | tf::vectorEigenToTF(imu_state.position,tf_imu_position); 648 | tf::Quaternion tf_imu_orientation; 649 | tf::quaternionEigenToTF(imu_state.orientation,tf_imu_orientation); 650 | tf::Transform T_w_i_tf; 651 | T_w_i_tf.setOrigin(tf_imu_position); 652 | T_w_i_tf.setRotation(tf_imu_orientation); 653 | tf_pub.sendTransform(tf::StampedTransform(T_w_i_tf,time,world_frame_id,body_frame_id)); 654 | 655 | //publish the odometry 656 | nav_msgs::Odometry odom_msg; 657 | odom_msg.header.stamp = time; 658 | odom_msg.header.frame_id = world_frame_id; 659 | odom_msg.child_frame_id = body_frame_id; 660 | tf::quaternionEigenToMsg(imu_state.orientation,odom_msg.pose.pose.orientation); 661 | tf::pointEigenToMsg(imu_state.position,odom_msg.pose.pose.position); 662 | tf::vectorEigenToMsg(imu_state.velocity,odom_msg.twist.twist.linear); 663 | //convert the covariance for pose 664 | Matrix3d P_pp = state_server.state_cov.block<3,3>(0,0); 665 | Matrix3d P_po = state_server.state_cov.block<3,3>(0,3); 666 | Matrix3d P_op = state_server.state_cov.block<3,3>(3,0); 667 | Matrix3d P_oo = state_server.state_cov.block<3,3>(3,3); 668 | Matrix P_imu_pose = Matrix::Zero(); 669 | P_imu_pose << P_pp, P_po,P_op,P_oo; 670 | for(int i = 0; i < 6; ++i) 671 | for(int j = 0; j < 6; ++j) 672 | odom_msg.pose.covariance[6*i+j] = P_imu_pose(i,j); 673 | //convert the covariance for velocity 674 | Matrix3d P_imu_vel = state_server.state_cov.block<3,3>(6,6); 675 | for(int i = 0; i < 3; ++i) 676 | for(int j = 0; j < 3; ++j) 677 | odom_msg.twist.covariance[3*i+j] = P_imu_vel(i,j); 678 | //publish odom topic 679 | odom_pub.publish(odom_msg); 680 | 681 | // // publish debug image 682 | cv_bridge::CvImage debug_image_msg; 683 | debug_image_msg.header.stamp = time; 684 | debug_image_msg.encoding = sensor_msgs::image_encodings::MONO8; 685 | debug_image_msg.image = debug_image; 686 | debug_img_pub.publish(debug_image_msg.toImageMsg()); 687 | 688 | //publish markers 689 | visualization_msgs::MarkerArray markers; 690 | geometry_msgs::PoseArray markers_pose_msg; 691 | markers_pose_msg.header.frame_id = world_frame_id; 692 | markers_pose_msg.header.stamp = time; 693 | const FeatureMap& features = state_server.fature_states; 694 | for(int i = 0; i < features.size(); ++i){ 695 | visualization_msgs::Marker marker; 696 | drawRosMarker(features[i].id,time,features[i].position,features[i].orientation, 697 | state_server.state_cov.block<3,3>(15+6*i,15+6*i),marker); 698 | markers.markers.push_back(marker); 699 | markers_pose_msg.poses.push_back(marker.pose); 700 | } 701 | feature_map_pub.publish(markers); 702 | features_pose_pub.publish(markers_pose_msg); 703 | 704 | //publish the trajectory 705 | static nav_msgs::Path traj_msg; 706 | traj_msg.header.frame_id = world_frame_id; 707 | traj_msg.header.stamp = time; 708 | geometry_msgs::PoseStamped pose; 709 | pose.header.stamp = time; 710 | pose.header.frame_id = world_frame_id; 711 | pose.pose = odom_msg.pose.pose; 712 | traj_msg.poses.push_back(pose); 713 | trajectory_pub.publish(traj_msg); 714 | 715 | } 716 | 717 | void VIEkfSlam::drawRosMarker(int id, const ros::Time& time, 718 | const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, 719 | const Eigen::Matrix3d& cov_pp, 720 | visualization_msgs::Marker& marker){ 721 | 722 | marker.header.frame_id = world_frame_id; 723 | marker.header.stamp = time; 724 | marker.ns = "ArucoMarker"; 725 | marker.id = id; 726 | marker.type = visualization_msgs::Marker::SPHERE; 727 | marker.action = visualization_msgs::Marker::ADD; 728 | marker.color.r = 0.0f; 729 | marker.color.g = 1.0f; 730 | marker.color.b = 0.0f; 731 | marker.color.a = 1.0f; 732 | marker.lifetime = ros::Duration(); 733 | 734 | tf::pointEigenToMsg(position,marker.pose.position); 735 | tf::quaternionEigenToMsg(orientation,marker.pose.orientation); 736 | SelfAdjointEigenSolver> eigen_solver( 737 | (cov_pp + cov_pp.transpose())/2); 738 | VectorXd eigen_values = eigen_solver.eigenvalues(); 739 | marker.scale.x = eigen_values(0) * 10; 740 | marker.scale.y = eigen_values(1) * 10; 741 | marker.scale.z = eigen_values(2) * 10; 742 | } 743 | 744 | 745 | void VIEkfSlam::undistortPoints(const std::vector& pts_in, 746 | const cv::Vec4d& intrinsics, 747 | const cv::Vec4d& distortion_coeffs, 748 | std::vector& pts_out){ 749 | 750 | if(pts_in.size() == 0) return; 751 | 752 | const cv::Matx33d K( 753 | intrinsics[0], 0.0, intrinsics[2], 754 | 0.0, intrinsics[1], intrinsics[3], 755 | 0.0, 0.0, 1.0); 756 | 757 | cv::undistortPoints(pts_in,pts_out,K,distortion_coeffs); 758 | 759 | } 760 | 761 | } 762 | 763 | -------------------------------------------------------------------------------- /VIEKFSLAM2/src/vi_ekfslam_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "VIEKFSLAM2/vi_ekfslam.hpp" 3 | 4 | int main(int argc,char** argv) 5 | { 6 | 7 | ros::init(argc,argv,"vi_ekfslam"); 8 | ros::NodeHandle nh; 9 | 10 | vi_ekfslam::VIEkfSlam viEkfSlam(nh); 11 | viEkfSlam.initialize(); 12 | 13 | ros::spin(); 14 | return 0; 15 | } --------------------------------------------------------------------------------