├── 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 ├── AR_demo.rviz ├── black_box │ └── black_box_config.yaml ├── extrinsic_parameter_example.pdf ├── fisheye_mask.jpg ├── fisheye_mask_752x480.jpg ├── realsense │ └── realsense_fisheye_config.yaml ├── tum │ └── tum_config.yaml └── vins_rviz_config.rviz ├── docker ├── Dockerfile ├── Makefile └── run.sh ├── feature_tracker ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── include │ ├── feature_tracker.h │ ├── parameters.h │ └── tic_toc.h ├── package.xml └── src │ ├── feature_tracker.cpp │ ├── feature_tracker_node.cpp │ └── parameters.cpp ├── pose_graph ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── include │ ├── ThirdParty │ │ ├── DBoW │ │ │ ├── BowVector.h │ │ │ ├── DBoW2.h │ │ │ ├── FBrief.h │ │ │ ├── FClass.h │ │ │ ├── FeatureVector.h │ │ │ ├── QueryResults.h │ │ │ ├── ScoringObject.h │ │ │ ├── TemplatedDatabase.h │ │ │ └── TemplatedVocabulary.h │ │ ├── DUtils │ │ │ ├── DException.h │ │ │ ├── DUtils.h │ │ │ ├── Random.h │ │ │ └── Timestamp.h │ │ ├── DVision │ │ │ ├── BRIEF.h │ │ │ └── DVision.h │ │ └── VocabularyBinary.hpp │ ├── keyframe.h │ ├── parameters.h │ ├── pose_graph.h │ └── utility │ │ ├── CameraPoseVisualization.h │ │ ├── tic_toc.h │ │ └── utility.h ├── package.xml └── src │ ├── ThirdParty │ ├── DBoW │ │ ├── BowVector.cpp │ │ ├── FBrief.cpp │ │ ├── FeatureVector.cpp │ │ ├── QueryResults.cpp │ │ └── ScoringObject.cpp │ ├── DUtils │ │ ├── Random.cpp │ │ └── Timestamp.cpp │ ├── DVision │ │ └── BRIEF.cpp │ └── VocabularyBinary.cpp │ ├── keyframe.cpp │ ├── pose_graph.cpp │ ├── pose_graph_node.cpp │ └── utility │ ├── CameraPoseVisualization.cpp │ └── utility.cpp └── vins_estimator ├── CMakeLists.txt ├── cmake └── FindEigen.cmake ├── include ├── backend.h ├── estimator.h ├── factor │ ├── imu_factor.h │ ├── integration_base.h │ ├── marginalization_factor.h │ ├── pose_local_parameterization.h │ ├── projection_factor.h │ └── projection_td_factor.h ├── feature_manager.h ├── initial.h ├── initial │ ├── initial_alignment.h │ ├── initial_ex_rotation.h │ ├── initial_sfm.h │ └── solve_5pts.h ├── parameters.h └── utility │ ├── CameraPoseVisualization.h │ ├── tic_toc.h │ ├── utility.h │ └── visualization.h ├── launch ├── 3dm.launch ├── black_box.launch ├── cla.launch ├── euroc.launch ├── euroc_no_extrinsic_param.launch ├── realsense_color.launch ├── realsense_fisheye.launch ├── tum.launch └── vins_rviz.launch ├── package.xml └── src ├── backend.cpp ├── estimator.cpp ├── estimator_node.cpp ├── factor ├── marginalization_factor.cpp ├── pose_local_parameterization.cpp ├── projection_factor.cpp └── projection_td_factor.cpp ├── feature_manager.cpp ├── initial.cpp ├── initial ├── initial_aligment.cpp ├── initial_ex_rotation.cpp ├── initial_sfm.cpp └── solve_5pts.cpp ├── parameters.cpp └── utility ├── CameraPoseVisualization.cpp ├── utility.cpp └── visualization.cpp /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++11") 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/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/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/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/extrinsic_parameter_example.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iwander-all/vins-mono-self-improved-/409e78923ba4ba84124fc20268fac864a07dcd95/config/extrinsic_parameter_example.pdf -------------------------------------------------------------------------------- /config/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iwander-all/vins-mono-self-improved-/409e78923ba4ba84124fc20268fac864a07dcd95/config/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fisheye_mask_752x480.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iwander-all/vins-mono-self-improved-/409e78923ba4ba84124fc20268fac864a07dcd95/config/fisheye_mask_752x480.jpg -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /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_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | sensor_msgs 12 | cv_bridge 13 | camera_model 14 | ) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | catkin_package() 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 25 | find_package(Eigen3) 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ${EIGEN3_INCLUDE_DIR} 29 | ) 30 | 31 | add_executable(feature_tracker 32 | src/feature_tracker_node.cpp 33 | src/parameters.cpp 34 | src/feature_tracker.cpp 35 | ) 36 | 37 | target_link_libraries(feature_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS}) 38 | -------------------------------------------------------------------------------- /feature_tracker/include/feature_tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef _FEATURE_TRACKER_H_ 2 | #define _FEATURE_TRACKER_H_ 3 | 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 | #include "parameters.h" 18 | #include "tic_toc.h" 19 | 20 | using namespace std; 21 | using namespace camodocal; 22 | using namespace Eigen; 23 | 24 | 25 | /** 26 | * @brief check whether the pixel coordinates of tracking features in border or not 27 | * @param Input cv::Point2f &pt 28 | */ 29 | bool inBorder(const cv::Point2f &pt); 30 | 31 | /** 32 | * @brief delete items in output value according to the input value 33 | * @param Input vector status 34 | * @param Output vector &v 35 | */ 36 | void reduceVector(vector &v, vector status); 37 | 38 | /** 39 | * @brief delete items in output value according to the input value 40 | * @param Input vector status 41 | * @param Output vector &v 42 | */ 43 | void reduceVector(vector &v, vector status); 44 | 45 | 46 | /** 47 | * @brief Class of FeatureTracker 48 | */ 49 | class FeatureTracker 50 | { 51 | public: 52 | FeatureTracker(); 53 | 54 | /** 55 | * @brief eualize the image 56 | * @param Input const cv::Mat &_img 57 | * @param Output cv::Mat &img 58 | */ 59 | void equalize(const cv::Mat &_img,cv::Mat &img); 60 | 61 | /** 62 | * @brief track existing features and delete failures 63 | */ 64 | void flowTrack(); 65 | 66 | /** 67 | * @brief track new features in forward image 68 | */ 69 | void trackNew(); 70 | 71 | /** 72 | * @brief main process of feature tracker 73 | * @param Input const cv::Mat &_img 74 | * @param Input double _cur_time 75 | */ 76 | void readImage(const cv::Mat &_img,double _cur_time); 77 | 78 | /** 79 | * @brief rank features according to their existing times and setMask for high rank features 80 | * to avoid crowded layout of features 81 | */ 82 | void setMask(); 83 | 84 | /** 85 | * @brief add new tracked features into buffer 86 | */ 87 | void addPoints(); 88 | 89 | /** 90 | * @brief update ID for features 91 | */ 92 | bool updateID(unsigned int i); 93 | 94 | /** 95 | * @brief read cameras' parameters 96 | * @param Input const string &calib_file 97 | */ 98 | void readIntrinsicParameter(const string &calib_file); 99 | 100 | /** 101 | * @brief show undistortion 102 | * TODO not important ,can be deleted 103 | * @param Input const string &name 104 | */ 105 | void showUndistortion(const string &name); 106 | 107 | /** 108 | * @brief use Fundamental matrix to delete outliers 109 | */ 110 | void rejectWithF(); 111 | 112 | /** 113 | * @brief get undistorted normalized coordinates of features 114 | * @param Input const string &name 115 | */ 116 | void undistortedPoints(); 117 | 118 | cv::Mat mask; 119 | cv::Mat fisheye_mask; 120 | 121 | cv::Mat prev_img, cur_img, forw_img; 122 | vector n_pts; 123 | vector prev_pts, cur_pts, forw_pts; 124 | vector prev_un_pts, cur_un_pts; 125 | vector pts_velocity; 126 | vector ids; 127 | vector track_cnt; 128 | map cur_un_pts_map; 129 | map prev_un_pts_map; 130 | 131 | camodocal::CameraPtr m_camera; 132 | 133 | double cur_time; 134 | double prev_time; 135 | 136 | static int n_id; 137 | }; 138 | 139 | #endif -------------------------------------------------------------------------------- /feature_tracker/include/parameters.h: -------------------------------------------------------------------------------- 1 | #ifndef _PARAMETERS_H_ 2 | #define _PARAMETERS_H_ 3 | 4 | #include 5 | #include 6 | 7 | extern int ROW; 8 | extern int COL; 9 | extern int FOCAL_LENGTH; 10 | const int NUM_OF_CAM = 1; 11 | 12 | 13 | extern std::string IMAGE_TOPIC; 14 | extern std::string IMU_TOPIC; 15 | extern std::string FISHEYE_MASK; 16 | extern std::vector CAM_NAMES; 17 | extern int MAX_CNT; 18 | extern int MIN_DIST; 19 | extern int WINDOW_SIZE; 20 | extern int FREQ; 21 | extern double F_THRESHOLD; 22 | extern int SHOW_TRACK; 23 | extern int STEREO_TRACK; 24 | extern int EQUALIZE; 25 | extern int FISHEYE; 26 | extern bool PUB_THIS_FRAME; 27 | 28 | void readParameters(ros::NodeHandle &n); 29 | 30 | #endif -------------------------------------------------------------------------------- /feature_tracker/include/tic_toc.h: -------------------------------------------------------------------------------- 1 | #ifndef _TIC_TOC_H_ 2 | #define _TIC_TOC_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class TicToc 9 | { 10 | public: 11 | TicToc() 12 | { 13 | tic(); 14 | } 15 | 16 | void tic() 17 | { 18 | start = std::chrono::system_clock::now(); 19 | } 20 | 21 | double toc() 22 | { 23 | end = std::chrono::system_clock::now(); 24 | std::chrono::duration elapsed_seconds = end - start; 25 | return elapsed_seconds.count() * 1000; 26 | } 27 | 28 | private: 29 | std::chrono::time_point start, end; 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /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/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "../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 = 460; 66 | PUB_THIS_FRAME = false; 67 | 68 | if (FREQ == 0) 69 | FREQ = 100; 70 | 71 | fsSettings.release(); 72 | 73 | 74 | } 75 | -------------------------------------------------------------------------------- /pose_graph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pose_graph) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 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 | nav_msgs 13 | camera_model 14 | cv_bridge 15 | roslib 16 | ) 17 | 18 | find_package(OpenCV) 19 | 20 | 21 | find_package(Ceres REQUIRED) 22 | 23 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 24 | find_package(Eigen3) 25 | 26 | include_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) 27 | 28 | catkin_package() 29 | 30 | add_executable(pose_graph 31 | src/pose_graph_node.cpp 32 | src/pose_graph.cpp 33 | src/keyframe.cpp 34 | src/utility/CameraPoseVisualization.cpp 35 | src/ThirdParty/DBoW/BowVector.cpp 36 | src/ThirdParty/DBoW/FBrief.cpp 37 | src/ThirdParty/DBoW/FeatureVector.cpp 38 | src/ThirdParty/DBoW/QueryResults.cpp 39 | src/ThirdParty/DBoW/ScoringObject.cpp 40 | src/ThirdParty/DUtils/Random.cpp 41 | src/ThirdParty/DUtils/Timestamp.cpp 42 | src/ThirdParty/DVision/BRIEF.cpp 43 | src/ThirdParty/VocabularyBinary.cpp 44 | ) 45 | 46 | target_link_libraries(pose_graph ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 47 | # message("catkin_lib ${catkin_LIBRARIES}") 48 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DBoW/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DBoW/DBoW2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DBoW2.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: Generic include file for the DBoW2 classes and 6 | * the specialized vocabularies and databases 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DBoW2 Library 12 | * 13 | * DBoW2 library for C++: 14 | * Bag-of-word image database for image retrieval. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://doriangalvez.com 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries, 23 | * as well as the boost::dynamic_bitset class. 24 | * 25 | * \section citation Citation 26 | * If you use this software in academic works, please cite: 27 | 
28 |    @@ARTICLE{GalvezTRO12,
29 |     author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
30 |     journal={IEEE Transactions on Robotics},
31 |     title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
32 |     year={2012},
33 |     month={October},
34 |     volume={28},
35 |     number={5},
36 |     pages={1188--1197},
37 |     doi={10.1109/TRO.2012.2197158},
38 |     ISSN={1552-3098}
39 |   }
40 |
41 | * 42 | * \section license License 43 | * This file is licensed under a Creative Commons 44 | * Attribution-NonCommercial-ShareAlike 3.0 license. 45 | * This file can be freely used and users can use, download and edit this file 46 | * provided that credit is attributed to the original author. No users are 47 | * permitted to use this file for commercial purposes unless explicit permission 48 | * is given by the original author. Derivative works must be licensed using the 49 | * same or similar license. 50 | * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further 51 | * details. 52 | * 53 | */ 54 | 55 | #ifndef __D_T_DBOW2__ 56 | #define __D_T_DBOW2__ 57 | 58 | /// Includes all the data structures to manage vocabularies and image databases 59 | namespace DBoW2 60 | { 61 | } 62 | 63 | #include "TemplatedVocabulary.h" 64 | #include "TemplatedDatabase.h" 65 | #include "BowVector.h" 66 | #include "FeatureVector.h" 67 | #include "QueryResults.h" 68 | #include "FBrief.h" 69 | 70 | /// BRIEF Vocabulary 71 | typedef DBoW2::TemplatedVocabulary 72 | BriefVocabulary; 73 | 74 | /// BRIEF Database 75 | typedef DBoW2::TemplatedDatabase 76 | BriefDatabase; 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DBoW/FBrief.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_BRIEF__ 11 | #define __D_T_F_BRIEF__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | #include "../DVision/DVision.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Functions to manipulate BRIEF descriptors 23 | class FBrief: protected FClass 24 | { 25 | public: 26 | 27 | typedef DVision::BRIEF::bitset TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | static void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean); 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | 68 | }; 69 | 70 | } // namespace DBoW2 71 | 72 | #endif 73 | 74 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DBoW/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DBoW/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DBoW/QueryResults.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.h 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_QUERY_RESULTS__ 11 | #define __D_T_QUERY_RESULTS__ 12 | 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | /// Id of entries of the database 18 | typedef unsigned int EntryId; 19 | 20 | /// Single result of a query 21 | class Result 22 | { 23 | public: 24 | 25 | /// Entry id 26 | EntryId Id; 27 | 28 | /// Score obtained 29 | double Score; 30 | 31 | /// debug 32 | int nWords; // words in common 33 | // !!! this is filled only by Bhatt score! 34 | // (and for BCMatching, BCThresholding then) 35 | 36 | double bhatScore, chiScore; 37 | /// debug 38 | 39 | // only done by ChiSq and BCThresholding 40 | double sumCommonVi; 41 | double sumCommonWi; 42 | double expectedChiScore; 43 | /// debug 44 | 45 | /** 46 | * Empty constructors 47 | */ 48 | inline Result(){} 49 | 50 | /** 51 | * Creates a result with the given data 52 | * @param _id entry id 53 | * @param _score score 54 | */ 55 | inline Result(EntryId _id, double _score): Id(_id), Score(_score){} 56 | 57 | /** 58 | * Compares the scores of two results 59 | * @return true iff this.score < r.score 60 | */ 61 | inline bool operator<(const Result &r) const 62 | { 63 | return this->Score < r.Score; 64 | } 65 | 66 | /** 67 | * Compares the scores of two results 68 | * @return true iff this.score > r.score 69 | */ 70 | inline bool operator>(const Result &r) const 71 | { 72 | return this->Score > r.Score; 73 | } 74 | 75 | /** 76 | * Compares the entry id of the result 77 | * @return true iff this.id == id 78 | */ 79 | inline bool operator==(EntryId id) const 80 | { 81 | return this->Id == id; 82 | } 83 | 84 | /** 85 | * Compares the score of this entry with a given one 86 | * @param s score to compare with 87 | * @return true iff this score < s 88 | */ 89 | inline bool operator<(double s) const 90 | { 91 | return this->Score < s; 92 | } 93 | 94 | /** 95 | * Compares the score of this entry with a given one 96 | * @param s score to compare with 97 | * @return true iff this score > s 98 | */ 99 | inline bool operator>(double s) const 100 | { 101 | return this->Score > s; 102 | } 103 | 104 | /** 105 | * Compares the score of two results 106 | * @param a 107 | * @param b 108 | * @return true iff a.Score > b.Score 109 | */ 110 | static inline bool gt(const Result &a, const Result &b) 111 | { 112 | return a.Score > b.Score; 113 | } 114 | 115 | /** 116 | * Compares the scores of two results 117 | * @return true iff a.Score > b.Score 118 | */ 119 | inline static bool ge(const Result &a, const Result &b) 120 | { 121 | return a.Score > b.Score; 122 | } 123 | 124 | /** 125 | * Returns true iff a.Score >= b.Score 126 | * @param a 127 | * @param b 128 | * @return true iff a.Score >= b.Score 129 | */ 130 | static inline bool geq(const Result &a, const Result &b) 131 | { 132 | return a.Score >= b.Score; 133 | } 134 | 135 | /** 136 | * Returns true iff a.Score >= s 137 | * @param a 138 | * @param s 139 | * @return true iff a.Score >= s 140 | */ 141 | static inline bool geqv(const Result &a, double s) 142 | { 143 | return a.Score >= s; 144 | } 145 | 146 | 147 | /** 148 | * Returns true iff a.Id < b.Id 149 | * @param a 150 | * @param b 151 | * @return true iff a.Id < b.Id 152 | */ 153 | static inline bool ltId(const Result &a, const Result &b) 154 | { 155 | return a.Id < b.Id; 156 | } 157 | 158 | /** 159 | * Prints a string version of the result 160 | * @param os ostream 161 | * @param ret Result to print 162 | */ 163 | friend std::ostream & operator<<(std::ostream& os, const Result& ret ); 164 | }; 165 | 166 | /// Multiple results from a query 167 | class QueryResults: public std::vector 168 | { 169 | public: 170 | 171 | /** 172 | * Multiplies all the scores in the vector by factor 173 | * @param factor 174 | */ 175 | inline void scaleScores(double factor); 176 | 177 | /** 178 | * Prints a string version of the results 179 | * @param os ostream 180 | * @param ret QueryResults to print 181 | */ 182 | friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret ); 183 | 184 | /** 185 | * Saves a matlab file with the results 186 | * @param filename 187 | */ 188 | void saveM(const std::string &filename) const; 189 | 190 | }; 191 | 192 | // -------------------------------------------------------------------------- 193 | 194 | inline void QueryResults::scaleScores(double factor) 195 | { 196 | for(QueryResults::iterator qit = begin(); qit != end(); ++qit) 197 | qit->Score *= factor; 198 | } 199 | 200 | // -------------------------------------------------------------------------- 201 | 202 | } // namespace TemplatedBoW 203 | 204 | #endif 205 | 206 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DBoW/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | }; 45 | 46 | /** 47 | * Macro for defining Scoring classes 48 | * @param NAME name of class 49 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 50 | * @param NORM type of norm to use when MUSTNORMALIZE 51 | */ 52 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 53 | NAME: public GeneralScoring \ 54 | { public: \ 55 | /** \ 56 | * Computes score between two vectors \ 57 | * @param v \ 58 | * @param w \ 59 | * @return score between v and w \ 60 | */ \ 61 | virtual double score(const BowVector &v, const BowVector &w) const; \ 62 | \ 63 | /** \ 64 | * Says if a vector must be normalized according to the scoring function \ 65 | * @param norm (out) if true, norm to use 66 | * @return true iff vectors must be normalized \ 67 | */ \ 68 | virtual inline bool mustNormalize(LNorm &norm) const \ 69 | { norm = NORM; return MUSTNORMALIZE; } \ 70 | } 71 | 72 | /// L1 Scoring object 73 | class __SCORING_CLASS(L1Scoring, true, L1); 74 | 75 | /// L2 Scoring object 76 | class __SCORING_CLASS(L2Scoring, true, L2); 77 | 78 | /// Chi square Scoring object 79 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 80 | 81 | /// KL divergence Scoring object 82 | class __SCORING_CLASS(KLScoring, true, L1); 83 | 84 | /// Bhattacharyya Scoring object 85 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 86 | 87 | /// Dot product Scoring object 88 | class __SCORING_CLASS(DotProductScoring, false, L1); 89 | 90 | #undef __SCORING_CLASS 91 | 92 | } // namespace DBoW2 93 | 94 | #endif 95 | 96 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DUtils/DException.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DException.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: general exception of the library 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | #ifndef __D_EXCEPTION__ 14 | #define __D_EXCEPTION__ 15 | 16 | #include 17 | #include 18 | using namespace std; 19 | 20 | namespace DUtils { 21 | 22 | /// General exception 23 | class DException : 24 | public exception 25 | { 26 | public: 27 | /** 28 | * Creates an exception with a general error message 29 | */ 30 | DException(void) throw(): m_message("DUtils exception"){} 31 | 32 | /** 33 | * Creates an exception with a custom error message 34 | * @param msg: message 35 | */ 36 | DException(const char *msg) throw(): m_message(msg){} 37 | 38 | /** 39 | * Creates an exception with a custom error message 40 | * @param msg: message 41 | */ 42 | DException(const string &msg) throw(): m_message(msg){} 43 | 44 | /** 45 | * Destructor 46 | */ 47 | virtual ~DException(void) throw(){} 48 | 49 | /** 50 | * Returns the exception message 51 | */ 52 | virtual const char* what() const throw() 53 | { 54 | return m_message.c_str(); 55 | } 56 | 57 | protected: 58 | /// Error message 59 | string m_message; 60 | }; 61 | 62 | } 63 | 64 | #endif 65 | 66 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DUtils/DUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DUtils.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: include file for including all the library functionalities 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DUtils Library 12 | * 13 | * DUtils library for C++: 14 | * Collection of classes with general utilities for C++ applications. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section license License 22 | * This program is free software: you can redistribute it and/or modify 23 | * it under the terms of the GNU Lesser General Public License (LGPL) as 24 | * published by the Free Software Foundation, either version 3 of the License, 25 | * or any later version. 26 | * 27 | */ 28 | 29 | 30 | #pragma once 31 | 32 | #ifndef __D_UTILS__ 33 | #define __D_UTILS__ 34 | 35 | /// Several utilities for C++ programs 36 | namespace DUtils 37 | { 38 | } 39 | 40 | // Exception 41 | #include "DException.h" 42 | 43 | #include "Timestamp.h" 44 | // Random numbers 45 | #include "Random.h" 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DUtils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | namespace DUtils { 19 | 20 | /// Functions to generate pseudo-random numbers 21 | class Random 22 | { 23 | public: 24 | class UnrepeatedRandomizer; 25 | 26 | public: 27 | /** 28 | * Sets the random number seed to the current time 29 | */ 30 | static void SeedRand(); 31 | 32 | /** 33 | * Sets the random number seed to the current time only the first 34 | * time this function is called 35 | */ 36 | static void SeedRandOnce(); 37 | 38 | /** 39 | * Sets the given random number seed 40 | * @param seed 41 | */ 42 | static void SeedRand(int seed); 43 | 44 | /** 45 | * Sets the given random number seed only the first time this function 46 | * is called 47 | * @param seed 48 | */ 49 | static void SeedRandOnce(int seed); 50 | 51 | /** 52 | * Returns a random number in the range [0..1] 53 | * @return random T number in [0..1] 54 | */ 55 | template 56 | static T RandomValue(){ 57 | return (T)rand()/(T)RAND_MAX; 58 | } 59 | 60 | /** 61 | * Returns a random number in the range [min..max] 62 | * @param min 63 | * @param max 64 | * @return random T number in [min..max] 65 | */ 66 | template 67 | static T RandomValue(T min, T max){ 68 | return Random::RandomValue() * (max - min) + min; 69 | } 70 | 71 | /** 72 | * Returns a random int in the range [min..max] 73 | * @param min 74 | * @param max 75 | * @return random int in [min..max] 76 | */ 77 | static int RandomInt(int min, int max); 78 | 79 | /** 80 | * Returns a random number from a gaussian distribution 81 | * @param mean 82 | * @param sigma standard deviation 83 | */ 84 | template 85 | static T RandomGaussianValue(T mean, T sigma) 86 | { 87 | // Box-Muller transformation 88 | T x1, x2, w, y1; 89 | 90 | do { 91 | x1 = (T)2. * RandomValue() - (T)1.; 92 | x2 = (T)2. * RandomValue() - (T)1.; 93 | w = x1 * x1 + x2 * x2; 94 | } while ( w >= (T)1. || w == (T)0. ); 95 | 96 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 97 | y1 = x1 * w; 98 | 99 | return( mean + y1 * sigma ); 100 | } 101 | 102 | private: 103 | 104 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 105 | static bool m_already_seeded; 106 | 107 | }; 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | /// Provides pseudo-random numbers with no repetitions 112 | class Random::UnrepeatedRandomizer 113 | { 114 | public: 115 | 116 | /** 117 | * Creates a randomizer that returns numbers in the range [min, max] 118 | * @param min 119 | * @param max 120 | */ 121 | UnrepeatedRandomizer(int min, int max); 122 | ~UnrepeatedRandomizer(){} 123 | 124 | /** 125 | * Copies a randomizer 126 | * @param rnd 127 | */ 128 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 129 | 130 | /** 131 | * Copies a randomizer 132 | * @param rnd 133 | */ 134 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 135 | 136 | /** 137 | * Returns a random number not given before. If all the possible values 138 | * were already given, the process starts again 139 | * @return unrepeated random number 140 | */ 141 | int get(); 142 | 143 | /** 144 | * Returns whether all the possible values between min and max were 145 | * already given. If get() is called when empty() is true, the behaviour 146 | * is the same than after creating the randomizer 147 | * @return true iff all the values were returned 148 | */ 149 | inline bool empty() const { return m_values.empty(); } 150 | 151 | /** 152 | * Returns the number of values still to be returned 153 | * @return amount of values to return 154 | */ 155 | inline unsigned int left() const { return m_values.size(); } 156 | 157 | /** 158 | * Resets the randomizer as it were just created 159 | */ 160 | void reset(); 161 | 162 | protected: 163 | 164 | /** 165 | * Creates the vector with available values 166 | */ 167 | void createValues(); 168 | 169 | protected: 170 | 171 | /// Min of range of values 172 | int m_min; 173 | /// Max of range of values 174 | int m_max; 175 | 176 | /// Available values 177 | std::vector m_values; 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif 184 | 185 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DUtils/Timestamp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_TIMESTAMP__ 11 | #define __D_TIMESTAMP__ 12 | 13 | #include 14 | using namespace std; 15 | 16 | namespace DUtils { 17 | 18 | /// Timestamp 19 | class Timestamp 20 | { 21 | public: 22 | 23 | /// Options to initiate a timestamp 24 | enum tOptions 25 | { 26 | NONE = 0, 27 | CURRENT_TIME = 0x1, 28 | ZERO = 0x2 29 | }; 30 | 31 | public: 32 | 33 | /** 34 | * Creates a timestamp 35 | * @param option option to set the initial time stamp 36 | */ 37 | Timestamp(Timestamp::tOptions option = NONE); 38 | 39 | /** 40 | * Destructor 41 | */ 42 | virtual ~Timestamp(void); 43 | 44 | /** 45 | * Says if the timestamp is "empty": seconds and usecs are both 0, as 46 | * when initiated with the ZERO flag 47 | * @return true iif secs == usecs == 0 48 | */ 49 | bool empty() const; 50 | 51 | /** 52 | * Sets this instance to the current time 53 | */ 54 | void setToCurrentTime(); 55 | 56 | /** 57 | * Sets the timestamp from seconds and microseconds 58 | * @param secs: seconds 59 | * @param usecs: microseconds 60 | */ 61 | inline void setTime(unsigned long secs, unsigned long usecs){ 62 | m_secs = secs; 63 | m_usecs = usecs; 64 | } 65 | 66 | /** 67 | * Returns the timestamp in seconds and microseconds 68 | * @param secs seconds 69 | * @param usecs microseconds 70 | */ 71 | inline void getTime(unsigned long &secs, unsigned long &usecs) const 72 | { 73 | secs = m_secs; 74 | usecs = m_usecs; 75 | } 76 | 77 | /** 78 | * Sets the timestamp from a string with the time in seconds 79 | * @param stime: string such as "1235603336.036609" 80 | */ 81 | void setTime(const string &stime); 82 | 83 | /** 84 | * Sets the timestamp from a number of seconds from the epoch 85 | * @param s seconds from the epoch 86 | */ 87 | void setTime(double s); 88 | 89 | /** 90 | * Returns this timestamp as the number of seconds in (long) float format 91 | */ 92 | double getFloatTime() const; 93 | 94 | /** 95 | * Returns this timestamp as the number of seconds in fixed length string format 96 | */ 97 | string getStringTime() const; 98 | 99 | /** 100 | * Returns the difference in seconds between this timestamp (greater) and t (smaller) 101 | * If the order is swapped, a negative number is returned 102 | * @param t: timestamp to subtract from this timestamp 103 | * @return difference in seconds 104 | */ 105 | double operator- (const Timestamp &t) const; 106 | 107 | /** 108 | * Returns a copy of this timestamp + s seconds + us microseconds 109 | * @param s seconds 110 | * @param us microseconds 111 | */ 112 | Timestamp plus(unsigned long s, unsigned long us) const; 113 | 114 | /** 115 | * Returns a copy of this timestamp - s seconds - us microseconds 116 | * @param s seconds 117 | * @param us microseconds 118 | */ 119 | Timestamp minus(unsigned long s, unsigned long us) const; 120 | 121 | /** 122 | * Adds s seconds to this timestamp and returns a reference to itself 123 | * @param s seconds 124 | * @return reference to this timestamp 125 | */ 126 | Timestamp& operator+= (double s); 127 | 128 | /** 129 | * Substracts s seconds to this timestamp and returns a reference to itself 130 | * @param s seconds 131 | * @return reference to this timestamp 132 | */ 133 | Timestamp& operator-= (double s); 134 | 135 | /** 136 | * Returns a copy of this timestamp + s seconds 137 | * @param s: seconds 138 | */ 139 | Timestamp operator+ (double s) const; 140 | 141 | /** 142 | * Returns a copy of this timestamp - s seconds 143 | * @param s: seconds 144 | */ 145 | Timestamp operator- (double s) const; 146 | 147 | /** 148 | * Returns whether this timestamp is at the future of t 149 | * @param t 150 | */ 151 | bool operator> (const Timestamp &t) const; 152 | 153 | /** 154 | * Returns whether this timestamp is at the future of (or is the same as) t 155 | * @param t 156 | */ 157 | bool operator>= (const Timestamp &t) const; 158 | 159 | /** 160 | * Returns whether this timestamp and t represent the same instant 161 | * @param t 162 | */ 163 | bool operator== (const Timestamp &t) const; 164 | 165 | /** 166 | * Returns whether this timestamp is at the past of t 167 | * @param t 168 | */ 169 | bool operator< (const Timestamp &t) const; 170 | 171 | /** 172 | * Returns whether this timestamp is at the past of (or is the same as) t 173 | * @param t 174 | */ 175 | bool operator<= (const Timestamp &t) const; 176 | 177 | /** 178 | * Returns the timestamp in a human-readable string 179 | * @param machine_friendly if true, the returned string is formatted 180 | * to yyyymmdd_hhmmss, without weekday or spaces 181 | * @note This has not been tested under Windows 182 | * @note The timestamp is truncated to seconds 183 | */ 184 | string Format(bool machine_friendly = false) const; 185 | 186 | /** 187 | * Returns a string version of the elapsed time in seconds, with the format 188 | * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us 189 | * @param s: elapsed seconds (given by getFloatTime) to format 190 | */ 191 | static string Format(double s); 192 | 193 | 194 | protected: 195 | /// Seconds 196 | unsigned long m_secs; // seconds 197 | /// Microseconds 198 | unsigned long m_usecs; // microseconds 199 | }; 200 | 201 | } 202 | 203 | #endif 204 | 205 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DVision/BRIEF.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BRIEF.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2011 5 | * Description: implementation of BRIEF (Binary Robust Independent 6 | * Elementary Features) descriptor by 7 | * Michael Calonder, Vincent Lepetit and Pascal Fua 8 | * + close binary tests (by Dorian Galvez-Lopez) 9 | * 10 | * If you use this code with the RANDOM_CLOSE descriptor version, please cite: 11 | @INPROCEEDINGS{GalvezIROS11, 12 | author={Galvez-Lopez, Dorian and Tardos, Juan D.}, 13 | booktitle={Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on}, 14 | title={Real-time loop detection with bags of binary words}, 15 | year={2011}, 16 | month={sept.}, 17 | volume={}, 18 | number={}, 19 | pages={51 -58}, 20 | keywords={}, 21 | doi={10.1109/IROS.2011.6094885}, 22 | ISSN={2153-0858} 23 | } 24 | * 25 | * License: see the LICENSE.txt file 26 | * 27 | */ 28 | 29 | #ifndef __D_BRIEF__ 30 | #define __D_BRIEF__ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace DVision { 37 | 38 | /// BRIEF descriptor 39 | class BRIEF 40 | { 41 | public: 42 | 43 | /// Bitset type 44 | typedef boost::dynamic_bitset<> bitset; 45 | 46 | /// Type of pairs 47 | enum Type 48 | { 49 | RANDOM, // random pairs (Calonder's original version) 50 | RANDOM_CLOSE, // random but close pairs (used in GalvezIROS11) 51 | }; 52 | 53 | public: 54 | 55 | /** 56 | * Creates the BRIEF a priori data for descriptors of nbits length 57 | * @param nbits descriptor length in bits 58 | * @param patch_size 59 | * @param type type of pairs to generate 60 | */ 61 | BRIEF(int nbits = 256, int patch_size = 48, Type type = RANDOM_CLOSE); 62 | virtual ~BRIEF(); 63 | 64 | /** 65 | * Returns the descriptor length in bits 66 | * @return descriptor length in bits 67 | */ 68 | inline int getDescriptorLengthInBits() const 69 | { 70 | return m_bit_length; 71 | } 72 | 73 | /** 74 | * Returns the type of classifier 75 | */ 76 | inline Type getType() const 77 | { 78 | return m_type; 79 | } 80 | 81 | /** 82 | * Returns the size of the patch 83 | */ 84 | inline int getPatchSize() const 85 | { 86 | return m_patch_size; 87 | } 88 | 89 | /** 90 | * Returns the BRIEF descriptors of the given keypoints in the given image 91 | * @param image 92 | * @param points 93 | * @param descriptors 94 | * @param treat_image (default: true) if true, the image is converted to 95 | * grayscale if needed and smoothed. If not, it is assumed the image has 96 | * been treated by the user 97 | * @note this function is similar to BRIEF::compute 98 | */ 99 | inline void operator() (const cv::Mat &image, 100 | const std::vector &points, 101 | std::vector &descriptors, 102 | bool treat_image = true) const 103 | { 104 | compute(image, points, descriptors, treat_image); 105 | } 106 | 107 | /** 108 | * Returns the BRIEF descriptors of the given keypoints in the given image 109 | * @param image 110 | * @param points 111 | * @param descriptors 112 | * @param treat_image (default: true) if true, the image is converted to 113 | * grayscale if needed and smoothed. If not, it is assumed the image has 114 | * been treated by the user 115 | * @note this function is similar to BRIEF::operator() 116 | */ 117 | void compute(const cv::Mat &image, 118 | const std::vector &points, 119 | std::vector &descriptors, 120 | bool treat_image = true) const; 121 | 122 | /** 123 | * Exports the test pattern 124 | * @param x1 x1 coordinates of pairs 125 | * @param y1 y1 coordinates of pairs 126 | * @param x2 x2 coordinates of pairs 127 | * @param y2 y2 coordinates of pairs 128 | */ 129 | inline void exportPairs(std::vector &x1, std::vector &y1, 130 | std::vector &x2, std::vector &y2) const 131 | { 132 | x1 = m_x1; 133 | y1 = m_y1; 134 | x2 = m_x2; 135 | y2 = m_y2; 136 | } 137 | 138 | /** 139 | * Sets the test pattern 140 | * @param x1 x1 coordinates of pairs 141 | * @param y1 y1 coordinates of pairs 142 | * @param x2 x2 coordinates of pairs 143 | * @param y2 y2 coordinates of pairs 144 | */ 145 | inline void importPairs(const std::vector &x1, 146 | const std::vector &y1, const std::vector &x2, 147 | const std::vector &y2) 148 | { 149 | m_x1 = x1; 150 | m_y1 = y1; 151 | m_x2 = x2; 152 | m_y2 = y2; 153 | m_bit_length = x1.size(); 154 | } 155 | 156 | /** 157 | * Returns the Hamming distance between two descriptors 158 | * @param a first descriptor vector 159 | * @param b second descriptor vector 160 | * @return hamming distance 161 | */ 162 | inline static int distance(const bitset &a, const bitset &b) 163 | { 164 | return (a^b).count(); 165 | } 166 | 167 | protected: 168 | 169 | /** 170 | * Generates random points in the patch coordinates, according to 171 | * m_patch_size and m_bit_length 172 | */ 173 | void generateTestPoints(); 174 | 175 | protected: 176 | 177 | /// Descriptor length in bits 178 | int m_bit_length; 179 | 180 | /// Patch size 181 | int m_patch_size; 182 | 183 | /// Type of pairs 184 | Type m_type; 185 | 186 | /// Coordinates of test points relative to the center of the patch 187 | std::vector m_x1, m_x2; 188 | std::vector m_y1, m_y2; 189 | 190 | }; 191 | 192 | } // namespace DVision 193 | 194 | #endif 195 | 196 | 197 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/DVision/DVision.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DVision.h 3 | * Project: DVision library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 4, 2010 6 | * Description: several functions for computer vision 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DVision Library 12 | * 13 | * DVision library for C++ and OpenCV: 14 | * Collection of classes with computer vision functionality 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils and DUtilsCV libraries and the OpenCV library. 23 | * 24 | * \section license License 25 | * This program is free software: you can redistribute it and/or modify 26 | * it under the terms of the GNU Lesser General Public License (LGPL) as 27 | * published by the Free Software Foundation, either version 3 of the License, 28 | * or any later version. 29 | * 30 | */ 31 | 32 | #ifndef __D_VISION__ 33 | #define __D_VISION__ 34 | 35 | 36 | /// Computer vision functions 37 | namespace DVision 38 | { 39 | } 40 | 41 | #include "BRIEF.h" 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /pose_graph/include/ThirdParty/VocabularyBinary.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VocabularyBinary_hpp 2 | #define VocabularyBinary_hpp 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace VINSLoop { 9 | 10 | struct Node { 11 | int32_t nodeId; 12 | int32_t parentId; 13 | double weight; 14 | uint64_t descriptor[4]; 15 | }; 16 | 17 | struct Word { 18 | int32_t nodeId; 19 | int32_t wordId; 20 | }; 21 | 22 | struct Vocabulary { 23 | int32_t k; 24 | int32_t L; 25 | int32_t scoringType; 26 | int32_t weightingType; 27 | 28 | int32_t nNodes; 29 | int32_t nWords; 30 | 31 | Node* nodes; 32 | Word* words; 33 | 34 | Vocabulary(); 35 | ~Vocabulary(); 36 | 37 | void serialize(std::ofstream& stream); 38 | void deserialize(std::ifstream& stream); 39 | 40 | inline static size_t staticDataSize() { 41 | return sizeof(Vocabulary) - sizeof(Node*) - sizeof(Word*); 42 | } 43 | }; 44 | 45 | } 46 | 47 | #endif /* VocabularyBinary_hpp */ 48 | -------------------------------------------------------------------------------- /pose_graph/include/keyframe.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "camodocal/camera_models/CameraFactory.h" 8 | #include "camodocal/camera_models/CataCamera.h" 9 | #include "camodocal/camera_models/PinholeCamera.h" 10 | #include "utility/tic_toc.h" 11 | #include "utility/utility.h" 12 | #include "parameters.h" 13 | #include "ThirdParty/DBoW/DBoW2.h" 14 | #include "ThirdParty/DVision/DVision.h" 15 | 16 | #define MIN_LOOP_NUM 25 17 | 18 | using namespace Eigen; 19 | using namespace std; 20 | using namespace DVision; 21 | 22 | 23 | class BriefExtractor 24 | { 25 | public: 26 | virtual void operator()(const cv::Mat &im, vector &keys, vector &descriptors) const; 27 | BriefExtractor(const std::string &pattern_file); 28 | 29 | DVision::BRIEF m_brief; 30 | }; 31 | 32 | class KeyFrame 33 | { 34 | public: 35 | KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image, 36 | vector &_point_3d, vector &_point_2d_uv, vector &_point_2d_normal, 37 | vector &_point_id, int _sequence); 38 | KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i, 39 | cv::Mat &_image, int _loop_index, Eigen::Matrix &_loop_info, 40 | vector &_keypoints, vector &_keypoints_norm, vector &_brief_descriptors); 41 | bool findConnection(KeyFrame* old_kf); 42 | void computeWindowBRIEFPoint(); 43 | void computeBRIEFPoint(); 44 | //void extractBrief(); 45 | int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b); 46 | bool searchInAera(const BRIEF::bitset window_descriptor, 47 | const std::vector &descriptors_old, 48 | const std::vector &keypoints_old, 49 | const std::vector &keypoints_old_norm, 50 | cv::Point2f &best_match, 51 | cv::Point2f &best_match_norm); 52 | void searchByBRIEFDes(std::vector &matched_2d_old, 53 | std::vector &matched_2d_old_norm, 54 | std::vector &status, 55 | const std::vector &descriptors_old, 56 | const std::vector &keypoints_old, 57 | const std::vector &keypoints_old_norm); 58 | void FundmantalMatrixRANSAC(const std::vector &matched_2d_cur_norm, 59 | const std::vector &matched_2d_old_norm, 60 | vector &status); 61 | void PnPRANSAC(const vector &matched_2d_old_norm, 62 | const std::vector &matched_3d, 63 | std::vector &status, 64 | Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old); 65 | void getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); 66 | void getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); 67 | void updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); 68 | void updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); 69 | void updateLoop(Eigen::Matrix &_loop_info); 70 | 71 | Eigen::Vector3d getLoopRelativeT(); 72 | double getLoopRelativeYaw(); 73 | Eigen::Quaterniond getLoopRelativeQ(); 74 | 75 | 76 | 77 | double time_stamp; 78 | int index; 79 | int local_index; 80 | Eigen::Vector3d vio_T_w_i; 81 | Eigen::Matrix3d vio_R_w_i; 82 | Eigen::Vector3d T_w_i; 83 | Eigen::Matrix3d R_w_i; 84 | Eigen::Vector3d origin_vio_T; 85 | Eigen::Matrix3d origin_vio_R; 86 | cv::Mat image; 87 | cv::Mat thumbnail; 88 | vector point_3d; 89 | vector point_2d_uv; 90 | vector point_2d_norm; 91 | vector point_id; 92 | vector keypoints; 93 | vector keypoints_norm; 94 | vector window_keypoints; 95 | vector brief_descriptors; 96 | vector window_brief_descriptors; 97 | bool has_fast_point; 98 | int sequence; 99 | 100 | bool has_loop; 101 | int loop_index; 102 | Eigen::Matrix loop_info; 103 | }; 104 | 105 | -------------------------------------------------------------------------------- /pose_graph/include/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "camodocal/camera_models/CameraFactory.h" 4 | #include "camodocal/camera_models/CataCamera.h" 5 | #include "camodocal/camera_models/PinholeCamera.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | extern camodocal::CameraPtr m_camera; 14 | extern Eigen::Vector3d tic; 15 | extern Eigen::Matrix3d qic; 16 | extern ros::Publisher pub_match_img; 17 | extern ros::Publisher pub_match_points; 18 | extern int VISUALIZATION_SHIFT_X; 19 | extern int VISUALIZATION_SHIFT_Y; 20 | extern std::string BRIEF_PATTERN_FILE; 21 | extern std::string POSE_GRAPH_SAVE_PATH; 22 | extern int ROW; 23 | extern int COL; 24 | extern std::string VINS_RESULT_PATH; 25 | extern int DEBUG_IMAGE; 26 | extern int FAST_RELOCALIZATION; 27 | 28 | 29 | -------------------------------------------------------------------------------- /pose_graph/include/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "../parameters.h" 11 | 12 | class CameraPoseVisualization { 13 | public: 14 | std::string m_marker_ns; 15 | 16 | CameraPoseVisualization(float r, float g, float b, float a); 17 | 18 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 19 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 20 | void setScale(double s); 21 | void setLineWidth(double width); 22 | 23 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 24 | void reset(); 25 | 26 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 27 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 28 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 29 | //void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src); 30 | void publish_image_by( ros::Publisher &pub, const std_msgs::Header &header); 31 | private: 32 | std::vector m_markers; 33 | std_msgs::ColorRGBA m_image_boundary_color; 34 | std_msgs::ColorRGBA m_optical_center_connector_color; 35 | double m_scale; 36 | double m_line_width; 37 | visualization_msgs::Marker image; 38 | int LOOP_EDGE_NUM; 39 | int tmp_loop_edge_num; 40 | 41 | static const Eigen::Vector3d imlt; 42 | static const Eigen::Vector3d imlb; 43 | static const Eigen::Vector3d imrt; 44 | static const Eigen::Vector3d imrb; 45 | static const Eigen::Vector3d oc ; 46 | static const Eigen::Vector3d lt0 ; 47 | static const Eigen::Vector3d lt1 ; 48 | static const Eigen::Vector3d lt2 ; 49 | }; 50 | -------------------------------------------------------------------------------- /pose_graph/include/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 | -------------------------------------------------------------------------------- /pose_graph/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pose_graph 4 | 0.0.0 5 | pose_graph 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 | camera_model 44 | camera_model 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "../../../include/ThirdParty/DBoW/BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/FBrief.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "../../../include/ThirdParty/DBoW/FBrief.h" 15 | 16 | using namespace std; 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | void FBrief::meanValue(const std::vector &descriptors, 23 | FBrief::TDescriptor &mean) 24 | { 25 | mean.reset(); 26 | 27 | if(descriptors.empty()) return; 28 | 29 | const int N2 = descriptors.size() / 2; 30 | const int L = descriptors[0]->size(); 31 | 32 | vector counters(L, 0); 33 | 34 | vector::const_iterator it; 35 | for(it = descriptors.begin(); it != descriptors.end(); ++it) 36 | { 37 | const FBrief::TDescriptor &desc = **it; 38 | for(int i = 0; i < L; ++i) 39 | { 40 | if(desc[i]) counters[i]++; 41 | } 42 | } 43 | 44 | for(int i = 0; i < L; ++i) 45 | { 46 | if(counters[i] > N2) mean.set(i); 47 | } 48 | 49 | } 50 | 51 | // -------------------------------------------------------------------------- 52 | 53 | double FBrief::distance(const FBrief::TDescriptor &a, 54 | const FBrief::TDescriptor &b) 55 | { 56 | return (double)DVision::BRIEF::distance(a, b); 57 | } 58 | 59 | // -------------------------------------------------------------------------- 60 | 61 | std::string FBrief::toString(const FBrief::TDescriptor &a) 62 | { 63 | // from boost::bitset 64 | string s; 65 | to_string(a, s); // reversed 66 | return s; 67 | } 68 | 69 | // -------------------------------------------------------------------------- 70 | 71 | void FBrief::fromString(FBrief::TDescriptor &a, const std::string &s) 72 | { 73 | // from boost::bitset 74 | stringstream ss(s); 75 | ss >> a; 76 | } 77 | 78 | // -------------------------------------------------------------------------- 79 | 80 | void FBrief::toMat32F(const std::vector &descriptors, 81 | cv::Mat &mat) 82 | { 83 | if(descriptors.empty()) 84 | { 85 | mat.release(); 86 | return; 87 | } 88 | 89 | const int N = descriptors.size(); 90 | const int L = descriptors[0].size(); 91 | 92 | mat.create(N, L, CV_32F); 93 | 94 | for(int i = 0; i < N; ++i) 95 | { 96 | const TDescriptor& desc = descriptors[i]; 97 | float *p = mat.ptr(i); 98 | for(int j = 0; j < L; ++j, ++p) 99 | { 100 | *p = (desc[j] ? 1 : 0); 101 | } 102 | } 103 | } 104 | 105 | // -------------------------------------------------------------------------- 106 | 107 | } // namespace DBoW2 108 | 109 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "../../../include/ThirdParty/DBoW/FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/QueryResults.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.cpp 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include "../../../include/ThirdParty/DBoW/QueryResults.h" 13 | 14 | using namespace std; 15 | 16 | namespace DBoW2 17 | { 18 | 19 | // --------------------------------------------------------------------------- 20 | 21 | ostream & operator<<(ostream& os, const Result& ret ) 22 | { 23 | os << ""; 24 | return os; 25 | } 26 | 27 | // --------------------------------------------------------------------------- 28 | 29 | ostream & operator<<(ostream& os, const QueryResults& ret ) 30 | { 31 | if(ret.size() == 1) 32 | os << "1 result:" << endl; 33 | else 34 | os << ret.size() << " results:" << endl; 35 | 36 | QueryResults::const_iterator rit; 37 | for(rit = ret.begin(); rit != ret.end(); ++rit) 38 | { 39 | os << *rit; 40 | if(rit + 1 != ret.end()) os << endl; 41 | } 42 | return os; 43 | } 44 | 45 | // --------------------------------------------------------------------------- 46 | 47 | void QueryResults::saveM(const std::string &filename) const 48 | { 49 | fstream f(filename.c_str(), ios::out); 50 | 51 | QueryResults::const_iterator qit; 52 | for(qit = begin(); qit != end(); ++qit) 53 | { 54 | f << qit->Id << " " << qit->Score << endl; 55 | } 56 | 57 | f.close(); 58 | } 59 | 60 | // --------------------------------------------------------------------------- 61 | 62 | } // namespace DBoW2 63 | 64 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "../../../include/ThirdParty/DUtils/Random.h" 12 | #include "../../../include/ThirdParty/DUtils/Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DUtils/Timestamp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.cpp 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * 7 | * Note: in windows, this class has a 1ms resolution 8 | * 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _WIN32 21 | #ifndef WIN32 22 | #define WIN32 23 | #endif 24 | #endif 25 | 26 | #ifdef WIN32 27 | #include 28 | #define sprintf sprintf_s 29 | #else 30 | #include 31 | #endif 32 | 33 | #include "../../../include/ThirdParty/DUtils/Timestamp.h" 34 | 35 | using namespace std; 36 | 37 | using namespace DUtils; 38 | 39 | Timestamp::Timestamp(Timestamp::tOptions option) 40 | { 41 | if(option & CURRENT_TIME) 42 | setToCurrentTime(); 43 | else if(option & ZERO) 44 | setTime(0.); 45 | } 46 | 47 | Timestamp::~Timestamp(void) 48 | { 49 | } 50 | 51 | bool Timestamp::empty() const 52 | { 53 | return m_secs == 0 && m_usecs == 0; 54 | } 55 | 56 | void Timestamp::setToCurrentTime(){ 57 | 58 | #ifdef WIN32 59 | struct __timeb32 timebuffer; 60 | _ftime32_s( &timebuffer ); // C4996 61 | // Note: _ftime is deprecated; consider using _ftime_s instead 62 | m_secs = timebuffer.time; 63 | m_usecs = timebuffer.millitm * 1000; 64 | #else 65 | struct timeval now; 66 | gettimeofday(&now, NULL); 67 | m_secs = now.tv_sec; 68 | m_usecs = now.tv_usec; 69 | #endif 70 | 71 | } 72 | 73 | void Timestamp::setTime(const string &stime){ 74 | string::size_type p = stime.find('.'); 75 | if(p == string::npos){ 76 | m_secs = atol(stime.c_str()); 77 | m_usecs = 0; 78 | }else{ 79 | m_secs = atol(stime.substr(0, p).c_str()); 80 | 81 | string s_usecs = stime.substr(p+1, 6); 82 | m_usecs = atol(stime.substr(p+1).c_str()); 83 | m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); 84 | } 85 | } 86 | 87 | void Timestamp::setTime(double s) 88 | { 89 | m_secs = (unsigned long)s; 90 | m_usecs = (s - (double)m_secs) * 1e6; 91 | } 92 | 93 | double Timestamp::getFloatTime() const { 94 | return double(m_secs) + double(m_usecs)/1000000.0; 95 | } 96 | 97 | string Timestamp::getStringTime() const { 98 | char buf[32]; 99 | sprintf(buf, "%.6lf", this->getFloatTime()); 100 | return string(buf); 101 | } 102 | 103 | double Timestamp::operator- (const Timestamp &t) const { 104 | return this->getFloatTime() - t.getFloatTime(); 105 | } 106 | 107 | Timestamp& Timestamp::operator+= (double s) 108 | { 109 | *this = *this + s; 110 | return *this; 111 | } 112 | 113 | Timestamp& Timestamp::operator-= (double s) 114 | { 115 | *this = *this - s; 116 | return *this; 117 | } 118 | 119 | Timestamp Timestamp::operator+ (double s) const 120 | { 121 | unsigned long secs = (long)floor(s); 122 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 123 | 124 | return this->plus(secs, usecs); 125 | } 126 | 127 | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const 128 | { 129 | Timestamp t; 130 | 131 | const unsigned long max = 1000000ul; 132 | 133 | if(m_usecs + usecs >= max) 134 | t.setTime(m_secs + secs + 1, m_usecs + usecs - max); 135 | else 136 | t.setTime(m_secs + secs, m_usecs + usecs); 137 | 138 | return t; 139 | } 140 | 141 | Timestamp Timestamp::operator- (double s) const 142 | { 143 | unsigned long secs = (long)floor(s); 144 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 145 | 146 | return this->minus(secs, usecs); 147 | } 148 | 149 | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const 150 | { 151 | Timestamp t; 152 | 153 | const unsigned long max = 1000000ul; 154 | 155 | if(m_usecs < usecs) 156 | t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); 157 | else 158 | t.setTime(m_secs - secs, m_usecs - usecs); 159 | 160 | return t; 161 | } 162 | 163 | bool Timestamp::operator> (const Timestamp &t) const 164 | { 165 | if(m_secs > t.m_secs) return true; 166 | else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; 167 | else return false; 168 | } 169 | 170 | bool Timestamp::operator>= (const Timestamp &t) const 171 | { 172 | if(m_secs > t.m_secs) return true; 173 | else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; 174 | else return false; 175 | } 176 | 177 | bool Timestamp::operator< (const Timestamp &t) const 178 | { 179 | if(m_secs < t.m_secs) return true; 180 | else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; 181 | else return false; 182 | } 183 | 184 | bool Timestamp::operator<= (const Timestamp &t) const 185 | { 186 | if(m_secs < t.m_secs) return true; 187 | else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; 188 | else return false; 189 | } 190 | 191 | bool Timestamp::operator== (const Timestamp &t) const 192 | { 193 | return(m_secs == t.m_secs && m_usecs == t.m_usecs); 194 | } 195 | 196 | 197 | string Timestamp::Format(bool machine_friendly) const 198 | { 199 | struct tm tm_time; 200 | 201 | time_t t = (time_t)getFloatTime(); 202 | 203 | #ifdef WIN32 204 | localtime_s(&tm_time, &t); 205 | #else 206 | localtime_r(&t, &tm_time); 207 | #endif 208 | 209 | char buffer[128]; 210 | 211 | if(machine_friendly) 212 | { 213 | strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); 214 | } 215 | else 216 | { 217 | strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 218 | } 219 | 220 | return string(buffer); 221 | } 222 | 223 | string Timestamp::Format(double s) { 224 | int days = int(s / (24. * 3600.0)); 225 | s -= days * (24. * 3600.0); 226 | int hours = int(s / 3600.0); 227 | s -= hours * 3600; 228 | int minutes = int(s / 60.0); 229 | s -= minutes * 60; 230 | int seconds = int(s); 231 | int ms = int((s - seconds)*1e6); 232 | 233 | stringstream ss; 234 | ss.fill('0'); 235 | bool b; 236 | if((b = (days > 0))) ss << days << "d "; 237 | if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; 238 | if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; 239 | if(b) ss << setw(2); 240 | ss << seconds; 241 | if(!b) ss << "." << setw(6) << ms; 242 | 243 | return ss.str(); 244 | } 245 | 246 | 247 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DVision/BRIEF.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BRIEF.cpp 3 | * Author: Dorian Galvez 4 | * Date: September 2010 5 | * Description: implementation of BRIEF (Binary Robust Independent 6 | * Elementary Features) descriptor by 7 | * Michael Calonder, Vincent Lepetitand Pascal Fua 8 | * + close binary tests (by Dorian Galvez-Lopez) 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include "../../../include/ThirdParty/DVision/BRIEF.h" 14 | #include "../../../include/ThirdParty/DUtils/DUtils.h" 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | using namespace DVision; 20 | 21 | // ---------------------------------------------------------------------------- 22 | 23 | BRIEF::BRIEF(int nbits, int patch_size, Type type): 24 | m_bit_length(nbits), m_patch_size(patch_size), m_type(type) 25 | { 26 | assert(patch_size > 1); 27 | assert(nbits > 0); 28 | generateTestPoints(); 29 | } 30 | 31 | // ---------------------------------------------------------------------------- 32 | 33 | BRIEF::~BRIEF() 34 | { 35 | } 36 | 37 | // --------------------------------------------------------------------------- 38 | 39 | void BRIEF::compute(const cv::Mat &image, 40 | const std::vector &points, 41 | vector &descriptors, 42 | bool treat_image) const 43 | { 44 | const float sigma = 2.f; 45 | const cv::Size ksize(9, 9); 46 | 47 | cv::Mat im; 48 | if(treat_image) 49 | { 50 | cv::Mat aux; 51 | if(image.depth() == 3) 52 | { 53 | cv::cvtColor(image, aux, CV_RGB2GRAY); 54 | } 55 | else 56 | { 57 | aux = image; 58 | } 59 | 60 | cv::GaussianBlur(aux, im, ksize, sigma, sigma); 61 | 62 | } 63 | else 64 | { 65 | im = image; 66 | } 67 | 68 | assert(im.type() == CV_8UC1); 69 | assert(im.isContinuous()); 70 | 71 | // use im now 72 | const int W = im.cols; 73 | const int H = im.rows; 74 | 75 | descriptors.resize(points.size()); 76 | std::vector::iterator dit; 77 | 78 | std::vector::const_iterator kit; 79 | 80 | int x1, y1, x2, y2; 81 | 82 | dit = descriptors.begin(); 83 | for(kit = points.begin(); kit != points.end(); ++kit, ++dit) 84 | { 85 | dit->resize(m_bit_length); 86 | dit->reset(); 87 | 88 | for(unsigned int i = 0; i < m_x1.size(); ++i) 89 | { 90 | x1 = (int)(kit->pt.x + m_x1[i]); 91 | y1 = (int)(kit->pt.y + m_y1[i]); 92 | x2 = (int)(kit->pt.x + m_x2[i]); 93 | y2 = (int)(kit->pt.y + m_y2[i]); 94 | 95 | if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H 96 | && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H) 97 | { 98 | if( im.ptr(y1)[x1] < im.ptr(y2)[x2] ) 99 | { 100 | dit->set(i); 101 | } 102 | } // if (x,y)_1 and (x,y)_2 are in the image 103 | 104 | } // for each (x,y) 105 | } // for each keypoint 106 | } 107 | 108 | // --------------------------------------------------------------------------- 109 | 110 | void BRIEF::generateTestPoints() 111 | { 112 | m_x1.resize(m_bit_length); 113 | m_y1.resize(m_bit_length); 114 | m_x2.resize(m_bit_length); 115 | m_y2.resize(m_bit_length); 116 | 117 | const float g_mean = 0.f; 118 | const float g_sigma = 0.2f * (float)m_patch_size; 119 | const float c_sigma = 0.08f * (float)m_patch_size; 120 | 121 | float sigma2; 122 | if(m_type == RANDOM) 123 | sigma2 = g_sigma; 124 | else 125 | sigma2 = c_sigma; 126 | 127 | const int max_v = m_patch_size / 2; 128 | 129 | DUtils::Random::SeedRandOnce(); 130 | 131 | for(int i = 0; i < m_bit_length; ++i) 132 | { 133 | int x1, y1, x2, y2; 134 | 135 | do 136 | { 137 | x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); 138 | } while( x1 > max_v || x1 < -max_v); 139 | 140 | do 141 | { 142 | y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); 143 | } while( y1 > max_v || y1 < -max_v); 144 | 145 | float meanx, meany; 146 | if(m_type == RANDOM) 147 | meanx = meany = g_mean; 148 | else 149 | { 150 | meanx = x1; 151 | meany = y1; 152 | } 153 | 154 | do 155 | { 156 | x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2); 157 | } while( x2 > max_v || x2 < -max_v); 158 | 159 | do 160 | { 161 | y2 = DUtils::Random::RandomGaussianValue(meany, sigma2); 162 | } while( y2 > max_v || y2 < -max_v); 163 | 164 | m_x1[i] = x1; 165 | m_y1[i] = y1; 166 | m_x2[i] = x2; 167 | m_y2[i] = y2; 168 | } 169 | 170 | } 171 | 172 | // ---------------------------------------------------------------------------- 173 | 174 | 175 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/VocabularyBinary.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/ThirdParty/VocabularyBinary.hpp" 2 | #include 3 | using namespace std; 4 | 5 | VINSLoop::Vocabulary::Vocabulary() 6 | : nNodes(0), nodes(nullptr), nWords(0), words(nullptr) { 7 | } 8 | 9 | VINSLoop::Vocabulary::~Vocabulary() { 10 | if (nodes != nullptr) { 11 | delete [] nodes; 12 | nodes = nullptr; 13 | } 14 | 15 | if (words != nullptr) { 16 | delete [] words; 17 | words = nullptr; 18 | } 19 | } 20 | 21 | void VINSLoop::Vocabulary::serialize(ofstream& stream) { 22 | stream.write((const char *)this, staticDataSize()); 23 | stream.write((const char *)nodes, sizeof(Node) * nNodes); 24 | stream.write((const char *)words, sizeof(Word) * nWords); 25 | } 26 | 27 | void VINSLoop::Vocabulary::deserialize(ifstream& stream) { 28 | stream.read((char *)this, staticDataSize()); 29 | 30 | nodes = new Node[nNodes]; 31 | stream.read((char *)nodes, sizeof(Node) * nNodes); 32 | 33 | words = new Word[nWords]; 34 | stream.read((char *)words, sizeof(Word) * nWords); 35 | } 36 | -------------------------------------------------------------------------------- /pose_graph/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/utility/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 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 12 | return R0; 13 | } 14 | -------------------------------------------------------------------------------- /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++11") 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/backend.cpp 40 | src/initial.cpp 41 | src/feature_manager.cpp 42 | src/factor/pose_local_parameterization.cpp 43 | src/factor/projection_factor.cpp 44 | src/factor/projection_td_factor.cpp 45 | src/factor/marginalization_factor.cpp 46 | src/utility/utility.cpp 47 | src/utility/visualization.cpp 48 | src/utility/CameraPoseVisualization.cpp 49 | src/initial/solve_5pts.cpp 50 | src/initial/initial_aligment.cpp 51 | src/initial/initial_sfm.cpp 52 | src/initial/initial_ex_rotation.cpp 53 | ) 54 | 55 | 56 | target_link_libraries(vins_estimator ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 57 | 58 | 59 | -------------------------------------------------------------------------------- /vins_estimator/include/backend.h: -------------------------------------------------------------------------------- 1 | #ifndef _BACKEND_H_ 2 | #define _BACKEND_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "parameters.h" 13 | #include "utility/tic_toc.h" 14 | #include "factor/imu_factor.h" 15 | #include "factor/pose_local_parameterization.h" 16 | #include "factor/projection_factor.h" 17 | #include "factor/projection_td_factor.h" 18 | #include "factor/marginalization_factor.h" 19 | #include "factor/integration_base.h" 20 | 21 | using namespace std; 22 | //#include "estimator.h" 23 | 24 | class Estimator; 25 | 26 | class Backend 27 | { 28 | public: 29 | Backend(); 30 | 31 | void clearState(); 32 | 33 | /** 34 | * @brief backend 35 | */ 36 | void backend(Estimator *estimator); 37 | 38 | /** 39 | * @brief main pocess of backend 40 | */ 41 | void solveOdometry(Estimator *estimator); 42 | 43 | /** 44 | * @brief main pocess of solveOdometry 45 | */ 46 | void optimization(Estimator *estimator); 47 | 48 | /** 49 | * @brief nonLinear Optimization 50 | * @param Input ceres::Problem &problem,ceres::LossFunction *loss_function 51 | */ 52 | void nonLinearOptimization(Estimator *estimator,ceres::Problem &problem,ceres::LossFunction *loss_function); 53 | 54 | /** 55 | * @brief change status values from vector to double in favor of ceres 56 | */ 57 | void vector2double(Estimator *estimator); 58 | 59 | /** 60 | * @brief recover status values from double to vector 61 | */ 62 | void double2vector(Estimator *estimator); 63 | 64 | /** 65 | * @brief detect failure 66 | */ 67 | bool failureDetection(Estimator *estimator); 68 | 69 | /** 70 | * @brief marginizate old frames for big Hessian matrix 71 | */ 72 | void margOld(Estimator *estimator,ceres::LossFunction *loss_function); 73 | 74 | /** 75 | * @brief marginizate new frames for big Hessian matrix 76 | */ 77 | void margNew(Estimator *estimator); 78 | 79 | 80 | /** 81 | * @brief main process of sliding window for status valuee 82 | */ 83 | void slideWindow(Estimator *estimator); 84 | 85 | /** 86 | * @brief marginizate new frames 87 | */ 88 | void slideWindowNew(Estimator *estimator); 89 | 90 | /** 91 | * @brief marginizate old frames 92 | */ 93 | void slideWindowOld(Estimator *estimator); 94 | 95 | public: 96 | Estimator *estimator; 97 | // @param temp values for ceres optimization 98 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 99 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 100 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 101 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 102 | double para_Retrive_Pose[SIZE_POSE]; 103 | double para_Td[1][1]; 104 | double para_Tr[1][1]; //TODO delete 105 | 106 | MarginalizationInfo *last_marginalization_info; 107 | vector last_marginalization_parameter_blocks; 108 | 109 | enum SolverFlag 110 | { 111 | INITIAL, 112 | NON_LINEAR 113 | }; 114 | 115 | // @param status flags 116 | enum MarginalizationFlag 117 | { 118 | MARGIN_OLD = 0, 119 | MARGIN_SECOND_NEW = 1 120 | }; 121 | }; 122 | 123 | #endif -------------------------------------------------------------------------------- /vins_estimator/include/estimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESTIMATOR_H_ 2 | #define _ESTIMATOR_H_ 3 | 4 | //#include "parameters.h" 5 | #include "feature_manager.h" 6 | //#include "utility/utility.h" 7 | //#include "utility/tic_toc.h" 8 | //#include "initial/solve_5pts.h" 9 | //#include "initial/initial_sfm.h" 10 | //#include "initial/initial_alignment.h" 11 | //#include "initial/initial_ex_rotation.h" 12 | #include 13 | #include 14 | 15 | #include "backend.h" 16 | #include "initial.h" 17 | 18 | //#include 19 | //#include 20 | //#include 21 | 22 | class Backend; 23 | 24 | class Estimator 25 | { 26 | public: 27 | Estimator(); 28 | /** 29 | * @brief set parameters from yaml 30 | */ 31 | void setParameter(); 32 | 33 | 34 | // ********************interface******************** 35 | /** 36 | * @brief pre-integrate IMU 37 | * @param Input double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity 38 | */ 39 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 40 | 41 | /** 42 | * @brief main process of vio 43 | * @param Input const map>>> &image, const std_msgs::Header &header 44 | */ 45 | void processImage(const map>>> &image, const std_msgs::Header &header); 46 | 47 | /** 48 | * @brief set relocolization frames 49 | * @param Input double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r 50 | */ 51 | void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); 52 | 53 | 54 | // ********************nornal functions******************** 55 | /** 56 | * @brief inite all values for reboot 57 | */ 58 | void clearState(); 59 | 60 | /** 61 | * @brief calibrate the rotation from cemera to IMU 62 | */ 63 | void calibrationExRotation(); 64 | 65 | // ********************initial function groups******************** 66 | /** 67 | * @brief initialize the system 68 | * @param Input const std_msgs::Header &header 69 | */ 70 | void initial(const std_msgs::Header &header); 71 | 72 | public: 73 | Backend backend; 74 | Initial intializer; 75 | 76 | // @param status flags 77 | enum SolverFlag 78 | { 79 | INITIAL, 80 | NON_LINEAR 81 | }; 82 | 83 | // @param status flags 84 | enum MarginalizationFlag 85 | { 86 | MARGIN_OLD = 0, 87 | MARGIN_SECOND_NEW = 1 88 | }; 89 | 90 | // @param status flags 91 | SolverFlag solver_flag; 92 | MarginalizationFlag marginalization_flag; 93 | 94 | // @param status flags 95 | bool first_imu; 96 | //bool is_valid, is_key;// @param seems useless, TODO can be deleted 97 | bool failure_occur; 98 | 99 | // @param at first, it is 0, after IMU-visual align, it is gc0, after intial, it is gw 100 | Vector3d g; 101 | 102 | // @param seems useless, TODO can be deleted 103 | MatrixXd Ap[2], backup_A; 104 | VectorXd bp[2], backup_b; 105 | 106 | // @param status value: camera->IMU 107 | Matrix3d ric[NUM_OF_CAM]; 108 | Vector3d tic[NUM_OF_CAM]; 109 | 110 | // @param status value: PVQ/bias in world 111 | Vector3d Ps[(WINDOW_SIZE + 1)]; 112 | Vector3d Vs[(WINDOW_SIZE + 1)]; 113 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 114 | Vector3d Bas[(WINDOW_SIZE + 1)]; 115 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 116 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 117 | double td; 118 | 119 | // @param temp buffer 120 | Matrix3d back_R0, last_R, last_R0; 121 | Vector3d back_P0, last_P, last_P0; 122 | 123 | // @param status value: pre-integration bk->bk+1 124 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 125 | Vector3d acc_0, gyr_0; 126 | 127 | // @param buffer 128 | vector dt_buf[(WINDOW_SIZE + 1)]; 129 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 130 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 131 | 132 | // @param frame count(max=10) 133 | int frame_count; 134 | 135 | // @param sum_of_outlier: no use, TODO can be deleted 136 | // @param ssum_of_back: how many second new frames are marged 137 | // @param sum_of_front: how many old frames are marged 138 | // @param sum_of_invalid: no use, TODO can be deleted 139 | //int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 140 | 141 | // @param status value: all features 142 | FeatureManager f_manager; 143 | 144 | // @param function value: estimate motion 145 | MotionEstimator m_estimator; 146 | 147 | InitialEXRotation initial_ex_rotation; 148 | 149 | //vector point_cloud;//no use, TODO can be deleted 150 | //vector margin_cloud;//no use, TODO can be deleted 151 | vector key_poses; 152 | double initial_timestamp; 153 | 154 | int loop_window_index;//TODO delete 155 | 156 | map all_image_frame; 157 | IntegrationBase *tmp_pre_integration; 158 | 159 | // @param relocalization variable 160 | bool relocalization_info; 161 | double relo_frame_stamp; 162 | double relo_frame_index; 163 | int relo_frame_local_index; 164 | vector match_points; 165 | double relo_Pose[SIZE_POSE]; 166 | 167 | Matrix3d drift_correct_r; 168 | Vector3d drift_correct_t; 169 | Vector3d prev_relo_t; 170 | Matrix3d prev_relo_r; 171 | Vector3d relo_relative_t; 172 | Quaterniond relo_relative_q; 173 | double relo_relative_yaw; 174 | }; 175 | 176 | #endif -------------------------------------------------------------------------------- /vins_estimator/include/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/include/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/include/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/include/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 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 14 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 15 | const double _td_i, const double _td_j, const double _row_i, const double _row_j); 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | void check(double **parameters); 18 | 19 | Eigen::Vector3d pts_i, pts_j; 20 | Eigen::Vector3d velocity_i, velocity_j; 21 | double td_i, td_j; 22 | Eigen::Matrix tangent_base; 23 | double row_i, row_j; 24 | static Eigen::Matrix2d sqrt_info; 25 | static double sum_t; 26 | }; 27 | -------------------------------------------------------------------------------- /vins_estimator/include/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 | FeaturePerFrame(const Eigen::Matrix &_point, double td) 22 | { 23 | point.x() = _point(0); 24 | point.y() = _point(1); 25 | point.z() = _point(2); 26 | uv.x() = _point(3); 27 | uv.y() = _point(4); 28 | velocity.x() = _point(5); 29 | velocity.y() = _point(6); 30 | cur_td = td; 31 | } 32 | double cur_td; 33 | Vector3d point; 34 | Vector2d uv; 35 | Vector2d velocity; 36 | double z; 37 | bool is_used; 38 | double parallax; 39 | MatrixXd A; 40 | VectorXd b; 41 | double dep_gradient; 42 | }; 43 | 44 | class FeaturePerId 45 | { 46 | public: 47 | const int feature_id; 48 | int start_frame; 49 | vector feature_per_frame; 50 | 51 | int used_num; 52 | bool is_outlier; 53 | bool is_margin; 54 | double estimated_depth; 55 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 56 | 57 | Vector3d gt_p; 58 | 59 | FeaturePerId(int _feature_id, int _start_frame) 60 | : feature_id(_feature_id), start_frame(_start_frame), 61 | used_num(0), estimated_depth(-1.0), solve_flag(0) 62 | { 63 | } 64 | 65 | int endFrame(); 66 | }; 67 | 68 | class FeatureManager 69 | { 70 | public: 71 | FeatureManager(Matrix3d _Rs[]); 72 | 73 | void setRic(Matrix3d _ric[]); 74 | 75 | void clearState(); 76 | 77 | int getFeatureCount(); 78 | 79 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 80 | void debugShow(); 81 | vector> getCorresponding(int frame_count_l, int frame_count_r); 82 | 83 | //void updateDepth(const VectorXd &x); 84 | void setDepth(const VectorXd &x); 85 | void removeFailures(); 86 | void clearDepth(const VectorXd &x); 87 | VectorXd getDepthVector(); 88 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 89 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 90 | void removeBack(); 91 | void removeFront(int frame_count); 92 | void removeOutlier(); 93 | list feature; 94 | int last_track_num; 95 | 96 | private: 97 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 98 | const Matrix3d *Rs; 99 | Matrix3d ric[NUM_OF_CAM]; 100 | }; 101 | 102 | #endif -------------------------------------------------------------------------------- /vins_estimator/include/initial.h: -------------------------------------------------------------------------------- 1 | #ifndef _INITIAL_H_ 2 | #define _INITIAL_H_ 3 | 4 | #include 5 | 6 | #include "parameters.h" 7 | #include "utility/tic_toc.h" 8 | #include "initial/solve_5pts.h" 9 | #include "initial/initial_sfm.h" 10 | #include "initial/initial_alignment.h" 11 | #include "initial/initial_ex_rotation.h" 12 | 13 | using namespace std; 14 | 15 | class Estimator; 16 | 17 | class Initial 18 | { 19 | public: 20 | // ********************initial function groups******************** 21 | 22 | /** 23 | * @brief main pocess of initialization 24 | */ 25 | bool initialStructure(Estimator *estimator); 26 | 27 | /** 28 | * @brief check IMU Observibility 29 | * TODO not necessary, can be deleted 30 | */ 31 | bool checkIMUObservibility(Estimator *estimator); 32 | 33 | /** 34 | * @brief build sfm_f for SfM 35 | * @param Input vector &sfm_f 36 | * @param output vector &sfm_f 37 | */ 38 | void buildSFMFeature(Estimator *estimator, vector &sfm_f); 39 | 40 | /** 41 | * @brief find base frame l in sliding windows and get relative rotation and translation 42 | * between frame l and newest frame 43 | * @param Input Matrix3d &relative_R, Vector3d &relative_T, int &l 44 | * @param output Matrix3d &relative_R, Vector3d &relative_T, int &l 45 | */ 46 | bool relativePose(Estimator *estimator, Matrix3d &relative_R, Vector3d &relative_T, int &l); 47 | 48 | /** 49 | * @brief get the rotation and translation for all frames and 3D coordinates of all features 50 | * in frame l without scaler 51 | * @param Input Quaterniond Q[], Vector3d T[],map &sfm_tracked_points 52 | */ 53 | bool solvePnPForAllFrame(Estimator *estimator, Quaterniond Q[], Vector3d T[],map &sfm_tracked_points); 54 | 55 | /** 56 | * @brief loosely coupled IMU-visual initialization 57 | */ 58 | bool visualInitialAlign(Estimator *estimator); 59 | 60 | /** 61 | * @brief get the rotation and translation for all frames and 3D coordinates of all features 62 | * in frame world with scaler 63 | * @param Input const VectorXd &x 64 | */ 65 | void recoverStatusValuesFromInitial(Estimator *estimator, const VectorXd &x); 66 | 67 | public: 68 | Estimator *estimator; 69 | 70 | // @param status flags 71 | enum SolverFlag 72 | { 73 | INITIAL, 74 | NON_LINEAR 75 | }; 76 | 77 | // @param status flags 78 | enum MarginalizationFlag 79 | { 80 | MARGIN_OLD = 0, 81 | MARGIN_SECOND_NEW = 1 82 | }; 83 | 84 | }; 85 | #endif -------------------------------------------------------------------------------- /vins_estimator/include/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 | ImageFrame(const map>>>& _points, double _t):t{_t},is_key_frame{false} 18 | { 19 | points = _points; 20 | }; 21 | map> > > points; 22 | double t; 23 | Matrix3d R; 24 | Vector3d T; 25 | IntegrationBase *pre_integration; 26 | bool is_key_frame; 27 | }; 28 | 29 | bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x); -------------------------------------------------------------------------------- /vins_estimator/include/initial/initial_ex_rotation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../parameters.h" 5 | using namespace std; 6 | 7 | #include 8 | 9 | #include 10 | using namespace Eigen; 11 | #include 12 | 13 | /* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */ 14 | class InitialEXRotation 15 | { 16 | public: 17 | InitialEXRotation(); 18 | bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result); 19 | private: 20 | Matrix3d solveRelativeR(const vector> &corres); 21 | 22 | double testTriangulation(const vector &l, 23 | const vector &r, 24 | cv::Mat_ R, cv::Mat_ t); 25 | void decomposeE(cv::Mat E, 26 | cv::Mat_ &R1, cv::Mat_ &R2, 27 | cv::Mat_ &t1, cv::Mat_ &t2); 28 | int frame_count; 29 | 30 | vector< Matrix3d > Rc; 31 | vector< Matrix3d > Rimu; 32 | vector< Matrix3d > Rc_g; 33 | Matrix3d ric; 34 | }; 35 | 36 | 37 | -------------------------------------------------------------------------------- /vins_estimator/include/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 | 15 | 16 | struct SFMFeature 17 | { 18 | bool state; 19 | int id; 20 | vector> observation; 21 | double position[3]; 22 | double depth; 23 | }; 24 | 25 | struct ReprojectionError3D 26 | { 27 | ReprojectionError3D(double observed_u, double observed_v) 28 | :observed_u(observed_u), observed_v(observed_v) 29 | {} 30 | 31 | template 32 | bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const 33 | { 34 | T p[3]; 35 | ceres::QuaternionRotatePoint(camera_R, point, p); 36 | p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; 37 | T xp = p[0] / p[2]; 38 | T yp = p[1] / p[2]; 39 | residuals[0] = xp - T(observed_u); 40 | residuals[1] = yp - T(observed_v); 41 | return true; 42 | } 43 | 44 | static ceres::CostFunction* Create(const double observed_x, 45 | const double observed_y) 46 | { 47 | return (new ceres::AutoDiffCostFunction< 48 | ReprojectionError3D, 2, 4, 3, 3>( 49 | new ReprojectionError3D(observed_x,observed_y))); 50 | } 51 | 52 | double observed_u; 53 | double observed_v; 54 | }; 55 | 56 | class GlobalSFM 57 | { 58 | public: 59 | GlobalSFM(); 60 | bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l, 61 | const Matrix3d relative_R, const Vector3d relative_T, 62 | vector &sfm_f, map &sfm_tracked_points); 63 | 64 | private: 65 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 66 | 67 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 68 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 69 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 70 | int frame1, Eigen::Matrix &Pose1, 71 | vector &sfm_f); 72 | 73 | int feature_num; 74 | }; -------------------------------------------------------------------------------- /vins_estimator/include/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | //#include 8 | #include 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | 17 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 18 | 19 | private: 20 | double testTriangulation(const vector &l, 21 | const vector &r, 22 | cv::Mat_ R, cv::Mat_ t); 23 | void decomposeE(cv::Mat E, 24 | cv::Mat_ &R1, cv::Mat_ &R2, 25 | cv::Mat_ &t1, cv::Mat_ &t2); 26 | }; 27 | 28 | 29 | -------------------------------------------------------------------------------- /vins_estimator/include/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 = 460.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 | 42 | void readParameters(ros::NodeHandle &n); 43 | 44 | enum SIZE_PARAMETERIZATION 45 | { 46 | SIZE_POSE = 7, 47 | SIZE_SPEEDBIAS = 9, 48 | SIZE_FEATURE = 1 49 | }; 50 | 51 | enum StateOrder 52 | { 53 | O_P = 0, 54 | O_R = 3, 55 | O_V = 6, 56 | O_BA = 9, 57 | O_BG = 12 58 | }; 59 | 60 | enum NoiseOrder 61 | { 62 | O_AN = 0, 63 | O_GN = 3, 64 | O_AW = 6, 65 | O_GW = 9 66 | }; 67 | -------------------------------------------------------------------------------- /vins_estimator/include/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/include/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/include/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); -------------------------------------------------------------------------------- /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 | 16 | 17 | 18 | 19 | 20 | 21 | 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/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/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/factor/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/initial/initial_ex_rotation.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/initial/initial_ex_rotation.h" 2 | 3 | InitialEXRotation::InitialEXRotation(){ 4 | frame_count = 0; 5 | Rc.push_back(Matrix3d::Identity()); 6 | Rc_g.push_back(Matrix3d::Identity()); 7 | Rimu.push_back(Matrix3d::Identity()); 8 | ric = Matrix3d::Identity(); 9 | } 10 | 11 | bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result) 12 | { 13 | frame_count++; 14 | Rc.push_back(solveRelativeR(corres)); 15 | Rimu.push_back(delta_q_imu.toRotationMatrix()); 16 | Rc_g.push_back(ric.inverse() * delta_q_imu * ric); 17 | 18 | Eigen::MatrixXd A(frame_count * 4, 4); 19 | A.setZero(); 20 | int sum_ok = 0; 21 | for (int i = 1; i <= frame_count; i++) 22 | { 23 | Quaterniond r1(Rc[i]); 24 | Quaterniond r2(Rc_g[i]); 25 | 26 | double angular_distance = 180 / M_PI * r1.angularDistance(r2); 27 | ROS_DEBUG( 28 | "%d %f", i, angular_distance); 29 | 30 | double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0; 31 | ++sum_ok; 32 | Matrix4d L, R; 33 | 34 | double w = Quaterniond(Rc[i]).w(); 35 | Vector3d q = Quaterniond(Rc[i]).vec(); 36 | L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q); 37 | L.block<3, 1>(0, 3) = q; 38 | L.block<1, 3>(3, 0) = -q.transpose(); 39 | L(3, 3) = w; 40 | 41 | Quaterniond R_ij(Rimu[i]); 42 | w = R_ij.w(); 43 | q = R_ij.vec(); 44 | R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q); 45 | R.block<3, 1>(0, 3) = q; 46 | R.block<1, 3>(3, 0) = -q.transpose(); 47 | R(3, 3) = w; 48 | 49 | A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R); 50 | } 51 | 52 | JacobiSVD svd(A, ComputeFullU | ComputeFullV); 53 | Matrix x = svd.matrixV().col(3); 54 | Quaterniond estimated_R(x); 55 | ric = estimated_R.toRotationMatrix().inverse(); 56 | //cout << svd.singularValues().transpose() << endl; 57 | //cout << ric << endl; 58 | Vector3d ric_cov; 59 | ric_cov = svd.singularValues().tail<3>(); 60 | if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) 61 | { 62 | calib_ric_result = ric; 63 | return true; 64 | } 65 | else 66 | return false; 67 | } 68 | 69 | Matrix3d InitialEXRotation::solveRelativeR(const vector> &corres) 70 | { 71 | if (corres.size() >= 9) 72 | { 73 | vector ll, rr; 74 | for (int i = 0; i < int(corres.size()); i++) 75 | { 76 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 77 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 78 | } 79 | cv::Mat E = cv::findFundamentalMat(ll, rr); 80 | cv::Mat_ R1, R2, t1, t2; 81 | decomposeE(E, R1, R2, t1, t2); 82 | 83 | if (determinant(R1) + 1.0 < 1e-09) 84 | { 85 | E = -E; 86 | decomposeE(E, R1, R2, t1, t2); 87 | } 88 | double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2)); 89 | double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2)); 90 | cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2; 91 | 92 | Matrix3d ans_R_eigen; 93 | for (int i = 0; i < 3; i++) 94 | for (int j = 0; j < 3; j++) 95 | ans_R_eigen(j, i) = ans_R_cv(i, j); 96 | return ans_R_eigen; 97 | } 98 | return Matrix3d::Identity(); 99 | } 100 | 101 | double InitialEXRotation::testTriangulation(const vector &l, 102 | const vector &r, 103 | cv::Mat_ R, cv::Mat_ t) 104 | { 105 | cv::Mat pointcloud; 106 | cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 107 | 0, 1, 0, 0, 108 | 0, 0, 1, 0); 109 | cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), 110 | R(1, 0), R(1, 1), R(1, 2), t(1), 111 | R(2, 0), R(2, 1), R(2, 2), t(2)); 112 | cv::triangulatePoints(P, P1, l, r, pointcloud); 113 | int front_count = 0; 114 | for (int i = 0; i < pointcloud.cols; i++) 115 | { 116 | double normal_factor = pointcloud.col(i).at(3); 117 | 118 | cv::Mat_ p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor); 119 | cv::Mat_ p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor); 120 | if (p_3d_l(2) > 0 && p_3d_r(2) > 0) 121 | front_count++; 122 | } 123 | ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols); 124 | return 1.0 * front_count / pointcloud.cols; 125 | } 126 | 127 | void InitialEXRotation::decomposeE(cv::Mat E, 128 | cv::Mat_ &R1, cv::Mat_ &R2, 129 | cv::Mat_ &t1, cv::Mat_ &t2) 130 | { 131 | cv::SVD svd(E, cv::SVD::MODIFY_A); 132 | cv::Matx33d W(0, -1, 0, 133 | 1, 0, 0, 134 | 0, 0, 1); 135 | cv::Matx33d Wt(0, 1, 0, 136 | -1, 0, 0, 137 | 0, 0, 1); 138 | R1 = svd.u * cv::Mat(W) * svd.vt; 139 | R2 = svd.u * cv::Mat(Wt) * svd.vt; 140 | t1 = svd.u.col(2); 141 | t2 = -svd.u.col(2); 142 | } 143 | -------------------------------------------------------------------------------- /vins_estimator/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "../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 | std::cout << "result path " << VINS_RESULT_PATH << std::endl; 63 | 64 | // create folder if not exists 65 | FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str()); 66 | 67 | std::ofstream fout(VINS_RESULT_PATH, std::ios::out); 68 | fout.close(); 69 | 70 | ACC_N = fsSettings["acc_n"]; 71 | ACC_W = fsSettings["acc_w"]; 72 | GYR_N = fsSettings["gyr_n"]; 73 | GYR_W = fsSettings["gyr_w"]; 74 | G.z() = fsSettings["g_norm"]; 75 | ROW = fsSettings["image_height"]; 76 | COL = fsSettings["image_width"]; 77 | ROS_INFO("ROW: %f COL: %f ", ROW, COL); 78 | 79 | ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; 80 | if (ESTIMATE_EXTRINSIC == 2) 81 | { 82 | ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); 83 | RIC.push_back(Eigen::Matrix3d::Identity()); 84 | TIC.push_back(Eigen::Vector3d::Zero()); 85 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 86 | 87 | } 88 | else 89 | { 90 | if ( ESTIMATE_EXTRINSIC == 1) 91 | { 92 | ROS_WARN(" Optimize extrinsic param around initial guess!"); 93 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 94 | } 95 | if (ESTIMATE_EXTRINSIC == 0) 96 | ROS_WARN(" fix extrinsic param "); 97 | 98 | cv::Mat cv_R, cv_T; 99 | fsSettings["extrinsicRotation"] >> cv_R; 100 | fsSettings["extrinsicTranslation"] >> cv_T; 101 | Eigen::Matrix3d eigen_R; 102 | Eigen::Vector3d eigen_T; 103 | cv::cv2eigen(cv_R, eigen_R); 104 | cv::cv2eigen(cv_T, eigen_T); 105 | Eigen::Quaterniond Q(eigen_R); 106 | eigen_R = Q.normalized(); 107 | RIC.push_back(eigen_R); 108 | TIC.push_back(eigen_T); 109 | ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]); 110 | ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose()); 111 | 112 | } 113 | 114 | INIT_DEPTH = 5.0; 115 | BIAS_ACC_THRESHOLD = 0.1; 116 | BIAS_GYR_THRESHOLD = 0.1; 117 | 118 | TD = fsSettings["td"]; 119 | ESTIMATE_TD = fsSettings["estimate_td"]; 120 | if (ESTIMATE_TD) 121 | ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); 122 | else 123 | ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); 124 | 125 | ROLLING_SHUTTER = fsSettings["rolling_shutter"]; 126 | if (ROLLING_SHUTTER) 127 | { 128 | TR = fsSettings["rolling_shutter_tr"]; 129 | ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); 130 | } 131 | else 132 | { 133 | TR = 0; 134 | } 135 | 136 | fsSettings.release(); 137 | } 138 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/utility/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 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 12 | return R0; 13 | } 14 | --------------------------------------------------------------------------------