├── .gitignore ├── README.md ├── amber_vins ├── CMakeLists.txt ├── CMakeModules │ └── FindEigen.cmake ├── launch │ ├── simu.launch │ └── vio.launch ├── package.xml ├── src │ ├── amber_vins_node.cpp │ ├── calib.cpp │ ├── calib.h │ ├── camera.cpp │ ├── camera.h │ ├── data_generator.cpp │ ├── data_generator.h │ ├── data_generator_node.cpp │ ├── mixed_node.cpp │ ├── odometry.cpp │ ├── odometry.h │ ├── utils.cpp │ └── utils.h └── test │ └── test_camera.cpp ├── dataset ├── 2015-02-05-13-44-35.bag └── calib_data │ ├── calibrationdata.tar.gz │ ├── calibrationdata │ ├── left-0000.png │ ├── left-0001.png │ ├── left-0002.png │ ├── left-0003.png │ ├── left-0004.png │ ├── left-0005.png │ ├── left-0006.png │ ├── left-0007.png │ ├── left-0008.png │ ├── left-0009.png │ ├── left-0010.png │ ├── left-0011.png │ ├── left-0012.png │ ├── left-0013.png │ ├── left-0014.png │ ├── left-0015.png │ ├── left-0016.png │ ├── left-0017.png │ ├── left-0018.png │ ├── left-0019.png │ ├── left-0020.png │ ├── left-0021.png │ ├── left-0022.png │ ├── left-0023.png │ ├── left-0024.png │ ├── left-0025.png │ ├── left-0026.png │ ├── left-0027.png │ ├── left-0028.png │ ├── left-0029.png │ ├── left-0030.png │ ├── left-0031.png │ ├── left-0032.png │ ├── left-0033.png │ ├── left-0034.png │ ├── left-0035.png │ ├── left-0036.png │ ├── left-0037.png │ ├── left-0038.png │ ├── left-0039.png │ ├── left-0040.png │ ├── left-0041.png │ ├── left-0042.png │ ├── left-0043.png │ ├── left-0044.png │ ├── left-0045.png │ ├── left-0046.png │ ├── left-0047.png │ ├── left-0048.png │ ├── left-0049.png │ ├── left-0050.png │ ├── left-0051.png │ ├── left-0052.png │ ├── left-0053.png │ ├── left-0054.png │ ├── left-0055.png │ ├── left-0056.png │ ├── left-0057.png │ ├── left-0058.png │ ├── left-0059.png │ ├── left-0060.png │ ├── left-0061.png │ ├── left-0062.png │ ├── left-0063.png │ ├── left-0064.png │ ├── left-0065.png │ ├── left-0066.png │ ├── left-0067.png │ ├── left-0068.png │ ├── left-0069.png │ └── ost.txt │ ├── camera.yml │ └── image_list.xml ├── msckf_vins ├── CMakeLists.txt ├── CMakeModules │ └── FindEigen.cmake ├── launch │ └── run.launch ├── package.xml └── src │ ├── Camera.cpp │ ├── Camera.h │ ├── FeatureRecord.h │ ├── MSCKF.cpp │ ├── MSCKF.h │ ├── data_generator.cpp │ ├── data_generator.h │ ├── g_param.h │ ├── main.cpp │ ├── math_tool.cpp │ ├── math_tool.h │ ├── old_main.cpp │ └── tic_toc.h ├── sensor_processor ├── CMakeLists.txt ├── config │ ├── left_25000704.yml │ └── right_25000709.yml ├── launch │ └── feature_tracker.launch ├── package.xml └── src │ └── sensor_processor_node.cpp ├── test_quaternion ├── FindEigen.cmake ├── c.cpp └── q.h ├── vins_mac.rviz └── vins_rviz.rviz /.gitignore: -------------------------------------------------------------------------------- 1 | **/build/** 2 | **/devel/** 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # my_VINS 2 | 3 | This project tries to implement paper: 4 | 5 | [Monocular Visual Inertial Odometry on a Mobile Device](https://vision.in.tum.de/_media/spezial/bib/shelley14msc.pdf "Monocular Visual Inertial Odometry on a Mobile Device") 6 | 7 | 8 | [Consistency of EKF-based Visual-Inertial Odometry](http://www.ee.ucr.edu/~mli/ICRA2012REPORT.pdf "Consistency of EKF-based Visual-Inertial Odometry") 9 | 10 | **-2015-4-11-** 11 | 12 | I finished whole framework, which is a simplified version of *shelley14msc*. Now I am going to write several ROS package to process image and do simulation. 13 | 14 | Gonna folk dvorak0's sensor_processor and data_generator 15 | 16 | **-2015-4-15-** 17 | 18 | add ROS packages 19 | 20 | 1. msckf_vins. core package, move all MyTriangulation code here, and wrap ROS interface to receive sensor data 21 | 2. sensor_processor. Read IMU and process image 22 | 3. data_generator. a simulated environment that can generate IMU and image 23 | 24 | **-2015-4-29-** 25 | 26 | To start the program 27 | 28 | roscore 29 | 30 | rosrun data\_generator data\_generator\_node 31 | 32 | rosrun msckf\_vins msckf\_vins\_node 33 | -------------------------------------------------------------------------------- /amber_vins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3) 2 | PROJECT(amber_vins) 3 | 4 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 5 | SET(CMAKE_CXX_FLAGS "-DNDEBUG -std=c++11 -march=native -O3 -Wall") 6 | #SET(CMAKE_BUILD_TYPE Debug) 7 | 8 | FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs nav_msgs cv_bridge tf) 9 | FIND_PACKAGE(Eigen REQUIRED) 10 | FIND_PACKAGE(OpenCv REQUIRED) 11 | 12 | CATKIN_PACKAGE( 13 | ) 14 | 15 | INCLUDE_DIRECTORIES( 16 | src 17 | ${Eigen_INCLUDE_DIRS} 18 | ${OpenCv_INCLUDE_DIRS} 19 | ) 20 | 21 | LIST(APPEND SOURCEFILES 22 | src/odometry.cpp 23 | src/data_generator.cpp 24 | src/calib.cpp 25 | src/camera.cpp 26 | src/utils.cpp 27 | ) 28 | 29 | ADD_EXECUTABLE(mixed_node 30 | src/mixed_node.cpp 31 | ${SOURCEFILES} 32 | ) 33 | TARGET_LINK_LIBRARIES(mixed_node 34 | ${catkin_LIBRARIES} 35 | ) 36 | 37 | ADD_EXECUTABLE(amber_vins_node 38 | src/amber_vins_node.cpp 39 | ${SOURCEFILES} 40 | ) 41 | 42 | TARGET_LINK_LIBRARIES(amber_vins_node 43 | ${catkin_LIBRARIES} 44 | ) 45 | 46 | ADD_EXECUTABLE(data_generator_node 47 | src/data_generator_node.cpp 48 | ${SOURCEFILES} 49 | ) 50 | 51 | TARGET_LINK_LIBRARIES(data_generator_node 52 | ${catkin_LIBRARIES} 53 | ) 54 | 55 | catkin_add_gtest(test_camera 56 | test/test_camera.cpp 57 | ${SOURCEFILES}) 58 | 59 | target_link_libraries(test_camera 60 | ${catkin_LIBRARIES} 61 | ${OpenCv_LIBS}) 62 | -------------------------------------------------------------------------------- /amber_vins/CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | 83 | -------------------------------------------------------------------------------- /amber_vins/launch/simu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /amber_vins/launch/vio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /amber_vins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | amber_vins 4 | 0.0.0 5 | The msckf_vins package 6 | 7 | 8 | 9 | 10 | Botao Hu 11 | Paul Yang 12 | 13 | 14 | 15 | 16 | 17 | TODO 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 | catkin 44 | roscpp 45 | roscpp 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /amber_vins/src/amber_vins_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "data_generator.h" 12 | #include "camera.h" 13 | #include "calib.h" 14 | #include "odometry.h" 15 | 16 | using namespace std; 17 | 18 | ros::Publisher pub_odometry; 19 | ros::Publisher pub_pose; 20 | ros::Publisher pub_path; 21 | ros::Subscriber sub_imu; 22 | ros::Subscriber sub_image; 23 | 24 | // visualize results 25 | nav_msgs::Path path; 26 | visualization_msgs::Marker path_line; 27 | 28 | Calib* calib; 29 | MSCKF* msckf; 30 | CameraMeasurements* cameraMeasurements; 31 | 32 | queue imu_buf; 33 | 34 | void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) 35 | { 36 | ROS_INFO("received imu data with stamp %lf", imu_msg->header.stamp.toSec()); 37 | 38 | imu_buf.push(*imu_msg); 39 | } 40 | 41 | void sendIMU(const sensor_msgs::Imu& imu_msg) 42 | { 43 | double t = imu_msg.header.stamp.toSec(); 44 | 45 | ROS_INFO("processing imu data with stamp %lf", t); 46 | 47 | double dx = imu_msg.linear_acceleration.x; 48 | double dy = imu_msg.linear_acceleration.y; 49 | double dz = imu_msg.linear_acceleration.z; 50 | 51 | double rx = imu_msg.angular_velocity.x; 52 | double ry = imu_msg.angular_velocity.y; 53 | double rz = imu_msg.angular_velocity.z; 54 | 55 | msckf->propagate(Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); 56 | } 57 | 58 | void publishOdometryResult() 59 | { 60 | Vector3d p = msckf->getPosition(); 61 | Vector3d v = msckf->getVelocity(); 62 | Quaterniond q = msckf->getQuaternion(); 63 | 64 | nav_msgs::Odometry odometry; 65 | odometry.header.stamp = ros::Time::now(); 66 | odometry.header.frame_id = "world"; 67 | odometry.pose.pose.position.x = p(0); 68 | odometry.pose.pose.position.y = p(1); 69 | odometry.pose.pose.position.z = p(2); 70 | odometry.pose.pose.orientation.x = q.x(); 71 | odometry.pose.pose.orientation.y = q.y(); 72 | odometry.pose.pose.orientation.z = q.z(); 73 | odometry.pose.pose.orientation.w = q.w(); 74 | odometry.twist.twist.linear.x = v(0); 75 | odometry.twist.twist.linear.y = v(1); 76 | odometry.twist.twist.linear.y = v(2); 77 | 78 | geometry_msgs::PoseStamped pose_stamped; 79 | pose_stamped.header.stamp = ros::Time::now(); 80 | pose_stamped.header.frame_id = "world"; 81 | pose_stamped.pose = odometry.pose.pose; 82 | 83 | path.poses.push_back(pose_stamped); 84 | 85 | pub_odometry.publish(odometry); 86 | pub_pose.publish(pose_stamped); 87 | pub_path.publish(path); 88 | } 89 | 90 | void imageCallback(const sensor_msgs::PointCloud::ConstPtr& image_msg_ptr) 91 | { 92 | double t = image_msg_ptr->header.stamp.toSec(); 93 | if (imu_buf.empty() || t < imu_buf.front().header.stamp.toSec()) 94 | { 95 | ROS_ERROR("wait for imu data"); 96 | return; 97 | } 98 | 99 | while (!imu_buf.empty() && t >= imu_buf.front().header.stamp.toSec()) 100 | { 101 | sendIMU(imu_buf.front()); 102 | imu_buf.pop(); 103 | } 104 | 105 | ROS_INFO("processing vision data with stamp %lf", t); 106 | 107 | vector > image; 108 | for (int i = 0; i < (int) image_msg_ptr->points.size(); i++) 109 | { 110 | int id = image_msg_ptr->channels[0].values[i]; 111 | double x = image_msg_ptr->points[i].x; 112 | double y = image_msg_ptr->points[i].y; 113 | 114 | image.push_back(make_pair(id, Vector2d(x, y))); 115 | } 116 | cameraMeasurements->addFeatures(image); 117 | msckf->updateCamera(*cameraMeasurements); 118 | //cout << *msckf << endl; 119 | 120 | publishOdometryResult(); 121 | } 122 | 123 | void setupROS() 124 | { 125 | ros::NodeHandle n("~"); 126 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 127 | 128 | // define pub topics 129 | pub_path = n.advertise("path", 1000); 130 | pub_odometry = n.advertise("odometry", 1000); 131 | pub_pose = n.advertise("pose", 1000); 132 | 133 | path_line.header.frame_id = "world"; 134 | path_line.header.stamp = ros::Time::now(); 135 | path_line.ns = "calibration"; 136 | path_line.action = visualization_msgs::Marker::ADD; 137 | path_line.pose.orientation.w = 1.0; 138 | path_line.type = visualization_msgs::Marker::LINE_STRIP; 139 | path_line.scale.x = 0.01; 140 | path_line.color.a = 1.0; 141 | path_line.color.r = 1.0; 142 | path_line.id = 1; 143 | 144 | path.header.frame_id = "world"; 145 | 146 | sub_imu = n.subscribe("/imu_3dm_gx4/imu", 1000, imuCallback); 147 | sub_image = n.subscribe("/sensors/image", 1000, imageCallback); 148 | } 149 | 150 | void setup() 151 | { 152 | calib = new Calib(); 153 | calib->camera.setIntrinsicParam(365.07984, 365.12127, 381.01962, 254.44318); 154 | calib->camera.setDistortionParam(-2.842958e-1, 8.7155025e-2, -1.4602925e-4, -6.149638e-4, -1.218237e-2); 155 | 156 | calib->delta_t = (double) 1 / DataGenerator::FREQ; 157 | calib->CI_q = Quaterniond((Matrix3d() << 0, -1, 0, 0, 0, 1, -1, 0, 0).finished()); 158 | calib->C_p_I << -0.02, -0.14, 0; 159 | calib->image_imu_offset_t = 0; 160 | 161 | calib->sigma_gc = 0.01; 162 | calib->sigma_ac = 0.1; 163 | calib->sigma_wgc = 0.01; 164 | calib->sigma_wac = 0.01; 165 | calib->sigma_Im = 10; 166 | 167 | DataGenerator* generator = new DataGenerator(calib); 168 | 169 | msckf = new MSCKF(calib); 170 | msckf->x.segment<4>(0) = Quaterniond(generator->getRotation()).coeffs(); 171 | msckf->x.segment<3>(0 + 4) = generator->getPosition(); 172 | msckf->x.segment<3>(0 + 4 + 3) = generator->getVelocity(); 173 | 174 | cameraMeasurements = new CameraMeasurements(); 175 | } 176 | 177 | int main(int argc, char **argv) 178 | { 179 | ros::init(argc, argv, "msckf_vins"); 180 | 181 | setup(); 182 | setupROS(); 183 | ros::spin(); 184 | 185 | return 0; 186 | } 187 | 188 | 189 | -------------------------------------------------------------------------------- /amber_vins/src/calib.cpp: -------------------------------------------------------------------------------- 1 | #include "calib.h" 2 | #include "odometry.h" 3 | 4 | Calib::Calib() 5 | { 6 | // 7 | // Camera calibration 8 | // 9 | camera = Camera(); 10 | 11 | /* Position */ 12 | CI_q = Quaterniond(1, 0, 0, 0); // Rotation from camera to IMU coordinates. [unit quaternion] 13 | C_p_I = Vector3d(0, 0, 0); // Position of inertial frame in camera coordinates [m,m,m] 14 | // 15 | // Physical properties 16 | // 17 | g = 9.8; // Gravitational acceleration [m/s^2] 18 | delta_t = 1; // Time between IMU data [s] 19 | image_imu_offset_t = 0; //Time offset beween Image and IMU [s] 20 | // 21 | // Noise levels 22 | // 23 | sigma_gc = 0; sigma_ac = 0; // Standard deviation of IMU noise [rad/s, m/s^2] 24 | sigma_wgc = 0; sigma_wac = 0; // Standard deviation of IMU random walk noise [rad/s, m/s^2] 25 | /* Camera */ 26 | sigma_Im = 0; // Image noise 27 | // 28 | // Options 29 | // 30 | maxFrame = ODO_MAX_FRAMES; // Maximum frames in FIFO 31 | minFrame = 0; 32 | } 33 | 34 | ostream& operator<<(ostream& out, const Calib& calib) 35 | { 36 | return out << calib.camera << "\n" << 37 | "CI_q: " << (calib.CI_q).coeffs().transpose() << " C_p_I: " << calib.C_p_I.transpose() << "\n" << 38 | "g: " << calib.g << "\n" << 39 | "delta_t: " << calib.delta_t << "\n" << 40 | "sigma_gc: " << calib.sigma_gc << " sigma_ac: " << calib.sigma_ac << "\n" << 41 | "sigma_wgc: " << calib.sigma_wgc << " sigma_wac: " << calib.sigma_wac << "\n" << 42 | "sigma_Im: " << calib.sigma_Im << "\n" << 43 | "maxFrame: " << calib.maxFrame << "\n" << 44 | "minFrame: " << calib.minFrame ; 45 | } 46 | 47 | -------------------------------------------------------------------------------- /amber_vins/src/calib.h: -------------------------------------------------------------------------------- 1 | #ifndef _CALIB_H_ 2 | #define _CALIB_H_ 3 | 4 | #include 5 | #include 6 | #include "camera.h" 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | class Calib 11 | { 12 | public: 13 | /* Camera */ 14 | Camera camera; // Camera 15 | 16 | /* Camera Position */ 17 | Quaterniond CI_q; // Rotation from camera to IMU coordinates. [unit quaternion] 18 | Vector3d C_p_I; // Position of inertial frame in camera coordinates [m,m,m] 19 | // 20 | // Physical properties 21 | // 22 | double g; // Gravitational acceleration [m/s^2] 23 | double delta_t; // Time between IMU data [s] 24 | double image_imu_offset_t; // Time delay for images [s] 25 | // 26 | // Noise levels 27 | // 28 | /* IMU TODO: add correct units */ 29 | double sigma_gc, sigma_ac, // Standard deviation of IMU noise [rad/s, m/s^2] 30 | sigma_wgc, sigma_wac; // Standard deviation of IMU random walk noise [rad/s, m/s^2] 31 | /* Camera */ 32 | double sigma_Im; // Image noise 33 | 34 | // 35 | // Options 36 | // 37 | unsigned int maxFrame; // Maximum frames in FIFO 38 | unsigned int minFrame; // Minimun frames in FIFO (is smaller before the first minFrames). Is used if features are only added when there is a match 39 | 40 | Calib(); 41 | 42 | friend ostream& operator<<(ostream& out, const Calib& calib); 43 | }; 44 | 45 | #endif //_CALIB_H_ 46 | 47 | -------------------------------------------------------------------------------- /amber_vins/src/camera.cpp: -------------------------------------------------------------------------------- 1 | #include "camera.h" 2 | #include 3 | #include 4 | 5 | Camera::Camera() 6 | { 7 | /* Projection */ 8 | ox = 0; oy = 0; // principal point [px,px] 9 | fx = 1; fy = 1; // focal length [px,px] 10 | k1 = 0; k2 = 0; k3 = 0; // radial distortion parameters [n/u,n/u] 11 | t1 = 0; t2 = 0; // tangential distortion parameters [n/u,n/u] 12 | } 13 | 14 | void Camera::setIntrinsicParam(double _fx, double _fy, double _ox, double _oy) 15 | { 16 | fx = _fx; 17 | fy = _fy; 18 | ox = _ox; 19 | oy = _oy; 20 | } 21 | 22 | void Camera::setDistortionParam(double _k1, double _k2, double _t1, double _t2, double _k3) 23 | { 24 | k1 = _k1; 25 | k2 = _k2; 26 | k3 = _k3; 27 | t1 = _t1; 28 | t2 = _t2; 29 | } 30 | 31 | // 32 | // Project a point from camera coordinates to pixel position 33 | // 34 | Vector2d Camera::cameraProject(const Vector3d& p) 35 | { 36 | double x = p(0); 37 | double y = p(1); 38 | double z = p(2); 39 | double u = x / z; 40 | double v = y / z; 41 | double r2 = u * u + v * v; 42 | double dr = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; 43 | Vector2d dt( 44 | 2 * u * v * t1 + (r2 + 2 * u * u) * t2, 45 | 2 * u * v * t2 + (r2 + 2 * v * v) * t1 46 | ); 47 | 48 | return Vector2d(ox + fx * (dr * u + dt(0)), oy + fy * (dr * v + dt(1))); 49 | } 50 | 51 | // 52 | // Jacobian of h (camera model) 53 | // 54 | Matrix Camera::jacobianH(const Vector3d& p) 55 | { 56 | double x = p(0); 57 | double y = p(1); 58 | double z = p(2); 59 | 60 | double u = x / z; 61 | double v = y / z; 62 | 63 | double r2 = u * u + v * v; 64 | 65 | double dr = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; 66 | Vector2d dt( 67 | 2 * u * v * t1 + (r2 + 2 * u * u) * t2, 68 | 2 * u * v * t2 + (r2 + 2 * v * v) * t1 69 | ); 70 | 71 | Vector3d Jdr = 2 / z * (k1 + 2 * k2 * r2 + 3 * k3 * r2 * r2) * Vector3d(u, v, -r2); 72 | 73 | Matrix Jdt; 74 | Jdt << 3 * t2 * u + t1 * v, t1 * u + t2 * v, -2 * t1 * u * v - t2 * (3 * u * u + v * v), 75 | t1 * u + t2 * v, t2 * u + 3 * t1 * v, -2 * t2 * u * v - t1 * (u * u + 3 * v * v); 76 | Jdt *= 2 / z; 77 | 78 | Matrix Jdistorted; 79 | Jdistorted = dr / z * (Matrix() << 1, 0, -u, 0, 1, -v).finished() + Vector2d(u, v) * Jdr.transpose() + Jdt; 80 | 81 | Matrix J = Vector2d(fx, fy).asDiagonal() * Jdistorted; 82 | 83 | return J; 84 | } 85 | 86 | Vector3d Camera::triangulateGN(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess) 87 | { 88 | int iterations = 1000; 89 | 90 | assert (CG_q.cols() == G_p_C.cols()); 91 | assert (CG_q.cols() == z.cols()); 92 | 93 | unsigned int n = z.cols(); 94 | 95 | Vector3d G_p_f = guess; 96 | 97 | // Residual 98 | VectorXd r(n * 2); 99 | // Jacobian 100 | MatrixX3d J(n * 2, 3); 101 | 102 | while (iterations--) 103 | { 104 | for (int i = 0; i < n; i++) 105 | { 106 | Matrix3d GCi_R = Quaterniond(CG_q.col(i)).inverse().toRotationMatrix(); 107 | Vector3d Ci_p_f = GCi_R * (G_p_f - G_p_C.col(i)); 108 | Matrix3d Jg = GCi_R; 109 | 110 | r.segment<2>(i * 2) = z.col(i) - cameraProject(Ci_p_f); 111 | J.block<2, 3>(i * 2, 0) = -jacobianH(Ci_p_f) * Jg; 112 | } 113 | 114 | if (r.norm() < 0.001) 115 | break; 116 | // New estimate 117 | G_p_f = G_p_f - (J.transpose() * J).inverse() * J.transpose() * r; 118 | } 119 | 120 | return G_p_f; 121 | } 122 | 123 | Vector3d Camera::triangulateGNInverseDepth(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess) 124 | { 125 | int iterations = 1000; 126 | 127 | assert (CG_q.cols() == G_p_C.cols()); 128 | assert (CG_q.cols() == z.cols()); 129 | 130 | unsigned int n = z.cols(); 131 | 132 | // Residual 133 | VectorXd r(n * 2); 134 | // Jacobian 135 | MatrixX3d J(n * 2, 3); 136 | 137 | Vector3d C0_guess = Quaterniond(CG_q.col(0)).inverse() * (guess - G_p_C.col(0)); 138 | Vector3d theta(C0_guess(0) / C0_guess(2), C0_guess(1) / C0_guess(2), 1.0 / C0_guess(2)); 139 | while (iterations--) 140 | { 141 | 142 | for (int i = 0; i < n; i++) 143 | { 144 | // Calculate i-th camera transition to camera 0 145 | Quaterniond GCi_q = Quaterniond(CG_q.col(i)).inverse(); 146 | Quaterniond C0G_q = Quaterniond(CG_q.col(0)); 147 | 148 | Matrix3d C0Ci_R = (GCi_q * C0G_q).toRotationMatrix(); 149 | Vector3d Ci_p_C0 = GCi_q * (G_p_C.col(0) - G_p_C.col(i)); 150 | 151 | // Calculate inverse depth 152 | Vector3d Ci_p_f = C0Ci_R * Vector3d(theta(0), theta(1), 1) + theta(2) * Ci_p_C0; 153 | Matrix3d Jg; 154 | Jg << C0Ci_R.col(0), C0Ci_R.col(1), Ci_p_C0; 155 | 156 | r.segment<2>(i * 2) = z.col(i) - cameraProject(Ci_p_f); 157 | J.block<2, 3>(i * 2, 0) = -jacobianH(Ci_p_f) * Jg; 158 | } 159 | 160 | if (r.norm() < 0.001) 161 | break; 162 | // New estimate 163 | theta = theta - (J.transpose() * J).inverse() * J.transpose() * r; 164 | 165 | } 166 | 167 | Vector3d C0_p_f(theta(0) / theta(2), theta(1) / theta(2), 1.0 / theta(2)); 168 | 169 | Vector3d G_p_f = Quaterniond(CG_q.col(0)) * C0_p_f + G_p_C.col(0); 170 | 171 | return G_p_f; 172 | } 173 | 174 | 175 | struct TriangulateFunctor 176 | { 177 | int n; 178 | 179 | Camera* camera; 180 | Matrix4Xd CG_q; 181 | Matrix3Xd G_p_C; 182 | Matrix2Xd z; 183 | 184 | TriangulateFunctor(Camera* _camera, const Matrix4Xd& _CG_q, const Matrix3Xd& _G_p_C, const Matrix2Xd& _z) 185 | : camera(_camera), CG_q(_CG_q), G_p_C(_G_p_C), z(_z) 186 | { 187 | n = z.cols(); 188 | } 189 | 190 | int operator()(const VectorXd &G_p_f, VectorXd &r) const 191 | { 192 | for (int i = 0; i < n; i++) 193 | { 194 | Matrix3d GCi_R = Quaterniond(CG_q.col(i)).inverse().toRotationMatrix(); 195 | Vector3d Ci_p_f = GCi_R * (G_p_f - G_p_C.col(i)); 196 | 197 | r.segment<2>(i * 2) = z.col(i) - camera->cameraProject(Ci_p_f); 198 | } 199 | 200 | return 0; 201 | } 202 | 203 | int df(const VectorXd &G_p_f, MatrixXd &J) const 204 | { 205 | for (int i = 0; i < n; i++) 206 | { 207 | Matrix3d GCi_R = Quaterniond(CG_q.col(i)).inverse().toRotationMatrix(); 208 | Vector3d Ci_p_f = GCi_R * (G_p_f - G_p_C.col(i)); 209 | Matrix3d Jg = GCi_R; 210 | 211 | J.block<2, 3>(i * 2, 0) = -camera->jacobianH(Ci_p_f) * Jg; 212 | } 213 | return 0; 214 | } 215 | 216 | int inputs() const { return 3; } 217 | int values() const { return n * 2; } 218 | }; 219 | 220 | Vector3d Camera::triangulateLM(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess) 221 | { 222 | TriangulateFunctor functor(this, CG_q, G_p_C, z); 223 | LevenbergMarquardt lm(functor); 224 | VectorXd G_p_f = guess; 225 | LevenbergMarquardtSpace::Status status = lm.minimize(G_p_f); 226 | return G_p_f; 227 | } 228 | 229 | struct InverseDepthTriangulateFunctor 230 | { 231 | int n; 232 | 233 | Camera* camera; 234 | Matrix4Xd CG_q; 235 | Matrix3Xd G_p_C; 236 | Matrix2Xd z; 237 | 238 | InverseDepthTriangulateFunctor(Camera* _camera, const Matrix4Xd& _CG_q, const Matrix3Xd& _G_p_C, const Matrix2Xd& _z) 239 | : camera(_camera), CG_q(_CG_q), G_p_C(_G_p_C), z(_z) 240 | { 241 | n = z.cols(); 242 | } 243 | 244 | int operator()(const VectorXd &theta, VectorXd &r) const 245 | { 246 | for (int i = 0; i < n; i++) 247 | { 248 | Quaterniond GCi_q = Quaterniond(CG_q.col(i)).inverse(); 249 | Quaterniond C0G_q = Quaterniond(CG_q.col(0)); 250 | 251 | Matrix3d C0Ci_R = (GCi_q * C0G_q).toRotationMatrix(); 252 | Vector3d Ci_p_C0 = GCi_q * (G_p_C.col(0) - G_p_C.col(i)); 253 | 254 | // Calculate inverse depth 255 | Vector3d Ci_p_f = C0Ci_R * Vector3d(theta(0), theta(1), 1) + theta(2) * Ci_p_C0; 256 | 257 | r.segment<2>(i * 2) = z.col(i) - camera->cameraProject(Ci_p_f); 258 | } 259 | 260 | return 0; 261 | } 262 | 263 | int df(const VectorXd &theta, MatrixXd &J) const 264 | { 265 | for (int i = 0; i < n; i++) 266 | { 267 | // Calculate i-th camera transition to camera 0 268 | Quaterniond GCi_q = Quaterniond(CG_q.col(i)).inverse(); 269 | Quaterniond C0G_q = Quaterniond(CG_q.col(0)); 270 | 271 | Matrix3d C0Ci_R = (GCi_q * C0G_q).toRotationMatrix(); 272 | Vector3d Ci_p_C0 = GCi_q * (G_p_C.col(0) - G_p_C.col(i)); 273 | 274 | // Calculate inverse depth 275 | Vector3d Ci_p_f = C0Ci_R * Vector3d(theta(0), theta(1), 1) + theta(2) * Ci_p_C0; 276 | Matrix3d Jg; 277 | Jg << C0Ci_R.col(0), C0Ci_R.col(1), Ci_p_C0; 278 | 279 | J.block<2, 3>(i * 2, 0) = -camera->jacobianH(Ci_p_f) * Jg; 280 | } 281 | return 0; 282 | } 283 | 284 | int inputs() const { return 3; } 285 | int values() const { return n * 2; } 286 | }; 287 | 288 | Vector3d Camera::triangulateLMInverseDepth(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess) 289 | { 290 | InverseDepthTriangulateFunctor functor(this, CG_q, G_p_C, z); 291 | LevenbergMarquardt lm(functor); 292 | Vector3d C0_guess = Quaterniond(CG_q.col(0)).inverse() * (guess - G_p_C.col(0)); 293 | VectorXd theta = Vector3d(C0_guess(0) / C0_guess(2), C0_guess(1) / C0_guess(2), 1.0 / C0_guess(2)); 294 | LevenbergMarquardtSpace::Status status = lm.minimize(theta); 295 | Vector3d C0_p_f(theta(0) / theta(2), theta(1) / theta(2), 1.0 / theta(2)); 296 | Vector3d G_p_f = Quaterniond(CG_q.col(0)) * C0_p_f + G_p_C.col(0); 297 | return G_p_f; 298 | } 299 | 300 | Vector3d Camera::triangulateFromTwoView(const Quaterniond& CiG_q, const Vector3d& G_p_Ci, const Vector2d& zi, 301 | const Quaterniond& CjG_q, const Vector3d& G_p_Cj, const Vector2d& zj) 302 | { 303 | Vector3d Ci_p_f((zi(0) - ox) / fx, (zi(1) - oy) / fy, 1); 304 | Vector3d Cj_p_f((zj(0) - ox) / fx, (zj(1) - oy) / fy, 1); 305 | 306 | Vector3d Ci_d_Cjf = CiG_q.inverse() * CjG_q * Cj_p_f; 307 | Vector3d Ci_p_Cj = CiG_q.inverse() * (G_p_Cj - G_p_Ci); 308 | 309 | //Solve depth_i * Ci_p_f == depth_j * Ci_d_Cjf + Ci_p_Cj 310 | // depth_i = Ci_d_Cjf.cross(Ci_p_Cj) / Ci_d_Cjf.cross(Ci_p_f) 311 | // depth_j = Ci_p_f.cross(Ci_p_Cj) / Ci_d_Cjf.cross(Ci_p_f) 312 | 313 | double depth_i = Ci_d_Cjf.cross(Ci_p_Cj)(2) / Ci_d_Cjf.cross(Ci_p_f)(2); 314 | Ci_p_f *= depth_i; 315 | // double depth_j = Ci_p_f.cross(Ci_p_Cj)(2) / Ci_d_Cjf.cross(Ci_p_f)(2); 316 | // Cj_p_f *= depth_j; 317 | 318 | Vector3d G_p_f = CiG_q * Ci_p_f + G_p_Ci; 319 | 320 | return G_p_f; 321 | } 322 | 323 | Vector3d Camera::triangulate(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, double* r_norm) 324 | { 325 | int n = z.cols(); 326 | TriangulateFunctor functor(this, CG_q, G_p_C, z); 327 | Vector3d G_p_f; 328 | double min_r_norm = numeric_limits::infinity(); 329 | 330 | for (int i = 0; i < n; i++) 331 | for (int j = 0; j < n; j++) 332 | if (i != j) 333 | { 334 | Vector3d G_p_f_guess = triangulateFromTwoView( 335 | Quaterniond(CG_q.col(i)), G_p_C.col(i), z.col(i), 336 | Quaterniond(CG_q.col(j)), G_p_C.col(j), z.col(j)); 337 | 338 | Vector3d G_p_f_hat = triangulateLM(CG_q, G_p_C, z, G_p_f_guess); 339 | VectorXd r(n * 2); 340 | functor(G_p_f_hat, r); 341 | if (r.norm() < min_r_norm) 342 | { 343 | min_r_norm = r.norm(); 344 | G_p_f = G_p_f_hat; 345 | } 346 | } 347 | 348 | if (r_norm != NULL) 349 | *r_norm = min_r_norm; 350 | 351 | // cout << "===triangulate====" << endl; 352 | // cout << "CG_q:" << endl << CG_q << endl; 353 | // cout << "G_p_C:" << endl << G_p_C << endl; 354 | // cout << "G_z_f:" << endl << z << endl; 355 | // cout << "G_p_f:" << G_p_f.transpose() << endl; 356 | 357 | return G_p_f; 358 | } 359 | 360 | ostream& operator<<(ostream& out, const Camera& camera) 361 | { 362 | return out << 363 | "ox: " << camera.ox << " oy: " << camera.oy << "\n" << 364 | "fx: " << camera.fx << " fy: " << camera.fy << "\n" << 365 | "k1: " << camera.k1 << " k2: " << camera.k2 << "\n" << 366 | "t1: " << camera.t1 << " t2: " << camera.t2; 367 | } 368 | -------------------------------------------------------------------------------- /amber_vins/src/camera.h: -------------------------------------------------------------------------------- 1 | #ifndef _CAMERA_H_ 2 | #define _CAMERA_H_ 3 | 4 | #include 5 | #include 6 | using namespace Eigen; 7 | using namespace std; 8 | 9 | class Camera 10 | { 11 | public: 12 | // 13 | // Camera calibration 14 | // 15 | /* Projection */ 16 | double ox, oy, // principal point [px,px] 17 | fx, fy, // focal length [px,px] 18 | k1, k2, k3, // radial distortion parameters [n/u,n/u] 19 | t1, t2; // tangential distortion parameters [n/u,n/u] 20 | 21 | Camera(); 22 | void setDistortionParam(double _k1, double _k2, double _t1, double _t2, double _k3); 23 | void setIntrinsicParam(double _fx, double _fy, double _ox, double _oy); 24 | 25 | Vector2d cameraProject(const Vector3d &p); 26 | Matrix jacobianH(const Vector3d &p); 27 | Vector3d triangulateGNInverseDepth(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess = Vector3d(0, 0, 0)); 28 | Vector3d triangulateGN(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess = Vector3d(0, 0, 0)); 29 | Vector3d triangulateLMInverseDepth(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess = Vector3d(0, 0, 0)); 30 | Vector3d triangulateLM(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, const Vector3d& guess = Vector3d(0, 0, 0)); 31 | 32 | Vector3d triangulateFromTwoView(const Quaterniond& CiG_q, const Vector3d& G_p_Ci, const Vector2d& zi, 33 | const Quaterniond& CjG_q, const Vector3d& G_p_Cj, const Vector2d& zj); 34 | Vector3d triangulate(const Matrix4Xd& CG_q, const Matrix3Xd& G_p_C, const Matrix2Xd& z, double* r_norm = NULL); 35 | 36 | friend ostream& operator<<(ostream& out, const Camera& camera); 37 | }; 38 | 39 | 40 | #endif //_CAMERA_H_ 41 | -------------------------------------------------------------------------------- /amber_vins/src/data_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "data_generator.h" 2 | //#define COMPLEX_TRAJECTORY 1 3 | //#define SIMPLE_TRAJECTORY 1 4 | 5 | #define WITH_NOISE 6 | 7 | Vector3d DataGenerator::generatePoint() 8 | { 9 | return Vector3d(rand() % (6 * MAX_BOX) - 3 * MAX_BOX, 10 | rand() % (6 * MAX_BOX) - 3 * MAX_BOX, 11 | rand() % (6 * MAX_BOX) - 3 * MAX_BOX); 12 | } 13 | 14 | DataGenerator::DataGenerator(Calib* _calib): calib(_calib) 15 | { 16 | srand(0); 17 | t = 0; 18 | current_id = 0; 19 | for (int i = 0; i < NUM_POINTS; i++) 20 | pts[i] = generatePoint(); 21 | random_generator = default_random_engine(0); 22 | distribution = normal_distribution(0, 1); 23 | } 24 | 25 | void DataGenerator::update() 26 | { 27 | t += 1.0 / FREQ; 28 | } 29 | 30 | double DataGenerator::getTime() 31 | { 32 | return t; 33 | } 34 | 35 | void DataGenerator::setTime(double _t) 36 | { 37 | t = _t; 38 | } 39 | 40 | Vector3d DataGenerator::getPoint(int i) 41 | { 42 | return pts[i]; 43 | } 44 | 45 | Vector3d DataGenerator::getPosition() 46 | { 47 | #ifdef COMPLEX_TRAJECTORY 48 | double x, y, z; 49 | if (t < MAX_TIME) 50 | { 51 | x = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI); 52 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2 * 2); 53 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2); 54 | } 55 | else if (t >= MAX_TIME && t < 2 * MAX_TIME) 56 | { 57 | x = MAX_BOX / 2.0 - MAX_BOX / 2.0; 58 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0; 59 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0; 60 | } 61 | else 62 | { 63 | double tt = t - 2 * MAX_TIME; 64 | x = -MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI); 65 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI * 2 * 2); 66 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI * 2); 67 | } 68 | #elif SIMPLE_TRAJECTORY 69 | double x, y, z; 70 | x = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI); 71 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2 * 2); 72 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2); 73 | #else 74 | double x = MAX_BOX * cos(t / MAX_TIME * M_PI * 2); 75 | double y = MAX_BOX * sin(t / MAX_TIME * M_PI * 2); 76 | double z = 3; 77 | #endif 78 | 79 | return Vector3d(x, y, z); 80 | } 81 | 82 | Matrix3d DataGenerator::getRotation() 83 | { 84 | // return (AngleAxisd(0.0 + M_PI * sin(t / 10), Vector3d::UnitY()) * AngleAxisd(0.0 + M_PI * sin(t / 10), Vector3d::UnitX())).toRotationMatrix(); 85 | 86 | #ifdef COMPLEX_TRAJECTORY 87 | return (AngleAxisd(30.0 / 180 * M_PI * sin(t / MAX_TIME * M_PI * 2), Vector3d::UnitX()) 88 | * AngleAxisd(40.0 / 180 * M_PI * sin(t / MAX_TIME * M_PI * 2), Vector3d::UnitY()) 89 | * AngleAxisd(0, Vector3d::UnitZ())).toRotationMatrix(); 90 | #elif SIMPLE_TRAJECTORY 91 | return (AngleAxisd(0.0 + M_PI * sin(t / 10), Vector3d::UnitY()) * AngleAxisd(0.0 + M_PI * sin(t / 10), Vector3d::UnitX())).toRotationMatrix(); 92 | #else 93 | return Matrix3d::Identity(); 94 | #endif 95 | } 96 | 97 | Vector3d DataGenerator::getVelocity() 98 | { 99 | #ifdef COMPLEX_TRAJECTORY 100 | double dx, dy, dz; 101 | if (t < MAX_TIME) 102 | { 103 | dx = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 104 | dy = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 105 | dz = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 106 | } 107 | else if (t >= MAX_TIME && t < 2 * MAX_TIME) 108 | { 109 | dx = 0.0; 110 | dy = 0.0; 111 | dz = 0.0; 112 | } 113 | else 114 | { 115 | double tt = t - 2 * MAX_TIME; 116 | dx = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 117 | dy = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 118 | dz = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 119 | } 120 | #elif SIMPLE_TRAJECTORY 121 | double dx, dy, dz; 122 | dx = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 123 | dy = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 124 | dz = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 125 | #else 126 | double dx = - MAX_BOX * sin(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 127 | double dy = MAX_BOX * cos(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 128 | double dz = 0; 129 | #endif 130 | return Vector3d(dx, dy, dz); 131 | } 132 | 133 | Vector3d DataGenerator::getIMUAngularVelocity() 134 | { 135 | const double delta_t = 0.00001; 136 | Matrix3d rot = getRotation(); 137 | t += delta_t; 138 | Matrix3d drot = (getRotation() - rot) / delta_t; 139 | t -= delta_t; 140 | Matrix3d skew = rot.inverse() * drot; 141 | #ifdef WITH_NOISE 142 | Vector3d disturb = Vector3d(distribution(random_generator) * calib->sigma_gc, 143 | distribution(random_generator) * calib->sigma_gc, 144 | distribution(random_generator) * calib->sigma_gc 145 | ); 146 | return disturb + Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0)); 147 | #else 148 | return Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0)); 149 | #endif 150 | } 151 | 152 | Vector3d DataGenerator::getIMULinearAcceleration() 153 | { 154 | #ifdef COMPLEX_TRAJECTORY 155 | double ddx, ddy, ddz; 156 | if (t < MAX_TIME) 157 | { 158 | ddx = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 159 | ddy = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 160 | ddz = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 161 | } 162 | else if (t >= MAX_TIME && t < 2 * MAX_TIME) 163 | { 164 | ddx = 0.0; 165 | ddy = 0.0; 166 | ddz = 0.0; 167 | } 168 | else 169 | { 170 | double tt = t - 2 * MAX_TIME; 171 | ddx = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 172 | ddy = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 173 | ddz = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 174 | } 175 | #elif SIMPLE_TRAJECTORY 176 | double ddx, ddy, ddz; 177 | ddx = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 178 | ddy = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 179 | ddz = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 180 | #else 181 | double ddx = -MAX_BOX * (1.0 / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2) * cos(t / MAX_TIME * M_PI * 2); 182 | double ddy = -MAX_BOX * (1.0 / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2) * sin(t / MAX_TIME * M_PI * 2); 183 | double ddz = 0; 184 | #endif 185 | 186 | #ifdef WITH_NOISE 187 | Vector3d disturb = Vector3d(distribution(random_generator) * calib->sigma_ac, 188 | distribution(random_generator) * calib->sigma_ac, 189 | distribution(random_generator) * calib->sigma_ac); 190 | #else 191 | Vector3d disturb = Vector3d(0, 0, 0); 192 | #endif 193 | return getRotation().transpose() * (disturb + Vector3d(ddx, ddy, ddz + calib->g)); 194 | } 195 | 196 | vector> DataGenerator::getImage() 197 | { 198 | printf("===frame start===\n"); 199 | 200 | vector> image; 201 | 202 | Vector3d position = getPosition(); 203 | Matrix3d GI_R = getRotation().transpose(); 204 | 205 | for (int i = 0; i < NUM_POINTS; i++) 206 | { 207 | Vector3d C_p_f = calib->CI_q.toRotationMatrix().transpose() * GI_R * (pts[i] - position) + calib->C_p_I; 208 | Vector2d C_z_f = calib->camera.cameraProject(C_p_f); 209 | 210 | if (abs(atan2(C_p_f(0), C_p_f(2))) <= M_PI * FOV / 2 / 180 211 | && abs(atan2(C_p_f(1), C_p_f(2))) <= M_PI * FOV / 2 / 180 212 | && C_p_f(2) > 0.1 213 | && 0 < C_z_f(0) && C_z_f(0) < COL && 0 < C_z_f(1) && C_z_f(1) < ROW) 214 | { 215 | // int n_id = before_feature_id.find(i) == before_feature_id.end() ? 216 | // current_id++ : before_feature_id[i]; 217 | int n_id = i; 218 | // #if WITH_NOISE 219 | // Vector2d disturb = Vector2d(distribution(random_generator) * calib->sigma_Im / C_p_f(2) / C_p_f(2), 220 | // distribution(random_generator) * calib->sigma_Im / C_p_f(2) / C_p_f(2)); 221 | // #else 222 | // Vector2d disturb = Vector2d(0, 0); 223 | // #endif 224 | Vector2d disturb = Vector2d(0, 0); 225 | 226 | image.push_back(make_pair(n_id, disturb + C_z_f)); 227 | 228 | printf("Feature Id: %d, p_gf: [%f, %f, %f], p_cf: [%f, %f, %f], z_cf: [%f, %f]\n", 229 | n_id, pts[i](0), pts[i](1), pts[i](2), C_p_f(0), C_p_f(1), C_p_f(2), C_z_f(0), C_z_f(1)); 230 | 231 | current_feature_id[i] = n_id; 232 | } 233 | } 234 | printf("===frame end===\n"); 235 | /*before_feature_id = current_feature_id; 236 | current_feature_id.clear(); 237 | */ 238 | sort(image.begin(), image.end(), [](const pair &a, 239 | const pair &b) 240 | { 241 | return a.first < b.first; 242 | }); 243 | return image; 244 | } 245 | 246 | vector DataGenerator::getCloud() 247 | { 248 | vector cloud; 249 | 250 | for (int i = 0; i < NUM_POINTS; i++) 251 | { 252 | cloud.push_back(pts[i]); 253 | } 254 | return cloud; 255 | } 256 | -------------------------------------------------------------------------------- /amber_vins/src/data_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef _DATA_GENERATOR_H_ 2 | #define _DATA_GENERATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "calib.h" 13 | using namespace std; 14 | using namespace Eigen; 15 | 16 | class DataGenerator 17 | { 18 | public: 19 | static int const NUM_POINTS = 200; 20 | static int const MAX_BOX = 10; 21 | static int const RANGE1 = 15; 22 | static int const RANGE2 = 25; 23 | static int const MAX_HEIGHT = 5; 24 | 25 | static int const FREQ = 800; 26 | static int const IMU_PER_IMG = 40; 27 | 28 | static int const MAX_TIME = 100; 29 | 30 | static int const FOV = 90; 31 | static int const ROW = 480; 32 | static int const COL = 752; 33 | 34 | public: 35 | Calib* calib; 36 | 37 | DataGenerator(Calib* _calib); 38 | void update(); 39 | double getTime(); 40 | void setTime(double _t); 41 | 42 | Vector3d generatePoint(); 43 | Vector3d getPoint(int i); 44 | vector getCloud(); 45 | Vector3d getPosition(); //G_p 46 | Matrix3d getRotation(); //IG_R 47 | Vector3d getVelocity(); //G_v 48 | 49 | Vector3d getIMUAngularVelocity(); //I_w body frame 50 | Vector3d getIMULinearAcceleration(); //I_a body frame 51 | 52 | vector> getImage(); 53 | 54 | private: 55 | Vector3d pts[NUM_POINTS]; 56 | double t; 57 | 58 | map before_feature_id; 59 | map current_feature_id; 60 | int current_id; 61 | 62 | default_random_engine random_generator; 63 | normal_distribution distribution; 64 | }; 65 | 66 | #endif //_DATA_GENERATOR_H_ -------------------------------------------------------------------------------- /amber_vins/src/data_generator_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "data_generator.h" 9 | 10 | using namespace std; 11 | using namespace Eigen; 12 | 13 | ros::Publisher pub_imu; 14 | ros::Publisher pub_image; 15 | ros::Publisher pub_sim_path; 16 | ros::Publisher pub_sim_odometry; 17 | ros::Publisher pub_sim_pose; 18 | ros::Publisher pub_sim_cloud; 19 | nav_msgs::Path sim_path; 20 | 21 | Calib* calib; 22 | DataGenerator* generator; 23 | 24 | void setupROS() 25 | { 26 | ros::NodeHandle n("~"); 27 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 28 | 29 | // define pub topics 30 | pub_imu = n.advertise("/imu_3dm_gx4/imu", 1000); 31 | pub_image = n.advertise("/sensors/image", 1000); 32 | 33 | pub_sim_path = n.advertise("/simulation/path", 1000); 34 | pub_sim_odometry = n.advertise("/simulation/odometry", 1000); 35 | pub_sim_pose = n.advertise("/simulation/pose", 1000); 36 | pub_sim_cloud = n.advertise("/simulation/cloud", 1000); 37 | 38 | sim_path.header.frame_id = "world"; 39 | } 40 | 41 | void publishIMUAndSimulation() 42 | { 43 | double current_time = generator->getTime(); 44 | cout << "current_time: " << current_time << endl; 45 | 46 | //get generated data 47 | Vector3d position = generator->getPosition(); 48 | Vector3d velocity = generator->getVelocity(); 49 | Matrix3d rotation = generator->getRotation(); 50 | Quaterniond quaternion = Quaterniond(rotation); 51 | Vector3d linear_acceleration = generator->getIMULinearAcceleration(); 52 | Vector3d angular_velocity = generator->getIMUAngularVelocity(); 53 | 54 | //publish odometry 55 | nav_msgs::Odometry odometry; 56 | odometry.header.frame_id = "world"; 57 | odometry.header.stamp = ros::Time(current_time); 58 | odometry.pose.pose.position.x = position(0); 59 | odometry.pose.pose.position.y = position(1); 60 | odometry.pose.pose.position.z = position(2); 61 | odometry.pose.pose.orientation.x = quaternion.x(); 62 | odometry.pose.pose.orientation.y = quaternion.y(); 63 | odometry.pose.pose.orientation.z = quaternion.z(); 64 | odometry.pose.pose.orientation.w = quaternion.w(); 65 | odometry.twist.twist.linear.x = velocity(0); 66 | odometry.twist.twist.linear.y = velocity(1); 67 | odometry.twist.twist.linear.z = velocity(2); 68 | pub_sim_odometry.publish(odometry); 69 | 70 | //publish pose 71 | geometry_msgs::PoseStamped pose_stamped; 72 | pose_stamped.header.frame_id = "world"; 73 | pose_stamped.header.stamp = ros::Time(current_time); 74 | pose_stamped.pose = odometry.pose.pose; 75 | pub_sim_pose.publish(pose_stamped); 76 | 77 | //publish path 78 | sim_path.poses.push_back(pose_stamped); 79 | pub_sim_path.publish(sim_path); 80 | 81 | //publish imu data 82 | sensor_msgs::Imu imu; 83 | imu.header.frame_id = "body"; 84 | imu.header.stamp = ros::Time(current_time); 85 | imu.linear_acceleration.x = linear_acceleration(0); 86 | imu.linear_acceleration.y = linear_acceleration(1); 87 | imu.linear_acceleration.z = linear_acceleration(2); 88 | imu.angular_velocity.x = angular_velocity(0); 89 | imu.angular_velocity.y = angular_velocity(1); 90 | imu.angular_velocity.z = angular_velocity(2); 91 | imu.orientation.x = quaternion.x(); 92 | imu.orientation.y = quaternion.y(); 93 | imu.orientation.z = quaternion.z(); 94 | imu.orientation.w = quaternion.w(); 95 | pub_imu.publish(imu); 96 | 97 | static tf::TransformBroadcaster br; 98 | tf::Transform transform; 99 | transform.setOrigin(tf::Vector3(position(0), position(1), position(2))); 100 | tf::Quaternion q; 101 | q.setW(quaternion.w()); 102 | q.setX(quaternion.x()); 103 | q.setY(quaternion.y()); 104 | q.setZ(quaternion.z()); 105 | transform.setRotation(q); 106 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "body")); 107 | 108 | sensor_msgs::PointCloud point_cloud; 109 | point_cloud.header.frame_id = "world"; 110 | point_cloud.header.stamp = ros::Time(current_time); 111 | sensor_msgs::ChannelFloat32 ids; 112 | int i = 0; 113 | for (auto & it : generator->getCloud()) 114 | { 115 | geometry_msgs::Point32 p; 116 | p.x = it(0); 117 | p.y = it(1); 118 | p.z = it(2); 119 | point_cloud.points.push_back(p); 120 | ids.values.push_back(i++); 121 | } 122 | point_cloud.channels.push_back(ids); 123 | pub_sim_cloud.publish(point_cloud); 124 | } 125 | 126 | void publishImageData() 127 | { 128 | sensor_msgs::PointCloud feature; 129 | sensor_msgs::ChannelFloat32 ids; 130 | sensor_msgs::ChannelFloat32 pixel; 131 | for (int i = 0; i < generator->ROW; i++) 132 | for (int j = 0; j < generator->COL; j++) 133 | pixel.values.push_back(255); 134 | feature.header.stamp = ros::Time(generator->getTime()); 135 | 136 | for (auto & id_pts : generator->getImage()) 137 | { 138 | int id = id_pts.first; 139 | geometry_msgs::Point32 p; 140 | p.x = id_pts.second(0); 141 | p.y = id_pts.second(1); 142 | p.z = 1; 143 | 144 | feature.points.push_back(p); 145 | ids.values.push_back(id); 146 | } 147 | feature.channels.push_back(ids); 148 | feature.channels.push_back(pixel); 149 | 150 | pub_image.publish(feature); 151 | } 152 | 153 | void setup() 154 | { 155 | calib = new Calib(); 156 | calib->camera.setIntrinsicParam(365.07984, 365.12127, 381.01962, 254.44318); 157 | calib->camera.setDistortionParam(-2.842958e-1, 8.7155025e-2, -1.4602925e-4, -6.149638e-4, -1.218237e-2); 158 | 159 | calib->delta_t = (double) 1 / DataGenerator::FREQ; 160 | calib->CI_q = Quaterniond((Matrix3d() << 0, -1, 0, 0, 0, 1, -1, 0, 0).finished()); 161 | calib->C_p_I << -0.02, -0.14, 0; 162 | calib->image_imu_offset_t = 0; 163 | 164 | calib->sigma_gc = 0.01; 165 | calib->sigma_ac = 0.1; 166 | calib->sigma_wgc = 0.01; 167 | calib->sigma_wac = 0.01; 168 | calib->sigma_Im = 1; 169 | 170 | generator = new DataGenerator(calib); 171 | } 172 | 173 | int main(int argc, char** argv) 174 | { 175 | ros::init(argc, argv, "data_generator"); 176 | 177 | setup(); 178 | setupROS(); 179 | 180 | ros::Rate loop_rate(DataGenerator::FREQ); 181 | while (pub_imu.getNumSubscribers() == 0) 182 | loop_rate.sleep(); 183 | 184 | for (int publish_count = 0; ros::ok(); publish_count++) 185 | { 186 | publishIMUAndSimulation(); 187 | 188 | //publish image data 189 | if (publish_count % generator->IMU_PER_IMG == generator->IMU_PER_IMG - 1) 190 | { 191 | publishImageData(); 192 | cout << "simulation p: " << generator->getPosition().transpose() << endl; 193 | cout << "simulation v: " << generator->getVelocity().transpose() << endl; 194 | cout << "simulation q: " << Quaterniond(generator->getRotation()).coeffs().transpose() << endl; 195 | } 196 | 197 | //update work 198 | generator->update(); 199 | 200 | ros::spinOnce(); 201 | loop_rate.sleep(); 202 | } 203 | 204 | return 0; 205 | } 206 | -------------------------------------------------------------------------------- /amber_vins/src/mixed_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "data_generator.h" 12 | #include "camera.h" 13 | #include "calib.h" 14 | #include "odometry.h" 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | 19 | ros::Publisher pub_imu; 20 | ros::Publisher pub_image; 21 | ros::Publisher pub_sim_path; 22 | ros::Publisher pub_sim_odometry; 23 | ros::Publisher pub_sim_pose; 24 | ros::Publisher pub_sim_cloud; 25 | nav_msgs::Path sim_path; 26 | ros::Publisher pub_odometry; 27 | ros::Publisher pub_pose; 28 | ros::Publisher pub_path; 29 | 30 | // visualize results 31 | nav_msgs::Path path; 32 | visualization_msgs::Marker path_line; 33 | 34 | Calib* calib; 35 | MSCKF* msckf; 36 | CameraMeasurements* cameraMeasurements; 37 | DataGenerator* generator; 38 | 39 | queue imu_buf; 40 | 41 | void imuCallback(const sensor_msgs::Imu imu_msg) 42 | { 43 | imu_buf.push(imu_msg); 44 | } 45 | 46 | void sendIMU(const sensor_msgs::Imu imu_msg) 47 | { 48 | double t = imu_msg.header.stamp.toSec(); 49 | 50 | ROS_INFO("processing imu data with stamp %lf", t); 51 | 52 | double dx = imu_msg.linear_acceleration.x; 53 | double dy = imu_msg.linear_acceleration.y; 54 | double dz = imu_msg.linear_acceleration.z; 55 | 56 | double rx = imu_msg.angular_velocity.x; 57 | double ry = imu_msg.angular_velocity.y; 58 | double rz = imu_msg.angular_velocity.z; 59 | 60 | msckf->propagate(Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); 61 | } 62 | 63 | void publishOdometryResult() 64 | { 65 | Vector3d p = msckf->getPosition(); 66 | Vector3d v = msckf->getVelocity(); 67 | Quaterniond q = msckf->getQuaternion(); 68 | 69 | nav_msgs::Odometry odometry; 70 | odometry.header.stamp = ros::Time::now(); 71 | odometry.header.frame_id = "world"; 72 | odometry.pose.pose.position.x = p(0); 73 | odometry.pose.pose.position.y = p(1); 74 | odometry.pose.pose.position.z = p(2); 75 | odometry.pose.pose.orientation.x = q.x(); 76 | odometry.pose.pose.orientation.y = q.y(); 77 | odometry.pose.pose.orientation.z = q.z(); 78 | odometry.pose.pose.orientation.w = q.w(); 79 | odometry.twist.twist.linear.x = v(0); 80 | odometry.twist.twist.linear.y = v(1); 81 | odometry.twist.twist.linear.y = v(2); 82 | 83 | geometry_msgs::PoseStamped pose_stamped; 84 | pose_stamped.header.stamp = ros::Time::now(); 85 | pose_stamped.header.frame_id = "world"; 86 | pose_stamped.pose = odometry.pose.pose; 87 | 88 | path.poses.push_back(pose_stamped); 89 | 90 | pub_odometry.publish(odometry); 91 | pub_pose.publish(pose_stamped); 92 | pub_path.publish(path); 93 | } 94 | 95 | void imageCallback(const sensor_msgs::PointCloud& image_msg) 96 | { 97 | double t = image_msg.header.stamp.toSec(); 98 | if (imu_buf.empty() || t < imu_buf.front().header.stamp.toSec()) 99 | { 100 | ROS_ERROR("wait for imu data"); 101 | return; 102 | } 103 | 104 | while (!imu_buf.empty() && t >= imu_buf.front().header.stamp.toSec()) 105 | { 106 | sendIMU(imu_buf.front()); 107 | imu_buf.pop(); 108 | } 109 | 110 | ROS_INFO("processing vision data with stamp %lf", t); 111 | 112 | vector > image; 113 | for (int i = 0; i < (int)image_msg.points.size(); i++) 114 | { 115 | int id = image_msg.channels[0].values[i]; 116 | double x = image_msg.points[i].x; 117 | double y = image_msg.points[i].y; 118 | 119 | image.push_back(make_pair(id, Vector2d(x, y))); 120 | } 121 | cameraMeasurements->addFeatures(image); 122 | msckf->updateCamera(*cameraMeasurements); 123 | cout << *msckf << endl; 124 | 125 | publishOdometryResult(); 126 | } 127 | 128 | void setupROS() 129 | { 130 | ros::NodeHandle n("~"); 131 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 132 | 133 | // define pub topics 134 | pub_imu = n.advertise("/imu_3dm_gx4/imu", 1000); 135 | pub_image = n.advertise("/sensors/image", 1000); 136 | 137 | pub_sim_path = n.advertise("/simulation/path", 100); 138 | pub_sim_odometry = n.advertise("/simulation/odometry", 100); 139 | pub_sim_pose = n.advertise("/simulation/pose", 100); 140 | pub_sim_cloud = n.advertise("/simulation/cloud", 1000); 141 | 142 | pub_path = n.advertise("path", 1000); 143 | pub_odometry = n.advertise("odometry", 1000); 144 | pub_pose = n.advertise("pose", 1000); 145 | 146 | path_line.header.frame_id = "world"; 147 | path_line.header.stamp = ros::Time::now(); 148 | path_line.ns = "calibration"; 149 | path_line.action = visualization_msgs::Marker::ADD; 150 | path_line.pose.orientation.w = 1.0; 151 | path_line.type = visualization_msgs::Marker::LINE_STRIP; 152 | path_line.scale.x = 0.01; 153 | path_line.color.a = 1.0; 154 | path_line.color.r = 1.0; 155 | path_line.id = 1; 156 | 157 | path.header.frame_id = "world"; 158 | sim_path.header.frame_id = "world"; 159 | } 160 | 161 | void publishIMUAndSimulation() 162 | { 163 | double current_time = generator->getTime(); 164 | 165 | //get generated data 166 | Vector3d position = generator->getPosition(); 167 | Vector3d velocity = generator->getVelocity(); 168 | Matrix3d rotation = generator->getRotation(); 169 | Quaterniond quaternion = Quaterniond(rotation); 170 | Vector3d linear_acceleration = generator->getIMULinearAcceleration(); 171 | Vector3d angular_velocity = generator->getIMUAngularVelocity(); 172 | 173 | //publish odometry 174 | nav_msgs::Odometry odometry; 175 | odometry.header.frame_id = "world"; 176 | odometry.header.stamp = ros::Time(current_time); 177 | odometry.pose.pose.position.x = position(0); 178 | odometry.pose.pose.position.y = position(1); 179 | odometry.pose.pose.position.z = position(2); 180 | odometry.pose.pose.orientation.x = quaternion.x(); 181 | odometry.pose.pose.orientation.y = quaternion.y(); 182 | odometry.pose.pose.orientation.z = quaternion.z(); 183 | odometry.pose.pose.orientation.w = quaternion.w(); 184 | odometry.twist.twist.linear.x = velocity(0); 185 | odometry.twist.twist.linear.y = velocity(1); 186 | odometry.twist.twist.linear.z = velocity(2); 187 | pub_sim_odometry.publish(odometry); 188 | 189 | //publish pose 190 | geometry_msgs::PoseStamped pose_stamped; 191 | pose_stamped.header.frame_id = "world"; 192 | pose_stamped.header.stamp = ros::Time(current_time); 193 | pose_stamped.pose = odometry.pose.pose; 194 | pub_sim_pose.publish(pose_stamped); 195 | 196 | //publish path 197 | sim_path.poses.push_back(pose_stamped); 198 | pub_sim_path.publish(sim_path); 199 | 200 | //publish imu data 201 | sensor_msgs::Imu imu; 202 | imu.header.frame_id = "body"; 203 | imu.header.stamp = ros::Time(current_time); 204 | imu.linear_acceleration.x = linear_acceleration(0); 205 | imu.linear_acceleration.y = linear_acceleration(1); 206 | imu.linear_acceleration.z = linear_acceleration(2); 207 | imu.angular_velocity.x = angular_velocity(0); 208 | imu.angular_velocity.y = angular_velocity(1); 209 | imu.angular_velocity.z = angular_velocity(2); 210 | imu.orientation.x = quaternion.x(); 211 | imu.orientation.y = quaternion.y(); 212 | imu.orientation.z = quaternion.z(); 213 | imu.orientation.w = quaternion.w(); 214 | pub_imu.publish(imu); 215 | imuCallback(imu); 216 | 217 | static tf::TransformBroadcaster br; 218 | tf::Transform transform; 219 | transform.setOrigin(tf::Vector3(position(0), position(1), position(2))); 220 | tf::Quaternion q; 221 | q.setW(quaternion.w()); 222 | q.setX(quaternion.x()); 223 | q.setY(quaternion.y()); 224 | q.setZ(quaternion.z()); 225 | transform.setRotation(q); 226 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "body")); 227 | 228 | sensor_msgs::PointCloud point_cloud; 229 | point_cloud.header.frame_id = "world"; 230 | point_cloud.header.stamp = ros::Time(); 231 | sensor_msgs::ChannelFloat32 ids; 232 | int i = 0; 233 | for (auto & it : generator->getCloud()) 234 | { 235 | geometry_msgs::Point32 p; 236 | p.x = it(0); 237 | p.y = it(1); 238 | p.z = it(2); 239 | point_cloud.points.push_back(p); 240 | ids.values.push_back(i++); 241 | } 242 | point_cloud.channels.push_back(ids); 243 | pub_sim_cloud.publish(point_cloud); 244 | } 245 | 246 | void publishImageData() 247 | { 248 | sensor_msgs::PointCloud feature; 249 | sensor_msgs::ChannelFloat32 ids; 250 | sensor_msgs::ChannelFloat32 pixel; 251 | for (int i = 0; i < generator->ROW; i++) 252 | for (int j = 0; j < generator->COL; j++) 253 | pixel.values.push_back(255); 254 | feature.header.stamp = ros::Time(generator->getTime()); 255 | 256 | for (auto & id_pts : generator->getImage()) 257 | { 258 | int id = id_pts.first; 259 | geometry_msgs::Point32 p; 260 | p.x = id_pts.second(0); 261 | p.y = id_pts.second(1); 262 | p.z = 1; 263 | 264 | feature.points.push_back(p); 265 | ids.values.push_back(id); 266 | } 267 | feature.channels.push_back(ids); 268 | feature.channels.push_back(pixel); 269 | 270 | pub_image.publish(feature); 271 | imageCallback(feature); 272 | } 273 | 274 | void setup() 275 | { 276 | calib = new Calib(); 277 | calib->camera.setIntrinsicParam(365.07984, 365.12127, 381.01962, 254.44318); 278 | calib->camera.setDistortionParam(-2.842958e-1, 8.7155025e-2, -1.4602925e-4, -6.149638e-4, -1.218237e-2); 279 | 280 | calib->delta_t = (double) 1 / DataGenerator::FREQ; 281 | calib->CI_q = Quaterniond((Matrix3d() << 0, -1, 0, 0, 0, 1, -1, 0, 0).finished()); 282 | calib->C_p_I << -0.02, -0.14, 0; 283 | calib->image_imu_offset_t = 0; 284 | 285 | calib->sigma_gc = 0.01; 286 | calib->sigma_ac = 0.1; 287 | calib->sigma_wgc = 0.01; 288 | calib->sigma_wac = 0.01; 289 | calib->sigma_Im = 1; 290 | 291 | generator = new DataGenerator(calib); 292 | 293 | msckf = new MSCKF(calib); 294 | msckf->x.segment<4>(0) = Quaterniond(generator->getRotation()).coeffs(); 295 | msckf->x.segment<3>(0 + 4) = generator->getPosition(); 296 | msckf->x.segment<3>(0 + 4 + 3) = generator->getVelocity(); 297 | 298 | cameraMeasurements = new CameraMeasurements(); 299 | } 300 | 301 | int main(int argc, char **argv) 302 | { 303 | ros::init(argc, argv, "msckf_vins"); 304 | 305 | setup(); 306 | setupROS(); 307 | 308 | ros::Rate loop_rate(generator->FREQ); 309 | for (int publish_count = 0; ros::ok(); publish_count++) 310 | { 311 | publishIMUAndSimulation(); 312 | 313 | //publish image data 314 | if (publish_count % generator->IMU_PER_IMG == generator->IMU_PER_IMG - 1) 315 | { 316 | publishImageData(); 317 | cout << "simulation p: " << generator->getPosition().transpose() << endl; 318 | cout << "simulation v: " << generator->getVelocity().transpose() << endl; 319 | cout << "simulation q: " << Quaterniond(generator->getRotation()).coeffs().transpose() << endl; 320 | } 321 | 322 | //update work 323 | generator->update(); 324 | 325 | ros::spinOnce(); 326 | loop_rate.sleep(); 327 | } 328 | 329 | return 0; 330 | } 331 | 332 | 333 | -------------------------------------------------------------------------------- /amber_vins/src/odometry.h: -------------------------------------------------------------------------------- 1 | #ifndef _ODOMETRY_H_ 2 | #define _ODOMETRY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "calib.h" 12 | #include "utils.h" 13 | using namespace Eigen; 14 | using namespace std; 15 | 16 | #define ODO_MAX_FRAMES (5) // used for static allocation of state and sigma 17 | #define ODO_STATE_SIZE (4 + 3 + 3 + 3 + 3) 18 | #define ODO_STATE_FRAME_SIZE (4 + 3 + 3) 19 | #define ODO_STATE_MAX_SIZE (ODO_STATE_SIZE + ODO_STATE_FRAME_SIZE * (ODO_MAX_FRAMES + 1)) 20 | #define ODO_SIGMA_SIZE (3 + 3 + 3 + 3 + 3) 21 | #define ODO_SIGMA_FRAME_SIZE (3 + 3 + 3) 22 | #define ODO_SIGMA_MAX_SIZE (ODO_SIGMA_SIZE + ODO_SIGMA_FRAME_SIZE * (ODO_MAX_FRAMES + 1)) 23 | 24 | typedef struct 25 | { 26 | int id; 27 | Matrix2Xd z; // length is stored as number of elements in z 28 | bool isLost; // Set if it is no longer tracked in current frame 29 | // TODO: add feature description here? 30 | } CameraMeas_t; 31 | 32 | class CameraMeasurements 33 | { 34 | public: 35 | list meas; 36 | // 37 | // Variables to hold feature info 38 | // 39 | map::iterator> link; 40 | 41 | void addToFeature(list::iterator& feature, const Vector2d& p); 42 | list::iterator addNewFeature(int id); 43 | list::iterator removeFeature(list::iterator& feature); 44 | void addFeatures(const vector>& features); 45 | }; 46 | 47 | /*** 48 | ** 49 | ** General 6DOF odometry with gyro and accelerometer (+biases) 50 | ** 51 | ***/ 52 | class Odometry 53 | { 54 | public: 55 | // 56 | // Calibration object 57 | // 58 | Calib* calib; 59 | // init 60 | Odometry(Calib* cal): calib(cal) {} 61 | }; 62 | 63 | class MSCKF: public Odometry 64 | { 65 | public: 66 | // 67 | // State 68 | // 69 | // Matrix x; // allocate statically 70 | VectorXd x; 71 | // 72 | // Covariance 73 | // 74 | // Matrix sigma; // allocate statically 75 | MatrixXd sigma; 76 | // 77 | // Local variables for integration 78 | // 79 | Vector3d I_a_prev, I_g_prev; 80 | 81 | bool initialized; 82 | 83 | public: 84 | MSCKF(Calib* cal); 85 | 86 | Quaterniond getQuaternion(); 87 | Vector3d getPosition(); 88 | Vector3d getVelocity(); 89 | 90 | void propagate(const Vector3d& I_a_m, const Vector3d& I_g_m, bool propagateError = true); 91 | void updateCamera(CameraMeasurements& cameraMeasurements); 92 | void augmentState(void); 93 | void removeOldStates(int n); 94 | 95 | void marginalize(const Matrix2Xd& z, const Vector3d& G_p_f, Ref r0, Ref H0); 96 | bool isInlinerCamera(const VectorXd& r0, const MatrixXd& H0); 97 | void performUpdate(const VectorXd& delta_x); 98 | 99 | // print 100 | friend ostream& operator<<(ostream& out, const MSCKF& msckf); 101 | }; 102 | 103 | #endif //_ODOMETRY_H_ 104 | -------------------------------------------------------------------------------- /amber_vins/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | Matrix3d crossMat(const Vector3d& v) 4 | { 5 | Matrix3d m; 6 | m << 0, -v(2), v(1), 7 | v(2), 0, -v(0), 8 | -v(1), v(0), 0; 9 | return m; 10 | } 11 | 12 | // 13 | // Omega( v ) = -[v] v 14 | // -v 0 15 | // 16 | Matrix4d Omega(const Vector3d& v) 17 | { 18 | Matrix4d m; 19 | m << 0, v(2), -v(1), v(0), 20 | -v(2), 0, v(0), v(1), 21 | v(1), -v(0), 0, v(2), 22 | -v(0), -v(1), -v(2), 0; 23 | return m; 24 | } 25 | 26 | -------------------------------------------------------------------------------- /amber_vins/src/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTILS_H_ 2 | #define _UTILS_H_ 3 | 4 | #include 5 | using namespace Eigen; 6 | 7 | const double chi2Inv[] = {0, 3.84145882069413, 5.99146454710798, 7.81472790325118, 9.48772903678115, 11.0704976935164, 12.591587243744, 14.0671404493402, 15.5073130558655, 16.9189776046204, 18.3070380532751, 19.6751375726825, 21.0260698174831, 22.3620324948269, 23.6847913048406, 24.9957901397286, 26.2962276048642, 27.5871116382753, 28.8692994303926, 30.1435272056462, 31.4104328442309, 32.6705733409173, 33.9244384714438, 35.1724616269081, 36.4150285018073, 37.6524841334828, 38.88513865983, 40.1132720694136, 41.3371381514274, 42.5569678042927, 43.7729718257422, 44.9853432803651, 46.1942595202785, 47.3998839190809, 48.6023673672942, 49.8018495682019, 50.9984601657106, 52.1923197301029, 53.3835406229693, 54.5722277589417, 55.758479278887, 56.9423871468241, 58.124037680868, 59.3035120268998, 60.4808865823364, 61.6562333762796, 62.8296204114082, 64.001111972218, 65.1707689035698, 66.3386488629688, 67.5048065495412, 68.6692939122858, 69.8321603398481, 70.9934528337823, 72.1532161670231, 73.3114930290833, 74.4683241593093, 75.6237484693761, 76.7778031560615, 77.9305238052304, 79.0819444878487, 80.2320978487627, 81.3810151888991, 82.5287265414718, 83.675260742721, 84.8206454976567, 85.9649074412309, 87.1080721953219, 88.2501644218741, 89.391207872508, 90.5312254348807, 91.6702391760549, 92.8082703831077, 93.9453396011923, 95.0814666692432, 96.2166707535038, 97.3509703790329, 98.4843834593404, 99.6169273242838, 100.74861874635, 101.879473965436, 103.009508712226, 104.138738230274, 105.26717729686, 106.394840242723, 107.521740970719, 108.647892973508, 109.773309350288, 110.898002822684, 112.021985749808, 113.145270142555, 114.267867677194, 115.389789708267, 116.511047280874, 117.631651142346, 118.751611753367, 119.870939298567, 120.98964369661, 122.107734609819, 123.225221453362, 124.342113404004, 125.458419408482, 126.574148191494, 127.689308263338, 128.803907927218, 129.917955286229, 131.031458250049, 132.144424541337, 133.256861701868, 134.368777098411, 135.480177928359, 136.591071225135, 137.701463863371, 138.811362563885, 139.920773898456, 141.02970429441, 142.138160039026, 143.246147283775, 144.353672048385, 145.460740224765, 146.567357580767, 147.673529763818, 148.779262304405, 149.884560619441, 150.989430015505, 152.093875691958, 153.197902743956, 154.30151616535, 155.404720851482, 156.507521601885, 157.609923122889, 158.711930030134, 159.813546850998, 160.914778026943, 162.015627915781, 163.11610079386, 164.216200858185, 165.315932228459, 166.415298949064, 167.514304990978, 168.61295425362, 169.711250566649, 170.809197691695, 171.90679932404, 173.004059094245, 174.100980569726, 175.197567256281, 176.29382259957, 177.389749986549, 178.485352746859, 179.580634154181, 180.675597427534, 181.770245732556, 182.864582182723, 183.958609840556, 185.052331718773, 186.145750781417, 187.238869944954, 188.331692079329, 189.424220009004, 190.516456513959, 191.608404330663, 192.700066153028, 193.791444633324, 194.88254238308, 195.973361973949, 197.063905938561, 198.154176771341, 199.244176929317, 200.333908832898, 201.42337486663, 202.512577379941, 203.601518687858, 204.69020107171, 205.778626779806, 206.866798028108, 207.95471700087, 209.042385851279, 210.12980670206, 211.216981646085, 212.303912746952, 213.390602039558, 214.477051530651, 215.563263199378, 216.649238997807, 217.734980851449, 218.820490659756, 219.905770296615, 220.990821610828, 222.075646426575, 223.160246543877, 224.244623739036, 225.328779765074, 226.412716352154, 227.496435208002, 228.579938018305, 229.663226447109, 230.74630213721, 231.829166710528, 232.911821768476, 233.994268892325, 235.076509643554, 236.158545564194, 237.24037817717, 238.322008986627, 239.403439478252, 240.484671119593, 241.565705360364, 242.646543632752, 243.727187351706, 244.807637915228, 245.887896704661, 246.967965084957, 248.047844404956, 249.127535997645, 250.20704118042, 251.286361255342, 252.365497509384, 253.444451214672, 254.523223628728, 255.601815994703, 256.680229541603, 257.758465484515, 258.836525024829, 259.914409350448, 260.992119636005, 262.069657043065, 263.147022720329, 264.224217803834, 265.301243417145, 266.378100671549, 267.45479066624, 268.531314488501, 269.607673213888, 270.683867906404, 271.759899618669, 272.835769392096, 273.911478257054, 274.987027233029, 276.062417328789, 277.137649542537, 278.212724862068, 279.287644264921, 280.362408718521, 281.437019180335, 282.511476598006, 283.585781909499, 284.659936043236, 285.733939918233, 286.807794444232, 287.881500521831, 288.955059042613, 290.028470889273, 291.10173693574, 292.174858047296, 293.247835080701, 294.320668884306, 295.39336029817, 296.465910154169, 297.538319276114, 298.610588479854, 299.682718573386, 300.754710356962, 301.826564623188, 302.898282157129, 303.96986373641, 305.041310131311, 306.112622104867, 307.18380041296, 308.254845804417, 309.325759021097, 310.396540797983, 311.467191863273, 312.537712938465, 313.608104738444, 314.678367971565, 315.748503339739, 316.818511538513, 317.888393257151, 318.958149178711, 320.027779980128, 321.097286332285, 322.166668900092, 323.235928342558, 324.305065312867, 325.374080458446, 326.44297442104, 327.511747836777, 328.58040133624, 329.648935544535, 330.717351081352, 331.785648561038, 332.853828592656, 333.921891780048, 334.989838721903, 336.057670011813, 337.125386238334, 338.192987985048, 339.260475830621, 340.327850348859, 341.395112108769, 342.462261674608, 343.529299605946, 344.596226457715, 345.663042780266, 346.72974911942, 347.796346016519, 348.862834008481, 349.929213627845, 350.995485402826, 352.061649857363, 353.127707511163, 354.193658879755, 355.259504474532, 356.325244802799, 357.390880367821, 358.456411668862, 359.521839201237, 360.587163456347, 361.652384921732, 362.717504081103, 363.782521414394, 364.847437397793, 365.912252503793, 366.976967201223, 368.041581955295, 369.106097227638, 370.170513476337, 371.234831155975, 372.299050717665, 373.36317260909, 374.427197274541, 375.491125154948, 376.554956687919, 377.618692307775, 378.682332445584, 379.745877529193, 380.809327983263, 381.872684229305, 382.935946685708, 383.999115767773, 385.062191887747, 386.125175454849, 387.188066875309, 388.25086655239, 389.313574886424, 390.37619227484, 391.438719112193, 392.501155790192, 393.563502697733, 394.625760220922, 395.687928743105, 396.750008644897, 397.812000304206, 398.873904096264, 399.935720393649, 400.997449566315, 402.059091981615, 403.120648004329, 404.182117996686, 405.243502318393, 406.304801326655, 407.366015376204, 408.427144819318, 409.488190005848, 410.549151283242, 411.610028996565, 412.670823488524, 413.73153509949, 414.792164167519, 415.852711028376, 416.913176015557, 417.973559460307, 419.033861691644, 420.09408303638, 421.154223819141, 422.214284362386, 423.27426498643, 424.334166009463, 425.393987747567, 426.45373051474, 427.51339462291, 428.57298038196, 429.632488099742, 430.691918082097, 431.751270632875, 432.810546053949, 433.869744645239, 434.928866704723, 435.987912528461, 437.046882410606, 438.105776643427, 439.164595517321, 440.223339320832, 441.282008340668, 442.340602861718, 443.399123167063, 444.457569537999, 445.515942254048, 446.574241592977, 447.632467830808, 448.690621241841, 449.748702098661, 450.80671067216, 451.864647231545, 452.92251204436, 453.980305376493, 455.038027492196, 456.095678654097, 457.153259123211, 458.21076915896, 459.268209019181, 460.325578960142, 461.382879236557, 462.440110101594, 463.497271806893, 464.554364602577, 465.611388737267, 466.668344458089, 467.725232010692, 468.782051639257, 469.838803586514, 470.895488093746, 471.952105400812, 473.008655746145, 474.065139366778, 475.121556498345, 476.177907375097, 477.234192229914, 478.290411294314, 479.346564798465, 480.402652971197, 481.458676040009, 482.514634231087, 483.570527769306, 484.626356878249, 485.682121780211, 486.737822696211, 487.793459846004, 488.84903344809, 489.904543719724, 490.959990876927, 492.01537513449, 493.070696705995, 494.125955803814, 495.181152639123, 496.236287421911, 497.29136036099, 498.346371664003, 499.401321537435, 500.456210186617, 501.511037815743, 502.565804627872, 503.62051082494, 504.675156607768, 505.729742176069, 506.78426772846, 507.838733462469, 508.89313957454, 509.947486260048, 511.001773713299, 512.056002127545, 513.110171694991, 514.164282606797, 515.218335053093, 516.272329222984, 517.326265304559, 518.380143484893, 519.433963950064, 520.487726885153, 521.541432474253, 522.59508090048, 523.648672345975, 524.702206991915, 525.755685018519, 526.809106605054, 527.862471929845, 528.915781170277, 529.969034502808, 531.022232102969, 532.075374145378, 533.128460803741, 534.181492250862, 535.234468658646, 536.28739019811, 537.340257039387, 538.393069351731, 539.445827303526, 540.498531062292, 541.551180794688, 542.603776666523, 543.656318842758, 544.708807487514, 545.761242764079, 546.813624834911, 547.865953861645, 548.918230005101, 549.970453425286, 551.022624281404, 552.074742731858, 553.126808934257, 554.178823045421, 555.230785221389, 556.282695617422, 557.334554388006, 558.386361686865, 559.438117666958, 560.489822480489, 561.541476278912, 562.593079212935, 563.644631432524, 564.696133086911, 565.747584324597, 566.798985293358, 567.850336140249, 568.901637011609, 569.952888053067, 571.004089409547, 572.055241225269, 573.106343643759, 574.157396807852, 575.208400859693, 576.259355940747, 577.310262191801, 578.361119752967, 579.41192876369, 580.46268936275, 581.513401688267, 582.564065877706, 583.61468206788, 584.665250394956, 585.715770994457, 586.76624400127, 587.816669549645, 588.867047773204, 589.917378804943, 590.967662777236, 592.017899821838, 593.068090069892, 594.118233651931, 595.168330697882, 596.218381337071, 597.268385698224, 598.318343909475, 599.368256098367, 600.418122391857, 601.467942916319, 602.517717797548, 603.567447160765, 604.617131130617, 605.666769831186, 606.716363385987, 607.765911917977, 608.815415549554, 609.864874402563, 610.9142885983, 611.963658257511, 613.012983500403, 614.062264446641, 615.111501215354, 616.160693925137, 617.209842694058, 618.258947639656, 619.308008878948, 620.35702652843, 621.406000704084, 622.454931521377, 623.503819095264, 624.552663540197, 625.60146497012, 626.650223498478, 627.698939238218, 628.747612301793, 629.796242801164, 630.844830847801, 631.893376552693, 632.941880026341, 633.99034137877, 635.038760719526, 636.087138157683, 637.135473801841, 638.183767760134, 639.23202014023, 640.280231049334, 641.32840059419, 642.376528881088, 643.424616015861, 644.47266210389, 645.520667250108, 646.568631559003, 647.616555134617, 648.664438080551, 649.71228049997, 650.760082495601, 651.807844169739, 652.855565624246, 653.903246960558, 654.950888279686, 655.998489682215, 657.046051268312, 658.093573137725, 659.141055389785, 660.18849812341, 661.23590143711, 662.283265428982, 663.330590196719, 664.377875837611, 665.425122448544, 666.472330126008, 667.519498966093, 668.566629064496, 669.613720516522, 670.660773417084, 671.70778786071, 672.754763941539, 673.80170175333, 674.848601389458, 675.89546294292, 676.942286506336, 677.98907217195, 679.035820031637, 680.082530176896, 681.129202698861, 682.1758376883, 683.222435235615, 684.268995430845, 685.315518363671, 686.362004123414, 687.40845279904, 688.454864479159, 689.501239252029, 690.547577205561, 691.593878427312, 692.640143004496, 693.686371023982, 694.732562572297, 695.778717735626, 696.824836599814, 697.870919250371, 698.916965772472, 699.962976250957, 701.008950770335, 702.054889414786, 703.100792268162, 704.146659413987, 705.192490935464, 706.23828691547, 707.284047436563, 708.329772580982, 709.375462430647, 710.421117067165, 711.466736571826, 712.512321025609, 713.557870509184, 714.60338510291, 715.648864886838, 716.694309940716, 717.739720343986, 718.785096175789, 719.830437514963, 720.875744440049, 721.921017029289, 722.96625536063, 724.011459511725, 725.056629559931, 726.101765582317, 727.146867655661, 728.191935856452, 729.236970260894, 730.281970944903, 731.326937984114, 732.371871453878, 733.416771429266, 734.461637985067, 735.506471195797, 736.551271135692, 737.596037878713, 738.640771498548, 739.685472068614, 740.730139662056, 741.774774351748, 742.8193762103, 743.863945310051, 744.908481723077, 745.952985521191, 746.997456775941, 748.041895558617, 749.086301940244, 750.130675991594, 751.175017783179, 752.219327385255, 753.263604867823, 754.307850300633, 755.35206375318, 756.39624529471, 757.440394994219, 758.484512920454, 759.528599141917, 760.572653726863, 761.616676743301, 762.660668258998, 763.70462834148, 764.748557058031, 765.792454475695, 766.836320661277, 767.880155681347, 768.923959602236, 769.967732490043, 771.011474410629, 772.055185429627, 773.098865612437, 774.142515024226, 775.186133729935, 776.229721794275, 777.273279281732, 778.316806256564, 779.360302782806, 780.403768924267, 781.447204744536, 782.490610306979, 783.533985674743, 784.577330910754, 785.62064607772, 786.663931238132, 787.707186454267, 788.750411788183, 789.793607301727, 790.836773056531, 791.879909114016, 792.923015535393, 793.96609238166, 795.009139713609, 796.052157591824, 797.095146076678, 798.138105228344, 799.181035106785, 800.223935771763, 801.266807282835, 802.309649699357, 803.352463080484, 804.39524748517, 805.438002972171, 806.480729600043, 807.523427427146, 808.566096511642, 809.608736911501, 810.651348684494, 811.693931888201, 812.736486580008, 813.779012817111, 814.821510656513, 815.863980155028, 816.90642136928, 817.948834355705, 818.991219170553, 820.033575869885, 821.075904509578, 822.118205145324, 823.16047783263, 824.20272262682, 825.244939583038, 826.287128756244, 827.329290201218, 828.37142397256, 829.413530124692, 830.455608711858, 831.497659788123, 832.539683407377, 833.581679623334, 834.623648489532, 835.665590059336, 836.707504385938, 837.749391522357, 838.791251521439, 839.833084435862, 840.874890318131, 841.916669220582, 842.958421195384, 844.000146294537, 845.041844569873, 846.083516073059, 847.125160855596, 848.166778968819, 849.208370463901, 850.24993539185, 851.291473803511, 852.332985749568, 853.374471280543, 854.415930446799, 855.457363298537, 856.4987698858, 857.540150258473, 858.581504466282, 859.622832558797, 860.664134585432, 861.705410595444, 862.746660637936, 863.787884761857, 864.829083016002, 865.870255449013, 866.911402109379, 867.95252304544, 868.993618305382, 870.034687937242, 871.075731988908, 872.116750508119, 873.157743542465, 874.198711139387, 875.239653346183, 876.280570210001, 877.321461777845, 878.362328096572, 879.403169212897, 880.44398517339, 881.484776024478, 882.525541812444, 883.56628258343, 884.606998383438, 885.647689258327, 886.688355253817, 887.728996415488, 888.769612788781, 889.810204418998, 890.850771351305, 891.891313630729, 892.931831302161, 893.972324410356, 895.012792999933, 896.053237115377, 897.093656801037, 898.134052101131, 899.174423059741, 900.214769720817, 901.255092128178, 902.295390325511, 903.335664356372, 904.375914264185, 905.416140092247, 906.456341883723, 907.496519681651, 908.536673528939, 909.576803468369, 910.616909542596, 911.656991794145, 912.69705026542, 913.737084998694, 914.777096036119, 915.81708341972, 916.857047191399, 917.896987392935, 918.936904065983, 919.976797252075, 921.016666992621, 922.056513328912, 923.096336302114, 924.136135953276, 925.175912323324, 926.215665453067, 927.255395383193, 928.295102154272, 929.334785806757, 930.374446380982, 931.414083917163, 932.453698455402, 933.493290035683, 934.532858697875, 935.572404481731, 936.61192742689, 937.651427572876, 938.690904959099, 939.730359624857, 940.769791609333, 941.809200951598, 942.848587690614, 943.887951865227, 944.927293514174, 945.966612676081, 947.005909389464, 948.045183692729, 949.084435624173, 950.123665221983, 951.162872524238, 952.202057568909, 953.241220393859, 954.280361036846, 955.319479535517, 956.358575927416, 957.39765024998, 958.436702540541, 959.475732836324, 960.514741174451, 961.55372759194, 962.592692125703, 963.631634812552, 964.670555689192, 965.709454792228, 966.748332158161, 967.787187823392, 968.826021824219, 969.86483419684, 970.903624977351, 971.942394201749, 972.98114190593, 974.019868125692, 975.058572896732, 976.097256254649, 977.135918234945, 978.174558873022, 979.213178204185, 980.251776263642, 981.290353086504, 982.328908707786, 983.367443162407, 984.405956485189, 985.444448710859, 986.48291987405, 987.521370009301, 988.559799151053, 989.598207333656, 990.636594591367, 991.674960958349, 992.71330646867, 993.751631156309, 994.789935055151, 995.828218198988, 996.866480621524, 997.904722356369, 998.942943437042, 999.981143896974, 1001.0193237695, 1002.05748308788, 1003.09562188527, 1004.13374019473, 1005.17183804926, 1006.20991548175, 1007.247972525, 1008.28600921172, 1009.32402557457, 1010.36202164606, 1011.39999745867, 1012.43795304477, 1013.47588843663, 1014.51380366646, 1015.55169876637, 1016.58957376839, 1017.62742870446, 1018.66526360644, 1019.70307850611, 1020.74087343515, 1021.77864842518, 1022.81640350771, 1023.8541387142, 1024.89185407598, 1025.92954962435, 1026.9672253905, 1028.00488140553, 1029.04251770048, 1030.0801343063, 1031.11773125385, 1032.15530857393, 1033.19286629724, 1034.23040445441, 1035.26792307598, 1036.30542219243, 1037.34290183414, 1038.38036203143, 1039.41780281453, 1040.45522421359, 1041.49262625868, 1042.53000897981, 1043.56737240689, 1044.60471656978, 1045.64204149822, 1046.67934722193, 1047.7166337705, 1048.75390117348, 1049.79114946033, 1050.82837866044, 1051.86558880311, 1052.90277991759, 1053.93995203303, 1054.97710517852, 1056.01423938308, 1057.05135467565, 1058.08845108508, 1059.12552864018, 1060.16258736966, 1061.19962730217, 1062.23664846628, 1063.27365089049, 1064.31063460324, 1065.34759963287, 1066.38454600768, 1067.42147375589, 1068.45838290563, 1069.49527348497, 1070.53214552193, 1071.56899904443, 1072.60583408033, 1073.64265065742, 1074.67944880344}; 8 | 9 | 10 | Matrix3d crossMat(const Vector3d& v); 11 | Matrix4d Omega(const Vector3d& v); 12 | 13 | #endif//_UTILS_H_ 14 | -------------------------------------------------------------------------------- /amber_vins/test/test_camera.cpp: -------------------------------------------------------------------------------- 1 | #include "data_generator.h" 2 | #include "camera.h" 3 | #include "calib.h" 4 | #include "odometry.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | using namespace std; 13 | 14 | Calib* calib; 15 | DataGenerator* generator; 16 | MSCKF* msckf; 17 | CameraMeasurements* cameraMeasurements; 18 | 19 | void setup() 20 | { 21 | calib = new Calib(); 22 | calib->camera.setIntrinsicParam(365.07984, 365.12127, 381.01962, 254.44318); 23 | calib->camera.setDistortionParam(-2.842958e-1, 8.7155025e-2, -1.4602925e-4, -6.149638e-4, -1.218237e-2); 24 | 25 | calib->delta_t = (double) 1 / DataGenerator::FREQ; 26 | calib->CI_q = Quaterniond((Matrix3d() << 0, -1, 0, 0, 0, 1, -1, 0, 0).finished()); 27 | calib->C_p_I << -0.02, -0.14, 0; 28 | calib->image_imu_offset_t = 0; 29 | 30 | calib->sigma_gc = 0; 31 | calib->sigma_ac = 0; 32 | calib->sigma_wgc = 0; 33 | calib->sigma_wac = 0; 34 | 35 | calib->sigma_Im = 0; 36 | 37 | calib->maxFrame = 5; 38 | calib->minFrame = 3; 39 | 40 | generator = new DataGenerator(calib); 41 | msckf = new MSCKF(calib); 42 | msckf->x.segment<4>(0) = Quaterniond(generator->getRotation()).coeffs(); 43 | msckf->x.segment<3>(0 + 4) = generator->getPosition(); 44 | msckf->x.segment<3>(0 + 4 + 3) = generator->getVelocity(); 45 | 46 | cameraMeasurements = new CameraMeasurements(); 47 | } 48 | 49 | Matrix2Xd cv2eigen(const std::vector& src) 50 | { 51 | Matrix2Xd dst(2, src.size()); 52 | for (int i = 0; i < src.size(); i++) 53 | { 54 | dst(0, i) = src[i].x; 55 | dst(1, i) = src[i].y; 56 | } 57 | return dst; 58 | } 59 | 60 | TEST(CameraTest, testProjectFunctionAndJacobian) 61 | { 62 | int n = 10; 63 | std::vector objectPoints; 64 | 65 | Matrix2Xd ourProjectedPoints(2, n); 66 | MatrixX3d ourJacobianPoints(2 * n, 3); 67 | 68 | for (int i = 0; i < n; i++) 69 | { 70 | Vector3d p = generator->generatePoint(); 71 | p(2) = abs(p(2)); 72 | objectPoints.push_back(cv::Point3d(p(0), p(1), p(2))); 73 | ourProjectedPoints.col(i) = calib->camera.cameraProject(p); 74 | ourJacobianPoints.block<2, 3>(i * 2, 0) = calib->camera.jacobianH(p); 75 | } 76 | 77 | cv::Mat K(3,3,cv::DataType::type); // intrinsic parameter matrix 78 | K.at(0, 0) = calib->camera.fx; 79 | K.at(1, 0) = 0; 80 | K.at(2, 0) = 0; 81 | 82 | K.at(0, 1) = 0; 83 | K.at(1, 1) = calib->camera.fy; 84 | K.at(2, 1) = 0; 85 | 86 | K.at(0, 2) = calib->camera.ox; 87 | K.at(1, 2) = calib->camera.oy; 88 | K.at(2, 2) = 1; 89 | 90 | cv::Mat distCoeffs(5, 1, cv::DataType::type); 91 | distCoeffs.at(0) = calib->camera.k1; 92 | distCoeffs.at(1) = calib->camera.k2; 93 | distCoeffs.at(2) = calib->camera.t1; 94 | distCoeffs.at(3) = calib->camera.t2; 95 | distCoeffs.at(4) = calib->camera.k3; 96 | 97 | cv::Mat rVec(3, 3, cv::DataType::type); // Rotation vector 98 | rVec.at(0, 0) = 1; 99 | rVec.at(1, 0) = 0; 100 | rVec.at(2, 0) = 0; 101 | 102 | rVec.at(0, 1) = 0; 103 | rVec.at(1, 1) = 1; 104 | rVec.at(2, 1) = 0; 105 | 106 | rVec.at(0, 2) = 0; 107 | rVec.at(1, 2) = 0; 108 | rVec.at(2, 2) = 1; 109 | 110 | cv::Mat tVec(3, 1, cv::DataType::type); // Translation vector 111 | tVec.at(0) = 0; 112 | tVec.at(1) = 0; 113 | tVec.at(2) = 0; 114 | 115 | 116 | cv::Mat rVecR(3,1,cv::DataType::type);//rodrigues rotation matrix 117 | 118 | cv::Rodrigues(rVec,rVecR); 119 | 120 | std::vector projectedPoints; 121 | 122 | cv::projectPoints(objectPoints, rVecR, tVec, K, distCoeffs, projectedPoints); 123 | 124 | ASSERT_LT((cv2eigen(projectedPoints) - ourProjectedPoints).norm(), 0.1); 125 | 126 | std::vector projectedPoints_x; 127 | std::vector objectPoints_x; 128 | std::vector projectedPoints_y; 129 | std::vector objectPoints_y; 130 | std::vector projectedPoints_z; 131 | std::vector objectPoints_z; 132 | 133 | double delta = 0.00000001; 134 | for (int i = 0; i < n; i++) 135 | { 136 | cv::Point3d p = objectPoints[i]; 137 | objectPoints_x.push_back(cv::Point3d(p.x + delta, p.y, p.z)); 138 | objectPoints_y.push_back(cv::Point3d(p.x, p.y + delta, p.z)); 139 | objectPoints_z.push_back(cv::Point3d(p.x, p.y, p.z + delta)); 140 | } 141 | cv::projectPoints(objectPoints_x, rVecR, tVec, K, distCoeffs, projectedPoints_x); 142 | cv::projectPoints(objectPoints_y, rVecR, tVec, K, distCoeffs, projectedPoints_y); 143 | cv::projectPoints(objectPoints_z, rVecR, tVec, K, distCoeffs, projectedPoints_z); 144 | 145 | MatrixXd jacobianPoints_x = (cv2eigen(projectedPoints_x) - cv2eigen(projectedPoints)) / delta; 146 | MatrixXd jacobianPoints_y = (cv2eigen(projectedPoints_y) - cv2eigen(projectedPoints)) / delta; 147 | MatrixXd jacobianPoints_z = (cv2eigen(projectedPoints_z) - cv2eigen(projectedPoints)) / delta; 148 | 149 | Map jacobianPointsCol_x(jacobianPoints_x.data(),jacobianPoints_x.size()); 150 | Map jacobianPointsCol_y(jacobianPoints_y.data(),jacobianPoints_y.size()); 151 | Map jacobianPointsCol_z(jacobianPoints_z.data(),jacobianPoints_z.size()); 152 | 153 | MatrixX3d jacobianPoints(2 * n, 3); 154 | 155 | jacobianPoints << jacobianPointsCol_x, jacobianPointsCol_y, jacobianPointsCol_z; 156 | 157 | ASSERT_LT((jacobianPoints - ourJacobianPoints).norm(), 1); 158 | } 159 | 160 | TEST(CameraTest, testTriangluateFromTrajectory) 161 | { 162 | int N = 4; 163 | Matrix4Xd CG_q(4, N); 164 | Matrix3Xd G_p_C(3, N); 165 | Matrix3Xd C_p_f(3, N); 166 | Matrix2Xd z(2, N); 167 | 168 | for (int i = 0; i < N; i++) 169 | { 170 | generator->setTime((double) i / 4); 171 | 172 | Quaterniond IiG_q = Quaterniond(generator->getRotation()); 173 | Quaterniond CiG_q = IiG_q * calib->CI_q; 174 | // Calculate camera state 175 | Vector3d G_p_Ii = generator->getPosition(); 176 | Vector3d G_p_Ci = G_p_Ii - CiG_q * calib->C_p_I; 177 | 178 | G_p_C.col(i) = G_p_Ci; 179 | CG_q.col(i) = CiG_q.coeffs(); 180 | } 181 | generator->setTime(0); 182 | 183 | bool valid; 184 | do 185 | { 186 | valid = true; 187 | Vector3d G_p_f = generator->generatePoint(); 188 | G_p_f = Vector3d(19.000000, 29.000000, 20.000000); 189 | for (int i = 0; i < N && valid; i++) 190 | { 191 | Quaterniond CiG_q = Quaterniond(CG_q.col(i)); 192 | Vector3d G_p_Ci = G_p_C.col(i); 193 | Vector3d Ci_p_f = CiG_q.inverse() * (G_p_f - G_p_Ci); 194 | Vector2d Ci_z_f = calib->camera.cameraProject(Ci_p_f); 195 | 196 | 197 | if (abs(atan2(Ci_p_f(0), Ci_p_f(2))) <= M_PI * DataGenerator::FOV / 2 / 180 198 | && abs(atan2(Ci_p_f(1), Ci_p_f(2))) <= M_PI * DataGenerator::FOV / 2 / 180 199 | && Ci_p_f(2) > 0.1 200 | && 0 < Ci_z_f(0) && Ci_z_f(0) < DataGenerator::COL 201 | && 0 < Ci_z_f(1) && Ci_z_f(1) < DataGenerator::ROW) 202 | { 203 | CG_q.col(i) = CiG_q.coeffs(); 204 | G_p_C.col(i) = G_p_Ci; 205 | C_p_f.col(i) = Ci_p_f; 206 | z.col(i) = Ci_z_f; 207 | } 208 | else 209 | { 210 | cout << "wrong G_p_f" << Ci_p_f.transpose() << "z_f" << Ci_z_f.transpose() << endl; 211 | cout << abs(atan2(Ci_p_f(0), Ci_p_f(2))) * 180 / M_PI * 2 << endl; 212 | cout << abs(atan2(Ci_p_f(1), Ci_p_f(2))) * 180 / M_PI * 2 << endl; 213 | 214 | valid = false; 215 | } 216 | } 217 | if (valid) 218 | { 219 | cout << "G_p_f:" << G_p_f.transpose() << endl; 220 | Vector3d G_p_f_hat = calib->camera.triangulate(CG_q, G_p_C, z); 221 | 222 | cout << ">>> G_p_f_hat:" << G_p_f_hat.transpose() << endl; 223 | } 224 | } 225 | while (!valid); 226 | } 227 | 228 | TEST(CameraTest, DISABLED_testTriangluate) 229 | { 230 | generator->setTime(0); 231 | int testNum = 100; 232 | VectorXd error(testNum); 233 | while (testNum--) 234 | { 235 | Vector3d G_p_f = generator->generatePoint(); 236 | int N = 10; 237 | Matrix4Xd CG_q(4, N); 238 | Matrix3Xd G_p_C(3, N); 239 | Matrix3Xd C_p_f(3, N); 240 | Matrix2Xd z(2, N); 241 | 242 | int n = 0; 243 | while (n < N) 244 | { 245 | Quaterniond CiG_q = Quaterniond(rand() % 10 - 5, rand() % 10 - 5, rand() % 10 - 5, rand() % 10 - 5); 246 | CiG_q.normalize(); 247 | Vector3d G_p_Ci = generator->generatePoint(); 248 | Vector3d Ci_p_f = CiG_q.inverse() * (G_p_f - G_p_Ci); 249 | Vector2d Ci_z_f = calib->camera.cameraProject(Ci_p_f); 250 | 251 | if (abs(atan2(Ci_p_f(0), Ci_p_f(2))) <= M_PI * DataGenerator::FOV / 2 / 180 252 | && abs(atan2(Ci_p_f(1), Ci_p_f(2))) <= M_PI * DataGenerator::FOV / 2 / 180 253 | && Ci_p_f(2) > 0.1 254 | && 0 < Ci_z_f(0) && Ci_z_f(0) < DataGenerator::COL 255 | && 0 < Ci_z_f(1) && Ci_z_f(1) < DataGenerator::ROW) 256 | { 257 | CG_q.col(n) = CiG_q.coeffs(); 258 | G_p_C.col(n) = G_p_Ci; 259 | C_p_f.col(n) = Ci_p_f; 260 | z.col(n) = Ci_z_f; 261 | n++; 262 | } 263 | } 264 | // cout << "=======" << endl; 265 | // cout << "CG_q:" << endl << CG_q << endl; 266 | // cout << "G_p_C:" << endl << G_p_C << endl; 267 | // cout << "C_p_f:" << endl << C_p_f << endl; 268 | // cout << "G_z_f:" << endl << z << endl; 269 | //cout << "G_p_f:" << G_p_f.transpose() << endl; 270 | 271 | Vector3d G_p_f_hat = calib->camera.triangulate(CG_q, G_p_C, z); 272 | //cout << ">>> G_p_f_hat:" << G_p_f_hat.transpose() << endl; 273 | 274 | error[testNum] = (G_p_f - G_p_f_hat).norm(); 275 | } 276 | //cout << "error: " << error.mean() << endl; 277 | ASSERT_LT(error.mean(), 0.5); 278 | } 279 | 280 | int main(int argc, char **argv) { 281 | testing::InitGoogleTest(&argc, argv); 282 | setup(); 283 | 284 | return RUN_ALL_TESTS(); 285 | } 286 | 287 | -------------------------------------------------------------------------------- /dataset/2015-02-05-13-44-35.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/2015-02-05-13-44-35.bag -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata.tar.gz -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0000.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0001.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0002.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0003.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0004.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0005.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0006.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0007.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0007.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0008.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0008.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0009.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0009.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0010.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0010.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0011.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0011.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0012.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0012.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0013.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0013.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0014.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0014.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0015.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0015.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0016.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0016.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0017.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0017.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0018.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0018.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0019.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0020.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0020.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0021.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0021.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0022.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0022.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0023.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0023.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0024.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0025.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0025.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0026.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0026.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0027.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0027.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0028.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0028.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0029.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0029.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0030.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0030.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0031.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0031.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0032.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0032.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0033.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0033.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0034.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0034.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0035.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0035.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0036.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0036.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0037.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0037.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0038.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0038.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0039.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0039.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0040.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0040.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0041.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0041.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0042.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0042.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0043.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0043.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0044.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0044.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0045.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0045.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0046.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0046.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0047.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0047.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0048.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0048.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0049.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0049.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0050.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0050.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0051.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0051.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0052.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0052.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0053.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0053.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0054.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0054.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0055.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0055.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0056.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0056.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0057.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0057.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0058.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0058.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0059.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0059.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0060.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0060.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0061.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0061.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0062.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0062.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0063.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0063.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0064.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0064.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0065.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0065.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0066.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0066.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0067.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0067.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0068.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0068.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/left-0069.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/my_VINS/6d5c9ebee1f5d982a9c42114bc3fd693f667a95f/dataset/calib_data/calibrationdata/left-0069.png -------------------------------------------------------------------------------- /dataset/calib_data/calibrationdata/ost.txt: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 752 8 | 9 | height 10 | 480 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 367.138488 0.000000 374.908081 16 | 0.000000 367.787173 254.282369 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.250538 0.046181 0.000083 -0.000928 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 249.522949 0.000000 373.123667 0.000000 29 | 0.000000 320.282593 258.620845 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /dataset/calib_data/image_list.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | calibrationdata/left-0000.png 5 | calibrationdata/left-0001.png 6 | calibrationdata/left-0002.png 7 | calibrationdata/left-0003.png 8 | calibrationdata/left-0004.png 9 | calibrationdata/left-0005.png 10 | calibrationdata/left-0006.png 11 | calibrationdata/left-0007.png 12 | calibrationdata/left-0008.png 13 | calibrationdata/left-0009.png 14 | calibrationdata/left-0010.png 15 | calibrationdata/left-0011.png 16 | calibrationdata/left-0012.png 17 | calibrationdata/left-0013.png 18 | calibrationdata/left-0014.png 19 | calibrationdata/left-0015.png 20 | calibrationdata/left-0016.png 21 | calibrationdata/left-0017.png 22 | calibrationdata/left-0018.png 23 | calibrationdata/left-0019.png 24 | calibrationdata/left-0020.png 25 | calibrationdata/left-0021.png 26 | calibrationdata/left-0022.png 27 | calibrationdata/left-0023.png 28 | calibrationdata/left-0024.png 29 | calibrationdata/left-0025.png 30 | calibrationdata/left-0026.png 31 | calibrationdata/left-0027.png 32 | calibrationdata/left-0028.png 33 | calibrationdata/left-0029.png 34 | calibrationdata/left-0030.png 35 | calibrationdata/left-0031.png 36 | calibrationdata/left-0032.png 37 | calibrationdata/left-0033.png 38 | calibrationdata/left-0034.png 39 | calibrationdata/left-0035.png 40 | calibrationdata/left-0036.png 41 | calibrationdata/left-0037.png 42 | calibrationdata/left-0038.png 43 | calibrationdata/left-0039.png 44 | calibrationdata/left-0040.png 45 | calibrationdata/left-0041.png 46 | calibrationdata/left-0042.png 47 | calibrationdata/left-0043.png 48 | calibrationdata/left-0044.png 49 | calibrationdata/left-0045.png 50 | calibrationdata/left-0046.png 51 | calibrationdata/left-0047.png 52 | calibrationdata/left-0048.png 53 | calibrationdata/left-0049.png 54 | calibrationdata/left-0050.png 55 | calibrationdata/left-0051.png 56 | calibrationdata/left-0052.png 57 | calibrationdata/left-0053.png 58 | calibrationdata/left-0054.png 59 | calibrationdata/left-0055.png 60 | calibrationdata/left-0056.png 61 | calibrationdata/left-0057.png 62 | calibrationdata/left-0058.png 63 | calibrationdata/left-0059.png 64 | calibrationdata/left-0060.png 65 | calibrationdata/left-0061.png 66 | calibrationdata/left-0062.png 67 | calibrationdata/left-0063.png 68 | calibrationdata/left-0064.png 69 | calibrationdata/left-0065.png 70 | calibrationdata/left-0066.png 71 | calibrationdata/left-0067.png 72 | calibrationdata/left-0068.png 73 | calibrationdata/left-0069.png 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /msckf_vins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(msckf_vins) 3 | 4 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 5 | set(CMAKE_CXX_FLAGS "-DNDEBUG -std=c++11 -march=native -O3 -Wall") 6 | #SET(CMAKE_BUILD_TYPE Debug) 7 | 8 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs nav_msgs cv_bridge tf ) 9 | FIND_PACKAGE(Eigen REQUIRED) 10 | 11 | catkin_package( 12 | # INCLUDE_DIRS include 13 | # LIBRARIES msckf_vins 14 | # CATKIN_DEPENDS other_catkin_pkg 15 | # DEPENDS system_lib 16 | ) 17 | 18 | include_directories( 19 | include 20 | ${Eigen_INCLUDE_DIRS} 21 | ) 22 | 23 | add_executable(msckf_vins_node 24 | src/main.cpp 25 | src/Camera.cpp 26 | src/math_tool.cpp 27 | src/MSCKF.cpp 28 | src/data_generator.cpp 29 | ) 30 | 31 | target_link_libraries(msckf_vins_node 32 | ${catkin_LIBRARIES} 33 | ) 34 | -------------------------------------------------------------------------------- /msckf_vins/CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /msckf_vins/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /msckf_vins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | msckf_vins 4 | 0.0.0 5 | The msckf_vins package 6 | 7 | 8 | 9 | 10 | paulyang 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 | catkin 43 | roscpp 44 | roscpp 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /msckf_vins/src/Camera.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Camera.cpp 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 18/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #include 10 | #include 11 | #include "Camera.h" 12 | #include "math_tool.h" 13 | using namespace std; 14 | using namespace Eigen; 15 | DistortCamera::DistortCamera() 16 | { 17 | optical = Vector2d::Zero(2,1); 18 | focusMtx = Matrix2d::Zero(2,2); 19 | } 20 | 21 | void DistortCamera::setImageSize(double _height, double _width) 22 | { 23 | width = _width; 24 | nCols = _width; 25 | 26 | height = _height; 27 | nRows = _height; 28 | } 29 | void DistortCamera::setIntrinsicMtx(double _fx, double _fy, double _ox, double _oy) 30 | { 31 | fx = _fx; 32 | fy = _fy; 33 | ox = _ox; 34 | oy = _oy; 35 | 36 | optical(0) = ox; 37 | optical(1) = oy; 38 | 39 | focusMtx(0,0) = fx; 40 | focusMtx(1,1) = fy; 41 | } 42 | void DistortCamera::setDistortionParam(double _k1, double _k2, double _p1, double _p2, double _k3) 43 | { 44 | k1 = _k1; 45 | k2 = _k2; 46 | k3 = _k3; 47 | p1 = _p1; 48 | p2 = _p2; 49 | } 50 | 51 | Vector2d DistortCamera::h(Vector3d ptr) 52 | { 53 | Vector2d z, dt, uv; 54 | double u, v, r, dr; 55 | u = ptr(0)/ptr(2); 56 | v = ptr(1)/ptr(2); 57 | 58 | r = u*u + v*v; 59 | dr = 1 + k1*r + k2 * r* r + k3* r * r * r; 60 | 61 | dt(0) = 2*u*v*p1+(r+2*u*u)*p2; 62 | dt(1) = 2*u*v*p2+(r+2*v*v)*p1; 63 | 64 | uv(0) = u; 65 | uv(1) = v; 66 | z = optical + focusMtx*(dr * uv + dt); 67 | 68 | // cout << "dr is" << dr << endl; 69 | // cout << "dt is" << dt << endl; 70 | // cout << p1 << endl; 71 | // cout << p2 << endl; 72 | // cout << u << endl; 73 | // cout << v << endl; 74 | return z; 75 | } 76 | 77 | 78 | MatrixXd DistortCamera::Jh(Vector3d ptr) 79 | { 80 | Vector2d dt; 81 | Matrix2d focus; 82 | MatrixXd J(2,3); 83 | MatrixXd Jdistort(2,3); 84 | double x, y, z, u, v, r, dr, x2, y2, z2, z3; 85 | double ddrdx, ddrdy, ddrdz; 86 | Vector2d ddtdx, ddtdy, ddtdz; 87 | 88 | focus(0,0) = fx; 89 | focus(1,1) = fy; 90 | 91 | x = ptr(0); 92 | y = ptr(1); 93 | z = ptr(2); 94 | x2 = x*x; 95 | y2 = y*y; 96 | z2 = z*z; 97 | z3 = z*z*z; 98 | 99 | u = x/z; 100 | v = y/z; 101 | 102 | r = u*u + v*v; 103 | 104 | dr = 1 + k1*r + k2 * r * r + k3 * r * r * r; 105 | 106 | dt(0) = 2*u*v*p1+(r+2*u*u)*p2; 107 | dt(1) = 2*u*v*p2+(r+2*v*v)*p1; 108 | 109 | Vector3d Jdr = 2 / z * (k1 + 2 * k2 * r + 3 * k3 * r * r) * Vector3d(u, v, -r); 110 | 111 | ddtdx(0) = (6*p2*x)/z2 + (2*p1*y)/z2; 112 | ddtdx(1) = (2*p1*x)/z2 + (2*p2*y)/z2; 113 | ddtdy(0) = (2*p1*x)/z2 + (2*p2*y)/z2; 114 | ddtdy(1) = (2*p2*x)/z2 + (6*p1*y)/z2; 115 | ddtdz(0) = - p2*((6*x2)/z3 + (2*y2)/z3) - (4*p1*x*y)/z3; 116 | ddtdz(1) = - p1*((2*x2)/z3 + (6*y2)/z3) - (4*p2*x*y)/z3; 117 | 118 | 119 | Jdistort = MatrixXd::Zero(2, 3); 120 | 121 | Jdistort(0,0) = dr/z + Jdr(0)*u + ddtdx(0); 122 | Jdistort(0,1) = Jdr(1)*u + ddtdy(0); 123 | Jdistort(0,2) = -dr/z*u + Jdr(2)*u + ddtdz(0); 124 | Jdistort(1,0) = Jdr(0)*v + ddtdx(1); 125 | Jdistort(1,1) = dr/z + Jdr(1)*v + ddtdy(1); 126 | Jdistort(1,2) = -dr/z*v + Jdr(2)*v + ddtdz(1); 127 | 128 | J = focus * Jdistort; 129 | 130 | return J; 131 | } 132 | 133 | // measure_mtx is 2f 134 | Vector3d DistortCamera::triangulate(MatrixXd measure_mtx, MatrixXd pose_mtx) 135 | { 136 | // std::cout << "measure_mtx is " << std::endl << measure_mtx << std::endl; 137 | // std::cout << "pose_mtx is " << std::endl<< pose_mtx << std::endl; 138 | //std::cout << std::endl; 139 | Vector3d return_pose = Vector3d(0.0f, 0.0f, 0.0f); 140 | int num_item = (int)pose_mtx.cols(); 141 | 142 | MatrixXd q_list = MatrixXd::Zero(4, num_item); 143 | MatrixXd t_list = MatrixXd::Zero(3, num_item); 144 | 145 | Matrix3d R_wc0 = quaternion_to_R(pose_mtx.block<4,1>(0,0)); 146 | Vector3d t_wc0 = pose_mtx.block<3,1>(4,0); 147 | 148 | Matrix3d R_wci = Matrix3d::Identity(3, 3); 149 | Vector3d t_wci = Vector3d::Zero(3, 1); 150 | Matrix3d R_c0ci, R_cic0; 151 | Vector3d t_c0ci, t_cic0; 152 | 153 | q_list.col(0) = Vector4d(0,0,0,1); 154 | t_list.col(0) = t_wci; 155 | 156 | // TODO: rewrite this piece use quaternion 157 | for (int i=1; i(0,i)); 160 | t_wci = pose_mtx.block<3,1>(4,i); 161 | 162 | R_c0ci = R_wc0.transpose()*R_wci; 163 | t_c0ci = R_wc0.transpose()*(t_wci - t_wc0); 164 | R_cic0 = R_c0ci.transpose(); 165 | t_cic0 = - R_c0ci.transpose()*t_c0ci; 166 | 167 | q_list.col(i) = R_to_quaternion(R_cic0); 168 | t_list.col(i) = t_cic0; 169 | } 170 | 171 | // cout << "q_list is" << endl << q_list << endl; 172 | // cout << "t_list is" << endl << t_list << endl; 173 | 174 | // obtain init estimation 175 | Vector2d mtx_A, mtx_B; 176 | Vector3d ptr_i, ptr_j, ti, tj, guess; 177 | MatrixXd nK = MatrixXd::Zero(2,3); 178 | Matrix3d R_wbi, R_wbj; 179 | double depth; 180 | 181 | ptr_i(0) = (measure_mtx(0,0)-ox)/fx; 182 | ptr_i(1) = (measure_mtx(1,0)-oy)/fy; 183 | ptr_i(2) = 1; 184 | ptr_j(0) = (measure_mtx(0,1)-ox)/fx; 185 | ptr_j(1) = (measure_mtx(1,1)-oy)/fy; 186 | ptr_j(2) = 1; 187 | 188 | R_wbi = quaternion_to_R(pose_mtx.block<4,1>(0,0)); 189 | R_wbj = quaternion_to_R(pose_mtx.block<4,1>(0,1)); 190 | ti = pose_mtx.block<3,1>(4,0); 191 | tj = pose_mtx.block<3,1>(4,1); 192 | 193 | nK(0,0) = 1; nK(1,1) = 1; 194 | nK(0,2) = - ptr_i(0); 195 | nK(1,2) = - ptr_i(1); 196 | 197 | mtx_A = nK*R_wbi.transpose()*R_wbj*ptr_j; 198 | mtx_B = nK*R_wbi.transpose()*(ti - tj); 199 | 200 | depth = (mtx_B(0)*mtx_A(0)+mtx_B(1)*mtx_A(1))/(mtx_A(0)*mtx_A(0)+mtx_A(1)*mtx_A(1)); 201 | guess = R_wbi.transpose()*R_wbj*depth*ptr_j; 202 | 203 | 204 | //Vector3d theta = Vector3d(1.0f, 1.0f, 1.0f); 205 | Vector3d theta = Vector3d(guess(0)/guess(2), guess(1)/guess(2), 1.0/guess(2)); 206 | 207 | Vector3d g_ptr = Vector3d(0.0f, 0.0f, 0.0f); 208 | VectorXd f = VectorXd::Zero(num_item*2); 209 | MatrixXd J = MatrixXd::Zero(num_item*2,3); 210 | Matrix3d Jg = Matrix3d::Zero(3,3); 211 | MatrixXd Ji; 212 | MatrixXd A; 213 | MatrixXd b; 214 | for (int itr = 0; itr < 100; itr++) 215 | { 216 | Vector3d tmp_theta = Vector3d(theta(0), theta(1), 1); 217 | for (int i = 0; i < num_item; i++) 218 | { 219 | R_cic0 = quaternion_to_R(q_list.col(i)); 220 | g_ptr = R_cic0*tmp_theta + theta(2)*t_list.col(i); 221 | 222 | f.segment(i*2, 2) = measure_mtx.col(i) - h(g_ptr); 223 | 224 | Jg.col(0) = R_cic0.col(0); 225 | Jg.col(1) = R_cic0.col(1); 226 | Jg.col(2) = t_list.col(i); 227 | 228 | Ji = -Jh(g_ptr) * Jg; 229 | J.block<2,3>(i*2,0) = Ji; 230 | } 231 | 232 | if (f.norm() < 1.0f) 233 | { 234 | break; 235 | } 236 | A = J.transpose()*J; 237 | b = J.transpose()*f; 238 | 239 | 240 | // cout << "f is" << endl << f << endl; 241 | // cout << "J is" << endl << J << endl; 242 | // cout << "solve is" << endl << A.ldlt().solve(b) << endl; 243 | 244 | //theta = theta - A.ldlt().solve(b); 245 | theta = theta - A.colPivHouseholderQr().solve(b); 246 | 247 | } 248 | 249 | return_pose(0) = theta(0)/theta(2); 250 | return_pose(1) = theta(1)/theta(2); 251 | return_pose(2) = 1.0f/theta(2); 252 | 253 | return_pose = R_wc0*return_pose+t_wc0; 254 | 255 | return return_pose; 256 | 257 | } 258 | 259 | 260 | 261 | -------------------------------------------------------------------------------- /msckf_vins/src/Camera.h: -------------------------------------------------------------------------------- 1 | // 2 | // Camera.h 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 18/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #ifndef MyTriangulation_Camera_h 10 | #define MyTriangulation_Camera_h 11 | #include 12 | 13 | class DistortCamera { 14 | int nRows; 15 | int nCols; 16 | 17 | double fx; 18 | double fy; 19 | 20 | double ox; 21 | double oy; 22 | 23 | // distortion parameters, opencv k1, k2, p1, p2, k3 24 | double k1; 25 | double k2; 26 | double p1; 27 | double p2; 28 | double k3; 29 | 30 | Eigen::Vector2d optical; 31 | Eigen::Matrix2d focusMtx; 32 | 33 | public: 34 | int width; 35 | int height; 36 | 37 | DistortCamera(); 38 | void setImageSize(double _height, double _width); 39 | void setIntrinsicMtx(double _fx, double _fy, double _ox, double _oy); 40 | void setDistortionParam(double _k1, double _k2, double _p1, double _p2, double _k3); 41 | 42 | Eigen::Vector2d h(Eigen::Vector3d ptr); 43 | 44 | Eigen::MatrixXd Jh(Eigen::Vector3d ptr); 45 | 46 | Eigen::Vector3d triangulate(Eigen::MatrixXd measure, Eigen::MatrixXd pose); 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /msckf_vins/src/FeatureRecord.h: -------------------------------------------------------------------------------- 1 | // 2 | // FeatureManager.h 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 26/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #ifndef __MyTriangulation__FeatureRecord__ 10 | #define __MyTriangulation__FeatureRecord__ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | using namespace std; 18 | 19 | #include 20 | using namespace Eigen; 21 | 22 | class FeatureInformation 23 | { 24 | public: 25 | Vector3d point; 26 | FeatureInformation(const Vector3d &_point): point(_point) 27 | {} 28 | }; 29 | 30 | class FeatureRecord 31 | { 32 | public: 33 | vector feature_points; 34 | 35 | bool is_used; 36 | bool is_lost; 37 | bool is_outlier; 38 | int start_frame; 39 | 40 | FeatureRecord() 41 | { 42 | start_frame = -1; 43 | is_used = false; 44 | is_lost = false; 45 | is_outlier = false; 46 | } 47 | 48 | 49 | FeatureRecord(int _start_frame, const Vector3d &_feature_point): 50 | feature_points {FeatureInformation(_feature_point)} 51 | { 52 | start_frame = _start_frame; 53 | is_used = false; 54 | is_lost = false; 55 | is_outlier = false; 56 | } 57 | }; 58 | 59 | 60 | #endif /* defined(__MyTriangulation__FeatureRecord__) */ 61 | -------------------------------------------------------------------------------- /msckf_vins/src/MSCKF.h: -------------------------------------------------------------------------------- 1 | // 2 | // MSCKF.h 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 18/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #ifndef __MyTriangulation__MSCKF__ 10 | #define __MyTriangulation__MSCKF__ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | //#include 22 | //#include 23 | 24 | using namespace Eigen; 25 | using namespace std; 26 | 27 | #include "FeatureRecord.h" 28 | #include "Camera.h" 29 | 30 | struct SlideState 31 | { 32 | Vector4d q; 33 | Vector3d p; 34 | Vector3d v; 35 | }; 36 | 37 | // chi-square test critical value table 38 | const double chi_test[] = 39 | { 40 | 0, // 0 41 | 0.004, // 1 42 | 0.103, // 2 43 | 0.352, 44 | 0.711, 45 | 1.145, 46 | 1.635, 47 | 2.167, 48 | 2.733, 49 | 3.325, 50 | 3.940, 51 | 4.575, 52 | 5.226, 53 | 5.892, 54 | 6.571, 55 | 7.261, 56 | 7.962, 57 | 8.672, 58 | 9.390, 59 | 10.117, 60 | 10.851, 61 | 11.591, 62 | 12.338, 63 | 13.091, 64 | 13.848, 65 | 14.611, 66 | 15.379, 67 | 16.151, 68 | 16.928, 69 | 17.708, 70 | 18.493 71 | }; 72 | 73 | class MSCKF 74 | { 75 | private: 76 | /* states */ 77 | // VectorXd nominalState; // dimension 4 + 3 + 3 + 3 + 3 = 16 78 | // VectorXd errorState; // dimension 3 + 3 + 3 + 3 + 3 = 15 79 | // VectorXd extrinsicP; // p_bc, dimension 3 80 | 81 | VectorXd fullNominalState; 82 | // VectorXd fullErrorState; 83 | 84 | list slidingWindow; 85 | 86 | /* covariance */ 87 | MatrixXd errorCovariance; 88 | MatrixXd fullErrorCovariance; 89 | MatrixXd phi; 90 | 91 | /* noise matrix */ 92 | MatrixXd Nc; 93 | double measure_noise; 94 | 95 | /* feature management */ 96 | map feature_record_dict; 97 | list residual_list; 98 | list H_mtx_list; 99 | list H_mtx_block_size_list; 100 | 101 | 102 | double current_time; // indicates the current time stamp 103 | int current_frame; // indicates the current frame in slidingWindow 104 | 105 | /* IMU measurements */ 106 | Vector3d prev_w, curr_w; 107 | Vector3d prev_a, curr_a; 108 | 109 | /* nominal state variables used only for calculation */ 110 | Vector4d spatial_quaternion; // q_gb 111 | Matrix3d spatial_rotation; // R_gb 112 | Vector3d spatial_position; 113 | Vector3d spatial_velocity; 114 | Vector3d gyro_bias; 115 | Vector3d acce_bias; 116 | 117 | 118 | 119 | /* fixed rotation between camera and the body frame */ 120 | Matrix3d R_cb; 121 | 122 | 123 | 124 | void correctNominalState(VectorXd delta); 125 | void addSlideState(); 126 | void removeSlideState(int index, int total); 127 | void addFeatures(const vector> &image); 128 | void removeFrameFeatures(int index); 129 | void removeUsedFeatures(); 130 | 131 | Vector2d projectPoint(Vector3d feature_pose, Matrix3d R_bg, Vector3d p_gb, Vector3d p_cb); 132 | bool getResidualH(VectorXd& ri, MatrixXd& Hi, Vector3d feature_pose, MatrixXd measure, MatrixXd pose_mtx, int frame_offset); 133 | 134 | public: 135 | /* camera */ 136 | DistortCamera cam; 137 | 138 | MSCKF(); 139 | ~MSCKF(); 140 | 141 | void resetError(); 142 | 143 | void processIMU(double t, Vector3d linear_acceleration, Vector3d angular_velocity); 144 | void processImage(const vector> &image); 145 | 146 | void setNominalState(Vector4d q, Vector3d p, Vector3d v, Vector3d bg, Vector3d ba); 147 | void setCalibParam(Vector3d p_cb, double fx, double fy, double ox, double oy, double k1, double k2, double p1, double p2, double k3); 148 | void setIMUCameraRotation(Matrix3d _R_cb); 149 | 150 | void setNoiseMatrix(double dgc, double dac, double dwgc, double dwac); 151 | void setMeasureNoise(double _noise); 152 | 153 | void setErrorCovarianceIdentity(); 154 | 155 | // test function... 156 | Vector2d projectCamPoint(Vector3d ptr); 157 | 158 | /* outputs */ 159 | Quaterniond getQuaternion(); 160 | Matrix3d getRotation(); 161 | Vector3d getPosition(); 162 | Vector3d getVelocity(); 163 | Vector3d getGyroBias(); 164 | Vector3d getAcceBias(); 165 | Vector3d getVIOffset(); 166 | 167 | 168 | /* debug outputs */ 169 | void printNominalState(bool is_full); 170 | void printErrorState(bool is_full); 171 | void printSlidingWindow(); 172 | void printErrorCovariance(bool is_full); 173 | 174 | map global_features; 175 | 176 | }; 177 | 178 | #endif /* defined(__MyTriangulation__MSCKF__) */ 179 | -------------------------------------------------------------------------------- /msckf_vins/src/data_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "data_generator.h" 2 | #define PI 3.1415926 3 | //#define WITH_NOISE 1 4 | #define COMPLEX_TRAJECTORY 1 5 | DataGenerator::DataGenerator() 6 | { 7 | srand(0); 8 | t = 0; 9 | current_id = 0; 10 | for (int i = 0; i < NUM_POINTS; i++) 11 | { 12 | pts[i](0) = rand() % (6 * MAX_BOX) - 3 * MAX_BOX; 13 | pts[i](1) = rand() % (6 * MAX_BOX) - 3 * MAX_BOX; 14 | pts[i](2) = rand() % (6 * MAX_BOX) - 3 * MAX_BOX; 15 | } 16 | R_cb << 0, -1, 0, 17 | 0, 0, 1, 18 | -1, 0, 0; 19 | //Tic << 4, 5, 6; 20 | p_cb << -0.14, -0.02, 0; 21 | 22 | //acc_cov << 1.3967e-04, 1.4357e-06, 2.1468e-06, 23 | // 1.4357e-06, 1.4352e-04, 5.7168e-05, 24 | // 2.1468e-06, 5.7168e-05, 1.5757e-04; 25 | //acc_cov << 1.3967e-04, 0, 0, 26 | // 0, 1.4352e-04, 0, 27 | // 0, 0, 1.5757e-04; 28 | acc_cov = Matrix3d::Identity(); 29 | gyr_cov = Matrix3d::Identity(); 30 | 31 | 32 | pts_cov << .1 * .1 / 3.6349576068362910e+02 / 3.6349576068362910e+02, 0, 33 | 0, .1 * .1 / 3.6356654972681025e+02 / 3.6356654972681025e+02; 34 | 35 | generator = default_random_engine(0); 36 | distribution = normal_distribution(0.0, 1); 37 | } 38 | 39 | 40 | 41 | void DataGenerator::update() 42 | { 43 | t += 1.0 / FREQ; 44 | } 45 | 46 | double DataGenerator::getTime() 47 | { 48 | return t; 49 | } 50 | 51 | Vector3d DataGenerator::getPoint(int i) 52 | { 53 | return pts[i]; 54 | } 55 | 56 | Vector3d DataGenerator::getPosition() 57 | { 58 | #ifdef COMPLEX_TRAJECTORY 59 | double x, y, z; 60 | if (t < MAX_TIME) 61 | { 62 | x = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI); 63 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2 * 2); 64 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2); 65 | } 66 | else if (t >= MAX_TIME && t < 2 * MAX_TIME) 67 | { 68 | x = MAX_BOX / 2.0 - MAX_BOX / 2.0; 69 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0; 70 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0; 71 | } 72 | else 73 | { 74 | double tt = t - 2 * MAX_TIME; 75 | x = -MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI); 76 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI * 2 * 2); 77 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI * 2); 78 | } 79 | #elif SIMPLE_TRAJECTORY 80 | double x, y, z; 81 | x = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI); 82 | y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2 * 2); 83 | z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * 2); 84 | #else 85 | double x = 10 * cos(t / 10); 86 | double y = 10 * sin(t / 10); 87 | double z = 3; 88 | #endif 89 | 90 | return Vector3d(x, y, z); 91 | } 92 | 93 | Matrix3d DataGenerator::getRotation() 94 | { 95 | #ifdef COMPLEX_TRAJECTORY 96 | return (AngleAxisd(30.0 / 180 * M_PI * sin(t / MAX_TIME * M_PI * 2), Vector3d::UnitX()) 97 | * AngleAxisd(40.0 / 180 * M_PI * sin(t / MAX_TIME * M_PI * 2), Vector3d::UnitY()) 98 | * AngleAxisd(0, Vector3d::UnitZ())).toRotationMatrix(); 99 | #elif SIMPLE_TRAJECTORY 100 | return (AngleAxisd(0.0 + M_PI * sin(t / 10), Vector3d::UnitY()) * AngleAxisd(0.0 + M_PI * sin(t / 10), Vector3d::UnitX())).toRotationMatrix(); 101 | #else 102 | return Matrix3d::Identity(); 103 | #endif 104 | } 105 | 106 | Vector3d DataGenerator::getAngularVelocity() 107 | { 108 | const double delta_t = 0.00001; 109 | Matrix3d rot = getRotation(); 110 | t += delta_t; 111 | Matrix3d drot = (getRotation() - rot) / delta_t; 112 | t -= delta_t; 113 | Matrix3d skew = rot.inverse() * drot; 114 | #ifdef WITH_NOISE 115 | Vector3d disturb = Vector3d(distribution(generator) * sqrt(gyr_cov(0, 0)), 116 | distribution(generator) * sqrt(gyr_cov(1, 1)), 117 | distribution(generator) * sqrt(gyr_cov(2, 2)) 118 | ); 119 | return disturb + Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0)); 120 | #else 121 | return Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0)); 122 | #endif 123 | } 124 | 125 | Vector3d DataGenerator::getVelocity() 126 | { 127 | #ifdef COMPLEX_TRAJECTORY 128 | double dx, dy, dz; 129 | if (t < MAX_TIME) 130 | { 131 | dx = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 132 | dy = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 133 | dz = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 134 | } 135 | else if (t >= MAX_TIME && t < 2 * MAX_TIME) 136 | { 137 | dx = 0.0; 138 | dy = 0.0; 139 | dz = 0.0; 140 | } 141 | else 142 | { 143 | double tt = t - 2 * MAX_TIME; 144 | dx = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 145 | dy = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 146 | dz = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 147 | } 148 | #elif SIMPLE_TRAJECTORY 149 | double dx, dy, dz; 150 | dx = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 151 | dy = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 152 | dz = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 153 | #else 154 | double dx = - sin(t / 10); 155 | double dy = cos(t / 10); 156 | double dz = 0; 157 | #endif 158 | return Vector3d(dx, dy, dz); 159 | } 160 | 161 | //I_am = GI_R(G_a - G_g), G_g = (0, 0, -9.8) 162 | Vector3d DataGenerator::getLinearAcceleration() 163 | { 164 | double ddx, ddy, ddz; 165 | if (t < MAX_TIME) 166 | { 167 | ddx = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 168 | ddy = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 169 | ddz = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 170 | } 171 | else if (t >= MAX_TIME && t < 2 * MAX_TIME) 172 | { 173 | ddx = 0.0; 174 | ddy = 0.0; 175 | ddz = 0.0; 176 | } 177 | else 178 | { 179 | double tt = t - 2 * MAX_TIME; 180 | ddx = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI); 181 | ddy = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2) * (1.0 / MAX_TIME * M_PI * 2 * 2); 182 | ddz = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2) * (1.0 / MAX_TIME * M_PI * 2); 183 | } 184 | #if WITH_NOISE 185 | Vector3d disturb = Vector3d(distribution(generator) * sqrt(acc_cov(0, 0)), 186 | distribution(generator) * sqrt(acc_cov(1, 1)), 187 | distribution(generator) * sqrt(acc_cov(2, 2)) 188 | ); 189 | return getRotation().inverse() * (disturb + Vector3d(ddx, ddy, ddz + 9.8)); 190 | #else 191 | return getRotation().inverse() * Vector3d(ddx, ddy, ddz + 9.8); 192 | #endif 193 | } 194 | 195 | vector> DataGenerator::getImage() 196 | { 197 | vector> image; 198 | Vector3d position = getPosition(); 199 | Matrix3d R_gb = getRotation(); //R_gb 200 | printf("===frame start===\n"); 201 | 202 | printf("max: %d\n", current_id); 203 | 204 | for (int i = 0; i < NUM_POINTS; i++) 205 | { 206 | Vector3d local_point = R_cb * R_gb.transpose() * (pts[i] - position) + p_cb; 207 | 208 | double xx = local_point(0); 209 | double yy = local_point(1); 210 | double zz = local_point(2); 211 | 212 | if (abs(atan2(xx, zz)) <= M_PI * FOV / 2 / 180 213 | && abs(atan2(yy, zz)) <= M_PI * FOV / 2 / 180 214 | && zz > 0) 215 | { 216 | //int n_id = before_feature_id.find(i) == before_feature_id.end() ? 217 | // current_id++ : before_feature_id[i]; 218 | int n_id = i; 219 | #if WITH_NOISE 220 | Vector3d disturb = Vector3d(distribution(generator) * sqrt(pts_cov(0, 0)) / zz / zz, 221 | distribution(generator) * sqrt(pts_cov(1, 1)) / zz / zz, 222 | 0 223 | ); 224 | image.push_back(make_pair(n_id, disturb + Vector3d(xx / zz, yy / zz, 1))); 225 | #else 226 | image.push_back(make_pair(n_id, local_point)); 227 | // printf("feature id: %d, p_gf: [%f, %f, %f], p_cf: [%f, %f, %f]\n", 228 | // n_id, pts[i](0), pts[i](1), pts[i](2), local_point(0), local_point(1), local_point(2)); 229 | 230 | #endif 231 | current_feature_id[i] = n_id; 232 | } 233 | } 234 | printf("===frame end===\n"); 235 | before_feature_id = current_feature_id; 236 | current_feature_id.clear(); 237 | sort(image.begin(), image.end(), [](const pair &a, 238 | const pair &b) 239 | { 240 | return a.first < b.first; 241 | }); 242 | return image; 243 | } 244 | 245 | vector DataGenerator::getCloud() 246 | { 247 | vector cloud; 248 | 249 | for (int i = 0; i < NUM_POINTS; i++) 250 | { 251 | cloud.push_back(pts[i]); 252 | } 253 | return cloud; 254 | } 255 | -------------------------------------------------------------------------------- /msckf_vins/src/data_generator.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace Eigen; 15 | 16 | class DataGenerator 17 | { 18 | public: 19 | DataGenerator(); 20 | void update(); 21 | 22 | double getTime(); 23 | 24 | Vector3d getPoint(int i); 25 | vector getCloud(); 26 | Vector3d getPosition(); 27 | Matrix3d getRotation(); 28 | Vector3d getVelocity(); 29 | 30 | Vector3d getAngularVelocity(); 31 | Vector3d getLinearAcceleration(); 32 | 33 | vector> getImage(); 34 | 35 | static int const NUM_POINTS = 2000; 36 | static int const MAX_BOX = 10; 37 | static int const RANGE1 = 15; 38 | static int const RANGE2 = 25; 39 | static int const MAX_HEIGHT = 5; 40 | static int const FREQ = 200; 41 | static int const IMU_PER_IMG = 50; 42 | static int const MAX_TIME = 10; 43 | static int const FOV = 90; 44 | private: 45 | Vector3d pts[NUM_POINTS]; 46 | double t; 47 | 48 | std::map before_feature_id; 49 | std::map current_feature_id; 50 | int current_id; 51 | 52 | Matrix3d R_cb; 53 | Vector3d p_cb; 54 | 55 | Matrix3d acc_cov, gyr_cov; 56 | Matrix2d pts_cov; 57 | default_random_engine generator; 58 | normal_distribution distribution; 59 | }; 60 | -------------------------------------------------------------------------------- /msckf_vins/src/g_param.h: -------------------------------------------------------------------------------- 1 | // 2 | // g_param.h 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 26/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #ifndef MyTriangulation_g_param_h 10 | #define MyTriangulation_g_param_h 11 | 12 | 13 | //#define DEBUG_FLAG 14 | 15 | #ifndef DEBUG_FLAG 16 | 17 | #define SLIDING_WINDOW_SIZE 10 18 | 19 | #define NOMINAL_STATE_SIZE 16 // 4 + 3 + 3 + 3 + 3 20 | #define ERROR_STATE_SIZE 15 // 3 + 3 + 3 + 3 + 3 21 | #define NOMINAL_POSE_STATE_SIZE 10 // 4 + 3 + 3 22 | #define ERROR_POSE_STATE_SIZE 9 // 3 + 3 + 3 23 | 24 | #else 25 | 26 | #define SLIDING_WINDOW_SIZE 2 27 | #define NOMINAL_STATE_SIZE 5 28 | #define ERROR_STATE_SIZE 4 29 | #define NOMINAL_POSE_STATE_SIZE 3 30 | #define ERROR_POSE_STATE_SIZE 2 31 | 32 | #endif 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /msckf_vins/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "data_generator.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "tic_toc.h" 21 | #include "MSCKF.h" 22 | #include "math_tool.h" 23 | using namespace std; 24 | 25 | const int ROW=480; 26 | const int COL=752; 27 | const double FOCAL_LENGTH = 365.1; 28 | 29 | 30 | queue imu_buf; 31 | 32 | MSCKF my_kf; 33 | 34 | // visualize results 35 | nav_msgs::Path path; 36 | double sum_of_path = 0.0; 37 | Vector3d last_path(0.0, 0.0, 0.0); 38 | Quaterniond curr_q; 39 | visualization_msgs::Marker path_line; 40 | ros::Publisher pub_odometry; 41 | ros::Publisher pub_path2, pub_path3, pub_path4; 42 | ros::Publisher pub_pose2, pub_pose3; 43 | 44 | void imu_callback(const sensor_msgs::Imu imu_msg) 45 | { 46 | imu_buf.push(imu_msg); 47 | } 48 | 49 | void send_imu(const sensor_msgs::Imu imu_msg) 50 | { 51 | double t = imu_msg.header.stamp.toSec(); 52 | 53 | //ROS_INFO("processing IMU data with stamp %lf", t); 54 | 55 | double dx = imu_msg.linear_acceleration.x; 56 | double dy = imu_msg.linear_acceleration.y; 57 | double dz = imu_msg.linear_acceleration.z; 58 | 59 | double rx = imu_msg.angular_velocity.x; 60 | double ry = imu_msg.angular_velocity.y; 61 | double rz = imu_msg.angular_velocity.z; 62 | 63 | my_kf.processIMU(t, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); 64 | } 65 | 66 | void image_callback(const sensor_msgs::PointCloud image_msg) 67 | { 68 | my_kf.setErrorCovarianceIdentity(); 69 | double t = image_msg.header.stamp.toSec(); 70 | if (imu_buf.empty() || t < imu_buf.front().header.stamp.toSec()) 71 | { 72 | ROS_ERROR("wait for imu data"); 73 | return; 74 | } 75 | TicToc t_s; 76 | while (!imu_buf.empty() && t >= imu_buf.front().header.stamp.toSec()) 77 | { 78 | send_imu(imu_buf.front()); 79 | imu_buf.pop(); 80 | } 81 | ROS_INFO("processing vision data with stamp %lf", t); 82 | vector> image; 83 | for (int i = 0; i < (int)image_msg.points.size(); i++) 84 | { 85 | int id = image_msg.channels[0].values[i]; 86 | double x = image_msg.points[i].x; 87 | double y = image_msg.points[i].y; 88 | double z = image_msg.points[i].z; 89 | Vector3d p_cf(x, y, z); 90 | //R_cb * R_gb.transpose() * (pts[i] - position) + p_cb 91 | Vector2d cam_ptr = my_kf.projectCamPoint(p_cf); 92 | //ROS_INFO("id %d cam pos (%f, %f, %f) project to (%f, %f)", id, x, y, z, cam_ptr(0), cam_ptr(1)); 93 | if(cam_ptr(0) > 0 && cam_ptr(0) < my_kf.cam.width && cam_ptr(1) > 0 && cam_ptr(1) < my_kf.cam.height) 94 | //image.push_back(make_pair(/*gr_id * 10000 + */id, Vector3d(cam_ptr(0), cam_ptr(1), 1))); 95 | // image.push_back(make_pair(/*gr_id * 10000 + */id, Vector3d(cam_ptr(0), cam_ptr(1), 1))); 96 | image.push_back(make_pair(/*gr_id * 10000 + */id, p_cf)); 97 | } 98 | 99 | my_kf.processImage(image); 100 | 101 | sum_of_path += (my_kf.getPosition() - last_path).norm(); 102 | last_path = my_kf.getPosition(); 103 | //Matrix3d Rota = my_kf.getRotation(); 104 | //curr_q = R_to_quaternion(Rota.transpose()); 105 | //cout << curr_q << endl; 106 | //cout << curr_q.norm() << endl; 107 | curr_q = my_kf.getQuaternion(); 108 | 109 | ROS_INFO("sum of path %lf", sum_of_path); 110 | ROS_INFO("vo solver costs: %lf ms", t_s.toc()); 111 | 112 | nav_msgs::Odometry odometry; 113 | odometry.header.stamp = ros::Time::now(); 114 | odometry.header.frame_id = "world"; 115 | odometry.pose.pose.position.x = last_path(0); 116 | odometry.pose.pose.position.y = last_path(1); 117 | odometry.pose.pose.position.z = last_path(2); 118 | odometry.pose.pose.orientation.x = curr_q.x(); 119 | odometry.pose.pose.orientation.y = curr_q.y(); 120 | odometry.pose.pose.orientation.z = curr_q.z(); 121 | odometry.pose.pose.orientation.w = curr_q.w(); 122 | // odometry.twist.twist.linear.x = solution.v(0); 123 | // odometry.twist.twist.linear.y = solution.v(1); 124 | // odometry.twist.twist.linear.z = solution.v(2); 125 | pub_odometry.publish(odometry); 126 | 127 | //fprintf(f, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", solution.p(0), solution.p(1), solution.p(2), 128 | // solution.v(0), solution.v(1), solution.v(2), 129 | // solution.q.x(), solution.q.y(), solution.q.z(), solution.q.w()); 130 | 131 | 132 | geometry_msgs::PoseStamped pose_stamped; 133 | pose_stamped.header.stamp = ros::Time::now(); 134 | pose_stamped.header.frame_id = "world"; 135 | pose_stamped.pose = odometry.pose.pose; 136 | path.poses.push_back(pose_stamped); 137 | pub_path2.publish(path); 138 | pub_pose2.publish(pose_stamped); 139 | 140 | geometry_msgs::Point pose_p; 141 | pose_p.x = last_path(0); 142 | pose_p.y = last_path(1); 143 | pose_p.z = last_path(2); 144 | 145 | path_line.points.push_back(pose_p); 146 | path_line.scale.x = 0.01; 147 | pub_path3.publish(path_line); 148 | path_line.scale.x = 0.5; 149 | pub_path4.publish(path_line); 150 | 151 | // broadcast tf 152 | 153 | static tf::TransformBroadcaster br; 154 | tf::Transform transform; 155 | //transform.setOrigin(tf::Vector3(last_path(0), last_path(1), last_path(2)) ); 156 | tf::Quaternion q; 157 | q.setW(curr_q.w()); 158 | q.setX(curr_q.x()); 159 | q.setY(curr_q.y()); 160 | q.setZ(curr_q.z()); 161 | transform.setRotation(q); 162 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "body")); 163 | 164 | // transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); 165 | // q.setEuler(0.0, M_PI, 0.0); 166 | // transform.setRotation(q); 167 | // br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "world_v")); 168 | // br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "body", "body_v")); 169 | 170 | } 171 | 172 | void cloud_callback(const sensor_msgs::PointCloud cloud_msg) 173 | { 174 | 175 | for (int i = 0; i < (int)cloud_msg.points.size(); i++) 176 | { 177 | int id = cloud_msg.channels[0].values[i]; 178 | double x = cloud_msg.points[i].x; 179 | double y = cloud_msg.points[i].y; 180 | double z = cloud_msg.points[i].z; 181 | Vector3d world_ptr(x, y, z); 182 | my_kf.global_features[id] = world_ptr; 183 | } 184 | 185 | } 186 | 187 | int main(int argc, char **argv) 188 | { 189 | ros::init(argc, argv, "msckf_vins"); 190 | ros::NodeHandle n("~"); 191 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 192 | 193 | // define pub topics 194 | ros::Publisher pub_imu = n.advertise("/imu_3dm_gx4/imu", 1000); 195 | ros::Publisher pub_image = n.advertise("/sensors/image", 1000); 196 | 197 | ros::Publisher pub_path = n.advertise("/simulation/path", 100); 198 | ros::Publisher pub_odometry = n.advertise("/simulation/odometry", 100); 199 | ros::Publisher pub_pose = n.advertise("/simulation/pose", 100); 200 | ros::Publisher pub_cloud = n.advertise("/simulation/cloud", 1000); 201 | pub_path2 = n.advertise("path", 1000); 202 | pub_path3 = n.advertise("path3", 1000); 203 | pub_path4 = n.advertise("path4", 1000); 204 | pub_odometry = n.advertise("odometry", 1000); 205 | pub_pose2 = n.advertise("pose", 1000); 206 | pub_pose3 = n.advertise("pose3", 1000); 207 | 208 | static tf::TransformBroadcaster br; 209 | tf::Transform transform; 210 | tf::Quaternion q; 211 | 212 | // transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); 213 | // q.setEuler(0.0, M_PI, 0.0); 214 | // transform.setRotation(q); 215 | // br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "world_v")); 216 | // br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "body", "body_v")); 217 | 218 | path_line.header.frame_id = "world"; 219 | path_line.header.stamp = ros::Time::now(); 220 | path_line.ns = "calibration"; 221 | path_line.action = visualization_msgs::Marker::ADD; 222 | path_line.pose.orientation.w = 1.0; 223 | path_line.type = visualization_msgs::Marker::LINE_STRIP; 224 | path_line.scale.x = 0.01; 225 | path_line.color.a = 1.0; 226 | path_line.color.r = 1.0; 227 | path_line.id = 1; 228 | path_line.points.push_back(geometry_msgs::Point()); 229 | pub_path2.publish(path_line); 230 | path_line.scale.x = 0.5; 231 | pub_path3.publish(path_line); 232 | 233 | path.header.frame_id = "world"; 234 | 235 | // init MSCKF 236 | bool inited = false; 237 | 238 | DataGenerator generator; 239 | ros::Rate loop_rate(generator.FREQ); 240 | int publish_count = 0; 241 | nav_msgs::Path path; 242 | path.header.frame_id = "world"; 243 | //prepare point cloud 244 | sensor_msgs::PointCloud point_cloud; 245 | point_cloud.header.frame_id = "world"; 246 | point_cloud.header.stamp = ros::Time(); 247 | sensor_msgs::ChannelFloat32 ids; 248 | int i = 0; 249 | for (auto & it : generator.getCloud()) 250 | { 251 | geometry_msgs::Point32 p; 252 | p.x = it(0); 253 | p.y = it(1); 254 | p.z = it(2); 255 | point_cloud.points.push_back(p); 256 | ids.values.push_back(i++); 257 | } 258 | point_cloud.channels.push_back(ids); 259 | pub_cloud.publish(point_cloud); 260 | 261 | while (ros::ok()) 262 | { 263 | double current_time = generator.getTime(); 264 | //ROS_INFO("time: %lf", current_time); 265 | 266 | //get generated data 267 | Vector3d position = generator.getPosition(); 268 | Vector3d velocity = generator.getVelocity(); 269 | Matrix3d rotation = generator.getRotation(); 270 | Quaterniond q(rotation); 271 | if (inited == false) 272 | { 273 | inited = true; 274 | Vector4d init_q(q.x(), q.y(), q.z(), q.w()); // x y z w 275 | Vector3d init_p(position(0), position(1), position(2)); 276 | Vector3d init_v(velocity(0), velocity(1), velocity(2)); 277 | Vector3d init_bg(0.0 ,0.0, 0.0); 278 | Vector3d init_ba(0.0 ,0.0, 0.0); 279 | Vector3d init_pcb(-0.14, -0.02, 0.0); 280 | my_kf.setNominalState(init_q, init_p, init_v, init_bg, init_ba); 281 | my_kf.setCalibParam(init_pcb, 365.07984, 365.12127, 381.0196, 254.4431, 282 | -2.842958e-1, 8.7155025e-2, -1.4602925e-4, -6.149638e-4, -1.218237e-2); 283 | 284 | my_kf.setMeasureNoise(10.0); 285 | my_kf.setNoiseMatrix(1.0, 3.5, 3.5, 1.0); 286 | } 287 | 288 | Vector3d linear_acceleration = generator.getLinearAcceleration(); 289 | Vector3d angular_velocity = generator.getAngularVelocity(); 290 | 291 | //publish visulization data 292 | nav_msgs::Odometry odometry; 293 | odometry.header.frame_id = "world"; 294 | odometry.header.stamp = ros::Time(current_time); 295 | odometry.pose.pose.position.x = position(0); 296 | odometry.pose.pose.position.y = position(1); 297 | odometry.pose.pose.position.z = position(2); 298 | odometry.pose.pose.orientation.x = q.x(); 299 | odometry.pose.pose.orientation.y = q.y(); 300 | odometry.pose.pose.orientation.z = q.z(); 301 | odometry.pose.pose.orientation.w = q.w(); 302 | odometry.twist.twist.linear.x = velocity(0); 303 | odometry.twist.twist.linear.y = velocity(1); 304 | odometry.twist.twist.linear.z = velocity(2); 305 | pub_odometry.publish(odometry); 306 | 307 | geometry_msgs::PoseStamped pose_stamped; 308 | pose_stamped.header.frame_id = "world"; 309 | pose_stamped.header.stamp = ros::Time(current_time); 310 | pose_stamped.pose = odometry.pose.pose; 311 | path.poses.push_back(pose_stamped); 312 | pub_path.publish(path); 313 | 314 | pub_pose.publish(pose_stamped); 315 | 316 | //publish imu data 317 | sensor_msgs::Imu imu; 318 | imu.header.frame_id = "world"; 319 | imu.header.stamp = ros::Time(current_time); 320 | imu.linear_acceleration.x = linear_acceleration(0); 321 | imu.linear_acceleration.y = linear_acceleration(1); 322 | imu.linear_acceleration.z = linear_acceleration(2); 323 | imu.angular_velocity.x = angular_velocity(0); 324 | imu.angular_velocity.y = angular_velocity(1); 325 | imu.angular_velocity.z = angular_velocity(2); 326 | imu.orientation.x = q.x(); 327 | imu.orientation.y = q.y(); 328 | imu.orientation.z = q.z(); 329 | imu.orientation.w = q.w(); 330 | imu_callback(imu); 331 | pub_imu.publish(imu); 332 | //printf("publish imu data with stamp %lf\n", imu.header.stamp.toSec()); 333 | cloud_callback(point_cloud); 334 | pub_cloud.publish(point_cloud); 335 | 336 | //publish image data 337 | if (publish_count % generator.IMU_PER_IMG == 0) 338 | { 339 | //publish image data 340 | 341 | sensor_msgs::PointCloud feature; 342 | sensor_msgs::ChannelFloat32 ids; 343 | sensor_msgs::ChannelFloat32 pixel; 344 | for (int i = 0; i < ROW; i++) 345 | for (int j = 0; j < COL; j++) 346 | pixel.values.push_back(255); 347 | feature.header.stamp = ros::Time(generator.getTime()); 348 | 349 | // printf("publish image data with stamp %lf\n", feature.header.stamp.toSec()); 350 | // printf("p_gb: [%f, %f, %f], R_gb: [%f, %f, %f, %f]\n", position(0), position(1), position(2), 351 | // q.x(), q.y(), q.z(), q.w()); 352 | 353 | cv::Mat simu_img(600, 600, CV_8UC3, cv::Scalar(0, 0, 0)); 354 | for (auto & id_pts : generator.getImage()) 355 | { 356 | int id = id_pts.first; 357 | geometry_msgs::Point32 p; 358 | p.x = id_pts.second(0); 359 | p.y = id_pts.second(1); 360 | p.z = id_pts.second(2); 361 | 362 | //project point 363 | //p.y /= p.x; 364 | //p.z /= p.x; 365 | //p.x = 1; 366 | 367 | feature.points.push_back(p); 368 | ids.values.push_back(id); 369 | 370 | p.x /= p.z; 371 | p.y /= p.z; 372 | 373 | char label[10]; 374 | sprintf(label, "%d", id); 375 | cv::putText(simu_img, label, cv::Point2d(p.y + 1, p.x + 1) * 0.5 * 600, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 376 | } 377 | feature.channels.push_back(ids); 378 | feature.channels.push_back(pixel); 379 | image_callback(feature); 380 | pub_image.publish(feature); 381 | // cv::imshow("camera image", simu_img); 382 | // cv::waitKey(100); 383 | //if (generator.getTime() > DataGenerator::MAX_TIME) 384 | // break; 385 | } 386 | 387 | //update work 388 | generator.update(); 389 | publish_count++; 390 | ros::spinOnce(); 391 | loop_rate.sleep(); 392 | } 393 | 394 | return 0; 395 | } 396 | 397 | 398 | -------------------------------------------------------------------------------- /msckf_vins/src/math_tool.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // math_tool.cpp 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 18/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #include "math_tool.h" 10 | 11 | /* my quaternion convention 12 | double w = nq(0); 13 | double x = nq(1); 14 | double y = nq(2); 15 | double z = nq(3); 16 | */ 17 | 18 | 19 | Matrix3d quaternion_to_R(const Vector4d& q) 20 | { 21 | return Quaterniond(q).matrix(); 22 | } 23 | 24 | Vector4d R_to_quaternion(const Matrix3d& R) 25 | { 26 | return Quaterniond(R).coeffs(); 27 | } 28 | 29 | 30 | Matrix3d skew_mtx(const Vector3d& w) 31 | { 32 | Matrix3d W; 33 | W << 0, -w(2), w(1), 34 | w(2), 0, -w(0), 35 | -w(1), w(0), 0; 36 | return W; 37 | } 38 | 39 | Matrix4d omega_mtx(const Vector3d& w) 40 | { 41 | Matrix4d omega; 42 | 43 | omega.block<3,3>(0,0) = - skew_mtx(w); 44 | omega.block<3,1>(0,3) = w; 45 | omega.block<1,3>(3,0) = - w.transpose(); 46 | omega(3,3) = 0.0f; 47 | return omega; 48 | } 49 | 50 | // shelley thesis 51 | // calculate small rotation using the fourth order Runge-Kutta method 52 | Quaterniond delta_quaternion(const Vector3d& w_prev, const Vector3d& w_curr, const double dt) 53 | { 54 | Vector4d q, q0, k1, k2, k3, k4; 55 | q0 << 0.0f, 0.0f, 0.0f, 1.0f; 56 | 57 | k1 = 0.5f * omega_mtx(w_prev) * q0; 58 | k2 = 0.5f * omega_mtx(0.5*(w_prev+w_curr)) * (q0 + 0.5*dt*k1); 59 | k3 = 0.5f * omega_mtx(0.5*(w_prev+w_curr)) * (q0 + 0.5*dt*k2); 60 | k4 = 0.5f * omega_mtx(w_curr) * (q0 + dt*k3); 61 | 62 | q = q0 + (dt*(k1+2*k2+2*k3+k4))/6.0f; 63 | 64 | q = q / q.norm(); 65 | 66 | return Quaterniond(q); 67 | } 68 | 69 | Vector4d quaternion_correct(Vector4d q, Vector3d d_theta) 70 | { 71 | Quaterniond qf(q); 72 | Quaterniond dq( 73 | 1, 74 | 0.5*d_theta(0), 75 | 0.5*d_theta(1), 76 | 0.5*d_theta(2) 77 | ); 78 | dq.w() = 1 - dq.vec().transpose() * dq.vec(); 79 | 80 | qf = (qf * dq).normalized(); 81 | return qf.coeffs(); 82 | } 83 | -------------------------------------------------------------------------------- /msckf_vins/src/math_tool.h: -------------------------------------------------------------------------------- 1 | // 2 | // math_tool.h 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 18/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #ifndef __MyTriangulation__math_tool__ 10 | #define __MyTriangulation__math_tool__ 11 | 12 | #include 13 | using namespace Eigen; 14 | Matrix3d quaternion_to_R(const Vector4d& q); 15 | Vector4d R_to_quaternion(const Matrix3d& R); 16 | Matrix3d skew_mtx(const Vector3d& w); 17 | Matrix4d omega_mtx(const Vector3d& w); 18 | Quaterniond delta_quaternion(const Vector3d& w_prev, const Vector3d& w_curr, const double dt); 19 | Vector4d quaternion_correct(Vector4d q, Vector3d d_theta); 20 | #endif /* defined(__MyTriangulation__math_tool__) */ 21 | -------------------------------------------------------------------------------- /msckf_vins/src/old_main.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // main.cpp 3 | // MyTriangulation 4 | // 5 | // Created by Yang Shuo on 18/3/15. 6 | // Copyright (c) 2015 Yang Shuo. All rights reserved. 7 | // 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | //#include 14 | //#include 15 | 16 | #include "Camera.h" 17 | #include "math_tool.h" 18 | #include "MSCKF.h" 19 | #include "FeatureRecord.h" 20 | using namespace Eigen; 21 | 22 | // global variable to test 23 | //FeatureManager myManager; 24 | MSCKF my_kf; 25 | void featureManagerTest(); 26 | void KFtest(); 27 | void test_March(); 28 | void test_Apr(); 29 | 30 | int main(int argc, char ** argv) { 31 | //ros::init(argc, argv, "msckf_vins"); 32 | //ros::NodeHandle n; 33 | //ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 34 | test_March(); 35 | //test_Apr(); 36 | return 0; 37 | } 38 | 39 | void test_Apr() 40 | { 41 | // do test here 42 | MatrixXd U(3,2); 43 | U << 0.388, 0.866, 44 | 0.712, -0.0634, 45 | -0.586, 0.496; 46 | Matrix2d S(2,2); 47 | S << 1.19, 0, 48 | 0, 0.899; 49 | Matrix2d V(2,2); 50 | V<< -0.183, 0.983, 51 | 0.983 , 0.183; 52 | cout << U*S*V.transpose()<> image; 74 | MatrixXd measure(2,5); 75 | MatrixXd pose(7,5); 76 | measure << 77 | 497.6229, 493.1491, 489.0450, 478.8504, 466.8726, 78 | 371.0596, 366.5853, 362.4808, 352.2850, 340.3059 79 | ; 80 | pose<< 81 | 0.777986724316206, 0.838539246498940, 0.775194258937213, 0.831502026804405, 0.770011014887736, 82 | 0.625646233952092, 0.489266453739099, 0.435311151332881, 0.225961749519687, 0.059184118324008, 83 | -0.057475618563915, -0.239729575410618, -0.457796966390153, -0.507489573463504, -0.635279684146888, 84 | 0, 0, 0, 0, 0, 85 | 1.098427340189829, 5.357568053111257, 9.092272676622571, 11.936962300957290, 13.613178885567471, 86 | 13.956842672263791, 12.934313455158014, 10.645683518400434, 7.314979906023285, 3.268235093982677, 87 | 1.414213562373095, -1.414213562373095, 1.414213562373095, -1.414213562373094, 1.414213562373095 88 | ; 89 | image.clear(); 90 | //0 91 | image.push_back(std::make_pair(5, Vector3d(measure(0,0), measure(1,0), 1))); 92 | my_kf.setNominalState(pose.block<4,1>(0,0), pose.block<3,1>(4,0), 93 | Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f)); 94 | my_kf.processImage(image); 95 | image.clear(); 96 | //1 97 | image.push_back(std::make_pair(5, Vector3d(measure(0,1), measure(1,1), 1))); 98 | my_kf.setNominalState(pose.block<4,1>(0,1), pose.block<3,1>(4,1), 99 | Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f)); 100 | my_kf.processImage(image); 101 | image.clear(); 102 | //2 103 | image.push_back(std::make_pair(5, Vector3d(measure(0,2), measure(1,2), 1))); 104 | my_kf.setNominalState(pose.block<4,1>(0,2), pose.block<3,1>(4,2), 105 | Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f)); 106 | my_kf.processImage(image); 107 | image.clear(); 108 | //3 109 | image.push_back(std::make_pair(5, Vector3d(measure(0,3), measure(1,3), 1))); 110 | my_kf.setNominalState(pose.block<4,1>(0,3), pose.block<3,1>(4,3), 111 | Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f)); 112 | my_kf.processImage(image); 113 | image.clear(); 114 | //4 115 | image.push_back(std::make_pair(5, Vector3d(measure(0,4), measure(1,4), 1))); 116 | my_kf.setNominalState(pose.block<4,1>(0,4), pose.block<3,1>(4,4), 117 | Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f), Vector3d(0.0f, 0.0f, 0.0f)); 118 | my_kf.processImage(image); 119 | image.clear(); 120 | 121 | // finally should do a triangulation 122 | my_kf.processImage(image); 123 | 124 | 125 | // 126 | // my_kf.processImage(image); 127 | // my_kf.processImage(image); 128 | 129 | std::cout<< "I have done this peacefully" << std::endl; 130 | } 131 | 132 | void test_March() { 133 | DistortCamera cam; 134 | cam.setImageSize(480, 752); 135 | cam.setIntrinsicMtx(365.07984, 365.12127, 381.0196, 254.4431); 136 | cam.setDistortionParam(-2.842958e-1, 137 | 8.7155025e-2, 138 | -1.4602925e-4, 139 | -6.149638e-4, 140 | -1.218237e-2); 141 | 142 | Vector3d ptr; 143 | ptr << 3.5f, 3.35f, 6.0f; 144 | Vector2d z = cam.h(ptr); 145 | MatrixXd Jacob_h = cam.Jh(ptr); 146 | 147 | std::cout << "projected point is " << z << std::endl; 148 | std::cout << "Jacobian matrix is " << std::endl << Jacob_h << std::endl; 149 | 150 | std::cout << "Jacobian matrix has cols " << Jacob_h.cols() << std::endl; 151 | 152 | Vector4d q; 153 | Matrix3d R; 154 | q << 1, 2, 3, 4; 155 | q = q/q.norm(); 156 | 157 | R = quaternion_to_R(q); 158 | 159 | q = Vector4d(1,0,0,0); 160 | 161 | std::cout << "q is " << q << std::endl; 162 | std::cout << "R is " << R << std::endl; 163 | 164 | Matrix4d omega; 165 | omega = omega_mtx(ptr); 166 | 167 | std::cout << "omega is " << omega << std::endl; 168 | 169 | 170 | MatrixXd measure(2,5); 171 | MatrixXd pose(7,5); 172 | measure << 173 | 497.6229, 493.1491, 489.0450, 478.8504, 466.8726, 174 | 371.0596, 366.5853, 362.4808, 352.2850, 340.3059 175 | ; 176 | pose<< 177 | 0.777986724316206, 0.838539246498940, 0.775194258937213, 0.831502026804405, 0.770011014887736, 178 | 0.625646233952092, 0.489266453739099, 0.435311151332881, 0.225961749519687, 0.059184118324008, 179 | -0.057475618563915, -0.239729575410618, -0.457796966390153, -0.507489573463504, -0.635279684146888, 180 | 0, 0, 0, 0, 0, 181 | 1.098427340189829, 5.357568053111257, 9.092272676622571, 11.936962300957290, 13.613178885567471, 182 | 13.956842672263791, 12.934313455158014, 10.645683518400434, 7.314979906023285, 3.268235093982677, 183 | 1.414213562373095, -1.414213562373095, 1.414213562373095, -1.414213562373094, 1.414213562373095 184 | ; 185 | 186 | 187 | std::cout << "measure is " << std::endl << measure << std::endl; 188 | std::cout << "pose is " << std::endl<< pose << std::endl; 189 | 190 | Vector3d ptr_pose = cam.triangulate(measure, pose); 191 | std::cout << "ptr_pose is " << std::endl<< ptr_pose << std::endl; 192 | 193 | 194 | Quaternionf q2(1.0, 0.0, 0.0, 0.0); 195 | Quaternionf q3(1.0, 0.0, 0.0, 0.0); 196 | Quaternionf dq(1, 197 | 0.7, 198 | 0.7, 199 | 0.7); 200 | dq.w() = 1 - dq.vec().transpose()*dq.vec(); 201 | q2 = (q2*dq).normalized(); 202 | std::cout << "q2 is " << std::endl<< q2.w() << q2.x() << q2.y() << q2.z() << std::endl; 203 | 204 | Vector3d w_prev(1.4,1.4,1.4); 205 | Vector3d w_curr(1.4,1.4,1.4); 206 | Vector4d dq2 = delta_quaternion(w_prev, w_curr, 1.0f); 207 | dq.w() = dq2(3); 208 | dq.x() = -dq2(0); 209 | dq.y() = -dq2(1); 210 | dq.z() = -dq2(2); 211 | q3 = (q3 * dq).normalized(); 212 | std::cout << "q3 is " << std::endl<< q3.w() << q3.x() << q3.y() << q3.z() << std::endl; 213 | 214 | //Matrix3d ff = q3.vec(); 215 | std::map mymap; 216 | mymap[100]="an element"; 217 | mymap[200]="another element"; 218 | mymap[300]=mymap[200]; 219 | 220 | std::cout << "mymap[100] is " << mymap[100] << '\n'; 221 | std::cout << "mymap[200] is " << mymap[200] << '\n'; 222 | std::cout << "mymap[300] is " << mymap[300] << '\n'; 223 | std::cout << "mymap[301] is " << mymap[301] << '\n'; 224 | 225 | std::cout << "find 301" << mymap.find(301)->second << '\n'; 226 | //std::cout << "find 302" << mymap.find(302)->second << '\n'; 227 | 228 | std::cout << "mymap now contains " << mymap.size() << " elements.\n"; 229 | 230 | int k = 0; 231 | for (int i = 0; i<0;i++) 232 | k++; 233 | std::cout<<"kkkkk" << k<(); 238 | // HouseholderQR qr(A); 239 | // MatrixXd RR = qr.matrixQR().triangularView(); 240 | // MatrixXd Q = qr.householderQ(); 241 | // std::cout << RR << "\n"; 242 | // std::cout << Q << "\n"; 243 | // MatrixXd D = Q*RR-A; 244 | MatrixXd A = pose; 245 | HouseholderQR qr(A); 246 | MatrixXd RR = qr.matrixQR().triangularView(); 247 | MatrixXd Q = qr.householderQ(); 248 | std::cout << RR << "\n"; 249 | std::cout << Q << "\n"; 250 | MatrixXd D = Q*RR-A; 251 | 252 | std::cout << "\n" << (Q*RR-A).norm() << " " << sqrt((D.adjoint()*D).trace()) << "\n"; 253 | } 254 | 255 | void KFtest() 256 | { 257 | Vector4d q(1.0f, 0.0f, 0.0f, 0.0f); 258 | Vector3d p(3.0f, 3.0f, 4.5f); 259 | Vector3d v(1.3f, 1.4f, 1.5f); 260 | Vector3d bg(0.0f ,0.0f, 0.0f); 261 | Vector3d ba(0.0f ,0.0f, 0.0f); 262 | Vector3d pcb(10.0f ,0.0f, 0.0f); 263 | my_kf.setCalibParam(pcb, 365.07984, 365.12127, 381.0196, 254.4431, 264 | -2.842958e-1, 8.7155025e-2, -1.4602925e-4, -6.149638e-4, -1.218237e-2); 265 | my_kf.setNominalState(q, p, v, bg, ba); 266 | my_kf.printNominalState(true); 267 | my_kf.printErrorCovariance(true); 268 | // my_kf.addSlideState(); 269 | my_kf.printNominalState(true); 270 | my_kf.printErrorCovariance(true); 271 | my_kf.printSlidingWindow(); 272 | 273 | v = Vector3d(1.9f, 2.0f, 2.1f); 274 | my_kf.setNominalState(q, p, v, bg, ba); 275 | 276 | my_kf.printNominalState(true); 277 | my_kf.printErrorCovariance(true); 278 | // my_kf.addSlideState(); 279 | my_kf.printSlidingWindow(); 280 | 281 | my_kf.printNominalState(true); 282 | my_kf.printErrorCovariance(true); 283 | // my_kf.removeSlideState(0, 2); 284 | 285 | my_kf.printNominalState(true); 286 | my_kf.printErrorCovariance(true); 287 | my_kf.printSlidingWindow(); 288 | // my_kf.removeSlideState(0, 1); 289 | 290 | my_kf.printErrorCovariance(true); 291 | my_kf.processIMU(1.0f, Vector3d(0.9f, 0.9f, 0.9f), Vector3d(0.5f, 0.0f, 0.0f)); 292 | my_kf.processIMU(1.4f, Vector3d(0.9f, 0.9f, 0.9f), Vector3d(0.5f, 0.0f, 0.0f)); 293 | my_kf.printErrorCovariance(true); 294 | } 295 | 296 | void foo() 297 | { 298 | SlideState state; 299 | Vector4d q(1.0f, 0.5f, 0.5f, 0.5f); 300 | Vector3d p(3.0f, 3.0f, 4.5f); 301 | Vector3d v(1.3f, 1.4f, 1.5f); 302 | state.p = p; 303 | state.q = q; 304 | state.v = v; 305 | 306 | //myManager.addSlideState(state); 307 | } 308 | -------------------------------------------------------------------------------- /msckf_vins/src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #ifndef TIC_TOC_H 2 | #define TIC_TOC_H 3 | 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc(): sum(0) 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | t = clock(); 18 | } 19 | 20 | double toc() 21 | { 22 | sum += (clock() - t) / CLOCKS_PER_SEC * 1000; 23 | return sum; 24 | } 25 | private: 26 | double sum, t; 27 | }; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /sensor_processor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sensor_processor) 3 | 4 | set(CMAKE_CXX_FLAGS "-DNDEBUG -std=c++11 -march=native -O3 -Wall") 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs cv_bridge) 7 | 8 | FIND_PACKAGE(OpenCV REQUIRED) 9 | 10 | catkin_package() 11 | 12 | add_executable(sensor_processor 13 | src/sensor_processor_node.cpp 14 | ) 15 | 16 | target_link_libraries(sensor_processor ${catkin_LIBRARIES} ${OpenCV_LIBS}) 17 | -------------------------------------------------------------------------------- /sensor_processor/config/right_25000709.yml: -------------------------------------------------------------------------------- 1 | K: [ 3.6507984214192066e+02, 0., 3.8101960885425410e+02, 0., 3.6512127672545773e+02, 2.5444318313980298e+02, 0., 0., 1. ] 2 | D: [-2.8429580149021449e-01, 8.7155025659709395e-02, -1.4602925538147228e-04, -6.1496385367527061e-04, -1.2182371303193360e-02 ] 3 | -------------------------------------------------------------------------------- /sensor_processor/launch/feature_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /sensor_processor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sensor_processor 4 | 0.0.0 5 | The sensor_processor package 6 | 7 | 8 | 9 | 10 | dvorak 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 | catkin 43 | roscpp 44 | roscpp 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /sensor_processor/src/sensor_processor_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | using namespace std; 4 | 5 | #include 6 | #include "sensor_msgs/Image.h" 7 | #include "sensor_msgs/image_encodings.h" 8 | #include "sensor_msgs/PointCloud.h" 9 | #include "cv_bridge/cv_bridge.h" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | using namespace cv; 17 | 18 | Mat K, D, map1, map2, map1_fixed, map2_fixed; 19 | const int MAX_CNT = 50; 20 | const int MIN_DIST = 30, HASH_DIST = 2 * MIN_DIST; 21 | const int ROW = 480, HASH_ROW = (ROW + HASH_DIST - 1) / HASH_DIST + 1; 22 | const int COL = 752, HASH_COL = (COL + HASH_DIST - 1) / HASH_DIST + 1; 23 | const double FREQ_TIME = 0.1; 24 | 25 | bool img_hash[HASH_COL][HASH_ROW]; 26 | 27 | Mat prev_img, cur_img, forw_img; 28 | vector prev_pts, cur_pts, forw_pts; 29 | double prev_time, cur_time, forw_time; 30 | 31 | int next_id = 0; 32 | vector id; 33 | 34 | ros::Publisher pub_image; 35 | 36 | template 37 | void reduce_vector(vector &v, vector status) 38 | { 39 | int j = 0; 40 | for (int i = 0; i < int(v.size()); i++) 41 | if (status[i]) 42 | v[j++] = v[i]; 43 | v.resize(j); 44 | } 45 | 46 | double sum_time = 0.0; 47 | int sum_cnt = 0; 48 | 49 | void image_callback(const sensor_msgs::ImagePtr &image_msg) 50 | { 51 | forw_time = image_msg->header.stamp.toSec(); 52 | ROS_DEBUG("current time %lf", forw_time); 53 | 54 | double t_s = clock(); 55 | 56 | Mat dist_img = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8)->image; 57 | forw_img.release(); 58 | //undistort(dist_img, forw_img, K, D); 59 | remap(dist_img, forw_img, map1, map2, INTER_LINEAR, BORDER_CONSTANT); 60 | ROS_DEBUG("read and undistort costs %lf", (clock() - t_s) / CLOCKS_PER_SEC * 1000); 61 | 62 | if (prev_img.empty()) 63 | { 64 | ROS_DEBUG("init"); 65 | 66 | prev_img = forw_img; 67 | goodFeaturesToTrack(prev_img, prev_pts, MAX_CNT, 0.05, MIN_DIST); 68 | prev_time = forw_time; 69 | 70 | for (int i = 0; i < int(prev_pts.size()); i++) 71 | id.push_back(next_id++); 72 | 73 | ROS_DEBUG("feature number: %d", int(prev_pts.size())); 74 | 75 | cur_img = prev_img; 76 | cur_pts = prev_pts; 77 | cur_time = prev_time; 78 | 79 | ROS_DEBUG("sum time %lf", sum_time); 80 | sum_time = 0.0; 81 | return; 82 | } 83 | 84 | 85 | double t_op = clock(); 86 | ROS_DEBUG("start tracking"); 87 | 88 | vector status; 89 | vector err; 90 | forw_pts.clear(); 91 | 92 | ROS_DEBUG("tracking number: %lu", cur_pts.size()); 93 | calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, Size(21, 21), 3); 94 | reduce_vector(prev_pts, status); 95 | reduce_vector(cur_pts, status); 96 | reduce_vector(forw_pts, status); 97 | reduce_vector(id, status); 98 | ROS_DEBUG("tracking number: %lu", forw_pts.size()); 99 | ROS_DEBUG("optical flow costs %lf", (clock() - t_op) / CLOCKS_PER_SEC * 1000); 100 | 101 | if (forw_time - prev_time < FREQ_TIME) 102 | { 103 | cv::swap(cur_img, forw_img); 104 | std::swap(cur_pts, forw_pts); 105 | std::swap(cur_time, forw_time); 106 | sum_time += (clock() - t_s) / CLOCKS_PER_SEC * 1000.0; 107 | } 108 | else 109 | { 110 | vector status; 111 | vector err; 112 | 113 | if (prev_pts.size() >= 9) 114 | { 115 | double t_f = clock(); 116 | findFundamentalMat(prev_pts, forw_pts, FM_RANSAC, 0.5, 0.99, status); 117 | reduce_vector(prev_pts, status); 118 | reduce_vector(forw_pts, status); 119 | reduce_vector(id, status); 120 | ROS_DEBUG("F costs %lf", (clock() - t_f) / CLOCKS_PER_SEC * 1000); 121 | } 122 | 123 | memset(img_hash, 0, sizeof(img_hash)); 124 | for (auto & i : forw_pts) 125 | { 126 | int x = int(i.x / HASH_DIST + 0.5); 127 | int y = int(i.y / HASH_DIST + 0.5); 128 | if (x >= 0 && x < HASH_COL && y >= 0 && y < HASH_ROW) 129 | img_hash[x][y] = true; 130 | else 131 | ROS_ERROR("Point(%lf %lf), (%d, %d) is out of (%d, %d)", i.x, i.y, x, y, HASH_COL, HASH_ROW); 132 | } 133 | 134 | double t_g = clock(); 135 | vector tmp_pts; 136 | goodFeaturesToTrack(forw_img, tmp_pts, MAX_CNT * 2, 0.05, MIN_DIST); 137 | ROS_DEBUG("tracking new feature costs %lf", (clock() - t_g) / CLOCKS_PER_SEC * 1000); 138 | 139 | int cnt = 0; 140 | for (auto & i : tmp_pts) 141 | { 142 | int x = int(i.x / HASH_DIST + 0.5); 143 | int y = int(i.y / HASH_DIST + 0.5); 144 | if (x >= 0 && x < HASH_COL && y >= 0 && y < HASH_ROW && !img_hash[x][y]) 145 | { 146 | forw_pts.push_back(i); 147 | id.push_back(next_id++); 148 | cnt++; 149 | } 150 | } 151 | 152 | sum_time += (clock() - t_s) / CLOCKS_PER_SEC * 1000.0; 153 | sum_cnt++; 154 | ROS_DEBUG("sum time %lf", sum_time); 155 | ROS_DEBUG("sum time %lf", sum_time / sum_cnt); 156 | 157 | sensor_msgs::PointCloud feature; 158 | sensor_msgs::ChannelFloat32 ids; 159 | sensor_msgs::ChannelFloat32 pixel; 160 | 161 | for (int i = 0; i < ROW; i++) 162 | for (int j = 0; j < COL; j++) 163 | pixel.values.push_back(forw_img.at(i, j)); 164 | 165 | feature.header = image_msg->header; 166 | vector un_pts; 167 | undistortPoints(forw_pts, un_pts, K, Mat()); 168 | for (int i = 0; i < int(un_pts.size()); i++) 169 | { 170 | int p_id = id[i]; 171 | geometry_msgs::Point32 p; 172 | p.x = un_pts[i].x; 173 | p.y = un_pts[i].y; 174 | p.z = 1; 175 | 176 | feature.points.push_back(p); 177 | ids.values.push_back(p_id); 178 | } 179 | feature.channels.push_back(ids); 180 | feature.channels.push_back(pixel); 181 | pub_image.publish(feature); 182 | 183 | Mat color_img; 184 | cvtColor(forw_img, color_img, CV_GRAY2RGB); 185 | for (int i = 0; i < int(prev_pts.size()); i++) 186 | line(color_img, forw_pts[i], prev_pts[i], Scalar(0, 255, 0), 3); 187 | for (int i = 0; i < int(forw_pts.size()); i++) 188 | circle(color_img, forw_pts[i], 3, Scalar(255, 0, 0)); 189 | 190 | imshow("tracking", color_img); 191 | waitKey(1); 192 | 193 | cv::swap(prev_img, forw_img); 194 | std::swap(prev_pts, forw_pts); 195 | std::swap(prev_time, forw_time); 196 | 197 | cur_img = prev_img; 198 | cur_pts = prev_pts; 199 | cur_time = prev_time; 200 | } 201 | puts(""); 202 | } 203 | 204 | 205 | 206 | int main(int argc, char **argv) 207 | { 208 | ros::init(argc, argv, "sensor_processor"); 209 | ros::NodeHandle n("~"); 210 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 211 | 212 | ros::Subscriber sub_image = n.subscribe("input_image", 1000, image_callback); 213 | 214 | 215 | XmlRpc::XmlRpcValue xml_value; 216 | float _K[3 * 3], _D[8]; 217 | if (n.getParam("K", xml_value)) 218 | { 219 | for (int i = 0; i < xml_value.size(); i++) 220 | _K[i] = static_cast(xml_value[i]); 221 | K = Mat(3, 3, CV_32FC1, _K); 222 | ROS_INFO_STREAM("K: " << K); 223 | } 224 | else 225 | { 226 | ROS_ERROR("Error K"); 227 | } 228 | 229 | if (n.getParam("D", xml_value)) 230 | { 231 | for (int i = 0; i < xml_value.size(); i++) 232 | _D[i] = static_cast(xml_value[i]); 233 | D = Mat(1, xml_value.size(), CV_32FC1, _D); 234 | ROS_INFO_STREAM("D: " << D); 235 | } 236 | else 237 | { 238 | ROS_ERROR("Error D"); 239 | } 240 | 241 | initUndistortRectifyMap(K, D, Mat(), Mat(), Size(COL, ROW), CV_32FC1, map1, map2); 242 | convertMaps(map1, map2, map1_fixed, map2_fixed, CV_16SC2); 243 | 244 | pub_image = n.advertise("output_image", 1000); 245 | 246 | ros::spin(); 247 | 248 | return 0; 249 | } 250 | -------------------------------------------------------------------------------- /test_quaternion/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | 83 | -------------------------------------------------------------------------------- /test_quaternion/c.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "q.h" 7 | using namespace Eigen; 8 | using namespace std; 9 | int main() 10 | { 11 | srand(0); 12 | int s = 100; 13 | while (s--) { 14 | Vector3d p(rand() % 10 - 5, rand() % 10 - 5, rand() % 10 - 5); 15 | Vector4d c(rand() % 10 - 5, rand() % 10 - 5, rand() % 10 - 5, rand() % 10 - 5); 16 | Vector4d c2(rand() % 10 - 5, rand() % 10 - 5, rand() % 10 - 5, rand() % 10 - 5); 17 | 18 | Quaterniond q1(c); 19 | q1.normalize(); 20 | Quaterniond q2(c2); 21 | q2.normalize(); 22 | 23 | 24 | QuaternionAlias q3(c); 25 | QuaternionAlias q4(c2); 26 | q3.normalize(); 27 | q4.normalize(); 28 | 29 | Vector3d p1 = q1 * (q2 * p); 30 | Vector3d p2 = q1.matrix() * q2.matrix() * p; 31 | 32 | cout << q1.toRotationMatrix() << endl; 33 | cout << q1.matrix() << endl; 34 | 35 | cout << p1.transpose() << endl; 36 | cout << p2.transpose() << endl; 37 | } 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /test_quaternion/q.h: -------------------------------------------------------------------------------- 1 | #ifndef _QUATERNION_ALIAS_H_ 2 | #define _QUATERNION_ALIAS_H_ 3 | #include 4 | #include 5 | 6 | namespace Eigen { 7 | 8 | template class QuaternionAlias; 9 | 10 | namespace internal { 11 | template 12 | struct traits > 13 | { 14 | typedef QuaternionAlias<_Scalar,_Options> PlainObject; 15 | typedef _Scalar Scalar; 16 | typedef Matrix<_Scalar,4,1,_Options> Coefficients; 17 | enum{ 18 | IsAligned = internal::traits::Flags & AlignedBit, 19 | Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit 20 | }; 21 | }; 22 | } 23 | 24 | template 25 | class QuaternionAlias : public Quaternion<_Scalar,_Options> 26 | { 27 | 28 | typedef Quaternion<_Scalar,_Options> Base; 29 | //typedef QuaternionAlias<_Scalar,_Options> This; 30 | enum { IsAligned = internal::traits::IsAligned }; 31 | 32 | public: 33 | typedef _Scalar Scalar; 34 | typedef Matrix Vector3; 35 | typedef Matrix Matrix3; 36 | 37 | typedef typename internal::traits::Coefficients Coefficients; 38 | typedef typename Base::AngleAxisType AngleAxisType; 39 | 40 | QuaternionAlias() : Base() {} 41 | 42 | QuaternionAlias(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : Base(w, x, y, z) {} 43 | 44 | QuaternionAlias(const Scalar* data) : Base(data) {} 45 | 46 | template 47 | QuaternionAlias(const QuaternionBase& other) : Base(other) {} 48 | 49 | QuaternionAlias(const AngleAxisType& aa) : Base(aa) {} 50 | 51 | template 52 | QuaternionAlias(const MatrixBase& other) : Base(other) {} 53 | 54 | /* 55 | template 56 | explicit inline QuaternionAlias(const QuaternionAlias& other) 57 | { m_coeffs = other.coeffs().template cast(); } 58 | */ 59 | 60 | template EIGEN_STRONG_INLINE QuaternionAlias<_Scalar, _Options> operator* (const QuaternionBase& other) const 61 | { 62 | // return (*this).Base::operator*( other ); 63 | return other.operator*( *this ); 64 | } 65 | QuaternionAlias conjugate() const 66 | { 67 | return QuaternionAlias(this->w(),-this->x(),-this->y(),-this->z()); 68 | } 69 | //template EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase& q) 70 | Matrix3 toRotationMatrix() const 71 | { 72 | return this->conjugate().Base::toRotationMatrix(); 73 | } 74 | EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const 75 | { 76 | return this->conjugate().Base::_transformVector( v ); 77 | } 78 | 79 | }; 80 | 81 | } 82 | 83 | #endif//_QUATERNION_ALIAS_H_ 84 | -------------------------------------------------------------------------------- /vins_mac.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 | - /Path1 10 | - /Pose1 11 | - /Pose2 12 | - /Path2 13 | - /PointCloud1 14 | Splitter Ratio: 0.5 15 | Tree Height: 573 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.588679 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: "" 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.03 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Buffer Length: 1 58 | Class: rviz/Path 59 | Color: 255; 10; 192 60 | Enabled: true 61 | Line Style: Lines 62 | Line Width: 0.03 63 | Name: Path 64 | Offset: 65 | X: 0 66 | Y: 0 67 | Z: 0 68 | Topic: /simulation/path 69 | Value: true 70 | - Alpha: 1 71 | Axes Length: 1 72 | Axes Radius: 0.1 73 | Class: rviz/Pose 74 | Color: 255; 10; 192 75 | Enabled: true 76 | Head Length: 0.3 77 | Head Radius: 0.1 78 | Name: Pose 79 | Shaft Length: 1 80 | Shaft Radius: 0.05 81 | Shape: Arrow 82 | Topic: /simulation/pose 83 | Value: true 84 | - Alpha: 1 85 | Axes Length: 1 86 | Axes Radius: 0.1 87 | Class: rviz/Pose 88 | Color: 25; 255; 0 89 | Enabled: true 90 | Head Length: 0.3 91 | Head Radius: 0.1 92 | Name: Pose 93 | Shaft Length: 1 94 | Shaft Radius: 0.05 95 | Shape: Arrow 96 | Topic: /msckf_vins/pose 97 | Value: true 98 | - Alpha: 1 99 | Buffer Length: 1 100 | Class: rviz/Path 101 | Color: 25; 255; 0 102 | Enabled: true 103 | Line Style: Lines 104 | Line Width: 0.03 105 | Name: Path 106 | Offset: 107 | X: 0 108 | Y: 0 109 | Z: 0 110 | Topic: /msckf_vins/path 111 | Value: true 112 | - Alpha: 1 113 | Autocompute Intensity Bounds: true 114 | Autocompute Value Bounds: 115 | Max Value: 10 116 | Min Value: -10 117 | Value: true 118 | Axis: Z 119 | Channel Name: intensity 120 | Class: rviz/PointCloud 121 | Color: 255; 255; 255 122 | Color Transformer: Intensity 123 | Decay Time: 0 124 | Enabled: true 125 | Invert Rainbow: false 126 | Max Color: 255; 255; 255 127 | Max Intensity: 4096 128 | Min Color: 0; 0; 0 129 | Min Intensity: 0 130 | Name: PointCloud 131 | Position Transformer: XYZ 132 | Queue Size: 10 133 | Selectable: true 134 | Size (Pixels): 3 135 | Size (m): 0.1 136 | Style: Flat Squares 137 | Topic: /simulation/cloud 138 | Use Fixed Frame: true 139 | Use rainbow: true 140 | Value: true 141 | Enabled: true 142 | Global Options: 143 | Background Color: 48; 48; 48 144 | Fixed Frame: world 145 | Frame Rate: 30 146 | Name: root 147 | Tools: 148 | - Class: rviz/Interact 149 | Hide Inactive Objects: true 150 | - Class: rviz/MoveCamera 151 | - Class: rviz/Select 152 | - Class: rviz/FocusCamera 153 | - Class: rviz/Measure 154 | - Class: rviz/SetInitialPose 155 | Topic: /initialpose 156 | - Class: rviz/SetGoal 157 | Topic: /move_base_simple/goal 158 | - Class: rviz/PublishPoint 159 | Single click: true 160 | Topic: /clicked_point 161 | Value: true 162 | Views: 163 | Current: 164 | Class: rviz/Orbit 165 | Distance: 41.4643 166 | Enable Stereo Rendering: 167 | Stereo Eye Separation: 0.06 168 | Stereo Focal Distance: 1 169 | Swap Stereo Eyes: false 170 | Value: false 171 | Focal Point: 172 | X: 0 173 | Y: 0 174 | Z: 0 175 | Name: Current View 176 | Near Clip Distance: 0.01 177 | Pitch: 0.500398 178 | Target Frame: 179 | Value: Orbit (rviz) 180 | Yaw: 0.9954 181 | Saved: ~ 182 | Window Geometry: 183 | Displays: 184 | collapsed: false 185 | Height: 846 186 | Hide Left Dock: false 187 | Hide Right Dock: false 188 | QMainWindow State: 000000ff00000000fd00000004000000000000010a000002c9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c000002c9000000da00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002c000002c9000000a300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000001fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000295000002c900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 189 | Selection: 190 | collapsed: false 191 | Time: 192 | collapsed: false 193 | Tool Properties: 194 | collapsed: false 195 | Views: 196 | collapsed: false 197 | Width: 1200 198 | X: 68 199 | Y: 32 200 | -------------------------------------------------------------------------------- /vins_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 108 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /Simu Pose1 11 | - /Environ PointCloud1 12 | - /VINS Path1 13 | Splitter Ratio: 0.5 14 | Tree Height: 501 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 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.03 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 | Marker Scale: 1 61 | Name: TF 62 | Show Arrows: true 63 | Show Axes: true 64 | Show Names: true 65 | Tree: 66 | {} 67 | Update Interval: 0 68 | Value: true 69 | - Alpha: 1 70 | Axes Length: 1 71 | Axes Radius: 0.1 72 | Class: rviz/Pose 73 | Color: 255; 25; 0 74 | Enabled: true 75 | Head Length: 0.3 76 | Head Radius: 0.1 77 | Name: Simu Pose 78 | Shaft Length: 1 79 | Shaft Radius: 0.05 80 | Shape: Arrow 81 | Topic: /simulation/pose 82 | Value: true 83 | - Alpha: 1 84 | Buffer Length: 1 85 | Class: rviz/Path 86 | Color: 25; 255; 0 87 | Enabled: true 88 | Line Style: Lines 89 | Line Width: 0.03 90 | Name: Simu Path 91 | Offset: 92 | X: 0 93 | Y: 0 94 | Z: 0 95 | Topic: /simulation/path 96 | Value: true 97 | - Alpha: 1 98 | Autocompute Intensity Bounds: true 99 | Autocompute Value Bounds: 100 | Max Value: 10 101 | Min Value: -10 102 | Value: true 103 | Axis: Z 104 | Channel Name: intensity 105 | Class: rviz/PointCloud 106 | Color: 255; 255; 255 107 | Color Transformer: Intensity 108 | Decay Time: 10 109 | Enabled: true 110 | Invert Rainbow: false 111 | Max Color: 255; 255; 255 112 | Max Intensity: 4096 113 | Min Color: 0; 0; 0 114 | Min Intensity: 0 115 | Name: Environ PointCloud 116 | Position Transformer: XYZ 117 | Queue Size: 35 118 | Selectable: true 119 | Size (Pixels): 100 120 | Size (m): 1 121 | Style: Boxes 122 | Topic: /simulation/cloud 123 | Use Fixed Frame: true 124 | Use rainbow: true 125 | Value: true 126 | - Alpha: 1 127 | Buffer Length: 1 128 | Class: rviz/Path 129 | Color: 85; 85; 255 130 | Enabled: true 131 | Line Style: Lines 132 | Line Width: 0.03 133 | Name: VINS Path 134 | Offset: 135 | X: 0 136 | Y: 0 137 | Z: 0 138 | Topic: /msckf_vins/path 139 | Value: true 140 | - Alpha: 1 141 | Axes Length: 1 142 | Axes Radius: 0.1 143 | Class: rviz/Pose 144 | Color: 255; 25; 0 145 | Enabled: true 146 | Head Length: 0.3 147 | Head Radius: 0.1 148 | Name: Pose 149 | Shaft Length: 1 150 | Shaft Radius: 0.05 151 | Shape: Arrow 152 | Topic: /msckf_vins/pose 153 | Value: true 154 | Enabled: true 155 | Global Options: 156 | Background Color: 255; 255; 255 157 | Fixed Frame: world 158 | Frame Rate: 30 159 | Name: root 160 | Tools: 161 | - Class: rviz/Interact 162 | Hide Inactive Objects: true 163 | - Class: rviz/MoveCamera 164 | - Class: rviz/Select 165 | - Class: rviz/FocusCamera 166 | - Class: rviz/Measure 167 | - Class: rviz/SetInitialPose 168 | Topic: /initialpose 169 | - Class: rviz/SetGoal 170 | Topic: /move_base_simple/goal 171 | - Class: rviz/PublishPoint 172 | Single click: true 173 | Topic: /clicked_point 174 | Value: true 175 | Views: 176 | Current: 177 | Class: rviz/Orbit 178 | Distance: 183.526 179 | Enable Stereo Rendering: 180 | Stereo Eye Separation: 0.06 181 | Stereo Focal Distance: 1 182 | Swap Stereo Eyes: false 183 | Value: false 184 | Focal Point: 185 | X: -3.07033 186 | Y: 121.703 187 | Z: -55.9159 188 | Name: Current View 189 | Near Clip Distance: 0.01 190 | Pitch: 0.464797 191 | Target Frame: 192 | Value: Orbit (rviz) 193 | Yaw: 4.81218 194 | Saved: ~ 195 | Window Geometry: 196 | Displays: 197 | collapsed: false 198 | Height: 744 199 | Hide Left Dock: false 200 | Hide Right Dock: true 201 | QMainWindow State: 000000ff00000000fd0000000400000000000001bf000002a2fc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002a2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001d7000000f30000000000000000000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000350000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 202 | Selection: 203 | collapsed: false 204 | Time: 205 | collapsed: false 206 | Tool Properties: 207 | collapsed: false 208 | Views: 209 | collapsed: true 210 | Width: 1301 211 | X: 65 212 | Y: 24 213 | --------------------------------------------------------------------------------