├── .gitignore ├── LICENCE ├── README.md ├── ar_demo ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── launch │ ├── 3dm_bag.launch │ ├── ar_rviz.launch │ └── realsense_ar.launch ├── package.xml └── src │ └── ar_demo_node.cpp ├── benchmark_publisher ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── config │ ├── MH_01_easy │ │ └── data.csv │ ├── MH_02_easy │ │ └── data.csv │ ├── MH_03_medium │ │ └── data.csv │ ├── MH_04_difficult │ │ └── data.csv │ ├── MH_05_difficult │ │ └── data.csv │ ├── V1_01_easy │ │ └── data.csv │ ├── V1_02_medium │ │ └── data.csv │ ├── V1_03_difficult │ │ └── data.csv │ ├── V2_01_easy │ │ └── data.csv │ ├── V2_02_medium │ │ └── data.csv │ └── V2_03_difficult │ │ └── data.csv ├── launch │ └── publish.launch ├── package.xml └── src │ └── benchmark_publisher_node.cpp ├── camera_model ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── include │ └── camodocal │ │ ├── calib │ │ └── CameraCalibration.h │ │ ├── camera_models │ │ ├── Camera.h │ │ ├── CameraFactory.h │ │ ├── CataCamera.h │ │ ├── CostFunctionFactory.h │ │ ├── EquidistantCamera.h │ │ ├── PinholeCamera.h │ │ └── ScaramuzzaCamera.h │ │ ├── chessboard │ │ ├── Chessboard.h │ │ ├── ChessboardCorner.h │ │ ├── ChessboardQuad.h │ │ └── Spline.h │ │ ├── gpl │ │ ├── EigenQuaternionParameterization.h │ │ ├── EigenUtils.h │ │ └── gpl.h │ │ └── sparse_graph │ │ └── Transform.h ├── instruction ├── package.xml ├── readme.md └── src │ ├── calib │ └── CameraCalibration.cc │ ├── camera_models │ ├── Camera.cc │ ├── CameraFactory.cc │ ├── CataCamera.cc │ ├── CostFunctionFactory.cc │ ├── EquidistantCamera.cc │ ├── PinholeCamera.cc │ └── ScaramuzzaCamera.cc │ ├── chessboard │ └── Chessboard.cc │ ├── gpl │ ├── EigenQuaternionParameterization.cc │ └── gpl.cc │ ├── intrinsic_calib.cc │ └── sparse_graph │ └── Transform.cc ├── config ├── 3dm │ └── 3dm_config.yaml ├── black_box │ └── black_box_config.yaml ├── cla │ └── cla_config.yaml ├── euroc │ ├── calib_results1216.txt │ └── euroc_config_no_extrinsic.yaml ├── extrinsic_parameter_example.pdf ├── fisheye_mask.jpg ├── fisheye_mask_752x480.jpg ├── mindvision │ └── mindvision.yaml ├── realsense │ ├── realsense_color_config.yaml │ ├── realsense_fisheye_config.yaml │ └── realsense_zr300 └── tum │ └── tum_config.yaml ├── demo.jpg ├── docker ├── Dockerfile ├── Makefile └── run.sh ├── feature_tracker ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── package.xml └── src │ ├── feature_tracker.cpp │ ├── feature_tracker.h │ ├── feature_tracker_node.cpp │ ├── parameters.cpp │ ├── parameters.h │ ├── random_array.cc │ ├── random_array.h │ └── tic_toc.h ├── figure └── LF-VIO-hardware.png ├── pointcloud_image_fusion ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── package.xml └── src │ ├── pal_mask.jpg │ ├── pal_mask1.jpg │ ├── parameters.cpp │ ├── parameters.h │ ├── pointcloud_image_fusion.cpp │ ├── pointcloud_image_fusion.h │ ├── pointcloud_image_fusion_node.cpp │ └── tic_toc.h ├── support_files ├── brief_k10L6.bin ├── brief_pattern.yml └── paper_bib.txt └── vins_estimator ├── CMakeLists.txt ├── cmake └── FindEigen.cmake ├── launch ├── 3dm.launch ├── black_box.launch ├── cla.launch ├── euroc.launch ├── euroc_no_extrinsic_param.launch ├── mindvision.launch ├── mynteye.launch ├── realsense.launch ├── realsense_color.launch ├── realsense_fisheye.launch ├── tum.launch └── vins_rviz.launch ├── package.xml └── src ├── estimator.cpp ├── estimator.h ├── estimator_node.cpp ├── factor ├── imu_factor.h ├── integration_base.h ├── marginalization_factor.cpp ├── marginalization_factor.h ├── pose_local_parameterization.cpp ├── pose_local_parameterization.h ├── projection_factor.cpp ├── projection_factor.h ├── projection_td_factor.cpp └── projection_td_factor.h ├── feature_manager.cpp ├── feature_manager.h ├── initial ├── initial_aligment.cpp ├── initial_alignment.h ├── initial_ex_rotation.cpp ├── initial_ex_rotation.h ├── initial_sfm.cpp ├── initial_sfm.h ├── random_array.cc ├── random_array.h ├── solve_5pts.cpp └── solve_5pts.h ├── parameters.cpp ├── parameters.h ├── pnp_solver.cpp ├── pnp_solver.h └── utility ├── CameraPoseVisualization.cpp ├── CameraPoseVisualization.h ├── tic_toc.h ├── utility.cpp ├── utility.h ├── visualization.cpp └── visualization.h /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.rviz 3 | ex_calib_result.yaml 4 | vins_result.csv 5 | simulation.launch 6 | simulation/ 7 | data_generator/ 8 | iphone.launch 9 | iphone/ 10 | A3_rs/ 11 | A3_rs.launch 12 | test/ 13 | test.launch 14 | .vscode/ 15 | -------------------------------------------------------------------------------- /ar_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ar_demo) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE") 6 | #-DEIGEN_USE_MKL_ALL") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rospy 12 | std_msgs 13 | image_transport 14 | sensor_msgs 15 | cv_bridge 16 | message_filters 17 | camera_model 18 | ) 19 | find_package(OpenCV REQUIRED) 20 | 21 | catkin_package( 22 | 23 | ) 24 | 25 | 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 31 | find_package(Eigen3) 32 | include_directories( 33 | ${catkin_INCLUDE_DIRS} 34 | ${EIGEN3_INCLUDE_DIR} 35 | ) 36 | 37 | add_executable(ar_demo_node src/ar_demo_node.cpp) 38 | 39 | target_link_libraries(ar_demo_node 40 | ${catkin_LIBRARIES} ${OpenCV_LIBS} 41 | ) 42 | 43 | 44 | -------------------------------------------------------------------------------- /ar_demo/launch/3dm_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ar_demo/launch/ar_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /ar_demo/launch/realsense_ar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /ar_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ar_demo 4 | 0.0.0 5 | The ar_demo package 6 | 7 | 8 | 9 | 10 | tony-ws 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 | rospy 45 | std_msgs 46 | camera_model 47 | roscpp 48 | rospy 49 | std_msgs 50 | camera_model 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /benchmark_publisher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(benchmark_publisher) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -rdynamic") 7 | 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | tf 12 | ) 13 | 14 | catkin_package() 15 | include_directories(${catkin_INCLUDE_DIRS}) 16 | 17 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 18 | find_package(Eigen3) 19 | include_directories( 20 | ${catkin_INCLUDE_DIRS} 21 | ${EIGEN3_INCLUDE_DIR} 22 | ) 23 | 24 | add_executable(benchmark_publisher 25 | src/benchmark_publisher_node.cpp 26 | ) 27 | 28 | target_link_libraries(benchmark_publisher ${catkin_LIBRARIES}) 29 | -------------------------------------------------------------------------------- /benchmark_publisher/launch/publish.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | -------------------------------------------------------------------------------- /benchmark_publisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | benchmark_publisher 4 | 0.0.0 5 | The benchmark_publisher 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 | -------------------------------------------------------------------------------- /benchmark_publisher/src/benchmark_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | using namespace Eigen; 13 | 14 | const int SKIP = 50; 15 | string benchmark_output_path; 16 | string estimate_output_path; 17 | template 18 | T readParam(ros::NodeHandle &n, std::string name) 19 | { 20 | T ans; 21 | if (n.getParam(name, ans)) 22 | { 23 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 24 | } 25 | else 26 | { 27 | ROS_ERROR_STREAM("Failed to load " << name); 28 | n.shutdown(); 29 | } 30 | return ans; 31 | } 32 | 33 | struct Data 34 | { 35 | Data(FILE *f) 36 | { 37 | if (fscanf(f, " %lf,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", &t, 38 | &px, &py, &pz, 39 | &qw, &qx, &qy, &qz, 40 | &vx, &vy, &vz, 41 | &wx, &wy, &wz, 42 | &ax, &ay, &az) != EOF) 43 | { 44 | t /= 1e9; 45 | } 46 | } 47 | double t; 48 | float px, py, pz; 49 | float qw, qx, qy, qz; 50 | float vx, vy, vz; 51 | float wx, wy, wz; 52 | float ax, ay, az; 53 | }; 54 | int idx = 1; 55 | vector benchmark; 56 | 57 | ros::Publisher pub_odom; 58 | ros::Publisher pub_path; 59 | nav_msgs::Path path; 60 | 61 | int init = 0; 62 | Quaterniond baseRgt; 63 | Vector3d baseTgt; 64 | tf::Transform trans; 65 | 66 | void odom_callback(const nav_msgs::OdometryConstPtr &odom_msg) 67 | { 68 | //ROS_INFO("odom callback!"); 69 | if (odom_msg->header.stamp.toSec() > benchmark.back().t) 70 | return; 71 | 72 | for (; idx < static_cast(benchmark.size()) && benchmark[idx].t <= odom_msg->header.stamp.toSec(); idx++) 73 | ; 74 | 75 | 76 | if (init++ < SKIP) 77 | { 78 | baseRgt = Quaterniond(odom_msg->pose.pose.orientation.w, 79 | odom_msg->pose.pose.orientation.x, 80 | odom_msg->pose.pose.orientation.y, 81 | odom_msg->pose.pose.orientation.z) * 82 | Quaterniond(benchmark[idx - 1].qw, 83 | benchmark[idx - 1].qx, 84 | benchmark[idx - 1].qy, 85 | benchmark[idx - 1].qz).inverse(); 86 | baseTgt = Vector3d{odom_msg->pose.pose.position.x, 87 | odom_msg->pose.pose.position.y, 88 | odom_msg->pose.pose.position.z} - 89 | baseRgt * Vector3d{benchmark[idx - 1].px, benchmark[idx - 1].py, benchmark[idx - 1].pz}; 90 | return; 91 | } 92 | 93 | nav_msgs::Odometry odometry; 94 | odometry.header.stamp = ros::Time(benchmark[idx - 1].t); 95 | odometry.header.frame_id = "world"; 96 | odometry.child_frame_id = "world"; 97 | 98 | Vector3d tmp_T = baseTgt + baseRgt * Vector3d{benchmark[idx - 1].px, benchmark[idx - 1].py, benchmark[idx - 1].pz}; 99 | odometry.pose.pose.position.x = tmp_T.x(); 100 | odometry.pose.pose.position.y = tmp_T.y(); 101 | odometry.pose.pose.position.z = tmp_T.z(); 102 | 103 | Quaterniond tmp_R = baseRgt * Quaterniond{benchmark[idx - 1].qw, 104 | benchmark[idx - 1].qx, 105 | benchmark[idx - 1].qy, 106 | benchmark[idx - 1].qz}; 107 | odometry.pose.pose.orientation.w = tmp_R.w(); 108 | odometry.pose.pose.orientation.x = tmp_R.x(); 109 | odometry.pose.pose.orientation.y = tmp_R.y(); 110 | odometry.pose.pose.orientation.z = tmp_R.z(); 111 | 112 | Vector3d tmp_V = baseRgt * Vector3d{benchmark[idx - 1].vx, 113 | benchmark[idx - 1].vy, 114 | benchmark[idx - 1].vz}; 115 | odometry.twist.twist.linear.x = tmp_V.x(); 116 | odometry.twist.twist.linear.y = tmp_V.y(); 117 | odometry.twist.twist.linear.z = tmp_V.z(); 118 | pub_odom.publish(odometry); 119 | 120 | geometry_msgs::PoseStamped pose_stamped; 121 | pose_stamped.header = odometry.header; 122 | pose_stamped.pose = odometry.pose.pose; 123 | path.header = odometry.header; 124 | path.poses.push_back(pose_stamped); 125 | pub_path.publish(path); 126 | } 127 | 128 | int main(int argc, char **argv) 129 | { 130 | ros::init(argc, argv, "benchmark_publisher"); 131 | ros::NodeHandle n("~"); 132 | 133 | string csv_file = readParam(n, "data_name"); 134 | std::cout << "load ground truth " << csv_file << std::endl; 135 | FILE *f = fopen(csv_file.c_str(), "r"); 136 | if (f==NULL) 137 | { 138 | ROS_WARN("can't load ground truth; wrong path"); 139 | //std::cerr << "can't load ground truth; wrong path " << csv_file << std::endl; 140 | return 0; 141 | } 142 | char tmp[10000]; 143 | if (fgets(tmp, 10000, f) == NULL) 144 | { 145 | ROS_WARN("can't load ground truth; no data available"); 146 | } 147 | while (!feof(f)) 148 | benchmark.emplace_back(f); 149 | fclose(f); 150 | benchmark.pop_back(); 151 | ROS_INFO("Data loaded: %d", (int)benchmark.size()); 152 | 153 | pub_odom = n.advertise("odometry", 1000); 154 | pub_path = n.advertise("path", 1000); 155 | 156 | ros::Subscriber sub_odom = n.subscribe("estimated_odometry", 1000, odom_callback); 157 | 158 | ros::Rate r(20); 159 | ros::spin(); 160 | } 161 | -------------------------------------------------------------------------------- /camera_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(camera_model) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | ) 12 | 13 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 14 | include_directories(${Boost_INCLUDE_DIRS}) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 19 | find_package(Ceres REQUIRED) 20 | include_directories(${CERES_INCLUDE_DIRS}) 21 | 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES camera_model 26 | CATKIN_DEPENDS roscpp std_msgs 27 | # DEPENDS system_lib 28 | ) 29 | 30 | include_directories( 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | include_directories("include") 35 | 36 | 37 | add_executable(Calibration 38 | src/intrinsic_calib.cc 39 | src/chessboard/Chessboard.cc 40 | src/calib/CameraCalibration.cc 41 | src/camera_models/Camera.cc 42 | src/camera_models/CameraFactory.cc 43 | src/camera_models/CostFunctionFactory.cc 44 | src/camera_models/PinholeCamera.cc 45 | src/camera_models/CataCamera.cc 46 | src/camera_models/EquidistantCamera.cc 47 | src/camera_models/ScaramuzzaCamera.cc 48 | src/sparse_graph/Transform.cc 49 | src/gpl/gpl.cc 50 | src/gpl/EigenQuaternionParameterization.cc) 51 | 52 | add_library(camera_model 53 | src/chessboard/Chessboard.cc 54 | src/calib/CameraCalibration.cc 55 | src/camera_models/Camera.cc 56 | src/camera_models/CameraFactory.cc 57 | src/camera_models/CostFunctionFactory.cc 58 | src/camera_models/PinholeCamera.cc 59 | src/camera_models/CataCamera.cc 60 | src/camera_models/EquidistantCamera.cc 61 | src/camera_models/ScaramuzzaCamera.cc 62 | src/sparse_graph/Transform.cc 63 | src/gpl/gpl.cc 64 | src/gpl/EigenQuaternionParameterization.cc) 65 | 66 | target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 67 | target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 68 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/calib/CameraCalibration.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERACALIBRATION_H 2 | #define CAMERACALIBRATION_H 3 | 4 | #include 5 | 6 | #include "camodocal/camera_models/Camera.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class CameraCalibration 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | CameraCalibration(); 16 | 17 | CameraCalibration(Camera::ModelType modelType, 18 | const std::string& cameraName, 19 | const cv::Size& imageSize, 20 | const cv::Size& boardSize, 21 | float squareSize); 22 | 23 | void clear(void); 24 | 25 | void addChessboardData(const std::vector& corners); 26 | 27 | bool calibrate(void); 28 | 29 | int sampleCount(void) const; 30 | std::vector >& imagePoints(void); 31 | const std::vector >& imagePoints(void) const; 32 | std::vector >& scenePoints(void); 33 | const std::vector >& scenePoints(void) const; 34 | CameraPtr& camera(void); 35 | const CameraConstPtr camera(void) const; 36 | 37 | Eigen::Matrix2d& measurementCovariance(void); 38 | const Eigen::Matrix2d& measurementCovariance(void) const; 39 | 40 | cv::Mat& cameraPoses(void); 41 | const cv::Mat& cameraPoses(void) const; 42 | 43 | void drawResults(std::vector& images) const; 44 | 45 | void writeParams(const std::string& filename) const; 46 | 47 | bool writeChessboardData(const std::string& filename) const; 48 | bool readChessboardData(const std::string& filename); 49 | 50 | void setVerbose(bool verbose); 51 | 52 | private: 53 | bool calibrateHelper(CameraPtr& camera, 54 | std::vector& rvecs, std::vector& tvecs) const; 55 | 56 | void optimize(CameraPtr& camera, 57 | std::vector& rvecs, std::vector& tvecs) const; 58 | 59 | template 60 | void readData(std::ifstream& ifs, T& data) const; 61 | 62 | template 63 | void writeData(std::ofstream& ofs, T data) const; 64 | 65 | cv::Size m_boardSize; 66 | float m_squareSize; 67 | 68 | CameraPtr m_camera; 69 | cv::Mat m_cameraPoses; 70 | 71 | std::vector > m_imagePoints; 72 | std::vector > m_scenePoints; 73 | 74 | Eigen::Matrix2d m_measurementCovariance; 75 | 76 | bool m_verbose; 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H 2 | #define CAMERA_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace camodocal 10 | { 11 | 12 | class Camera 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | enum ModelType 17 | { 18 | KANNALA_BRANDT, 19 | MEI, 20 | PINHOLE, 21 | SCARAMUZZA 22 | }; 23 | 24 | class Parameters 25 | { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | Parameters(ModelType modelType); 29 | 30 | Parameters(ModelType modelType, const std::string& cameraName, 31 | int w, int h); 32 | 33 | ModelType& modelType(void); 34 | std::string& cameraName(void); 35 | int& imageWidth(void); 36 | int& imageHeight(void); 37 | 38 | ModelType modelType(void) const; 39 | const std::string& cameraName(void) const; 40 | int imageWidth(void) const; 41 | int imageHeight(void) const; 42 | 43 | int nIntrinsics(void) const; 44 | 45 | virtual bool readFromYamlFile(const std::string& filename) = 0; 46 | virtual void writeToYamlFile(const std::string& filename) const = 0; 47 | 48 | protected: 49 | ModelType m_modelType; 50 | int m_nIntrinsics; 51 | std::string m_cameraName; 52 | int m_imageWidth; 53 | int m_imageHeight; 54 | }; 55 | 56 | virtual ModelType modelType(void) const = 0; 57 | virtual const std::string& cameraName(void) const = 0; 58 | virtual int imageWidth(void) const = 0; 59 | virtual int imageHeight(void) const = 0; 60 | 61 | virtual cv::Mat& mask(void); 62 | virtual const cv::Mat& mask(void) const; 63 | 64 | virtual void estimateIntrinsics(const cv::Size& boardSize, 65 | const std::vector< std::vector >& objectPoints, 66 | const std::vector< std::vector >& imagePoints) = 0; 67 | virtual void estimateExtrinsics(const std::vector& objectPoints, 68 | const std::vector& imagePoints, 69 | cv::Mat& rvec, cv::Mat& tvec) const; 70 | 71 | // Lift points from the image plane to the sphere 72 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 73 | //%output P 74 | 75 | // Lift points from the image plane to the projective space 76 | virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 77 | //%output P 78 | 79 | // Projects 3D points to the image plane (Pi function) 80 | virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; 81 | //%output p 82 | 83 | // Projects 3D points to the image plane (Pi function) 84 | // and calculates jacobian 85 | //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 86 | // Eigen::Matrix& J) const = 0; 87 | //%output p 88 | //%output J 89 | 90 | virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; 91 | //%output p 92 | 93 | //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0; 94 | virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 95 | float fx = -1.0f, float fy = -1.0f, 96 | cv::Size imageSize = cv::Size(0, 0), 97 | float cx = -1.0f, float cy = -1.0f, 98 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0; 99 | 100 | virtual int parameterCount(void) const = 0; 101 | 102 | virtual void readParameters(const std::vector& parameters) = 0; 103 | virtual void writeParameters(std::vector& parameters) const = 0; 104 | 105 | virtual void writeParametersToYamlFile(const std::string& filename) const = 0; 106 | 107 | virtual std::string parametersToString(void) const = 0; 108 | 109 | /** 110 | * \brief Calculates the reprojection distance between points 111 | * 112 | * \param P1 first 3D point coordinates 113 | * \param P2 second 3D point coordinates 114 | * \return euclidean distance in the plane 115 | */ 116 | double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; 117 | 118 | double reprojectionError(const std::vector< std::vector >& objectPoints, 119 | const std::vector< std::vector >& imagePoints, 120 | const std::vector& rvecs, 121 | const std::vector& tvecs, 122 | cv::OutputArray perViewErrors = cv::noArray()) const; 123 | 124 | double reprojectionError(const Eigen::Vector3d& P, 125 | const Eigen::Quaterniond& camera_q, 126 | const Eigen::Vector3d& camera_t, 127 | const Eigen::Vector2d& observed_p) const; 128 | 129 | void projectPoints(const std::vector& objectPoints, 130 | const cv::Mat& rvec, 131 | const cv::Mat& tvec, 132 | std::vector& imagePoints) const; 133 | protected: 134 | cv::Mat m_mask; 135 | }; 136 | 137 | typedef boost::shared_ptr CameraPtr; 138 | typedef boost::shared_ptr CameraConstPtr; 139 | 140 | } 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/CameraFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAFACTORY_H 2 | #define CAMERAFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace camodocal 10 | { 11 | 12 | class CameraFactory 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | CameraFactory(); 17 | 18 | static boost::shared_ptr instance(void); 19 | 20 | CameraPtr generateCamera(Camera::ModelType modelType, 21 | const std::string& cameraName, 22 | cv::Size imageSize) const; 23 | 24 | CameraPtr generateCameraFromYamlFile(const std::string& filename); 25 | 26 | private: 27 | static boost::shared_ptr m_instance; 28 | }; 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/CataCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef CATACAMERA_H 2 | #define CATACAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | /** 14 | * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration 15 | * from Planar Grids, ICRA 2007 16 | */ 17 | 18 | class CataCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double xi, 28 | double k1, double k2, double p1, double p2, 29 | double gamma1, double gamma2, double u0, double v0); 30 | 31 | double& xi(void); 32 | double& k1(void); 33 | double& k2(void); 34 | double& p1(void); 35 | double& p2(void); 36 | double& gamma1(void); 37 | double& gamma2(void); 38 | double& u0(void); 39 | double& v0(void); 40 | 41 | double xi(void) const; 42 | double k1(void) const; 43 | double k2(void) const; 44 | double p1(void) const; 45 | double p2(void) const; 46 | double gamma1(void) const; 47 | double gamma2(void) const; 48 | double u0(void) const; 49 | double v0(void) const; 50 | 51 | bool readFromYamlFile(const std::string& filename); 52 | void writeToYamlFile(const std::string& filename) const; 53 | 54 | Parameters& operator=(const Parameters& other); 55 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 56 | 57 | private: 58 | double m_xi; 59 | double m_k1; 60 | double m_k2; 61 | double m_p1; 62 | double m_p2; 63 | double m_gamma1; 64 | double m_gamma2; 65 | double m_u0; 66 | double m_v0; 67 | }; 68 | 69 | CataCamera(); 70 | 71 | /** 72 | * \brief Constructor from the projection model parameters 73 | */ 74 | CataCamera(const std::string& cameraName, 75 | int imageWidth, int imageHeight, 76 | double xi, double k1, double k2, double p1, double p2, 77 | double gamma1, double gamma2, double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | CataCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 121 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 122 | Eigen::Matrix2d& J) const; 123 | 124 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 125 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 126 | float fx = -1.0f, float fy = -1.0f, 127 | cv::Size imageSize = cv::Size(0, 0), 128 | float cx = -1.0f, float cy = -1.0f, 129 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 130 | 131 | int parameterCount(void) const; 132 | 133 | const Parameters& getParameters(void) const; 134 | void setParameters(const Parameters& parameters); 135 | 136 | void readParameters(const std::vector& parameterVec); 137 | void writeParameters(std::vector& parameterVec) const; 138 | 139 | void writeParametersToYamlFile(const std::string& filename) const; 140 | 141 | std::string parametersToString(void) const; 142 | 143 | private: 144 | Parameters mParameters; 145 | 146 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 147 | bool m_noDistortion; 148 | }; 149 | 150 | typedef boost::shared_ptr CataCameraPtr; 151 | typedef boost::shared_ptr CataCameraConstPtr; 152 | 153 | template 154 | void 155 | CataCamera::spaceToPlane(const T* const params, 156 | const T* const q, const T* const t, 157 | const Eigen::Matrix& P, 158 | Eigen::Matrix& p) 159 | { 160 | T P_w[3]; 161 | P_w[0] = T(P(0)); 162 | P_w[1] = T(P(1)); 163 | P_w[2] = T(P(2)); 164 | 165 | // Convert quaternion from Eigen convention (x, y, z, w) 166 | // to Ceres convention (w, x, y, z) 167 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 168 | 169 | T P_c[3]; 170 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 171 | 172 | P_c[0] += t[0]; 173 | P_c[1] += t[1]; 174 | P_c[2] += t[2]; 175 | 176 | // project 3D object point to the image plane 177 | T xi = params[0]; 178 | T k1 = params[1]; 179 | T k2 = params[2]; 180 | T p1 = params[3]; 181 | T p2 = params[4]; 182 | T gamma1 = params[5]; 183 | T gamma2 = params[6]; 184 | T alpha = T(0); //cameraParams.alpha(); 185 | T u0 = params[7]; 186 | T v0 = params[8]; 187 | 188 | // Transform to model plane 189 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 190 | P_c[0] /= len; 191 | P_c[1] /= len; 192 | P_c[2] /= len; 193 | 194 | T u = P_c[0] / (P_c[2] + xi); 195 | T v = P_c[1] / (P_c[2] + xi); 196 | 197 | T rho_sqr = u * u + v * v; 198 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 199 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 200 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 201 | 202 | u = L * u + du; 203 | v = L * v + dv; 204 | p(0) = gamma1 * (u + alpha * v) + u0; 205 | p(1) = gamma2 * v + v0; 206 | } 207 | 208 | } 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTFUNCTIONFACTORY_H 2 | #define COSTFUNCTIONFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace ceres 10 | { 11 | class CostFunction; 12 | } 13 | 14 | namespace camodocal 15 | { 16 | 17 | enum 18 | { 19 | CAMERA_INTRINSICS = 1 << 0, 20 | CAMERA_POSE = 1 << 1, 21 | POINT_3D = 1 << 2, 22 | ODOMETRY_INTRINSICS = 1 << 3, 23 | ODOMETRY_3D_POSE = 1 << 4, 24 | ODOMETRY_6D_POSE = 1 << 5, 25 | CAMERA_ODOMETRY_TRANSFORM = 1 << 6 26 | }; 27 | 28 | class CostFunctionFactory 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | CostFunctionFactory(); 33 | 34 | static boost::shared_ptr instance(void); 35 | 36 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 37 | const Eigen::Vector3d& observed_P, 38 | const Eigen::Vector2d& observed_p, 39 | int flags) const; 40 | 41 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 42 | const Eigen::Vector3d& observed_P, 43 | const Eigen::Vector2d& observed_p, 44 | const Eigen::Matrix2d& sqrtPrecisionMat, 45 | int flags) const; 46 | 47 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 48 | const Eigen::Vector2d& observed_p, 49 | int flags, bool optimize_cam_odo_z = true) const; 50 | 51 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 52 | const Eigen::Vector2d& observed_p, 53 | const Eigen::Matrix2d& sqrtPrecisionMat, 54 | int flags, bool optimize_cam_odo_z = true) const; 55 | 56 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 57 | const Eigen::Vector3d& odo_pos, 58 | const Eigen::Vector3d& odo_att, 59 | const Eigen::Vector2d& observed_p, 60 | int flags, bool optimize_cam_odo_z = true) const; 61 | 62 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 63 | const Eigen::Quaterniond& cam_odo_q, 64 | const Eigen::Vector3d& cam_odo_t, 65 | const Eigen::Vector3d& odo_pos, 66 | const Eigen::Vector3d& odo_att, 67 | const Eigen::Vector2d& observed_p, 68 | int flags) const; 69 | 70 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, 71 | const CameraConstPtr& cameraRight, 72 | const Eigen::Vector3d& observed_P, 73 | const Eigen::Vector2d& observed_p_left, 74 | const Eigen::Vector2d& observed_p_right) const; 75 | 76 | private: 77 | static boost::shared_ptr m_instance; 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/EquidistantCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef EQUIDISTANTCAMERA_H 2 | #define EQUIDISTANTCAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | /** 14 | * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method 15 | * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 16 | */ 17 | 18 | class EquidistantCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double k2, double k3, double k4, double k5, 28 | double mu, double mv, 29 | double u0, double v0); 30 | 31 | double& k2(void); 32 | double& k3(void); 33 | double& k4(void); 34 | double& k5(void); 35 | double& mu(void); 36 | double& mv(void); 37 | double& u0(void); 38 | double& v0(void); 39 | 40 | double k2(void) const; 41 | double k3(void) const; 42 | double k4(void) const; 43 | double k5(void) const; 44 | double mu(void) const; 45 | double mv(void) const; 46 | double u0(void) const; 47 | double v0(void) const; 48 | 49 | bool readFromYamlFile(const std::string& filename); 50 | void writeToYamlFile(const std::string& filename) const; 51 | 52 | Parameters& operator=(const Parameters& other); 53 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 54 | 55 | private: 56 | // projection 57 | double m_k2; 58 | double m_k3; 59 | double m_k4; 60 | double m_k5; 61 | 62 | double m_mu; 63 | double m_mv; 64 | double m_u0; 65 | double m_v0; 66 | }; 67 | 68 | EquidistantCamera(); 69 | 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | EquidistantCamera(const std::string& cameraName, 74 | int imageWidth, int imageHeight, 75 | double k2, double k3, double k4, double k5, 76 | double mu, double mv, 77 | double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | EquidistantCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 121 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 122 | float fx = -1.0f, float fy = -1.0f, 123 | cv::Size imageSize = cv::Size(0, 0), 124 | float cx = -1.0f, float cy = -1.0f, 125 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 126 | 127 | int parameterCount(void) const; 128 | 129 | const Parameters& getParameters(void) const; 130 | void setParameters(const Parameters& parameters); 131 | 132 | void readParameters(const std::vector& parameterVec); 133 | void writeParameters(std::vector& parameterVec) const; 134 | 135 | void writeParametersToYamlFile(const std::string& filename) const; 136 | 137 | std::string parametersToString(void) const; 138 | 139 | private: 140 | template 141 | static T r(T k2, T k3, T k4, T k5, T theta); 142 | 143 | 144 | void fitOddPoly(const std::vector& x, const std::vector& y, 145 | int n, std::vector& coeffs) const; 146 | 147 | void backprojectSymmetric(const Eigen::Vector2d& p_u, 148 | double& theta, double& phi) const; 149 | 150 | Parameters mParameters; 151 | 152 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 153 | }; 154 | 155 | typedef boost::shared_ptr EquidistantCameraPtr; 156 | typedef boost::shared_ptr EquidistantCameraConstPtr; 157 | 158 | template 159 | T 160 | EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) 161 | { 162 | // k1 = 1 163 | return theta + 164 | k2 * theta * theta * theta + 165 | k3 * theta * theta * theta * theta * theta + 166 | k4 * theta * theta * theta * theta * theta * theta * theta + 167 | k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; 168 | } 169 | 170 | template 171 | void 172 | EquidistantCamera::spaceToPlane(const T* const params, 173 | const T* const q, const T* const t, 174 | const Eigen::Matrix& P, 175 | Eigen::Matrix& p) 176 | { 177 | T P_w[3]; 178 | P_w[0] = T(P(0)); 179 | P_w[1] = T(P(1)); 180 | P_w[2] = T(P(2)); 181 | 182 | // Convert quaternion from Eigen convention (x, y, z, w) 183 | // to Ceres convention (w, x, y, z) 184 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 185 | 186 | T P_c[3]; 187 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 188 | 189 | P_c[0] += t[0]; 190 | P_c[1] += t[1]; 191 | P_c[2] += t[2]; 192 | 193 | // project 3D object point to the image plane; 194 | T k2 = params[0]; 195 | T k3 = params[1]; 196 | T k4 = params[2]; 197 | T k5 = params[3]; 198 | T mu = params[4]; 199 | T mv = params[5]; 200 | T u0 = params[6]; 201 | T v0 = params[7]; 202 | 203 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 204 | T theta = acos(P_c[2] / len); 205 | T phi = atan2(P_c[1], P_c[0]); 206 | 207 | Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); 208 | 209 | p(0) = mu * p_u(0) + u0; 210 | p(1) = mv * p_u(1) + v0; 211 | } 212 | 213 | } 214 | 215 | #endif 216 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/PinholeCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef PINHOLECAMERA_H 2 | #define PINHOLECAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | class PinholeCamera: public Camera 14 | { 15 | public: 16 | class Parameters: public Camera::Parameters 17 | { 18 | public: 19 | Parameters(); 20 | Parameters(const std::string& cameraName, 21 | int w, int h, 22 | double k1, double k2, double p1, double p2, 23 | double fx, double fy, double cx, double cy); 24 | 25 | double& k1(void); 26 | double& k2(void); 27 | double& p1(void); 28 | double& p2(void); 29 | double& fx(void); 30 | double& fy(void); 31 | double& cx(void); 32 | double& cy(void); 33 | 34 | double xi(void) const; 35 | double k1(void) const; 36 | double k2(void) const; 37 | double p1(void) const; 38 | double p2(void) const; 39 | double fx(void) const; 40 | double fy(void) const; 41 | double cx(void) const; 42 | double cy(void) const; 43 | 44 | bool readFromYamlFile(const std::string& filename); 45 | void writeToYamlFile(const std::string& filename) const; 46 | 47 | Parameters& operator=(const Parameters& other); 48 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 49 | 50 | private: 51 | double m_k1; 52 | double m_k2; 53 | double m_p1; 54 | double m_p2; 55 | double m_fx; 56 | double m_fy; 57 | double m_cx; 58 | double m_cy; 59 | }; 60 | 61 | PinholeCamera(); 62 | 63 | /** 64 | * \brief Constructor from the projection model parameters 65 | */ 66 | PinholeCamera(const std::string& cameraName, 67 | int imageWidth, int imageHeight, 68 | double k1, double k2, double p1, double p2, 69 | double fx, double fy, double cx, double cy); 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | PinholeCamera(const Parameters& params); 74 | 75 | Camera::ModelType modelType(void) const; 76 | const std::string& cameraName(void) const; 77 | int imageWidth(void) const; 78 | int imageHeight(void) const; 79 | 80 | void estimateIntrinsics(const cv::Size& boardSize, 81 | const std::vector< std::vector >& objectPoints, 82 | const std::vector< std::vector >& imagePoints); 83 | 84 | // Lift points from the image plane to the sphere 85 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 86 | //%output P 87 | 88 | // Lift points from the image plane to the projective space 89 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 90 | //%output P 91 | 92 | // Projects 3D points to the image plane (Pi function) 93 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 94 | //%output p 95 | 96 | // Projects 3D points to the image plane (Pi function) 97 | // and calculates jacobian 98 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 99 | Eigen::Matrix& J) const; 100 | //%output p 101 | //%output J 102 | 103 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 104 | //%output p 105 | 106 | template 107 | static void spaceToPlane(const T* const params, 108 | const T* const q, const T* const t, 109 | const Eigen::Matrix& P, 110 | Eigen::Matrix& p); 111 | 112 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 113 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 114 | Eigen::Matrix2d& J) const; 115 | 116 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 117 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 118 | float fx = -1.0f, float fy = -1.0f, 119 | cv::Size imageSize = cv::Size(0, 0), 120 | float cx = -1.0f, float cy = -1.0f, 121 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 122 | 123 | int parameterCount(void) const; 124 | 125 | const Parameters& getParameters(void) const; 126 | void setParameters(const Parameters& parameters); 127 | 128 | void readParameters(const std::vector& parameterVec); 129 | void writeParameters(std::vector& parameterVec) const; 130 | 131 | void writeParametersToYamlFile(const std::string& filename) const; 132 | 133 | std::string parametersToString(void) const; 134 | 135 | private: 136 | Parameters mParameters; 137 | 138 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 139 | bool m_noDistortion; 140 | }; 141 | 142 | typedef boost::shared_ptr PinholeCameraPtr; 143 | typedef boost::shared_ptr PinholeCameraConstPtr; 144 | 145 | template 146 | void 147 | PinholeCamera::spaceToPlane(const T* const params, 148 | const T* const q, const T* const t, 149 | const Eigen::Matrix& P, 150 | Eigen::Matrix& p) 151 | { 152 | T P_w[3]; 153 | P_w[0] = T(P(0)); 154 | P_w[1] = T(P(1)); 155 | P_w[2] = T(P(2)); 156 | 157 | // Convert quaternion from Eigen convention (x, y, z, w) 158 | // to Ceres convention (w, x, y, z) 159 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 160 | 161 | T P_c[3]; 162 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 163 | 164 | P_c[0] += t[0]; 165 | P_c[1] += t[1]; 166 | P_c[2] += t[2]; 167 | 168 | // project 3D object point to the image plane 169 | T k1 = params[0]; 170 | T k2 = params[1]; 171 | T p1 = params[2]; 172 | T p2 = params[3]; 173 | T fx = params[4]; 174 | T fy = params[5]; 175 | T alpha = T(0); //cameraParams.alpha(); 176 | T cx = params[6]; 177 | T cy = params[7]; 178 | 179 | // Transform to model plane 180 | T u = P_c[0] / P_c[2]; 181 | T v = P_c[1] / P_c[2]; 182 | 183 | T rho_sqr = u * u + v * v; 184 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 185 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 186 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 187 | 188 | u = L * u + du; 189 | v = L * v + dv; 190 | p(0) = fx * (u + alpha * v) + cx; 191 | p(1) = fy * v + cy; 192 | } 193 | 194 | } 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/Chessboard.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARD_H 2 | #define CHESSBOARD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | // forward declarations 11 | class ChessboardCorner; 12 | typedef boost::shared_ptr ChessboardCornerPtr; 13 | class ChessboardQuad; 14 | typedef boost::shared_ptr ChessboardQuadPtr; 15 | 16 | class Chessboard 17 | { 18 | public: 19 | Chessboard(cv::Size boardSize, cv::Mat& image); 20 | 21 | void findCorners(bool useOpenCV = false); 22 | const std::vector& getCorners(void) const; 23 | bool cornersFound(void) const; 24 | 25 | const cv::Mat& getImage(void) const; 26 | const cv::Mat& getSketch(void) const; 27 | 28 | private: 29 | bool findChessboardCorners(const cv::Mat& image, 30 | const cv::Size& patternSize, 31 | std::vector& corners, 32 | int flags, bool useOpenCV); 33 | 34 | bool findChessboardCornersImproved(const cv::Mat& image, 35 | const cv::Size& patternSize, 36 | std::vector& corners, 37 | int flags); 38 | 39 | void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); 40 | 41 | void findConnectedQuads(std::vector& quads, 42 | std::vector& group, 43 | int group_idx, int dilation); 44 | 45 | // int checkQuadGroup(std::vector& quadGroup, 46 | // std::vector& outCorners, 47 | // cv::Size patternSize); 48 | 49 | void labelQuadGroup(std::vector& quad_group, 50 | cv::Size patternSize, bool firstRun); 51 | 52 | void findQuadNeighbors(std::vector& quads, int dilation); 53 | 54 | int augmentBestRun(std::vector& candidateQuads, int candidateDilation, 55 | std::vector& existingQuads, int existingDilation); 56 | 57 | void generateQuads(std::vector& quads, 58 | cv::Mat& image, int flags, 59 | int dilation, bool firstRun); 60 | 61 | bool checkQuadGroup(std::vector& quads, 62 | std::vector& corners, 63 | cv::Size patternSize); 64 | 65 | void getQuadrangleHypotheses(const std::vector< std::vector >& contours, 66 | std::vector< std::pair >& quads, 67 | int classId) const; 68 | 69 | bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; 70 | 71 | bool checkBoardMonotony(std::vector& corners, 72 | cv::Size patternSize); 73 | 74 | bool matchCorners(ChessboardQuadPtr& quad1, int corner1, 75 | ChessboardQuadPtr& quad2, int corner2) const; 76 | 77 | cv::Mat mImage; 78 | cv::Mat mSketch; 79 | std::vector mCorners; 80 | cv::Size mBoardSize; 81 | bool mCornersFound; 82 | }; 83 | 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/ChessboardCorner.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDCORNER_H 2 | #define CHESSBOARDCORNER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | class ChessboardCorner; 11 | typedef boost::shared_ptr ChessboardCornerPtr; 12 | 13 | class ChessboardCorner 14 | { 15 | public: 16 | ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} 17 | 18 | float meanDist(int &n) const 19 | { 20 | float sum = 0; 21 | n = 0; 22 | for (int i = 0; i < 4; ++i) 23 | { 24 | if (neighbors[i].get()) 25 | { 26 | float dx = neighbors[i]->pt.x - pt.x; 27 | float dy = neighbors[i]->pt.y - pt.y; 28 | sum += sqrt(dx*dx + dy*dy); 29 | n++; 30 | } 31 | } 32 | return sum / std::max(n, 1); 33 | } 34 | 35 | cv::Point2f pt; // X and y coordinates 36 | int row; // Row and column of the corner 37 | int column; // in the found pattern 38 | bool needsNeighbor; // Does the corner require a neighbor? 39 | int count; // number of corner neighbors 40 | ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors 41 | }; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/ChessboardQuad.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDQUAD_H 2 | #define CHESSBOARDQUAD_H 3 | 4 | #include 5 | 6 | #include "camodocal/chessboard/ChessboardCorner.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class ChessboardQuad; 12 | typedef boost::shared_ptr ChessboardQuadPtr; 13 | 14 | class ChessboardQuad 15 | { 16 | public: 17 | ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} 18 | 19 | int count; // Number of quad neighbors 20 | int group_idx; // Quad group ID 21 | float edge_len; // Smallest side length^2 22 | ChessboardCornerPtr corners[4]; // Coordinates of quad corners 23 | ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors 24 | bool labeled; // Has this corner been labeled? 25 | }; 26 | 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGENQUATERNIONPARAMETERIZATION_H 2 | #define EIGENQUATERNIONPARAMETERIZATION_H 3 | 4 | #include "ceres/local_parameterization.h" 5 | 6 | namespace camodocal 7 | { 8 | 9 | class EigenQuaternionParameterization : public ceres::LocalParameterization 10 | { 11 | public: 12 | virtual ~EigenQuaternionParameterization() {} 13 | virtual bool Plus(const double* x, 14 | const double* delta, 15 | double* x_plus_delta) const; 16 | virtual bool ComputeJacobian(const double* x, 17 | double* jacobian) const; 18 | virtual int GlobalSize() const { return 4; } 19 | virtual int LocalSize() const { return 3; } 20 | 21 | private: 22 | template 23 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; 24 | }; 25 | 26 | 27 | template 28 | void 29 | EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const 30 | { 31 | zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; 32 | zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; 33 | zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; 34 | zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; 35 | } 36 | 37 | } 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/gpl/gpl.h: -------------------------------------------------------------------------------- 1 | #ifndef GPL_H 2 | #define GPL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | template 12 | const T clamp(const T& v, const T& a, const T& b) 13 | { 14 | return std::min(b, std::max(a, v)); 15 | } 16 | 17 | double hypot3(double x, double y, double z); 18 | float hypot3f(float x, float y, float z); 19 | 20 | template 21 | const T normalizeTheta(const T& theta) 22 | { 23 | T normTheta = theta; 24 | 25 | while (normTheta < - M_PI) 26 | { 27 | normTheta += 2.0 * M_PI; 28 | } 29 | while (normTheta > M_PI) 30 | { 31 | normTheta -= 2.0 * M_PI; 32 | } 33 | 34 | return normTheta; 35 | } 36 | 37 | double d2r(double deg); 38 | float d2r(float deg); 39 | double r2d(double rad); 40 | float r2d(float rad); 41 | 42 | double sinc(double theta); 43 | 44 | template 45 | const T square(const T& x) 46 | { 47 | return x * x; 48 | } 49 | 50 | template 51 | const T cube(const T& x) 52 | { 53 | return x * x * x; 54 | } 55 | 56 | template 57 | const T random(const T& a, const T& b) 58 | { 59 | return static_cast(rand()) / RAND_MAX * (b - a) + a; 60 | } 61 | 62 | template 63 | const T randomNormal(const T& sigma) 64 | { 65 | T x1, x2, w; 66 | 67 | do 68 | { 69 | x1 = 2.0 * random(0.0, 1.0) - 1.0; 70 | x2 = 2.0 * random(0.0, 1.0) - 1.0; 71 | w = x1 * x1 + x2 * x2; 72 | } 73 | while (w >= 1.0 || w == 0.0); 74 | 75 | w = sqrt((-2.0 * log(w)) / w); 76 | 77 | return x1 * w * sigma; 78 | } 79 | 80 | unsigned long long timeInMicroseconds(void); 81 | 82 | double timeInSeconds(void); 83 | 84 | void colorDepthImage(cv::Mat& imgDepth, 85 | cv::Mat& imgColoredDepth, 86 | float minRange, float maxRange); 87 | 88 | bool colormap(const std::string& name, unsigned char idx, 89 | float& r, float& g, float& b); 90 | 91 | std::vector bresLine(int x0, int y0, int x1, int y1); 92 | std::vector bresCircle(int x0, int y0, int r); 93 | 94 | void fitCircle(const std::vector& points, 95 | double& centerX, double& centerY, double& radius); 96 | 97 | std::vector intersectCircles(double x1, double y1, double r1, 98 | double x2, double y2, double r2); 99 | 100 | void LLtoUTM(double latitude, double longitude, 101 | double& utmNorthing, double& utmEasting, 102 | std::string& utmZone); 103 | void UTMtoLL(double utmNorthing, double utmEasting, 104 | const std::string& utmZone, 105 | double& latitude, double& longitude); 106 | 107 | long int timestampDiff(uint64_t t1, uint64_t t2); 108 | 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/sparse_graph/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORM_H 2 | #define TRANSFORM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | class Transform 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | Transform(); 17 | Transform(const Eigen::Matrix4d& H); 18 | 19 | Eigen::Quaterniond& rotation(void); 20 | const Eigen::Quaterniond& rotation(void) const; 21 | double* rotationData(void); 22 | const double* const rotationData(void) const; 23 | 24 | Eigen::Vector3d& translation(void); 25 | const Eigen::Vector3d& translation(void) const; 26 | double* translationData(void); 27 | const double* const translationData(void) const; 28 | 29 | Eigen::Matrix4d toMatrix(void) const; 30 | 31 | private: 32 | Eigen::Quaterniond m_q; 33 | Eigen::Vector3d m_t; 34 | }; 35 | 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /camera_model/instruction: -------------------------------------------------------------------------------- 1 | rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/ 2 | -------------------------------------------------------------------------------- /camera_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | camera_model 4 | 0.0.0 5 | The camera_model 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 | std_msgs 45 | roscpp 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /camera_model/readme.md: -------------------------------------------------------------------------------- 1 | part of [camodocal](https://github.com/hengli/camodocal) 2 | 3 | [Google Ceres](http://ceres-solver.org) is needed. 4 | 5 | # Calibration: 6 | 7 | Use [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera. 8 | 9 | # Undistortion: 10 | 11 | See [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: 12 | 13 | - liftProjective: Lift points from the image plane to the projective space. 14 | - spaceToPlane: Projects 3D points to the image plane (Pi function) 15 | 16 | -------------------------------------------------------------------------------- /camera_model/src/camera_models/Camera.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/camera_models/Camera.h" 2 | #include "camodocal/camera_models/ScaramuzzaCamera.h" 3 | 4 | #include 5 | 6 | namespace camodocal 7 | { 8 | 9 | Camera::Parameters::Parameters(ModelType modelType) 10 | : m_modelType(modelType) 11 | , m_imageWidth(0) 12 | , m_imageHeight(0) 13 | { 14 | switch (modelType) 15 | { 16 | case KANNALA_BRANDT: 17 | m_nIntrinsics = 8; 18 | break; 19 | case PINHOLE: 20 | m_nIntrinsics = 8; 21 | break; 22 | case SCARAMUZZA: 23 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 24 | break; 25 | case MEI: 26 | default: 27 | m_nIntrinsics = 9; 28 | } 29 | } 30 | 31 | Camera::Parameters::Parameters(ModelType modelType, 32 | const std::string& cameraName, 33 | int w, int h) 34 | : m_modelType(modelType) 35 | , m_cameraName(cameraName) 36 | , m_imageWidth(w) 37 | , m_imageHeight(h) 38 | { 39 | switch (modelType) 40 | { 41 | case KANNALA_BRANDT: 42 | m_nIntrinsics = 8; 43 | break; 44 | case PINHOLE: 45 | m_nIntrinsics = 8; 46 | break; 47 | case SCARAMUZZA: 48 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 49 | break; 50 | case MEI: 51 | default: 52 | m_nIntrinsics = 9; 53 | } 54 | } 55 | 56 | Camera::ModelType& 57 | Camera::Parameters::modelType(void) 58 | { 59 | return m_modelType; 60 | } 61 | 62 | std::string& 63 | Camera::Parameters::cameraName(void) 64 | { 65 | return m_cameraName; 66 | } 67 | 68 | int& 69 | Camera::Parameters::imageWidth(void) 70 | { 71 | return m_imageWidth; 72 | } 73 | 74 | int& 75 | Camera::Parameters::imageHeight(void) 76 | { 77 | return m_imageHeight; 78 | } 79 | 80 | Camera::ModelType 81 | Camera::Parameters::modelType(void) const 82 | { 83 | return m_modelType; 84 | } 85 | 86 | const std::string& 87 | Camera::Parameters::cameraName(void) const 88 | { 89 | return m_cameraName; 90 | } 91 | 92 | int 93 | Camera::Parameters::imageWidth(void) const 94 | { 95 | return m_imageWidth; 96 | } 97 | 98 | int 99 | Camera::Parameters::imageHeight(void) const 100 | { 101 | return m_imageHeight; 102 | } 103 | 104 | int 105 | Camera::Parameters::nIntrinsics(void) const 106 | { 107 | return m_nIntrinsics; 108 | } 109 | 110 | cv::Mat& 111 | Camera::mask(void) 112 | { 113 | return m_mask; 114 | } 115 | 116 | const cv::Mat& 117 | Camera::mask(void) const 118 | { 119 | return m_mask; 120 | } 121 | 122 | void 123 | Camera::estimateExtrinsics(const std::vector& objectPoints, 124 | const std::vector& imagePoints, 125 | cv::Mat& rvec, cv::Mat& tvec) const 126 | { 127 | std::vector Ms(imagePoints.size()); 128 | for (size_t i = 0; i < Ms.size(); ++i) 129 | { 130 | Eigen::Vector3d P; 131 | liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); 132 | 133 | P /= P(2); 134 | 135 | Ms.at(i).x = P(0); 136 | Ms.at(i).y = P(1); 137 | } 138 | 139 | // assume unit focal length, zero principal point, and zero distortion 140 | cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); 141 | } 142 | 143 | double 144 | Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const 145 | { 146 | Eigen::Vector2d p1, p2; 147 | 148 | spaceToPlane(P1, p1); 149 | spaceToPlane(P2, p2); 150 | 151 | return (p1 - p2).norm(); 152 | } 153 | 154 | double 155 | Camera::reprojectionError(const std::vector< std::vector >& objectPoints, 156 | const std::vector< std::vector >& imagePoints, 157 | const std::vector& rvecs, 158 | const std::vector& tvecs, 159 | cv::OutputArray _perViewErrors) const 160 | { 161 | int imageCount = objectPoints.size(); 162 | size_t pointsSoFar = 0; 163 | double totalErr = 0.0; 164 | 165 | bool computePerViewErrors = _perViewErrors.needed(); 166 | cv::Mat perViewErrors; 167 | if (computePerViewErrors) 168 | { 169 | _perViewErrors.create(imageCount, 1, CV_64F); 170 | perViewErrors = _perViewErrors.getMat(); 171 | } 172 | 173 | for (int i = 0; i < imageCount; ++i) 174 | { 175 | size_t pointCount = imagePoints.at(i).size(); 176 | 177 | pointsSoFar += pointCount; 178 | 179 | std::vector estImagePoints; 180 | projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), 181 | estImagePoints); 182 | 183 | double err = 0.0; 184 | for (size_t j = 0; j < imagePoints.at(i).size(); ++j) 185 | { 186 | err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); 187 | } 188 | 189 | if (computePerViewErrors) 190 | { 191 | perViewErrors.at(i) = err / pointCount; 192 | } 193 | 194 | totalErr += err; 195 | } 196 | 197 | return totalErr / pointsSoFar; 198 | } 199 | 200 | double 201 | Camera::reprojectionError(const Eigen::Vector3d& P, 202 | const Eigen::Quaterniond& camera_q, 203 | const Eigen::Vector3d& camera_t, 204 | const Eigen::Vector2d& observed_p) const 205 | { 206 | Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; 207 | 208 | Eigen::Vector2d p; 209 | spaceToPlane(P_cam, p); 210 | 211 | return (p - observed_p).norm(); 212 | } 213 | 214 | void 215 | Camera::projectPoints(const std::vector& objectPoints, 216 | const cv::Mat& rvec, 217 | const cv::Mat& tvec, 218 | std::vector& imagePoints) const 219 | { 220 | // project 3D object points to the image plane 221 | imagePoints.reserve(objectPoints.size()); 222 | 223 | //double 224 | cv::Mat R0; 225 | cv::Rodrigues(rvec, R0); 226 | 227 | Eigen::MatrixXd R(3,3); 228 | R << R0.at(0,0), R0.at(0,1), R0.at(0,2), 229 | R0.at(1,0), R0.at(1,1), R0.at(1,2), 230 | R0.at(2,0), R0.at(2,1), R0.at(2,2); 231 | 232 | Eigen::Vector3d t; 233 | t << tvec.at(0), tvec.at(1), tvec.at(2); 234 | 235 | for (size_t i = 0; i < objectPoints.size(); ++i) 236 | { 237 | const cv::Point3f& objectPoint = objectPoints.at(i); 238 | 239 | // Rotate and translate 240 | Eigen::Vector3d P; 241 | P << objectPoint.x, objectPoint.y, objectPoint.z; 242 | 243 | P = R * P + t; 244 | 245 | Eigen::Vector2d p; 246 | spaceToPlane(P, p); 247 | 248 | imagePoints.push_back(cv::Point2f(p(0), p(1))); 249 | } 250 | } 251 | 252 | } 253 | -------------------------------------------------------------------------------- /camera_model/src/camera_models/CameraFactory.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/camera_models/CameraFactory.h" 2 | 3 | #include 4 | 5 | 6 | #include "camodocal/camera_models/CataCamera.h" 7 | #include "camodocal/camera_models/EquidistantCamera.h" 8 | #include "camodocal/camera_models/PinholeCamera.h" 9 | #include "camodocal/camera_models/ScaramuzzaCamera.h" 10 | 11 | #include "ceres/ceres.h" 12 | 13 | namespace camodocal 14 | { 15 | 16 | boost::shared_ptr CameraFactory::m_instance; 17 | 18 | CameraFactory::CameraFactory() 19 | { 20 | 21 | } 22 | 23 | boost::shared_ptr 24 | CameraFactory::instance(void) 25 | { 26 | if (m_instance.get() == 0) 27 | { 28 | m_instance.reset(new CameraFactory); 29 | } 30 | 31 | return m_instance; 32 | } 33 | 34 | CameraPtr 35 | CameraFactory::generateCamera(Camera::ModelType modelType, 36 | const std::string& cameraName, 37 | cv::Size imageSize) const 38 | { 39 | switch (modelType) 40 | { 41 | case Camera::KANNALA_BRANDT: 42 | { 43 | EquidistantCameraPtr camera(new EquidistantCamera); 44 | 45 | EquidistantCamera::Parameters params = camera->getParameters(); 46 | params.cameraName() = cameraName; 47 | params.imageWidth() = imageSize.width; 48 | params.imageHeight() = imageSize.height; 49 | camera->setParameters(params); 50 | return camera; 51 | } 52 | case Camera::PINHOLE: 53 | { 54 | PinholeCameraPtr camera(new PinholeCamera); 55 | 56 | PinholeCamera::Parameters params = camera->getParameters(); 57 | params.cameraName() = cameraName; 58 | params.imageWidth() = imageSize.width; 59 | params.imageHeight() = imageSize.height; 60 | camera->setParameters(params); 61 | return camera; 62 | } 63 | case Camera::SCARAMUZZA: 64 | { 65 | OCAMCameraPtr camera(new OCAMCamera); 66 | 67 | OCAMCamera::Parameters params = camera->getParameters(); 68 | params.cameraName() = cameraName; 69 | params.imageWidth() = imageSize.width; 70 | params.imageHeight() = imageSize.height; 71 | camera->setParameters(params); 72 | return camera; 73 | } 74 | case Camera::MEI: 75 | default: 76 | { 77 | CataCameraPtr camera(new CataCamera); 78 | 79 | CataCamera::Parameters params = camera->getParameters(); 80 | params.cameraName() = cameraName; 81 | params.imageWidth() = imageSize.width; 82 | params.imageHeight() = imageSize.height; 83 | camera->setParameters(params); 84 | return camera; 85 | } 86 | } 87 | } 88 | 89 | CameraPtr 90 | CameraFactory::generateCameraFromYamlFile(const std::string& filename) 91 | { 92 | cv::FileStorage fs(filename, cv::FileStorage::READ); 93 | 94 | if (!fs.isOpened()) 95 | { 96 | return CameraPtr(); 97 | } 98 | 99 | Camera::ModelType modelType = Camera::MEI; 100 | if (!fs["model_type"].isNone()) 101 | { 102 | std::string sModelType; 103 | fs["model_type"] >> sModelType; 104 | 105 | if (boost::iequals(sModelType, "kannala_brandt")) 106 | { 107 | modelType = Camera::KANNALA_BRANDT; 108 | } 109 | else if (boost::iequals(sModelType, "mei")) 110 | { 111 | modelType = Camera::MEI; 112 | } 113 | else if (boost::iequals(sModelType, "scaramuzza")) 114 | { 115 | modelType = Camera::SCARAMUZZA; 116 | } 117 | else if (boost::iequals(sModelType, "pinhole")) 118 | { 119 | modelType = Camera::PINHOLE; 120 | } 121 | else 122 | { 123 | std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; 124 | return CameraPtr(); 125 | } 126 | } 127 | 128 | switch (modelType) 129 | { 130 | case Camera::KANNALA_BRANDT: 131 | { 132 | EquidistantCameraPtr camera(new EquidistantCamera); 133 | 134 | EquidistantCamera::Parameters params = camera->getParameters(); 135 | params.readFromYamlFile(filename); 136 | camera->setParameters(params); 137 | return camera; 138 | } 139 | case Camera::PINHOLE: 140 | { 141 | PinholeCameraPtr camera(new PinholeCamera); 142 | 143 | PinholeCamera::Parameters params = camera->getParameters(); 144 | params.readFromYamlFile(filename); 145 | camera->setParameters(params); 146 | return camera; 147 | } 148 | case Camera::SCARAMUZZA: 149 | { 150 | OCAMCameraPtr camera(new OCAMCamera); 151 | 152 | OCAMCamera::Parameters params = camera->getParameters(); 153 | params.readFromYamlFile(filename); 154 | camera->setParameters(params); 155 | return camera; 156 | } 157 | case Camera::MEI: 158 | default: 159 | { 160 | CataCameraPtr camera(new CataCamera); 161 | 162 | CataCamera::Parameters params = camera->getParameters(); 163 | params.readFromYamlFile(filename); 164 | camera->setParameters(params); 165 | return camera; 166 | } 167 | } 168 | 169 | return CameraPtr(); 170 | } 171 | 172 | } 173 | 174 | -------------------------------------------------------------------------------- /camera_model/src/gpl/EigenQuaternionParameterization.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/gpl/EigenQuaternionParameterization.h" 2 | 3 | #include 4 | 5 | namespace camodocal 6 | { 7 | 8 | bool 9 | EigenQuaternionParameterization::Plus(const double* x, 10 | const double* delta, 11 | double* x_plus_delta) const 12 | { 13 | const double norm_delta = 14 | sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); 15 | if (norm_delta > 0.0) 16 | { 17 | const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); 18 | double q_delta[4]; 19 | q_delta[0] = sin_delta_by_delta * delta[0]; 20 | q_delta[1] = sin_delta_by_delta * delta[1]; 21 | q_delta[2] = sin_delta_by_delta * delta[2]; 22 | q_delta[3] = cos(norm_delta); 23 | EigenQuaternionProduct(q_delta, x, x_plus_delta); 24 | } 25 | else 26 | { 27 | for (int i = 0; i < 4; ++i) 28 | { 29 | x_plus_delta[i] = x[i]; 30 | } 31 | } 32 | return true; 33 | } 34 | 35 | bool 36 | EigenQuaternionParameterization::ComputeJacobian(const double* x, 37 | double* jacobian) const 38 | { 39 | jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT 40 | jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT 41 | jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT 42 | jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT 43 | return true; 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /camera_model/src/sparse_graph/Transform.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace camodocal 4 | { 5 | 6 | Transform::Transform() 7 | { 8 | m_q.setIdentity(); 9 | m_t.setZero(); 10 | } 11 | 12 | Transform::Transform(const Eigen::Matrix4d& H) 13 | { 14 | m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); 15 | m_t = H.block<3,1>(0,3); 16 | } 17 | 18 | Eigen::Quaterniond& 19 | Transform::rotation(void) 20 | { 21 | return m_q; 22 | } 23 | 24 | const Eigen::Quaterniond& 25 | Transform::rotation(void) const 26 | { 27 | return m_q; 28 | } 29 | 30 | double* 31 | Transform::rotationData(void) 32 | { 33 | return m_q.coeffs().data(); 34 | } 35 | 36 | const double* const 37 | Transform::rotationData(void) const 38 | { 39 | return m_q.coeffs().data(); 40 | } 41 | 42 | Eigen::Vector3d& 43 | Transform::translation(void) 44 | { 45 | return m_t; 46 | } 47 | 48 | const Eigen::Vector3d& 49 | Transform::translation(void) const 50 | { 51 | return m_t; 52 | } 53 | 54 | double* 55 | Transform::translationData(void) 56 | { 57 | return m_t.data(); 58 | } 59 | 60 | const double* const 61 | Transform::translationData(void) const 62 | { 63 | return m_t.data(); 64 | } 65 | 66 | Eigen::Matrix4d 67 | Transform::toMatrix(void) const 68 | { 69 | Eigen::Matrix4d H; 70 | H.setIdentity(); 71 | H.block<3,3>(0,0) = m_q.toRotationMatrix(); 72 | H.block<3,1>(0,3) = m_t; 73 | 74 | return H; 75 | } 76 | 77 | } 78 | -------------------------------------------------------------------------------- /config/3dm/3dm_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu_3dm_gx4/imu" 5 | image_topic: "/mv_25001498/image_raw" 6 | output_path: "/home/tony-ws1/output/" 7 | 8 | # MEI is better than PINHOLE for large FOV camera 9 | model_type: MEI 10 | camera_name: camera 11 | image_width: 752 12 | image_height: 480 13 | mirror_parameters: 14 | xi: 2.057e+00 15 | distortion_parameters: 16 | k1: 7.145e-02 17 | k2: 5.059e-01 18 | p1: 4.727e-05 19 | p2: -5.492e-04 20 | projection_parameters: 21 | gamma1: 1.115e+03 22 | gamma2: 1.114e+03 23 | u0: 3.672e+02 24 | v0: 2.385e+02 25 | 26 | # Extrinsic parameter between IMU and Camera. 27 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 28 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 29 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 30 | #If you choose 0 or 1, you should write down the following matrix. 31 | #Rotation from camera frame to imu frame, imu^R_cam 32 | extrinsicRotation: !!opencv-matrix 33 | rows: 3 34 | cols: 3 35 | dt: d 36 | data: [ 0, 0, -1, 37 | -1, 0, 0, 38 | 0, 1, 0] 39 | 40 | #Translation from camera frame to imu frame, imu^T_cam 41 | extrinsicTranslation: !!opencv-matrix 42 | rows: 3 43 | cols: 1 44 | dt: d 45 | data: [ 0, 0, 0.02] 46 | 47 | #feature traker paprameters 48 | 49 | max_cnt: 150 # max feature number in feature tracking 50 | min_dist: 20 # min distance between two features 51 | freq: 20 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 52 | F_threshold: 1.0 # ransac threshold (pixel) 53 | show_track: 1 # publish tracking image as topic 54 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 55 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 56 | #optimization parameters 57 | 58 | max_solver_time: 0.035 # max solver itration time (ms), to guarantee real time 59 | max_num_iterations: 10 # max solver itrations, to guarantee real time 60 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 61 | 62 | #imu parameters The more accurate parameters you provide, the better performance 63 | acc_n: 0.2 # accelerometer measurement noise standard deviation. 64 | gyr_n: 0.05 # gyroscope measurement noise standard deviation. 65 | acc_w: 0.002 # accelerometer bias random work noise standard deviation. 66 | gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. 67 | g_norm: 9.805 # 68 | 69 | #loop closure parameters 70 | loop_closure: 0 # start loop closure 71 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 72 | fast_relocalization: 0 # useful in real-time and large project 73 | pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 74 | 75 | #unsynchronization parameters 76 | estimate_td: 0 # online estimate time offset between camera and imu 77 | td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 78 | 79 | #rolling shutter parameters 80 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 81 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 82 | 83 | #visualization parameters 84 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 85 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 86 | visualize_camera_size: 0.4 # size of camera marker in RVIZ -------------------------------------------------------------------------------- /config/black_box/black_box_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/djiros/imu" 5 | image_topic: "/djiros/image" 6 | output_path: "/home/urop/output/" # vins outputs will be wrttento vins_folder_path + output_path 7 | 8 | #camera calibration 9 | model_type: MEI 10 | camera_name: camera 11 | image_width: 752 12 | image_height: 480 13 | mirror_parameters: 14 | xi: 2.2134257311108083e+00 15 | distortion_parameters: 16 | k1: 1.4213768437132895e-01 17 | k2: 9.1226950620748259e-01 18 | p1: 1.2056297779277966e-03 19 | p2: 2.0300076091651340e-03 20 | projection_parameters: 21 | gamma1: 1.1659242643040975e+03 22 | gamma2: 1.1656143723709608e+03 23 | u0: 3.9238492754088008e+02 24 | v0: 2.4392485271819217e+02 25 | 26 | 27 | # Extrinsic parameter between IMU and Camera. 28 | estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 29 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 30 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 31 | #If you choose 0 or 1, you should write down the following matrix. 32 | #Rotation from camera frame to imu frame, imu^R_cam 33 | extrinsicRotation: !!opencv-matrix 34 | rows: 3 35 | cols: 3 36 | dt: d 37 | data: [ 3.5547377966504534e-02, -9.9819308302994181e-01, 38 | -4.8445360055281682e-02, 9.9855985185796758e-01, 39 | 3.3527772724371241e-02, 4.1882104932016398e-02, 40 | -4.0182162424389177e-02, -4.9864390574059170e-02, 41 | 9.9794736152543517e-01 ] 42 | extrinsicTranslation: !!opencv-matrix 43 | rows: 3 44 | cols: 1 45 | dt: d 46 | data: [ 6.5272135116678912e-02, -6.8544177447541876e-02, 47 | 3.7304589197375740e-02 ] 48 | 49 | 50 | #feature traker paprameters 51 | 52 | max_cnt: 120 # max feature number in feature tracking 53 | min_dist: 30 # min distance between two features 54 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 55 | F_threshold: 1.0 # ransac threshold (pixel) 56 | show_track: 1 # publish tracking image as topic 57 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 58 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 59 | 60 | #optimization parameters 61 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 62 | max_num_iterations: 8 # max solver itrations, to guarantee real time 63 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 64 | 65 | #imu parameters The more accurate parameters you provide, the better performance 66 | acc_n: 0.2 # accelerometer measurement noise standard deviation. 67 | gyr_n: 0.05 # gyroscope measurement noise standard deviation. 68 | acc_w: 0.002 # accelerometer bias random work noise standard deviation. 69 | gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. 70 | g_norm: 9.805 # 71 | 72 | #loop closure parameters 73 | loop_closure: 1 # start loop closure 74 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 75 | fast_relocalization: 1 # useful in real-time and large project 76 | pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 77 | 78 | #unsynchronization parameters 79 | estimate_td: 1 # online estimate time offset between camera and imu 80 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 81 | 82 | #rolling shutter parameters 83 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 84 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 85 | 86 | #visualization parameters 87 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 88 | visualize_imu_forward: 1 # output imu forward propogation to achieve low latency and high frequence results 89 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 90 | -------------------------------------------------------------------------------- /config/cla/cla_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/tony-ws1/output/" 7 | 8 | #camera calibration 9 | model_type: KANNALA_BRANDT 10 | camera_name: camera 11 | image_width: 752 12 | image_height: 480 13 | projection_parameters: 14 | k2: -0.005740195474458931 15 | k3: 0.02878252863739417 16 | k4: -0.04010621197185408 17 | k5: 0.02008469575876223 18 | mu: 472.2863830700696 19 | mv: 470.83759684346785 20 | u0: 368.8316828103749 21 | v0: 232.23688706965652 22 | 23 | # Extrinsic parameter between IMU and Camera. 24 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 25 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 26 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 27 | #If you choose 0 or 1, you should write down the following matrix. 28 | #Rotation from camera frame to imu frame, imu^R_cam 29 | extrinsicRotation: !!opencv-matrix 30 | rows: 3 31 | cols: 3 32 | dt: d 33 | data: [ 0.99992185, -0.01241689, -0.00145542, 34 | 0.01242827, 0.99989004, 0.00809021, 35 | 0.0013548, -0.00810766, 0.99996621 ] 36 | extrinsicTranslation: !!opencv-matrix 37 | rows: 3 38 | cols: 1 39 | dt: d 40 | data: [ 0.03661314, -0.01027102, -0.00654466 ] 41 | 42 | #feature traker paprameters 43 | max_cnt: 150 # max feature number in feature tracking 44 | min_dist: 30 # min distance between two features 45 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 46 | F_threshold: 1.0 # ransac threshold (pixel) 47 | show_track: 1 # publish tracking image as topic 48 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 49 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 50 | 51 | #optimization parameters 52 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 53 | max_num_iterations: 8 # max solver itrations, to guarantee real time 54 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 55 | 56 | #imu parameters The more accurate parameters you provide, the better performance 57 | acc_n: 4e-2 # accelerometer measurement noise standard deviation. #0.2 4e-2 58 | gyr_n: 1e-3 # gyroscope measurement noise standard deviation. #0.05 1e-3 59 | acc_w: 1e-3 # accelerometer bias random work noise standard deviation. #0.02 4e-2 4e-3 1e-2 60 | gyr_w: 1e-4 # gyroscope bias random work noise standard deviation. #4.0e-5 1e-3 1e-4 1e-4 61 | g_norm: 9.81 # gravity magnitude 62 | 63 | 64 | #loop closure parameters 65 | loop_closure: 1 # start loop closure 66 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 67 | fast_relocalization: 0 # useful in real-time and large project 68 | pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 69 | 70 | #unsynchronization parameters 71 | estimate_td: 0 # online estimate time offset between camera and imu 72 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 73 | 74 | #rolling shutter parameters 75 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 76 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 77 | 78 | #visualization parameters 79 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 80 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 81 | visualize_camera_size: 0.4 # size of camera marker in RVIZ -------------------------------------------------------------------------------- /config/euroc/calib_results1216.txt: -------------------------------------------------------------------------------- 1 | #polynomial coefficients for the DIRECT mapping function (ocam_model.ss in MATLAB). These are used by cam2world 2 | 3 | 5 -2.463506e+02 0.000000e+00 1.872966e-03 -2.284953e-06 5.082365e-09 4 | 5 | #polynomial coefficients for the inverse mapping function (ocam_model.invpol in MATLAB). These are used by world2cam 6 | 7 | 14 376.943961 246.867309 17.112154 20.656636 19.342198 6.661639 0.101614 5.635293 4.316109 -1.987921 -1.951446 0.989571 1.198098 0.280377 8 | 9 | #center: "row" and "column", starting from 0 (C convention) 10 | 11 | 485.978236 645.104835 12 | 13 | #affine parameters "c", "d", "e" 14 | 15 | 1.000045 0.000016 -0.000022 16 | 17 | #image size: "height" and "width" 18 | 19 | 960 1280 20 | 21 | -------------------------------------------------------------------------------- /config/euroc/euroc_config_no_extrinsic.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/tony-ws1/output/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 752 12 | image_height: 480 13 | distortion_parameters: 14 | k1: -2.917e-01 15 | k2: 8.228e-02 16 | p1: 5.333e-05 17 | p2: -1.578e-04 18 | projection_parameters: 19 | fx: 4.616e+02 20 | fy: 4.603e+02 21 | cx: 3.630e+02 22 | cy: 2.481e+02 23 | 24 | # Extrinsic parameter between IMU and Camera. 25 | estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 26 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 27 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 28 | #If you choose 0 or 1, you should write down the following matrix. 29 | 30 | #feature traker paprameters 31 | max_cnt: 150 # max feature number in feature tracking 32 | min_dist: 30 # min distance between two features 33 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 34 | F_threshold: 1.0 # ransac threshold (pixel) 35 | show_track: 1 # publish tracking image as topic 36 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 37 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 38 | 39 | #optimization parameters 40 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 41 | max_num_iterations: 8 # max solver itrations, to guarantee real time 42 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 43 | 44 | #imu parameters The more accurate parameters you provide, the better performance 45 | acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 46 | gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05 47 | acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02 48 | gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 49 | g_norm: 9.81007 # gravity magnitude 50 | 51 | #loop closure parameters 52 | loop_closure: 1 # start loop closure 53 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 54 | fast_relocalization: 0 # useful in real-time and large project 55 | pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 56 | 57 | #unsynchronization parameters 58 | estimate_td: 0 # online estimate time offset between camera and imu 59 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 60 | 61 | #rolling shutter parameters 62 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 63 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 64 | 65 | #visualization parameters 66 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 67 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 68 | visualize_camera_size: 0.4 # size of camera marker in RVIZ -------------------------------------------------------------------------------- /config/extrinsic_parameter_example.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/config/extrinsic_parameter_example.pdf -------------------------------------------------------------------------------- /config/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/config/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fisheye_mask_752x480.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/config/fisheye_mask_752x480.jpg -------------------------------------------------------------------------------- /config/mindvision/mindvision.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/mavros/imu/data" 5 | #image_topic: "/cam0/image_raw" 6 | image_topic: "/mindvision/mindvision/left/image_mono" 7 | output_path: "/home/wt/slam/0118/src/VINS-Mono/output/" 8 | 9 | #camera calibration 10 | model_type: scaramuzza 11 | camera_name: pal 12 | image_width: 1280 13 | image_height: 960 14 | # poly_parameters: 15 | # p0: -2.463506e+02 16 | # p1: 0.000000e+00 17 | # p2: 1.872966e-03 18 | # p3: -2.284953e-06 19 | # p4: 5.082365e-09 20 | # inv_poly_parameters: 21 | # p0: 376.943961 22 | # p1: 246.867309 23 | # p2: 17.112154 24 | # p3: 20.656636 25 | # p4: 19.342198 26 | # p5: 6.661639 27 | # p6: 0.101614 28 | # p7: 5.635293 29 | # p8: 4.316109 30 | # p9: -1.987921 31 | # p10: -1.951446 32 | # p11: 0.989571 33 | # p12: 1.198098 34 | # p13: 0.280377 35 | # p14: 0.0 36 | # p15: 0.0 37 | # p16: 0.0 38 | # p17: 0.0 39 | # p18: 0.0 40 | # p19: 0.0 41 | # affine_parameters: 42 | # ac: 1.000045 43 | # ad: 0.000016 44 | # ae: -0.000022 45 | # cx: 645.104835 46 | # cy: 485.978236 47 | 48 | poly_parameters: 49 | p0: -2.445239e+02 50 | p1: 0.000000e+00 51 | p2: 1.748610e-03 52 | p3: -1.757770e-06 53 | p4: 4.475965e-09 54 | inv_poly_parameters: 55 | p0: 376.845565 56 | p1: 246.746504 57 | p2: 19.035187 58 | p3: 23.840497 59 | p4: 18.991943 60 | p5: 6.066253 61 | p6: 1.560387 62 | p7: 5.854280 63 | p8: 3.458320 64 | p9: -1.995166 65 | p10: -1.509264 66 | p11: 1.089614 67 | p12: 1.340245 68 | p13: 0.255323 69 | p14: 0.0 70 | p15: 0.0 71 | p16: 0.0 72 | p17: 0.0 73 | p18: 0.0 74 | p19: 0.0 75 | affine_parameters: 76 | ac: 1.000000 77 | ad: 0.000000 78 | ae: 0.000000 79 | cx: 645.107791 80 | cy: 486.025172 81 | 82 | # Extrinsic parameter between IMU and Camera. 83 | estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 84 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 85 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 86 | #If you choose 0 or 1, you should write down the following matrix. 87 | #Rotation from camera frame to imu frame, imu^R_cam 88 | extrinsicRotation: !!opencv-matrix 89 | rows: 3 90 | cols: 3 91 | dt: d 92 | # data: [0.0148655429818, -0.999880929698, 0.00414029679422, 93 | # 0.999557249008, 0.0149672133247, 0.025715529948, 94 | # -0.0257744366974, 0.00375618835797, 0.999660727178] 95 | # data: [-0.999949700263649, 0.007760552460550 , 0.006330159910637, 96 | # -0.007778710351654 ,-0.999965377760029 , -0.002848281184658, 97 | # 0.006307846959537, -0.002897386539466, 0.999975815700400] 98 | data: [-1.0, 0.0, 0.0, 99 | 0.0,-1.0, 0.0, 100 | 0.0, 0.0, 1.0] 101 | #Translation from camera frame to imu frame, imu^T_cam 102 | extrinsicTranslation: !!opencv-matrix 103 | rows: 3 104 | cols: 1 105 | dt: d 106 | # data: [-0.0216401454975,-0.064676986768, 0.00981073058949] 107 | data: [0.0,0.0,0.03] 108 | 109 | #feature traker paprameters 110 | max_cnt: 200 # max feature number in feature tracking 111 | min_dist: 20 # min distance between two features 112 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 113 | F_threshold: 1.0 # ransac threshold (pixel) 114 | show_track: 1 # publish tracking image as topic 115 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 116 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 117 | 118 | #added by wz 119 | center_x: 640.991301 120 | center_y: 490.937512 121 | max_r: 500 122 | min_r: 160 123 | 124 | # max_r: 500 125 | # min_r: 381 126 | 127 | 128 | 129 | # max_r: 500 130 | # min_r: 344 131 | 132 | #optimization parameters 133 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 134 | max_num_iterations: 8 # max solver itrations, to guarantee real time 135 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 136 | 137 | #imu parameters The more accurate parameters you provide, the better performance 138 | acc_n: 0.02 # accelerometer measurement noise standard deviation. #0.2 0.04 139 | gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004 140 | acc_w: 0.04 # accelerometer bias random work noise standard deviation. #0.02 141 | gyr_w: 0.001 # gyroscope bias random work noise standard deviation. #4.0e-5 142 | g_norm: 9.81007 # gravity magnitude 143 | 144 | #loop closure parameters 145 | loop_closure: 1 # start loop closure 146 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 147 | fast_relocalization: 0 # useful in real-time and large project 148 | pose_graph_save_path: "/home/shaozu/output/pose_graph/" # save and load path 149 | 150 | #unsynchronization parameters 151 | estimate_td: 1 # online estimate time offset between camera and imu 152 | td: -0.008 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 153 | 154 | #rolling shutter parameters 155 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 156 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 157 | 158 | #visualization parameters 159 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 160 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 161 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 162 | -------------------------------------------------------------------------------- /config/realsense/realsense_color_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/camera/imu/data_raw" 5 | image_topic: "/camera/color/image_raw" 6 | output_path: "/home/tony-ws1/output/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 640 12 | image_height: 480 13 | distortion_parameters: 14 | k1: 9.2615504465028850e-02 15 | k2: -1.8082438825995681e-01 16 | p1: -6.5484100374765971e-04 17 | p2: -3.5829351558557421e-04 18 | projection_parameters: 19 | fx: 6.0970550296798035e+02 20 | fy: 6.0909579671294716e+02 21 | cx: 3.1916667152289227e+02 22 | cy: 2.3558360480225772e+02 23 | 24 | # Extrinsic parameter between IMU and Camera. 25 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 26 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 27 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 28 | #If you choose 0 or 1, you should write down the following matrix. 29 | #Rotation from camera frame to imu frame, imu^R_cam 30 | extrinsicRotation: !!opencv-matrix 31 | rows: 3 32 | cols: 3 33 | dt: d 34 | data: [ 0.99964621, 0.01105994, 0.02418954, 35 | -0.01088975, 0.9999151, -0.00715601, 36 | -0.02426663, 0.00689006, 0.99968178] 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [0.07494282, -0.01077138, -0.00641822] 43 | 44 | #feature traker paprameters 45 | max_cnt: 150 # max feature number in feature tracking 46 | min_dist: 25 # min distance between two features 47 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 60 | gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 61 | acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.805 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 1 # start loop closure 67 | fast_relocalization: 1 # useful in real-time and large project 68 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 69 | pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 1 # online estimate time offset between camera and imu 73 | td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 1 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0.033 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /config/realsense/realsense_fisheye_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/camera/imu/data_raw" 5 | image_topic: "/camera/fisheye/image_raw" 6 | output_path: "/home/tony-ws1/output/" 7 | 8 | #camera calibration 9 | model_type: KANNALA_BRANDT 10 | camera_name: camera 11 | image_width: 640 12 | image_height: 480 13 | projection_parameters: 14 | k2: 1.7280355035195181e-02 15 | k3: -2.5505200860040985e-02 16 | k4: 2.2621441637715487e-02 17 | k5: -7.3355871719731113e-03 18 | mu: 2.7723712054408202e+02 19 | mv: 2.7699784668734617e+02 20 | u0: 3.3625356873985868e+02 21 | v0: 2.3603924727453901e+02 22 | 23 | # Extrinsic parameter between IMU and Camera. 24 | estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 25 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 26 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 27 | #If you choose 0 or 1, you should write down the following matrix. 28 | #Rotation from camera frame to imu frame, imu^R_cam 29 | extrinsicRotation: !!opencv-matrix 30 | rows: 3 31 | cols: 3 32 | dt: d 33 | data: [0.99992917, 0.00878151, 0.00803387, 34 | -0.00870674, 0.9999189, -0.0092943, 35 | -0.00811483, 0.00922369, 0.99992453] 36 | #Translation from camera frame to imu frame, imu^T_cam 37 | extrinsicTranslation: !!opencv-matrix 38 | rows: 3 39 | cols: 1 40 | dt: d 41 | data: [0.00188568, 0.00123801, 0.01044055] 42 | 43 | #feature traker paprameters 44 | max_cnt: 120 # max feature number in feature tracking 45 | min_dist: 30 # min distance between two features 46 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 47 | F_threshold: 1.0 # ransac threshold (pixel) 48 | show_track: 1 # publish tracking image as topic 49 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 50 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 51 | 52 | #optimization parameters 53 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 54 | max_num_iterations: 8 # max solver itrations, to guarantee real time 55 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 56 | 57 | #imu parameters The more accurate parameters you provide, the better performance 58 | acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04 59 | gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 60 | acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02 61 | gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5 62 | g_norm: 9.805 # gravity magnitude 63 | 64 | #loop closure parameters 65 | loop_closure: 1 # start loop closure 66 | fast_relocalization: 1 # useful in real-time and large project 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 69 | 70 | #unsynchronization parameters 71 | estimate_td: 1 # online estimate time offset between camera and imu 72 | td: 0.010 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 73 | 74 | #rolling shutter parameters 75 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 76 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 77 | 78 | #visualization parameters 79 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 80 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 81 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 82 | -------------------------------------------------------------------------------- /config/realsense/realsense_zr300: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/config/realsense/realsense_zr300 -------------------------------------------------------------------------------- /config/tum/tum_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/tony-ws1/output/" 7 | 8 | #camera calibration 9 | model_type: KANNALA_BRANDT 10 | camera_name: camera 11 | image_width: 512 12 | image_height: 512 13 | projection_parameters: 14 | k2: 0.0034823894022493434 15 | k3: 0.0007150348452162257 16 | k4: -0.0020532361418706202 17 | k5: 0.00020293673591811182 18 | mu: 190.97847715128717 19 | mv: 190.9733070521226 20 | u0: 254.93170605935475 21 | v0: 256.8974428996504 22 | 23 | # Extrinsic parameter between IMU and Camera. 24 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 25 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 26 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 27 | #If you choose 0 or 1, you should write down the following matrix. 28 | #Rotation from camera frame to imu frame, imu^R_cam 29 | extrinsicRotation: !!opencv-matrix 30 | rows: 3 31 | cols: 3 32 | dt: d 33 | data: [ -9.9951465899298464e-01, 7.5842033363785165e-03, 34 | -3.0214670573904204e-02, 2.9940114644659861e-02, 35 | -3.4023430206013172e-02, -9.9897246995704592e-01, 36 | -8.6044170750674241e-03, -9.9939225835343004e-01, 37 | 3.3779845322755464e-02 ] 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [ 4.4511917113940799e-02, -7.3197096234105752e-02, 43 | -4.7972907300764499e-02 ] 44 | 45 | #feature traker paprameters 46 | max_cnt: 150 # max feature number in feature tracking 47 | min_dist: 25 # min distance between two features 48 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 49 | F_threshold: 1.0 # ransac threshold (pixel) 50 | show_track: 1 # publish tracking image as topic 51 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 52 | fisheye: 1 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 53 | 54 | #optimization parameters 55 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 56 | max_num_iterations: 8 # max solver itrations, to guarantee real time 57 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 58 | 59 | #imu parameters The more accurate parameters you provide, the better performance 60 | acc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04 61 | gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 62 | acc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02 63 | gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 64 | g_norm: 9.80766 # gravity magnitude 65 | 66 | #loop closure parameters 67 | loop_closure: 0 # start loop closure 68 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 69 | fast_relocalization: 0 # useful in real-time and large project 70 | pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 71 | 72 | #unsynchronization parameters 73 | estimate_td: 0 # online estimate time offset between camera and imu 74 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 75 | 76 | #rolling shutter parameters 77 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 78 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 79 | 80 | #visualization parameters 81 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 82 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 83 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 84 | -------------------------------------------------------------------------------- /demo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/demo.jpg -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-perception 2 | 3 | ENV CERES_VERSION="1.12.0" 4 | ENV CATKIN_WS=/root/catkin_ws 5 | 6 | # set up thread number for building 7 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; \ 8 | else export USE_PROC=$(($(nproc)/2)) ; fi && \ 9 | apt-get update && apt-get install -y \ 10 | cmake \ 11 | libatlas-base-dev \ 12 | libeigen3-dev \ 13 | libgoogle-glog-dev \ 14 | libsuitesparse-dev \ 15 | python-catkin-tools \ 16 | ros-${ROS_DISTRO}-cv-bridge \ 17 | ros-${ROS_DISTRO}-image-transport \ 18 | ros-${ROS_DISTRO}-message-filters \ 19 | ros-${ROS_DISTRO}-tf && \ 20 | rm -rf /var/lib/apt/lists/* && \ 21 | # Build and install Ceres 22 | git clone https://ceres-solver.googlesource.com/ceres-solver && \ 23 | cd ceres-solver && \ 24 | git checkout tags/${CERES_VERSION} && \ 25 | mkdir build && cd build && \ 26 | cmake .. && \ 27 | make -j$(USE_PROC) install && \ 28 | rm -rf ../../ceres-solver && \ 29 | mkdir -p $CATKIN_WS/src/VINS-Mono/ 30 | 31 | # Copy VINS-Mono 32 | COPY ./ $CATKIN_WS/src/VINS-Mono/ 33 | # use the following line if you only have this dockerfile 34 | # RUN git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git 35 | 36 | # Build VINS-Mono 37 | WORKDIR $CATKIN_WS 38 | ENV TERM xterm 39 | ENV PYTHONIOENCODING UTF-8 40 | RUN catkin config \ 41 | --extend /opt/ros/$ROS_DISTRO \ 42 | --cmake-args \ 43 | -DCMAKE_BUILD_TYPE=Release && \ 44 | catkin build && \ 45 | sed -i '/exec "$@"/i \ 46 | source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh 47 | -------------------------------------------------------------------------------- /docker/Makefile: -------------------------------------------------------------------------------- 1 | all: help 2 | 3 | help: 4 | @echo "" 5 | @echo "-- Help Menu" 6 | @echo "" 7 | @echo " 1. make build - build all images" 8 | # @echo " 1. make pull - pull all images" 9 | @echo " 1. make clean - remove all images" 10 | @echo "" 11 | 12 | build: 13 | @docker build --tag ros:vins-mono -f ./Dockerfile .. 14 | 15 | clean: 16 | @docker rmi -f ros:vins-mono 17 | -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | trap : SIGTERM SIGINT 3 | 4 | function abspath() { 5 | # generate absolute path from relative path 6 | # $1 : relative filename 7 | # return : absolute path 8 | if [ -d "$1" ]; then 9 | # dir 10 | (cd "$1"; pwd) 11 | elif [ -f "$1" ]; then 12 | # file 13 | if [[ $1 = /* ]]; then 14 | echo "$1" 15 | elif [[ $1 == */* ]]; then 16 | echo "$(cd "${1%/*}"; pwd)/${1##*/}" 17 | else 18 | echo "$(pwd)/$1" 19 | fi 20 | fi 21 | } 22 | 23 | if [ "$#" -ne 1 ]; then 24 | echo "Usage: $0 LAUNCH_FILE" >&2 25 | exit 1 26 | fi 27 | 28 | roscore & 29 | ROSCORE_PID=$! 30 | sleep 1 31 | 32 | rviz -d ../config/vins_rviz_config.rviz & 33 | RVIZ_PID=$! 34 | 35 | VINS_MONO_DIR=$(abspath "..") 36 | 37 | docker run \ 38 | -it \ 39 | --rm \ 40 | --net=host \ 41 | -v ${VINS_MONO_DIR}:/root/catkin_ws/src/VINS-Mono/ \ 42 | ros:vins-mono \ 43 | /bin/bash -c \ 44 | "cd /root/catkin_ws/; \ 45 | catkin config \ 46 | --env-cache \ 47 | --extend /opt/ros/$ROS_DISTRO \ 48 | --cmake-args \ 49 | -DCMAKE_BUILD_TYPE=Release; \ 50 | catkin build; \ 51 | source devel/setup.bash; \ 52 | roslaunch vins_estimator ${1}" 53 | 54 | wait $ROSCORE_PID 55 | wait $RVIZ_PID 56 | 57 | if [[ $? -gt 128 ]] 58 | then 59 | kill $ROSCORE_PID 60 | kill $RVIZ_PID 61 | fi 62 | -------------------------------------------------------------------------------- /feature_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(feature_tracker) 3 | 4 | #set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS "-std=c++14") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | cv_bridge 14 | camera_model 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | 19 | catkin_package() 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 26 | find_package(Eigen3) 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ${EIGEN3_INCLUDE_DIR} 30 | ) 31 | 32 | add_executable(feature_tracker 33 | src/feature_tracker_node.cpp 34 | src/parameters.cpp 35 | src/feature_tracker.cpp 36 | src/random_array.cc 37 | ) 38 | 39 | target_link_libraries(feature_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS}) 40 | -------------------------------------------------------------------------------- /feature_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | feature_tracker 4 | 0.0.0 5 | The feature_tracker 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 | camera_model 45 | message_generation 46 | roscpp 47 | camera_model 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /feature_tracker/src/feature_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "camodocal/camera_models/CameraFactory.h" 13 | #include "camodocal/camera_models/CataCamera.h" 14 | #include "camodocal/camera_models/PinholeCamera.h" 15 | 16 | #include "parameters.h" 17 | #include "tic_toc.h" 18 | #include "random_array.h" 19 | 20 | using namespace std; 21 | using namespace camodocal; 22 | using namespace Eigen; 23 | 24 | bool inBorder(const cv::Point2f &pt); 25 | 26 | void reduceVector(vector &v, vector status); 27 | void reduceVector(vector &v, vector status); 28 | 29 | 30 | //added by wz 31 | Eigen::Matrix3d myfindFundamentalMat(vector corr1,vector corr2,int method , double theshold , double unknown ,vector &status); 32 | Eigen::Matrix3d compute_E_21(vector & bearings_1,vector & bearings_2); 33 | double check_inliers(Eigen::Matrix3d &E_21, vector &is_inlier_match,vector corr1,vector corr2); 34 | class FeatureTracker 35 | { 36 | public: 37 | FeatureTracker(); 38 | 39 | void readImage(const cv::Mat &_img,double _cur_time); 40 | 41 | void setMask(); 42 | 43 | void addPoints(); 44 | 45 | bool updateID(unsigned int i); 46 | 47 | void readIntrinsicParameter(const string &calib_file); 48 | 49 | void showUndistortion(const string &name); 50 | 51 | void rejectWithF(); 52 | 53 | void undistortedPoints(); 54 | 55 | cv::Mat mask; 56 | cv::Mat fisheye_mask; 57 | cv::Mat prev_img, cur_img, forw_img; 58 | vector n_pts; 59 | vector prev_pts, cur_pts, forw_pts; 60 | vector prev_un_pts, cur_un_pts; 61 | //changed by wz 62 | vector pts_velocity; 63 | vector pts_velocity_3d; 64 | 65 | //added by wz 66 | vector cur_un_pts_3d; 67 | 68 | vector ids; 69 | vector track_cnt; 70 | map cur_un_pts_map; 71 | map prev_un_pts_map; 72 | //added by wz 73 | map cur_un_pts_map_3d; 74 | map prev_un_pts_map_3d; 75 | camodocal::CameraPtr m_camera; 76 | double cur_time; 77 | double prev_time; 78 | 79 | static int n_id; 80 | }; 81 | -------------------------------------------------------------------------------- /feature_tracker/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC; 4 | std::string IMU_TOPIC; 5 | std::vector CAM_NAMES; 6 | std::string FISHEYE_MASK; 7 | int MAX_CNT; 8 | int MIN_DIST; 9 | int WINDOW_SIZE; 10 | int FREQ; 11 | double F_THRESHOLD; 12 | int SHOW_TRACK; 13 | int STEREO_TRACK; 14 | int EQUALIZE; 15 | int ROW; 16 | int COL; 17 | int FOCAL_LENGTH; 18 | int FISHEYE; 19 | bool PUB_THIS_FRAME; 20 | 21 | //added by wz 22 | double CENTER_X; 23 | double CENTER_Y; 24 | double MIN_R; 25 | double MAX_R; 26 | 27 | template 28 | T readParam(ros::NodeHandle &n, std::string name) 29 | { 30 | T ans; 31 | if (n.getParam(name, ans)) 32 | { 33 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 34 | } 35 | else 36 | { 37 | ROS_ERROR_STREAM("Failed to load " << name); 38 | n.shutdown(); 39 | } 40 | return ans; 41 | } 42 | 43 | void readParameters(ros::NodeHandle &n) 44 | { 45 | std::string config_file; 46 | config_file = readParam(n, "config_file"); 47 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 48 | if (!fsSettings.isOpened()) 49 | { 50 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 51 | } 52 | std::string VINS_FOLDER_PATH = readParam(n, "vins_folder"); 53 | 54 | fsSettings["image_topic"] >> IMAGE_TOPIC; 55 | fsSettings["imu_topic"] >> IMU_TOPIC; 56 | MAX_CNT = fsSettings["max_cnt"]; 57 | MIN_DIST = fsSettings["min_dist"]; 58 | ROW = fsSettings["image_height"]; 59 | COL = fsSettings["image_width"]; 60 | FREQ = fsSettings["freq"]; 61 | F_THRESHOLD = fsSettings["F_threshold"]; 62 | SHOW_TRACK = fsSettings["show_track"]; 63 | EQUALIZE = fsSettings["equalize"]; 64 | FISHEYE = fsSettings["fisheye"]; 65 | //added by wz 66 | CENTER_X = fsSettings["center_x"]; 67 | CENTER_Y = fsSettings["center_y"]; 68 | MAX_R = fsSettings["max_r"]; 69 | MIN_R = fsSettings["min_r"]; 70 | 71 | if (FISHEYE == 1) 72 | FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; 73 | CAM_NAMES.push_back(config_file); 74 | 75 | WINDOW_SIZE = 20; 76 | STEREO_TRACK = false; 77 | FOCAL_LENGTH = 160; 78 | PUB_THIS_FRAME = false; 79 | 80 | if (FREQ == 0) 81 | FREQ = 100; 82 | 83 | fsSettings.release(); 84 | } 85 | -------------------------------------------------------------------------------- /feature_tracker/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | extern int ROW; 6 | extern int COL; 7 | extern int FOCAL_LENGTH; 8 | const int NUM_OF_CAM = 1; 9 | 10 | 11 | extern std::string IMAGE_TOPIC; 12 | extern std::string IMU_TOPIC; 13 | extern std::string FISHEYE_MASK; 14 | extern std::vector CAM_NAMES; 15 | extern int MAX_CNT; 16 | extern int MIN_DIST; 17 | extern int WINDOW_SIZE; 18 | extern int FREQ; 19 | extern double F_THRESHOLD; 20 | extern int SHOW_TRACK; 21 | extern int STEREO_TRACK; 22 | extern int EQUALIZE; 23 | extern int FISHEYE; 24 | extern bool PUB_THIS_FRAME; 25 | 26 | //added by wz 27 | extern double CENTER_X; 28 | extern double CENTER_Y; 29 | extern double MAX_R; 30 | extern double MIN_R; 31 | 32 | void readParameters(ros::NodeHandle &n); 33 | -------------------------------------------------------------------------------- /feature_tracker/src/random_array.cc: -------------------------------------------------------------------------------- 1 | #include "random_array.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace util 10 | { 11 | 12 | std::mt19937 create_random_engine() 13 | { 14 | std::random_device random_device; 15 | std::vector v(10); 16 | std::generate(v.begin(), v.end(), std::ref(random_device)); 17 | std::seed_seq seed(v.begin(), v.end()); 18 | return std::mt19937(seed); 19 | } 20 | 21 | template 22 | std::vector create_random_array(const size_t size, const T rand_min, const T rand_max) 23 | { 24 | assert(rand_min <= rand_max); 25 | assert(size <= static_cast(rand_max - rand_min + 1)); 26 | 27 | auto random_engine = create_random_engine(); 28 | std::uniform_int_distribution uniform_int_distribution(rand_min, rand_max); 29 | const auto make_size = static_cast(size * 1.2); 30 | std::vector v; 31 | v.reserve(size); 32 | while (v.size() != size) 33 | { 34 | while (v.size() < make_size) 35 | { 36 | v.push_back(uniform_int_distribution(random_engine)); 37 | } 38 | 39 | std::sort(v.begin(), v.end()); 40 | auto unique_end = std::unique(v.begin(), v.end()); 41 | if (size < static_cast(std::distance(v.begin(), unique_end))) 42 | { 43 | unique_end = std::next(v.begin(), size); 44 | } 45 | v.erase(unique_end, v.end()); 46 | } 47 | std::shuffle(v.begin(), v.end(), random_engine); 48 | 49 | return v; 50 | } 51 | template std::vector create_random_array(size_t, int, int); 52 | template std::vector create_random_array(size_t, unsigned int, unsigned int); 53 | 54 | } 55 | -------------------------------------------------------------------------------- /feature_tracker/src/random_array.h: -------------------------------------------------------------------------------- 1 | #ifndef VINS_UTIL_RANDOM_ARRAY_H 2 | #define VINS_UTIL_RANDOM_ARRAY_H 3 | 4 | #include 5 | #include 6 | 7 | namespace util 8 | { 9 | 10 | std::mt19937 create_random_engine(); 11 | 12 | template 13 | std::vector create_random_array(const size_t size, const T rand_min, const T rand_max); 14 | 15 | } 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /feature_tracker/src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /figure/LF-VIO-hardware.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/figure/LF-VIO-hardware.png -------------------------------------------------------------------------------- /pointcloud_image_fusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pointcloud_image_fusion) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(PCL REQUIRED) 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | nav_msgs 15 | cv_bridge 16 | camera_model 17 | ) 18 | 19 | find_package(OpenCV REQUIRED) 20 | 21 | catkin_package() 22 | 23 | include_directories( 24 | ${catkin_INCLUDE_DIRS} 25 | ${PCL_INCLUDE_DIRS} 26 | ) 27 | link_directories(${PCL_LIBRARY_DIRS}) 28 | 29 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 30 | find_package(Eigen3) 31 | include_directories( 32 | ${catkin_INCLUDE_DIRS} 33 | ${EIGEN3_INCLUDE_DIR} 34 | ) 35 | 36 | add_executable(pointcloud_image_fusion 37 | src/pointcloud_image_fusion_node.cpp 38 | src/parameters.cpp 39 | src/pointcloud_image_fusion.cpp 40 | ) 41 | 42 | target_link_libraries(pointcloud_image_fusion ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES}) 43 | -------------------------------------------------------------------------------- /pointcloud_image_fusion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pointcloud_image_fusion 4 | 0.0.0 5 | The pointcloud_image_fusion 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 | camera_model 45 | message_generation 46 | roscpp 47 | camera_model 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /pointcloud_image_fusion/src/pal_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/pointcloud_image_fusion/src/pal_mask.jpg -------------------------------------------------------------------------------- /pointcloud_image_fusion/src/pal_mask1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/pointcloud_image_fusion/src/pal_mask1.jpg -------------------------------------------------------------------------------- /pointcloud_image_fusion/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC; 4 | std::string IMU_TOPIC; 5 | std::vector CAM_NAMES; 6 | std::string FISHEYE_MASK; 7 | int MAX_CNT; 8 | int MIN_DIST; 9 | int WINDOW_SIZE; 10 | int FREQ; 11 | double F_THRESHOLD; 12 | int SHOW_TRACK; 13 | int STEREO_TRACK; 14 | int EQUALIZE; 15 | int ROW; 16 | int COL; 17 | int FOCAL_LENGTH; 18 | int FISHEYE; 19 | bool PUB_THIS_FRAME; 20 | 21 | template 22 | T readParam(ros::NodeHandle &n, std::string name) 23 | { 24 | T ans; 25 | if (n.getParam(name, ans)) 26 | { 27 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 28 | } 29 | else 30 | { 31 | ROS_ERROR_STREAM("Failed to load " << name); 32 | n.shutdown(); 33 | } 34 | return ans; 35 | } 36 | 37 | void readParameters(ros::NodeHandle &n) 38 | { 39 | std::string config_file; 40 | config_file = readParam(n, "config_file"); 41 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 42 | if(!fsSettings.isOpened()) 43 | { 44 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 45 | } 46 | std::string VINS_FOLDER_PATH = readParam(n, "vins_folder"); 47 | 48 | fsSettings["image_topic"] >> IMAGE_TOPIC; 49 | fsSettings["imu_topic"] >> IMU_TOPIC; 50 | MAX_CNT = fsSettings["max_cnt"]; 51 | MIN_DIST = fsSettings["min_dist"]; 52 | ROW = fsSettings["image_height"]; 53 | COL = fsSettings["image_width"]; 54 | FREQ = fsSettings["freq"]; 55 | F_THRESHOLD = fsSettings["F_threshold"]; 56 | SHOW_TRACK = fsSettings["show_track"]; 57 | EQUALIZE = fsSettings["equalize"]; 58 | FISHEYE = fsSettings["fisheye"]; 59 | if (FISHEYE == 1) 60 | FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; 61 | CAM_NAMES.push_back(config_file); 62 | 63 | WINDOW_SIZE = 20; 64 | STEREO_TRACK = false; 65 | FOCAL_LENGTH = 160; 66 | PUB_THIS_FRAME = false; 67 | 68 | if (FREQ == 0) 69 | FREQ = 100; 70 | 71 | fsSettings.release(); 72 | 73 | 74 | } 75 | -------------------------------------------------------------------------------- /pointcloud_image_fusion/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | extern int ROW; 6 | extern int COL; 7 | extern int FOCAL_LENGTH; 8 | const int NUM_OF_CAM = 1; 9 | 10 | 11 | extern std::string IMAGE_TOPIC; 12 | extern std::string IMU_TOPIC; 13 | extern std::string FISHEYE_MASK; 14 | extern std::vector CAM_NAMES; 15 | extern int MAX_CNT; 16 | extern int MIN_DIST; 17 | extern int WINDOW_SIZE; 18 | extern int FREQ; 19 | extern double F_THRESHOLD; 20 | extern int SHOW_TRACK; 21 | extern int STEREO_TRACK; 22 | extern int EQUALIZE; 23 | extern int FISHEYE; 24 | extern bool PUB_THIS_FRAME; 25 | 26 | void readParameters(ros::NodeHandle &n); 27 | -------------------------------------------------------------------------------- /pointcloud_image_fusion/src/pointcloud_image_fusion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include "camodocal/camera_models/CameraFactory.h" 14 | #include "camodocal/camera_models/CataCamera.h" 15 | #include "camodocal/camera_models/PinholeCamera.h" 16 | 17 | 18 | #include "parameters.h" 19 | #include "tic_toc.h" 20 | 21 | using namespace std; 22 | using namespace camodocal; 23 | using namespace Eigen; 24 | 25 | bool inBorder(const cv::Point2f &pt); 26 | 27 | void reduceVector(vector &v, vector status); 28 | void reduceVector(vector &v, vector status); 29 | 30 | class Pointcloud_image_fusion 31 | { 32 | public: 33 | Pointcloud_image_fusion(); 34 | 35 | cv::Mat euqual_rectangle_expansion(cv::Mat &img,int rows,int cols); 36 | 37 | void GetremapMat(cv::Mat &remap_x,cv::Mat &remap_y,int rows,int cols); 38 | 39 | void readImage(const cv::Mat &_img,double _cur_time); 40 | 41 | void setMask(); 42 | 43 | void addPoints(); 44 | 45 | bool updateID(unsigned int i); 46 | 47 | void readIntrinsicParameter(const string &calib_file); 48 | 49 | void showUndistortion(const string &name); 50 | 51 | void rejectWithF(); 52 | 53 | void undistortedPoints(); 54 | 55 | void mygoodFeaturesToTrack( cv::InputArray _image, cv::OutputArray _corners, 56 | int maxCorners, double qualityLevel, double minDistance, 57 | cv::InputArray _mask, int blockSize=3, 58 | bool useHarrisDetector=false, double harrisK=0.04 ); 59 | 60 | cv::Mat mask; 61 | cv::Mat fisheye_mask; 62 | cv::Mat prev_img, cur_img, forw_img; 63 | vector n_pts; 64 | vector prev_pts, cur_pts, forw_pts; 65 | vector prev_un_pts, cur_un_pts; 66 | //added by wz 67 | vector cur_un_pts_3d; 68 | vector pts_velocity; 69 | vector pts_velocity_3d; 70 | vector ids; 71 | vector track_cnt; 72 | map cur_un_pts_map; 73 | map prev_un_pts_map; 74 | //added by wz 75 | map cur_un_pts_map_3d; 76 | map prev_un_pts_map_3d; 77 | camodocal::CameraPtr m_camera; 78 | double cur_time; 79 | double prev_time; 80 | 81 | static int n_id; 82 | }; 83 | -------------------------------------------------------------------------------- /pointcloud_image_fusion/src/pointcloud_image_fusion_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "pointcloud_image_fusion.h" 20 | 21 | queue img_buf; 22 | 23 | ros::Publisher pub_equal_rectangle_img; 24 | 25 | Pointcloud_image_fusion fusion[NUM_OF_CAM]; 26 | 27 | Eigen::Matrix3d camera_R_lidar; 28 | Eigen::Vector3d camera_T_lidar; 29 | 30 | pcl::PointCloud laserCloudIn; 31 | 32 | int rows = 480; 33 | int cols = 960; 34 | 35 | void img_callback(const sensor_msgs::ImageConstPtr &img_msg) 36 | { 37 | static bool init_flag = 0; 38 | 39 | cv_bridge::CvImageConstPtr ptr; 40 | if (img_msg->encoding == "8UC1") 41 | { 42 | sensor_msgs::Image img; 43 | img.header = img_msg->header; 44 | img.height = img_msg->height; 45 | img.width = img_msg->width; 46 | img.is_bigendian = img_msg->is_bigendian; 47 | img.step = img_msg->step; 48 | img.data = img_msg->data; 49 | img.encoding = "mono8"; 50 | // ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); 51 | ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); 52 | } 53 | else 54 | // ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); 55 | ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); 56 | 57 | cv::Mat show_img = ptr->image; 58 | cv::Mat tmp_img = show_img.clone(); 59 | cv::Mat equal_rectangle_img; 60 | TicToc t; 61 | 62 | static cv::Mat remap_x = cv::Mat::zeros(rows, cols, CV_32FC1); 63 | static cv::Mat remap_y = cv::Mat::zeros(rows, cols, CV_32FC1); 64 | 65 | for (int i = 0; i < NUM_OF_CAM; i++) 66 | { 67 | // equal_rectangle_img=fusion[i].euqual_rectangle_expansion(tmp_img,500,1000); 68 | fusion[i].readImage(equal_rectangle_img, img_msg->header.stamp.toSec()); 69 | 70 | if (init_flag == 0) 71 | { 72 | fusion[i].GetremapMat(remap_x, remap_y, rows, cols); 73 | init_flag = 1; 74 | } 75 | 76 | // cv::imshow("vis", equal_rectangle_img); 77 | // cv::waitKey(5); 78 | } 79 | 80 | // equal_rectangle_img=fusion[0].euqual_rectangle_expansion(show_img,rows,cols); 81 | cv::remap(show_img, equal_rectangle_img, remap_x, remap_y, 0); 82 | ROS_FATAL("equal_rectangle_img costs %fms", t.toc()); 83 | 84 | cv_bridge::CvImage img_bridge; 85 | sensor_msgs::Image pub_img_msg; // >> message to be sent 86 | 87 | std_msgs::Header header; // empty header 88 | header = img_msg->header; // user defined counter 89 | 90 | // cv::cvtColor(equal_rectangle_img, equal_rectangle_img, CV_GRAY2RGB); 91 | 92 | // for(auto i=0;i(idx_y,idx_x)[0]=255; 112 | // equal_rectangle_img.at(idx_y,idx_x)[1]=0; 113 | // equal_rectangle_img.at(idx_y,idx_x)[2]=0; 114 | 115 | // } 116 | 117 | img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, equal_rectangle_img); 118 | img_bridge.toImageMsg(pub_img_msg); // from cv_bridge to sensor_msgs::Image 119 | pub_equal_rectangle_img.publish(pub_img_msg); 120 | } 121 | void pointcloud_callback(const sensor_msgs::PointCloud2ConstPtr &pointcloud_msg) 122 | { 123 | 124 | pcl::fromROSMsg(*pointcloud_msg, laserCloudIn); 125 | } 126 | int main(int argc, char **argv) 127 | { 128 | ros::init(argc, argv, "pointcloud_image_fusion"); 129 | ros::NodeHandle n("~"); 130 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 131 | readParameters(n); 132 | 133 | for (int i = 0; i < NUM_OF_CAM; i++) 134 | fusion[i].readIntrinsicParameter(CAM_NAMES[i]); 135 | 136 | if (FISHEYE) 137 | { 138 | for (int i = 0; i < NUM_OF_CAM; i++) 139 | { 140 | fusion[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0); 141 | if (!fusion[i].fisheye_mask.data) 142 | { 143 | ROS_INFO("load mask fail"); 144 | ROS_BREAK(); 145 | } 146 | else 147 | ROS_INFO("load mask success"); 148 | } 149 | } 150 | 151 | string POINTCLOUD_TOPIC = "/os_cloud_node/points"; 152 | ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 1, img_callback, ros::TransportHints().tcpNoDelay()); 153 | // ros::Subscriber sub_img = n.subscribe("/feature_tracker/feature_img", 1, img_callback,ros::TransportHints().tcpNoDelay()); 154 | ros::Subscriber sub_pointcloud = n.subscribe(POINTCLOUD_TOPIC, 1, pointcloud_callback); 155 | pub_equal_rectangle_img = n.advertise("equal_rectangle_img", 100); 156 | ros::spin(); 157 | return 0; 158 | } 159 | -------------------------------------------------------------------------------- /pointcloud_image_fusion/src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /support_files/brief_k10L6.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flysoaryun/LF-VIO/1add21e62e6ca8dda186c5d311ebe66144fd0fce/support_files/brief_k10L6.bin -------------------------------------------------------------------------------- /support_files/paper_bib.txt: -------------------------------------------------------------------------------- 1 | 2 | @inproceedings{qin2018online, 3 | title={Online Temporal Calibration for Monocular Visual-Inertial Systems}, 4 | author={Qin, Tong and Shen, Shaojie}, 5 | booktitle={2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 6 | pages={3662--3669}, 7 | year={2018}, 8 | organization={IEEE} 9 | } 10 | 11 | 12 | 13 | @article{qin2017vins, 14 | title={VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator}, 15 | author={Qin, Tong and Li, Peiliang and Shen, Shaojie}, 16 | journal={IEEE Transactions on Robotics}, 17 | year={2018}, 18 | volume={34}, 19 | number={4}, 20 | pages={1004-1020} 21 | } 22 | -------------------------------------------------------------------------------- /vins_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vins_estimator) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | #-DEIGEN_USE_MKL_ALL") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | geometry_msgs 13 | nav_msgs 14 | tf 15 | cv_bridge 16 | ) 17 | 18 | find_package(OpenCV REQUIRED) 19 | 20 | # message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}") 21 | 22 | find_package(Ceres REQUIRED) 23 | 24 | include_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS}) 25 | 26 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 27 | find_package(Eigen3) 28 | include_directories( 29 | ${catkin_INCLUDE_DIRS} 30 | ${EIGEN3_INCLUDE_DIR} 31 | ) 32 | 33 | catkin_package() 34 | 35 | add_executable(vins_estimator 36 | src/estimator_node.cpp 37 | src/parameters.cpp 38 | src/estimator.cpp 39 | src/feature_manager.cpp 40 | src/factor/pose_local_parameterization.cpp 41 | src/factor/projection_factor.cpp 42 | src/factor/projection_td_factor.cpp 43 | src/factor/marginalization_factor.cpp 44 | src/utility/utility.cpp 45 | src/utility/visualization.cpp 46 | src/utility/CameraPoseVisualization.cpp 47 | src/initial/solve_5pts.cpp 48 | src/initial/random_array.cc 49 | src/initial/initial_aligment.cpp 50 | src/initial/initial_sfm.cpp 51 | src/initial/initial_ex_rotation.cpp 52 | 53 | src/pnp_solver.cpp 54 | ) 55 | 56 | 57 | target_link_libraries(vins_estimator ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 58 | 59 | 60 | -------------------------------------------------------------------------------- /vins_estimator/launch/3dm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /vins_estimator/launch/black_box.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /vins_estimator/launch/cla.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /vins_estimator/launch/euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /vins_estimator/launch/euroc_no_extrinsic_param.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /vins_estimator/launch/mindvision.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 27 | 28 | 33 | 34 | -------------------------------------------------------------------------------- /vins_estimator/launch/mynteye.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 22 | 23 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /vins_estimator/launch/realsense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 27 | 28 | 33 | 34 | -------------------------------------------------------------------------------- /vins_estimator/launch/realsense_color.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /vins_estimator/launch/realsense_fisheye.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /vins_estimator/launch/tum.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /vins_estimator/launch/vins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /vins_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vins_estimator 4 | 0.0.0 5 | The vins_estimator package 6 | 7 | 8 | 9 | 10 | qintong 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 | -------------------------------------------------------------------------------- /vins_estimator/src/estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | #include "feature_manager.h" 5 | #include "utility/utility.h" 6 | #include "utility/tic_toc.h" 7 | #include "initial/solve_5pts.h" 8 | #include "initial/initial_sfm.h" 9 | #include "initial/initial_alignment.h" 10 | #include "initial/initial_ex_rotation.h" 11 | #include 12 | #include 13 | 14 | #include 15 | #include "factor/imu_factor.h" 16 | #include "factor/pose_local_parameterization.h" 17 | #include "factor/projection_factor.h" 18 | #include "factor/projection_td_factor.h" 19 | #include "factor/marginalization_factor.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | class Estimator 26 | { 27 | public: 28 | Estimator(); 29 | 30 | void setParameter(); 31 | 32 | // interface 33 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 34 | // changed by wz 35 | void processImage(const map>>> &image, const std_msgs::Header &header); 36 | void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); 37 | 38 | // internal 39 | void clearState(); 40 | bool initialStructure(); 41 | bool visualInitialAlign(); 42 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 43 | void slideWindow(); 44 | void solveOdometry(); 45 | void slideWindowNew(); 46 | void slideWindowOld(); 47 | void optimization(); 48 | void vector2double(); 49 | void double2vector(); 50 | bool failureDetection(); 51 | 52 | enum SolverFlag 53 | { 54 | INITIAL, 55 | NON_LINEAR 56 | }; 57 | 58 | enum MarginalizationFlag 59 | { 60 | MARGIN_OLD = 0, 61 | MARGIN_SECOND_NEW = 1 62 | }; 63 | 64 | SolverFlag solver_flag; 65 | MarginalizationFlag marginalization_flag; 66 | Vector3d g; 67 | MatrixXd Ap[2], backup_A; 68 | VectorXd bp[2], backup_b; 69 | 70 | Matrix3d ric[NUM_OF_CAM]; 71 | Vector3d tic[NUM_OF_CAM]; 72 | 73 | Vector3d Ps[(WINDOW_SIZE + 1)]; 74 | Vector3d Vs[(WINDOW_SIZE + 1)]; 75 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 76 | Vector3d Bas[(WINDOW_SIZE + 1)]; 77 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 78 | double td; 79 | 80 | Matrix3d back_R0, last_R, last_R0; 81 | Vector3d back_P0, last_P, last_P0; 82 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 83 | 84 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 85 | Vector3d acc_0, gyr_0; 86 | 87 | vector dt_buf[(WINDOW_SIZE + 1)]; 88 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 89 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 90 | 91 | int frame_count; 92 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 93 | 94 | FeatureManager f_manager; 95 | MotionEstimator m_estimator; 96 | InitialEXRotation initial_ex_rotation; 97 | 98 | bool first_imu; 99 | bool is_valid, is_key; 100 | bool failure_occur; 101 | 102 | vector point_cloud; 103 | vector margin_cloud; 104 | vector key_poses; 105 | double initial_timestamp; 106 | 107 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 108 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 109 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 110 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 111 | double para_Retrive_Pose[SIZE_POSE]; 112 | double para_Td[1][1]; 113 | double para_Tr[1][1]; 114 | 115 | int loop_window_index; 116 | 117 | MarginalizationInfo *last_marginalization_info; 118 | vector last_marginalization_parameter_blocks; 119 | 120 | map all_image_frame; 121 | IntegrationBase *tmp_pre_integration; 122 | 123 | // relocalization variable 124 | bool relocalization_info; 125 | double relo_frame_stamp; 126 | double relo_frame_index; 127 | int relo_frame_local_index; 128 | vector match_points; 129 | double relo_Pose[SIZE_POSE]; 130 | Matrix3d drift_correct_r; 131 | Vector3d drift_correct_t; 132 | Vector3d prev_relo_t; 133 | Matrix3d prev_relo_r; 134 | Vector3d relo_relative_t; 135 | Quaterniond relo_relative_q; 136 | double relo_relative_yaw; 137 | }; 138 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "../utility/utility.h" 11 | #include "../utility/tic_toc.h" 12 | 13 | const int NUM_THREADS = 4; 14 | 15 | struct ResidualBlockInfo 16 | { 17 | ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector _parameter_blocks, std::vector _drop_set) 18 | : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {} 19 | 20 | void Evaluate(); 21 | 22 | ceres::CostFunction *cost_function; 23 | ceres::LossFunction *loss_function; 24 | std::vector parameter_blocks; 25 | std::vector drop_set; 26 | 27 | double **raw_jacobians; 28 | std::vector> jacobians; 29 | Eigen::VectorXd residuals; 30 | 31 | int localSize(int size) 32 | { 33 | return size == 7 ? 6 : size; 34 | } 35 | }; 36 | 37 | struct ThreadsStruct 38 | { 39 | std::vector sub_factors; 40 | Eigen::MatrixXd A; 41 | Eigen::VectorXd b; 42 | std::unordered_map parameter_block_size; //global size 43 | std::unordered_map parameter_block_idx; //local size 44 | }; 45 | 46 | class MarginalizationInfo 47 | { 48 | public: 49 | ~MarginalizationInfo(); 50 | int localSize(int size) const; 51 | int globalSize(int size) const; 52 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 53 | void preMarginalize(); 54 | void marginalize(); 55 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 56 | 57 | std::vector factors; 58 | int m, n; 59 | std::unordered_map parameter_block_size; //global size 60 | int sum_block_size; 61 | std::unordered_map parameter_block_idx; //local size 62 | std::unordered_map parameter_block_data; 63 | 64 | std::vector keep_block_size; //global size 65 | std::vector keep_block_idx; //local size 66 | std::vector keep_block_data; 67 | 68 | Eigen::MatrixXd linearized_jacobians; 69 | Eigen::VectorXd linearized_residuals; 70 | const double eps = 1e-8; 71 | 72 | }; 73 | 74 | class MarginalizationFactor : public ceres::CostFunction 75 | { 76 | public: 77 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 78 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 79 | 80 | MarginalizationInfo* marginalization_info; 81 | }; 82 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 4 | { 5 | Eigen::Map _p(x); 6 | Eigen::Map _q(x + 3); 7 | 8 | Eigen::Map dp(delta); 9 | 10 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 11 | 12 | Eigen::Map p(x_plus_delta); 13 | Eigen::Map q(x_plus_delta + 3); 14 | 15 | p = _p + dp; 16 | q = (_q * dq).normalized(); 17 | 18 | return true; 19 | } 20 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 21 | { 22 | Eigen::Map> j(jacobian); 23 | j.topRows<6>().setIdentity(); 24 | j.bottomRows<1>().setZero(); 25 | 26 | return true; 27 | } 28 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../utility/utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 10 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 11 | virtual int GlobalSize() const { return 7; }; 12 | virtual int LocalSize() const { return 6; }; 13 | }; 14 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 11 | { 12 | public: 13 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); 14 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 15 | void check(double **parameters); 16 | 17 | Eigen::Vector3d pts_i, pts_j; 18 | Eigen::Matrix tangent_base; 19 | static Eigen::Matrix2d sqrt_info; 20 | static double sum_t; 21 | }; 22 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_td_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 11 | { 12 | public: 13 | // changed by wz 14 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 15 | const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, 16 | const double _td_i, const double _td_j, const double _row_i, const double _row_j, int _track_size); 17 | 18 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 19 | void check(double **parameters); 20 | 21 | Eigen::Vector3d pts_i, pts_j; 22 | Eigen::Vector3d velocity_i, velocity_j; 23 | double td_i, td_j; 24 | Eigen::Matrix tangent_base; 25 | double row_i, row_j; 26 | static Eigen::Matrix2d sqrt_info; 27 | static double sum_t; 28 | // added by wz 29 | int track_size; 30 | }; 31 | -------------------------------------------------------------------------------- /vins_estimator/src/feature_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_MANAGER_H 2 | #define FEATURE_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | #include 11 | using namespace Eigen; 12 | 13 | #include 14 | #include 15 | 16 | #include "parameters.h" 17 | 18 | class FeaturePerFrame 19 | { 20 | public: 21 | // changed by wz 22 | FeaturePerFrame(const Eigen::Matrix &_point, double td) 23 | { 24 | point.x() = _point(0); 25 | point.y() = _point(1); 26 | point.z() = _point(2); 27 | uv.x() = _point(3); 28 | uv.y() = _point(4); 29 | velocity.x() = _point(5); 30 | velocity.y() = _point(6); 31 | 32 | // added by wz 33 | velocity.z() = _point(7); 34 | cur_td = td; 35 | } 36 | double cur_td; 37 | Vector3d point; 38 | Vector2d uv; 39 | // changed by wz 40 | Vector3d velocity; 41 | double z; 42 | bool is_used; 43 | double parallax; 44 | MatrixXd A; 45 | VectorXd b; 46 | double dep_gradient; 47 | }; 48 | 49 | class FeaturePerId 50 | { 51 | public: 52 | const int feature_id; 53 | int start_frame; 54 | vector feature_per_frame; 55 | 56 | int used_num; 57 | bool is_outlier; 58 | bool is_margin; 59 | double estimated_depth; 60 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 61 | 62 | Vector3d gt_p; 63 | 64 | FeaturePerId(int _feature_id, int _start_frame) 65 | : feature_id(_feature_id), start_frame(_start_frame), 66 | used_num(0), estimated_depth(-1.0), solve_flag(0) 67 | { 68 | } 69 | 70 | int endFrame(); 71 | }; 72 | 73 | class FeatureManager 74 | { 75 | public: 76 | FeatureManager(Matrix3d _Rs[]); 77 | 78 | void setRic(Matrix3d _ric[]); 79 | 80 | void clearState(); 81 | 82 | int getFeatureCount(); 83 | 84 | // changed by wz 85 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 86 | void debugShow(); 87 | vector> getCorresponding(int frame_count_l, int frame_count_r); 88 | 89 | // void updateDepth(const VectorXd &x); 90 | void setDepth(const VectorXd &x); 91 | void removeFailures(); 92 | void clearDepth(const VectorXd &x); 93 | VectorXd getDepthVector(); 94 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 95 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 96 | void removeBack(); 97 | void removeFront(int frame_count); 98 | void removeOutlier(); 99 | list feature; 100 | int last_track_num; 101 | 102 | private: 103 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 104 | const Matrix3d *Rs; 105 | Matrix3d ric[NUM_OF_CAM]; 106 | }; 107 | 108 | #endif -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_alignment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "../factor/imu_factor.h" 5 | #include "../utility/utility.h" 6 | #include 7 | #include 8 | #include "../feature_manager.h" 9 | 10 | using namespace Eigen; 11 | using namespace std; 12 | 13 | class ImageFrame 14 | { 15 | public: 16 | ImageFrame(){}; 17 | // changed by wz 18 | ImageFrame(const map>>> &_points, double _t) : t{_t}, is_key_frame{false} 19 | { 20 | points = _points; 21 | }; 22 | // changed by wz 23 | map>>> points; 24 | double t; 25 | Matrix3d R; 26 | Vector3d T; 27 | IntegrationBase *pre_integration; 28 | bool is_key_frame; 29 | }; 30 | 31 | bool VisualIMUAlignment(map &all_image_frame, Vector3d *Bgs, Vector3d &g, VectorXd &x); -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_ex_rotation.h: -------------------------------------------------------------------------------- 1 | // changed by wz 2 | #pragma once 3 | 4 | #include 5 | #include "../parameters.h" 6 | using namespace std; 7 | 8 | #include 9 | 10 | #include 11 | using namespace Eigen; 12 | #include 13 | #include "random_array.h" 14 | 15 | /* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */ 16 | class InitialEXRotation 17 | { 18 | public: 19 | InitialEXRotation(); 20 | bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result); 21 | 22 | private: 23 | Eigen::Matrix3d compute_E_21(vector &bearings_1, vector &bearings_2); 24 | double check_inliers(Eigen::Matrix3d &E_21, vector &is_inlier_match, vector corr1, vector corr2); 25 | Eigen::Matrix3d myfindFundamentalMat(vector corr1, vector corr2); 26 | 27 | Matrix3d solveRelativeR(const vector> &corres); 28 | 29 | double testTriangulation(const vector &l, 30 | const vector &r, 31 | cv::Mat_ R, cv::Mat_ t); 32 | void decomposeE(cv::Mat E, 33 | cv::Mat_ &R1, cv::Mat_ &R2, 34 | cv::Mat_ &t1, cv::Mat_ &t2); 35 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 36 | const Vector3d &point0, const Vector3d &point1, Vector3d &point_3d); 37 | void triangulatePoints(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 38 | const vector &points0, const vector &points1, vector &points_3d); 39 | int frame_count; 40 | 41 | vector Rc; 42 | vector Rimu; 43 | vector Rc_g; 44 | Matrix3d ric; 45 | }; 46 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | struct SFMFeature 15 | { 16 | bool state; 17 | int id; 18 | //changed by wz 19 | vector> observation; 20 | 21 | double position[3]; 22 | double depth; 23 | }; 24 | 25 | struct ReprojectionError3D 26 | { 27 | ReprojectionError3D(double observed_u, double observed_v, double observed_z) 28 | : observed_u(observed_u), observed_v(observed_v), observed_z(observed_z) 29 | { 30 | } 31 | 32 | //changed by wz 33 | template 34 | bool operator()(const T *const camera_R, const T *const camera_T, const T *point, T *residuals) const 35 | { 36 | T p[3]; 37 | ceres::QuaternionRotatePoint(camera_R, point, p); 38 | p[0] += camera_T[0]; 39 | p[1] += camera_T[1]; 40 | p[2] += camera_T[2]; 41 | T xp = p[0] / sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); 42 | T yp = p[1] / sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); 43 | T zp = p[2] / sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); 44 | residuals[0] = xp - T(observed_u); 45 | residuals[1] = yp - T(observed_v); 46 | residuals[2] = zp - T(observed_z); 47 | return true; 48 | } 49 | 50 | //changed by wz 51 | static ceres::CostFunction *Create(const double observed_x, 52 | const double observed_y, 53 | const double observed_z) 54 | { 55 | return (new ceres::AutoDiffCostFunction< 56 | ReprojectionError3D, 3, 4, 3, 3>( 57 | new ReprojectionError3D(observed_x, observed_y, observed_z))); 58 | } 59 | 60 | double observed_u; 61 | double observed_v; 62 | double observed_z; 63 | }; 64 | 65 | class GlobalSFM 66 | { 67 | public: 68 | GlobalSFM(); 69 | bool construct(int frame_num, Quaterniond *q, Vector3d *T, int l, 70 | const Matrix3d relative_R, const Vector3d relative_T, 71 | vector &sfm_f, map &sfm_tracked_points); 72 | 73 | private: 74 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 75 | //changed by wz 76 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 77 | Vector3d &point0, Vector3d &point1, Vector3d &point_3d); 78 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 79 | int frame1, Eigen::Matrix &Pose1, 80 | vector &sfm_f); 81 | 82 | int feature_num; 83 | }; -------------------------------------------------------------------------------- /vins_estimator/src/initial/random_array.cc: -------------------------------------------------------------------------------- 1 | #include "random_array.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace util 10 | { 11 | 12 | std::mt19937 create_random_engine() 13 | { 14 | std::random_device random_device; 15 | std::vector v(10); 16 | std::generate(v.begin(), v.end(), std::ref(random_device)); 17 | std::seed_seq seed(v.begin(), v.end()); 18 | return std::mt19937(seed); 19 | } 20 | 21 | template 22 | std::vector create_random_array(const size_t size, const T rand_min, const T rand_max) 23 | { 24 | assert(rand_min <= rand_max); 25 | assert(size <= static_cast(rand_max - rand_min + 1)); 26 | 27 | auto random_engine = create_random_engine(); 28 | std::uniform_int_distribution uniform_int_distribution(rand_min, rand_max); 29 | const auto make_size = static_cast(size * 1.2); 30 | std::vector v; 31 | v.reserve(size); 32 | while (v.size() != size) 33 | { 34 | while (v.size() < make_size) 35 | { 36 | v.push_back(uniform_int_distribution(random_engine)); 37 | } 38 | std::sort(v.begin(), v.end()); 39 | auto unique_end = std::unique(v.begin(), v.end()); 40 | if (size < static_cast(std::distance(v.begin(), unique_end))) 41 | { 42 | unique_end = std::next(v.begin(), size); 43 | } 44 | v.erase(unique_end, v.end()); 45 | } 46 | std::shuffle(v.begin(), v.end(), random_engine); 47 | 48 | return v; 49 | } 50 | template std::vector create_random_array(size_t, int, int); 51 | template std::vector create_random_array(size_t, unsigned int, unsigned int); 52 | } 53 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/random_array.h: -------------------------------------------------------------------------------- 1 | #ifndef VINS_UTIL_RANDOM_ARRAY_H 2 | #define VINS_UTIL_RANDOM_ARRAY_H 3 | 4 | #include 5 | #include 6 | 7 | namespace util 8 | { 9 | 10 | std::mt19937 create_random_engine(); 11 | 12 | template 13 | std::vector create_random_array(const size_t size, const T rand_min, const T rand_max); 14 | 15 | } 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | #include 8 | #include "random_array.h" 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | Eigen::Matrix3d myfindFundamentalMat(vector corr1, vector corr2, int method, double theshold, double unknown, vector &status); 17 | Eigen::Matrix3d compute_E_21(vector &bearings_1, vector &bearings_2); 18 | double check_inliers(Eigen::Matrix3d &E_21, vector &is_inlier_match, 19 | vector corr1, vector corr2); 20 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 21 | 22 | private: 23 | double testTriangulation(const vector &l, 24 | const vector &r, 25 | cv::Mat_ R, cv::Mat_ t); 26 | void decomposeE(cv::Mat E, 27 | cv::Mat_ &R1, cv::Mat_ &R2, 28 | cv::Mat_ &t1, cv::Mat_ &t2); 29 | void decomposeEssentialMat(Eigen::Matrix3d _E, Eigen::Matrix3d &_R1, Eigen::Matrix3d &_R2, Eigen::Vector3d &_t); 30 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 31 | const Vector3d &point0, const Vector3d &point1, Vector3d &point_3d); 32 | void triangulatePoints(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 33 | const vector &points0, const vector &points1, vector &points_3d); 34 | int recoverPose(Eigen::Matrix3d E, vector _points1, vector _points2, 35 | Matrix3d _R, Vector3d _t, vector _mask); 36 | }; 37 | -------------------------------------------------------------------------------- /vins_estimator/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | double INIT_DEPTH; 4 | double MIN_PARALLAX; 5 | double ACC_N, ACC_W; 6 | double GYR_N, GYR_W; 7 | 8 | std::vector RIC; 9 | std::vector TIC; 10 | 11 | Eigen::Vector3d G{0.0, 0.0, 9.8}; 12 | 13 | double BIAS_ACC_THRESHOLD; 14 | double BIAS_GYR_THRESHOLD; 15 | double SOLVER_TIME; 16 | int NUM_ITERATIONS; 17 | int ESTIMATE_EXTRINSIC; 18 | int ESTIMATE_TD; 19 | int ROLLING_SHUTTER; 20 | std::string EX_CALIB_RESULT_PATH; 21 | std::string VINS_RESULT_PATH; 22 | std::string IMU_TOPIC; 23 | double ROW, COL; 24 | double TD, TR; 25 | 26 | template 27 | T readParam(ros::NodeHandle &n, std::string name) 28 | { 29 | T ans; 30 | if (n.getParam(name, ans)) 31 | { 32 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 33 | } 34 | else 35 | { 36 | ROS_ERROR_STREAM("Failed to load " << name); 37 | n.shutdown(); 38 | } 39 | return ans; 40 | } 41 | 42 | void readParameters(ros::NodeHandle &n) 43 | { 44 | std::string config_file; 45 | config_file = readParam(n, "config_file"); 46 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 47 | if (!fsSettings.isOpened()) 48 | { 49 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 50 | } 51 | 52 | fsSettings["imu_topic"] >> IMU_TOPIC; 53 | 54 | SOLVER_TIME = fsSettings["max_solver_time"]; 55 | NUM_ITERATIONS = fsSettings["max_num_iterations"]; 56 | MIN_PARALLAX = fsSettings["keyframe_parallax"]; 57 | MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH; 58 | 59 | std::string OUTPUT_PATH; 60 | fsSettings["output_path"] >> OUTPUT_PATH; 61 | // VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv"; 62 | VINS_RESULT_PATH = OUTPUT_PATH + "/stamped_traj_estimate.txt"; 63 | 64 | std::cout << "result path " << VINS_RESULT_PATH << std::endl; 65 | 66 | // create folder if not exists 67 | FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str()); 68 | 69 | std::ofstream fout(VINS_RESULT_PATH, std::ios::out); 70 | fout.close(); 71 | 72 | ACC_N = fsSettings["acc_n"]; 73 | ACC_W = fsSettings["acc_w"]; 74 | GYR_N = fsSettings["gyr_n"]; 75 | GYR_W = fsSettings["gyr_w"]; 76 | G.z() = fsSettings["g_norm"]; 77 | ROW = fsSettings["image_height"]; 78 | COL = fsSettings["image_width"]; 79 | ROS_INFO("ROW: %f COL: %f ", ROW, COL); 80 | 81 | ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; 82 | if (ESTIMATE_EXTRINSIC == 2) 83 | { 84 | ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); 85 | RIC.push_back(Eigen::Matrix3d::Identity()); 86 | TIC.push_back(Eigen::Vector3d::Zero()); 87 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 88 | } 89 | else 90 | { 91 | if (ESTIMATE_EXTRINSIC == 1) 92 | { 93 | ROS_WARN(" Optimize extrinsic param around initial guess!"); 94 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 95 | } 96 | if (ESTIMATE_EXTRINSIC == 0) 97 | ROS_WARN(" fix extrinsic param "); 98 | 99 | cv::Mat cv_R, cv_T; 100 | fsSettings["extrinsicRotation"] >> cv_R; 101 | fsSettings["extrinsicTranslation"] >> cv_T; 102 | Eigen::Matrix3d eigen_R; 103 | Eigen::Vector3d eigen_T; 104 | cv::cv2eigen(cv_R, eigen_R); 105 | cv::cv2eigen(cv_T, eigen_T); 106 | Eigen::Quaterniond Q(eigen_R); 107 | eigen_R = Q.normalized(); 108 | RIC.push_back(eigen_R); 109 | TIC.push_back(eigen_T); 110 | ROS_INFO_STREAM("Extrinsic_R : " << std::endl 111 | << RIC[0]); 112 | ROS_INFO_STREAM("Extrinsic_T : " << std::endl 113 | << TIC[0].transpose()); 114 | } 115 | 116 | INIT_DEPTH = 5.0; 117 | BIAS_ACC_THRESHOLD = 0.1; 118 | BIAS_GYR_THRESHOLD = 0.1; 119 | 120 | TD = fsSettings["td"]; 121 | ESTIMATE_TD = fsSettings["estimate_td"]; 122 | if (ESTIMATE_TD) 123 | ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); 124 | else 125 | ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); 126 | 127 | ROLLING_SHUTTER = fsSettings["rolling_shutter"]; 128 | if (ROLLING_SHUTTER) 129 | { 130 | TR = fsSettings["rolling_shutter_tr"]; 131 | ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); 132 | } 133 | else 134 | { 135 | TR = 0; 136 | } 137 | 138 | fsSettings.release(); 139 | } 140 | -------------------------------------------------------------------------------- /vins_estimator/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "utility/utility.h" 7 | #include 8 | #include 9 | #include 10 | 11 | const double FOCAL_LENGTH = 160.0; 12 | const int WINDOW_SIZE = 10; 13 | const int NUM_OF_CAM = 1; 14 | const int NUM_OF_F = 1000; 15 | #define UNIT_SPHERE_ERROR 16 | 17 | extern double INIT_DEPTH; 18 | extern double MIN_PARALLAX; 19 | extern int ESTIMATE_EXTRINSIC; 20 | 21 | extern double ACC_N, ACC_W; 22 | extern double GYR_N, GYR_W; 23 | 24 | extern std::vector RIC; 25 | extern std::vector TIC; 26 | extern Eigen::Vector3d G; 27 | 28 | extern double BIAS_ACC_THRESHOLD; 29 | extern double BIAS_GYR_THRESHOLD; 30 | extern double SOLVER_TIME; 31 | extern int NUM_ITERATIONS; 32 | extern std::string EX_CALIB_RESULT_PATH; 33 | extern std::string VINS_RESULT_PATH; 34 | extern std::string IMU_TOPIC; 35 | extern double TD; 36 | extern double TR; 37 | extern int ESTIMATE_TD; 38 | extern int ROLLING_SHUTTER; 39 | extern double ROW, COL; 40 | 41 | void readParameters(ros::NodeHandle &n); 42 | 43 | enum SIZE_PARAMETERIZATION 44 | { 45 | SIZE_POSE = 7, 46 | SIZE_SPEEDBIAS = 9, 47 | SIZE_FEATURE = 1 48 | }; 49 | 50 | enum StateOrder 51 | { 52 | O_P = 0, 53 | O_R = 3, 54 | O_V = 6, 55 | O_BA = 9, 56 | O_BG = 12 57 | }; 58 | 59 | enum NoiseOrder 60 | { 61 | O_AN = 0, 62 | O_GN = 3, 63 | O_AW = 6, 64 | O_GW = 9 65 | }; 66 | -------------------------------------------------------------------------------- /vins_estimator/src/pnp_solver.h: -------------------------------------------------------------------------------- 1 | #ifndef PNP_SOLVER_H 2 | #define PNP_SOLVER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | class PnpSolver 11 | { 12 | public: 13 | PnpSolver(); 14 | ~PnpSolver(){}; 15 | void set_internal_parameters(double cx, double cy, double fx, double fy); 16 | void set_maximum_number_of_correspondences(int n); 17 | void reset_number_of_correspondences(); 18 | void add_correspondence(Eigen::Vector3d point3d, Eigen::Vector3d uvz); 19 | 20 | double compute_pose(Eigen::Matrix3d &R, Eigen::Vector3d &T); 21 | void projectPoints(std::vector &src, std::vector &dst, Eigen::Matrix3d &R, Eigen::Vector3d &T); 22 | 23 | double cx, cy, fx, fy; 24 | 25 | private: 26 | void choose_control_points(); 27 | void compute_barycentric_coordinates(); 28 | 29 | inline double dot(const double *v1, const double *v2); 30 | void compute_L_6x10(const Eigen::Matrix &Ut, Eigen::Matrix &L_6x10); 31 | void compute_rho(Eigen::Matrix &Rho); 32 | void find_betas_0(const Eigen::Matrix &L_6x10, const Eigen::Matrix &Rho, 33 | Eigen::Vector4d &Betas); 34 | void find_betas_1(const Eigen::Matrix &L_6x10, const Eigen::Matrix &Rho, 35 | Eigen::Vector4d &Betas); 36 | void find_betas_2(const Eigen::Matrix &L_6x10, const Eigen::Matrix &Rho, 37 | Eigen::Vector4d &Betas); 38 | void compute_ccs(const Eigen::Vector4d &Betas, const Eigen::Matrix &Ut); 39 | void compute_pcs(); 40 | void solve_for_sign(); 41 | void estimate_R_and_t(Eigen::Matrix3d &R, Eigen::Vector3d &T); 42 | double reprojection_error(const Eigen::Matrix3d &R, const Eigen::Vector3d &T); 43 | double compute_R_and_t(const Eigen::Matrix &Ut, const Eigen::Vector4d &Betas, 44 | Eigen::Matrix3d &R, Eigen::Vector3d &T); 45 | void gauss_newton(const Eigen::Matrix &L_6x10, const Eigen::Matrix &Rho, Eigen::Vector4d &betas); 46 | void compute_A_and_b_gauss_newton(const Eigen::Matrix &L_6x10, const Eigen::Matrix &Rho, 47 | Eigen::Vector4d &betas, Eigen::Matrix &A, Eigen::Matrix &b); 48 | void qr_solve(Eigen::Matrix &A, Eigen::Matrix &B, Eigen::Vector4d &X); 49 | 50 | int maximum_number_of_correspondences; 51 | int number_of_correspondences; 52 | Eigen::MatrixXd pws, us, alphas, pcs, signs; 53 | Eigen::Matrix cws, ccs; 54 | double cws_determinant; 55 | }; 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/CameraPoseVisualization.cpp: -------------------------------------------------------------------------------- 1 | #include "CameraPoseVisualization.h" 2 | 3 | const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0); 4 | const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0); 5 | const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0); 6 | const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0); 7 | const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0); 8 | const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0); 9 | const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0); 10 | const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0); 11 | 12 | void Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) { 13 | p.x = v.x(); 14 | p.y = v.y(); 15 | p.z = v.z(); 16 | } 17 | 18 | CameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a) 19 | : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) { 20 | m_image_boundary_color.r = r; 21 | m_image_boundary_color.g = g; 22 | m_image_boundary_color.b = b; 23 | m_image_boundary_color.a = a; 24 | m_optical_center_connector_color.r = r; 25 | m_optical_center_connector_color.g = g; 26 | m_optical_center_connector_color.b = b; 27 | m_optical_center_connector_color.a = a; 28 | } 29 | 30 | void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) { 31 | m_image_boundary_color.r = r; 32 | m_image_boundary_color.g = g; 33 | m_image_boundary_color.b = b; 34 | m_image_boundary_color.a = a; 35 | } 36 | 37 | void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) { 38 | m_optical_center_connector_color.r = r; 39 | m_optical_center_connector_color.g = g; 40 | m_optical_center_connector_color.b = b; 41 | m_optical_center_connector_color.a = a; 42 | } 43 | 44 | void CameraPoseVisualization::setScale(double s) { 45 | m_scale = s; 46 | } 47 | void CameraPoseVisualization::setLineWidth(double width) { 48 | m_line_width = width; 49 | } 50 | void CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ 51 | visualization_msgs::Marker marker; 52 | 53 | marker.ns = m_marker_ns; 54 | marker.id = m_markers.size() + 1; 55 | marker.type = visualization_msgs::Marker::LINE_LIST; 56 | marker.action = visualization_msgs::Marker::ADD; 57 | marker.scale.x = 0.005; 58 | 59 | marker.color.g = 1.0f; 60 | marker.color.a = 1.0; 61 | 62 | geometry_msgs::Point point0, point1; 63 | 64 | Eigen2Point(p0, point0); 65 | Eigen2Point(p1, point1); 66 | 67 | marker.points.push_back(point0); 68 | marker.points.push_back(point1); 69 | 70 | m_markers.push_back(marker); 71 | } 72 | 73 | void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ 74 | visualization_msgs::Marker marker; 75 | 76 | marker.ns = m_marker_ns; 77 | marker.id = m_markers.size() + 1; 78 | marker.type = visualization_msgs::Marker::LINE_LIST; 79 | marker.action = visualization_msgs::Marker::ADD; 80 | marker.scale.x = 0.04; 81 | //marker.scale.x = 0.3; 82 | 83 | marker.color.r = 1.0f; 84 | marker.color.b = 1.0f; 85 | marker.color.a = 1.0; 86 | 87 | geometry_msgs::Point point0, point1; 88 | 89 | Eigen2Point(p0, point0); 90 | Eigen2Point(p1, point1); 91 | 92 | marker.points.push_back(point0); 93 | marker.points.push_back(point1); 94 | 95 | m_markers.push_back(marker); 96 | } 97 | 98 | 99 | void CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) { 100 | visualization_msgs::Marker marker; 101 | 102 | marker.ns = m_marker_ns; 103 | marker.id = m_markers.size() + 1; 104 | marker.type = visualization_msgs::Marker::LINE_STRIP; 105 | marker.action = visualization_msgs::Marker::ADD; 106 | marker.scale.x = m_line_width; 107 | 108 | marker.pose.position.x = 0.0; 109 | marker.pose.position.y = 0.0; 110 | marker.pose.position.z = 0.0; 111 | marker.pose.orientation.w = 1.0; 112 | marker.pose.orientation.x = 0.0; 113 | marker.pose.orientation.y = 0.0; 114 | marker.pose.orientation.z = 0.0; 115 | 116 | 117 | geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; 118 | 119 | Eigen2Point(q * (m_scale *imlt) + p, pt_lt); 120 | Eigen2Point(q * (m_scale *imlb) + p, pt_lb); 121 | Eigen2Point(q * (m_scale *imrt) + p, pt_rt); 122 | Eigen2Point(q * (m_scale *imrb) + p, pt_rb); 123 | Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0); 124 | Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1); 125 | Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2); 126 | Eigen2Point(q * (m_scale *oc ) + p, pt_oc); 127 | 128 | // image boundaries 129 | marker.points.push_back(pt_lt); 130 | marker.points.push_back(pt_lb); 131 | marker.colors.push_back(m_image_boundary_color); 132 | marker.colors.push_back(m_image_boundary_color); 133 | 134 | marker.points.push_back(pt_lb); 135 | marker.points.push_back(pt_rb); 136 | marker.colors.push_back(m_image_boundary_color); 137 | marker.colors.push_back(m_image_boundary_color); 138 | 139 | marker.points.push_back(pt_rb); 140 | marker.points.push_back(pt_rt); 141 | marker.colors.push_back(m_image_boundary_color); 142 | marker.colors.push_back(m_image_boundary_color); 143 | 144 | marker.points.push_back(pt_rt); 145 | marker.points.push_back(pt_lt); 146 | marker.colors.push_back(m_image_boundary_color); 147 | marker.colors.push_back(m_image_boundary_color); 148 | 149 | // top-left indicator 150 | marker.points.push_back(pt_lt0); 151 | marker.points.push_back(pt_lt1); 152 | marker.colors.push_back(m_image_boundary_color); 153 | marker.colors.push_back(m_image_boundary_color); 154 | 155 | marker.points.push_back(pt_lt1); 156 | marker.points.push_back(pt_lt2); 157 | marker.colors.push_back(m_image_boundary_color); 158 | marker.colors.push_back(m_image_boundary_color); 159 | 160 | // optical center connector 161 | marker.points.push_back(pt_lt); 162 | marker.points.push_back(pt_oc); 163 | marker.colors.push_back(m_optical_center_connector_color); 164 | marker.colors.push_back(m_optical_center_connector_color); 165 | 166 | 167 | marker.points.push_back(pt_lb); 168 | marker.points.push_back(pt_oc); 169 | marker.colors.push_back(m_optical_center_connector_color); 170 | marker.colors.push_back(m_optical_center_connector_color); 171 | 172 | marker.points.push_back(pt_rt); 173 | marker.points.push_back(pt_oc); 174 | marker.colors.push_back(m_optical_center_connector_color); 175 | marker.colors.push_back(m_optical_center_connector_color); 176 | 177 | marker.points.push_back(pt_rb); 178 | marker.points.push_back(pt_oc); 179 | marker.colors.push_back(m_optical_center_connector_color); 180 | marker.colors.push_back(m_optical_center_connector_color); 181 | 182 | m_markers.push_back(marker); 183 | } 184 | 185 | void CameraPoseVisualization::reset() { 186 | m_markers.clear(); 187 | } 188 | 189 | void CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::Header &header ) { 190 | visualization_msgs::MarkerArray markerArray_msg; 191 | 192 | for(auto& marker : m_markers) { 193 | marker.header = header; 194 | markerArray_msg.markers.push_back(marker); 195 | } 196 | 197 | pub.publish(markerArray_msg); 198 | } -------------------------------------------------------------------------------- /vins_estimator/src/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class CameraPoseVisualization { 11 | public: 12 | std::string m_marker_ns; 13 | 14 | CameraPoseVisualization(float r, float g, float b, float a); 15 | 16 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 17 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 18 | void setScale(double s); 19 | void setLineWidth(double width); 20 | 21 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 22 | void reset(); 23 | 24 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 25 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 26 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 27 | private: 28 | std::vector m_markers; 29 | std_msgs::ColorRGBA m_image_boundary_color; 30 | std_msgs::ColorRGBA m_optical_center_connector_color; 31 | double m_scale; 32 | double m_line_width; 33 | 34 | static const Eigen::Vector3d imlt; 35 | static const Eigen::Vector3d imlb; 36 | static const Eigen::Vector3d imrt; 37 | static const Eigen::Vector3d imrb; 38 | static const Eigen::Vector3d oc ; 39 | static const Eigen::Vector3d lt0 ; 40 | static const Eigen::Vector3d lt1 ; 41 | static const Eigen::Vector3d lt2 ; 42 | }; 43 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 4 | { 5 | Eigen::Matrix3d R0; 6 | Eigen::Vector3d ng1 = g.normalized(); 7 | Eigen::Vector3d ng2{0, 0, 1.0}; 8 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 9 | double yaw = Utility::R2ypr(R0).x(); 10 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 11 | return R0; 12 | } 13 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class Utility 13 | { 14 | public: 15 | template 16 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 17 | { 18 | typedef typename Derived::Scalar Scalar_t; 19 | 20 | Eigen::Quaternion dq; 21 | Eigen::Matrix half_theta = theta; 22 | half_theta /= static_cast(2.0); 23 | dq.w() = static_cast(1.0); 24 | dq.x() = half_theta.x(); 25 | dq.y() = half_theta.y(); 26 | dq.z() = half_theta.z(); 27 | return dq; 28 | } 29 | 30 | template 31 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 32 | { 33 | Eigen::Matrix ans; 34 | ans << typename Derived::Scalar(0), -q(2), q(1), 35 | q(2), typename Derived::Scalar(0), -q(0), 36 | -q(1), q(0), typename Derived::Scalar(0); 37 | return ans; 38 | } 39 | 40 | template 41 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 42 | { 43 | return q; 44 | } 45 | 46 | template 47 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 48 | { 49 | Eigen::Quaternion qq = positify(q); 50 | Eigen::Matrix ans; 51 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 52 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 53 | return ans; 54 | } 55 | 56 | template 57 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 58 | { 59 | Eigen::Quaternion pp = positify(p); 60 | Eigen::Matrix ans; 61 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 62 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 63 | return ans; 64 | } 65 | 66 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 67 | { 68 | Eigen::Vector3d n = R.col(0); 69 | Eigen::Vector3d o = R.col(1); 70 | Eigen::Vector3d a = R.col(2); 71 | 72 | Eigen::Vector3d ypr(3); 73 | double y = atan2(n(1), n(0)); 74 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 75 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 76 | ypr(0) = y; 77 | ypr(1) = p; 78 | ypr(2) = r; 79 | 80 | return ypr / M_PI * 180.0; 81 | } 82 | 83 | template 84 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 85 | { 86 | typedef typename Derived::Scalar Scalar_t; 87 | 88 | Scalar_t y = ypr(0) / 180.0 * M_PI; 89 | Scalar_t p = ypr(1) / 180.0 * M_PI; 90 | Scalar_t r = ypr(2) / 180.0 * M_PI; 91 | 92 | Eigen::Matrix Rz; 93 | Rz << cos(y), -sin(y), 0, 94 | sin(y), cos(y), 0, 95 | 0, 0, 1; 96 | 97 | Eigen::Matrix Ry; 98 | Ry << cos(p), 0., sin(p), 99 | 0., 1., 0., 100 | -sin(p), 0., cos(p); 101 | 102 | Eigen::Matrix Rx; 103 | Rx << 1., 0., 0., 104 | 0., cos(r), -sin(r), 105 | 0., sin(r), cos(r); 106 | 107 | return Rz * Ry * Rx; 108 | } 109 | 110 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 111 | 112 | template 113 | struct uint_ 114 | { 115 | }; 116 | 117 | template 118 | void unroller(const Lambda &f, const IterT &iter, uint_) 119 | { 120 | unroller(f, iter, uint_()); 121 | f(iter + N); 122 | } 123 | 124 | template 125 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 126 | { 127 | f(iter); 128 | } 129 | 130 | template 131 | static T normalizeAngle(const T &angle_degrees) 132 | { 133 | T two_pi(2.0 * 180); 134 | if (angle_degrees > 0) 135 | return angle_degrees - 136 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 137 | else 138 | return angle_degrees + 139 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 140 | }; 141 | }; 142 | 143 | class FileSystemHelper 144 | { 145 | public: 146 | /****************************************************************************** 147 | * Recursively create directory if `path` not exists. 148 | * Return 0 if success. 149 | *****************************************************************************/ 150 | static int createDirectoryIfNotExists(const char *path) 151 | { 152 | struct stat info; 153 | int statRC = stat(path, &info); 154 | if (statRC != 0) 155 | { 156 | if (errno == ENOENT) 157 | { 158 | printf("%s not exists, trying to create it \n", path); 159 | if (!createDirectoryIfNotExists(dirname(strdupa(path)))) 160 | { 161 | if (mkdir(path, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) != 0) 162 | { 163 | fprintf(stderr, "Failed to create folder %s \n", path); 164 | return 1; 165 | } 166 | else 167 | return 0; 168 | } 169 | else 170 | return 1; 171 | } // directory not exists 172 | if (errno == ENOTDIR) 173 | { 174 | fprintf(stderr, "%s is not a directory path \n", path); 175 | return 1; 176 | } // something in path prefix is not a dir 177 | return 1; 178 | } 179 | return (info.st_mode & S_IFDIR) ? 0 : 1; 180 | } 181 | }; 182 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/visualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "CameraPoseVisualization.h" 17 | #include 18 | #include "../estimator.h" 19 | #include "../parameters.h" 20 | #include 21 | 22 | extern ros::Publisher pub_odometry; 23 | extern ros::Publisher pub_path, pub_pose; 24 | extern ros::Publisher pub_cloud, pub_map; 25 | extern ros::Publisher pub_key_poses; 26 | extern ros::Publisher pub_ref_pose, pub_cur_pose; 27 | extern ros::Publisher pub_key; 28 | extern nav_msgs::Path path; 29 | extern ros::Publisher pub_pose_graph; 30 | extern int IMAGE_ROW, IMAGE_COL; 31 | 32 | void registerPub(ros::NodeHandle &n); 33 | 34 | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header); 35 | 36 | void printStatistics(const Estimator &estimator, double t); 37 | 38 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 39 | 40 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 41 | 42 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 43 | 44 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 45 | 46 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 47 | 48 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 49 | 50 | void pubKeyframe(const Estimator &estimator); 51 | 52 | void pubRelocalization(const Estimator &estimator); --------------------------------------------------------------------------------