├── src ├── CMakeLists.txt ├── camera_model │ ├── instruction │ ├── readme.md │ ├── include │ │ └── camodocal │ │ │ ├── camera_models │ │ │ ├── CameraFactory.h │ │ │ ├── CostFunctionFactory.h │ │ │ ├── Camera.h │ │ │ ├── PinholeCamera.h │ │ │ ├── CataCamera.h │ │ │ ├── EquidistantCamera.h │ │ │ └── ScaramuzzaCamera.h │ │ │ ├── chessboard │ │ │ ├── ChessboardQuad.h │ │ │ ├── ChessboardCorner.h │ │ │ ├── Chessboard.h │ │ │ └── Spline.h │ │ │ ├── sparse_graph │ │ │ └── Transform.h │ │ │ ├── gpl │ │ │ ├── EigenQuaternionParameterization.h │ │ │ └── gpl.h │ │ │ └── calib │ │ │ └── CameraCalibration.h │ ├── src │ │ ├── sparse_graph │ │ │ └── Transform.cc │ │ ├── gpl │ │ │ └── EigenQuaternionParameterization.cc │ │ ├── camera_models │ │ │ ├── CameraFactory.cc │ │ │ └── Camera.cc │ │ └── intrinsic_calib.cc │ ├── CMakeLists.txt │ ├── package.xml │ └── cmake │ │ └── FindEigen.cmake ├── feature_tracker │ ├── src │ │ ├── tic_toc.h │ │ ├── parameters.h │ │ ├── feature_tracker.h │ │ ├── parameters.cpp │ │ ├── feature_tracker_node.cpp │ │ └── feature_tracker.cpp │ ├── CMakeLists.txt │ ├── package.xml │ └── cmake │ │ └── FindEigen.cmake └── gtsam_backend │ ├── src │ ├── GraphSolver_FEAT.cpp │ ├── GraphSolver_IMU.cpp │ ├── GraphSolver.cpp │ └── main_estimator.cpp │ ├── include │ └── gtsam_backend │ │ ├── utils │ │ ├── Config.h │ │ ├── Convert.h │ │ └── State.h │ │ └── GraphSolver.h │ ├── package.xml │ ├── CMakeLists.txt │ └── cmake │ └── FindMKL.cmake ├── github └── expected_result.png ├── .gitignore ├── README.md ├── launch └── mvsec_test.launch ├── config └── mvsec_vins_config.yaml └── rviz └── mvsec_test.rviz /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/camera_model/instruction: -------------------------------------------------------------------------------- 1 | rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/ 2 | -------------------------------------------------------------------------------- /github/expected_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ganlumomo/VisualInertialOdometry/HEAD/github/expected_result.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | devel/ 3 | 4 | 5 | .catkin_workspace 6 | 7 | # Prerequisites 8 | *.d 9 | 10 | # Compiled Object files 11 | *.slo 12 | *.lo 13 | *.o 14 | *.obj 15 | 16 | # Precompiled Headers 17 | *.gch 18 | *.pch 19 | 20 | # Compiled Dynamic libraries 21 | *.so 22 | *.dylib 23 | *.dll 24 | 25 | # Fortran module files 26 | *.mod 27 | *.smod 28 | 29 | # Compiled Static libraries 30 | *.lai 31 | *.la 32 | *.a 33 | *.lib 34 | 35 | # Executables 36 | *.exe 37 | *.out 38 | *.app 39 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/feature_tracker/src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /src/feature_tracker/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | extern int ROW; 6 | extern int COL; 7 | extern int FOCAL_LENGTH; 8 | const int NUM_OF_CAM = 1; 9 | 10 | 11 | extern std::string IMAGE_TOPIC; 12 | extern std::string IMU_TOPIC; 13 | extern std::string FISHEYE_MASK; 14 | extern std::vector CAM_NAMES; 15 | extern int MAX_CNT; 16 | extern int MIN_DIST; 17 | extern int WINDOW_SIZE; 18 | extern int FREQ; 19 | extern double F_THRESHOLD; 20 | extern int SHOW_TRACK; 21 | extern int STEREO_TRACK; 22 | extern int EQUALIZE; 23 | extern int FISHEYE; 24 | extern bool PUB_THIS_FRAME; 25 | 26 | void readParameters(ros::NodeHandle &n); 27 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/feature_tracker/src/feature_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "camodocal/camera_models/CameraFactory.h" 13 | #include "camodocal/camera_models/CataCamera.h" 14 | #include "camodocal/camera_models/PinholeCamera.h" 15 | 16 | #include "parameters.h" 17 | #include "tic_toc.h" 18 | 19 | using namespace std; 20 | using namespace camodocal; 21 | using namespace Eigen; 22 | 23 | bool inBorder(const cv::Point2f &pt); 24 | 25 | void reduceVector(vector &v, vector status); 26 | void reduceVector(vector &v, vector status); 27 | 28 | class FeatureTracker 29 | { 30 | public: 31 | FeatureTracker(); 32 | 33 | void readImage(const cv::Mat &_img,double _cur_time); 34 | 35 | void setMask(); 36 | 37 | void addPoints(); 38 | 39 | bool updateID(unsigned int i); 40 | 41 | void readIntrinsicParameter(const string &calib_file); 42 | 43 | void showUndistortion(const string &name); 44 | 45 | void rejectWithF(); 46 | 47 | void undistortedPoints(); 48 | 49 | cv::Mat mask; 50 | cv::Mat fisheye_mask; 51 | cv::Mat prev_img, cur_img, forw_img; 52 | vector n_pts; 53 | vector prev_pts, cur_pts, forw_pts; 54 | vector prev_un_pts, cur_un_pts; 55 | vector pts_velocity; 56 | vector ids; 57 | vector track_cnt; 58 | map cur_un_pts_map; 59 | map prev_un_pts_map; 60 | camodocal::CameraPtr m_camera; 61 | double cur_time; 62 | double prev_time; 63 | 64 | static int n_id; 65 | }; 66 | -------------------------------------------------------------------------------- /src/gtsam_backend/src/GraphSolver_FEAT.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "GraphSolver.h" 3 | 4 | void GraphSolver::process_feat_smart(double timestamp, std::vector leftids, std::vector leftuv) { 5 | 6 | //============================================================================== 7 | // Loop through LEFT features 8 | for(size_t i=0; iadd(gtsam::Point2(leftuv.at(i)), X(ct_state)); 14 | continue; 15 | } 16 | 17 | // If we know it is not in the graph 18 | // Create a smart factor for the new feature 19 | gtsam::noiseModel::Isotropic::shared_ptr measurementNoise = gtsam::noiseModel::Isotropic::Sigma(2, config->sigma_camera); 20 | gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2()); 21 | 22 | // Transformation from camera frame to imu frame, i.e., pose of imu frame in camera frame 23 | gtsam::Pose3 sensor_P_body = gtsam::Pose3(gtsam::Rot3(config->R_C0toI), gtsam::Point3(config->p_IinC0)); 24 | SmartFactor::shared_ptr smartfactor_left(new SmartFactor(measurementNoise, K, sensor_P_body.inverse())); 25 | measurement_smart_lookup_left[leftids.at(i)] = smartfactor_left; 26 | 27 | // Insert measurements to a smart factor 28 | smartfactor_left->add(gtsam::Point2(leftuv.at(i)), X(ct_state)); 29 | 30 | // Add smart factor to FORSTER2 model 31 | graph_new->push_back(smartfactor_left); 32 | graph->push_back(smartfactor_left); 33 | } 34 | } 35 | 36 | -------------------------------------------------------------------------------- /src/gtsam_backend/include/gtsam_backend/utils/Config.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class Config { 6 | public: 7 | /// Default constructor 8 | Config(){} 9 | 10 | std::string fixedId; 11 | 12 | /// Assumed "true" global gravity 13 | Eigen::Matrix gravity; 14 | 15 | /// Amount of IMU and FEAT we should wait to initialize to 16 | int imuWait; 17 | int featWait; 18 | 19 | /// Relative transform between CAM0 and IMU 20 | Eigen::Matrix p_IinC0; 21 | Eigen::Matrix R_C0toI; 22 | 23 | /// Priors 24 | Eigen::Matrix prior_qGtoI; 25 | Eigen::Matrix prior_pIinG; 26 | Eigen::Matrix prior_vIinG; 27 | Eigen::Matrix prior_ba; 28 | Eigen::Matrix prior_bg; 29 | 30 | /// Noise values for the image 31 | double sigma_camera; ///< Noise value for CAMERA points 32 | double sigma_camera_sq; ///< Noise value for CAMERA points squared 33 | 34 | // Noise values for the IMU 35 | double sigma_a; ///< Acceleration white noise 36 | double sigma_a_sq; ///< Acceleration white noise squared 37 | double sigma_g; ///< Gyro white noise 38 | double sigma_g_sq; ///< Gyro white noise squared 39 | double sigma_wa; ///< Acceleration bias walk 40 | double sigma_wa_sq; ///< Acceleration bias walk squared 41 | double sigma_wg; ///< Gyro bias walk 42 | double sigma_wg_sq; ///< Gyro bias walk squared 43 | 44 | // Noise values for Initialization 45 | double sigma_prior_rotation; // 0.1 rad on roll, pitch, yaw 46 | double sigma_prior_translation; // 30cm std on x, y, z 47 | double sigma_velocity; // 0.1 m/s 48 | double sigma_bias; // 0.1 m/s 49 | double sigma_pose_rotation; // 0.1 rad on roll, pitch, yaw 50 | double sigma_pose_translation; // 30cm std on x, y, z 51 | }; 52 | -------------------------------------------------------------------------------- /src/feature_tracker/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC; 4 | std::string IMU_TOPIC; 5 | std::vector CAM_NAMES; 6 | std::string FISHEYE_MASK; 7 | int MAX_CNT; 8 | int MIN_DIST; 9 | int WINDOW_SIZE; 10 | int FREQ; 11 | double F_THRESHOLD; 12 | int SHOW_TRACK; 13 | int STEREO_TRACK; 14 | int EQUALIZE; 15 | int ROW; 16 | int COL; 17 | int FOCAL_LENGTH; 18 | int FISHEYE; 19 | bool PUB_THIS_FRAME; 20 | 21 | 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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/gtsam_backend/include/gtsam_backend/utils/Convert.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | void ToPose(const gtsam::Pose3& i_pose, geometry_msgs::Pose& pose) { 7 | pose.orientation.x = i_pose.rotation().toQuaternion().x(); 8 | pose.orientation.y = i_pose.rotation().toQuaternion().y(); 9 | pose.orientation.z = i_pose.rotation().toQuaternion().z(); 10 | pose.orientation.w = i_pose.rotation().toQuaternion().w(); 11 | pose.position.x = i_pose.translation().x(); 12 | pose.position.y = i_pose.translation().y(); 13 | pose.position.z = i_pose.translation().z(); 14 | } 15 | 16 | void ToPoseWithCovariance(const gtsam::Pose3& i_pose, 17 | const Eigen::Matrix& covariance, 18 | geometry_msgs::PoseWithCovariance& pose) { 19 | ToPose(i_pose, pose.pose); 20 | 21 | // Finally set the covariance in the message 22 | pose.covariance[0] = covariance(0,0); // 0 23 | pose.covariance[1] = covariance(0,1); 24 | pose.covariance[2] = covariance(0,2); 25 | pose.covariance[3] = covariance(0,3); 26 | pose.covariance[4] = covariance(0,4); 27 | pose.covariance[5] = covariance(0,5); 28 | pose.covariance[6] = covariance(1,0); // 1 29 | pose.covariance[7] = covariance(1,1); 30 | pose.covariance[8] = covariance(1,2); 31 | pose.covariance[9] = covariance(1,3); 32 | pose.covariance[10] = covariance(1,4); 33 | pose.covariance[11] = covariance(1,5); 34 | pose.covariance[12] = covariance(2,0); // 2 35 | pose.covariance[13] = covariance(2,1); 36 | pose.covariance[14] = covariance(2,2); 37 | pose.covariance[15] = covariance(2,3); 38 | pose.covariance[16] = covariance(2,4); 39 | pose.covariance[17] = covariance(2,5); 40 | pose.covariance[18] = covariance(3,0); // 3 41 | pose.covariance[19] = covariance(3,1); 42 | pose.covariance[20] = covariance(3,2); 43 | pose.covariance[21] = covariance(3,3); 44 | pose.covariance[22] = covariance(3,4); 45 | pose.covariance[23] = covariance(3,5); 46 | pose.covariance[24] = covariance(4,0); // 4 47 | pose.covariance[25] = covariance(4,1); 48 | pose.covariance[26] = covariance(4,2); 49 | pose.covariance[27] = covariance(4,3); 50 | pose.covariance[28] = covariance(4,4); 51 | pose.covariance[29] = covariance(4,5); 52 | pose.covariance[30] = covariance(5,0); // 5 53 | pose.covariance[31] = covariance(5,1); 54 | pose.covariance[32] = covariance(5,2); 55 | pose.covariance[33] = covariance(5,3); 56 | pose.covariance[34] = covariance(5,4); 57 | pose.covariance[35] = covariance(5,5); 58 | } 59 | -------------------------------------------------------------------------------- /src/gtsam_backend/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gtsam_backend 4 | 0.0.0 5 | The gtsam_backend package 6 | 7 | 8 | 9 | 10 | ganlu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # VisualInertialOdometry 2 | 3 | ## Introduction 4 | 5 | This project is designed for students to learn the front-end and back-end in a Simultaneous Localization and Mapping (SLAM) system. The objective is that using feature_tracker in [VINS-MONO](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) as front-end, and [GTSAM](https://github.com/borglab/gtsam) as back-end to implement a visual inertial odometry (VIO) algorithm for real-data collected by a vehicle: [The MVSEC Dataset](https://daniilidis-group.github.io/mvsec/). The code is modified based on original code from [CPI](https://github.com/rpng/cpi). 6 | 7 | Specifically, we are learning how to utilize a front-end package and use IMUFactor, SmartProjectionPoseFactor and ISAM2 optimizer in GTSAM to achieve a simple but straight-forward VIO system. 8 | 9 | ## Instruction 10 | 11 | ### Step 1 12 | 13 | Install ROS: 14 | 15 | Ubuntu 18.04: [http://wiki.ros.org/melodic/Installation/Ubuntu](http://wiki.ros.org/melodic/Installation/Ubuntu). 16 | 17 | Ubuntu 16.04: [http://wiki.ros.org/kinetic/Installation/Ubuntu](http://wiki.ros.org/kinetic/Installation/Ubuntu). 18 | 19 | Install GTSAM as a thirdparty: [https://github.com/borglab/gtsam](https://github.com/borglab/gtsam). 20 | 21 | ### Step 2 22 | 23 | Clone this ros workspace: 24 | 25 | ```bash 26 | $ git clone https://github.com/ganlumomo/VisualInertialOdometry.git 27 | ``` 28 | 29 | Try out the feature_tracker package to see how does it work and what does it output. 30 | 31 | ### Step 3 32 | Refer examples [ImuFactorsExample.cpp](https://bitbucket.org/gtborg/gtsam/src/develop/examples/ImuFactorsExample.cpp) and [ISAM2Example_SmartFactor.cpp](https://bitbucket.org/gtborg/gtsam/src/develop/examples/ISAM2Example_SmartFactor.cpp) to implement the following functions defined in [GraphSolver.h](https://github.com/ganlumomo/VisualInertialOdometry/blob/master/src/gtsam_backend/include/gtsam_backend/GraphSolver.h#L115): 33 | ```bash 34 | // For IMU preintegration 35 | set_imu_preintegration 36 | create_imu_factor 37 | get_predicted_state 38 | reset_imu_integration 39 | 40 | // For smart vision factor 41 | process_feat_smart 42 | ``` 43 | You are free to change the function signature, but make sure change other places accordingly. 44 | 45 | ### Step 4 46 | 47 | Build the code and launch the ros node with rosbag data: 48 | ```bash 49 | $ cd VisualInertialOdometry 50 | $ catkin_make 51 | $ source devel/setup.bash 52 | $ roslaunch launch/mvsec_test.launch 53 | $ rosbag play mvsec_test.bag 54 | ``` 55 | Test data can be downloaded here: [mvsec_test.bag](https://drive.google.com/file/d/1kjzadWbivMe3tH3uULDono-XcWwR_2l0/view?usp=sharing). 56 | 57 | ### Expected Results 58 | 59 | 60 | 61 | Write one-page project summary about your implementation and result. 62 | 63 | ## References 64 | 65 | VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator ([PDF](https://ieeexplore.ieee.org/document/8421746)) 66 | 67 | IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation ([PDF](http://www.roboticsproceedings.org/rss11/p06.pdf)) 68 | 69 | Eliminating conditionally independent sets in factor graphs: A unifying perspective based on smart factors ([PDF](https://ieeexplore.ieee.org/abstract/document/6907483)) 70 | 71 | The Multivehicle Stereo Event Camera Dataset: An Event Camera Dataset for 3D Perception ([PDF](https://ieeexplore.ieee.org/document/8288670)) 72 | -------------------------------------------------------------------------------- /src/gtsam_backend/include/gtsam_backend/utils/State.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace gtsam { 7 | 8 | typedef imuBias::ConstantBias Bias; 9 | 10 | class State { 11 | private: 12 | Pose3 pose_; // Rotation from global to IMU, Position of IMU in global 13 | Vector3 velocity_; // Velocity of IMU in global 14 | Bias bias_; // Bias of IMU 15 | 16 | public: 17 | // Default Constructor 18 | State() : pose_(Pose3()), velocity_(Vector3()), bias_(Bias()) {} 19 | 20 | // Copy Constructor 21 | State(const State& state) { 22 | this->pose_ = state.pose_; 23 | this->velocity_ = state.velocity_; 24 | this->bias_ = state.bias_; 25 | } 26 | 27 | // Constructor 28 | State(const Pose3& pose, const Vector3& velocity, const Bias& bias) { 29 | this->pose_ = pose; 30 | this->velocity_ = velocity; 31 | this->bias_ = bias; 32 | } 33 | 34 | // Set pose 35 | void set_pose(const Pose3& pose) { 36 | this->pose_ = pose; 37 | } 38 | 39 | // Set velocity 40 | void set_velocity(const Vector3& velocity) { 41 | this->velocity_ = velocity; 42 | } 43 | 44 | // set_bias 45 | void set_bias(const Bias& bias) { 46 | this->bias_ = bias; 47 | } 48 | 49 | // Return pose as Pose3 50 | const Pose3& pose() const { 51 | return pose_; 52 | } 53 | 54 | // Return velocity as Vector3 55 | const Vector3& v() const { 56 | return velocity_; 57 | } 58 | 59 | // Return bias as Bias 60 | const Bias& b() const { 61 | return bias_; 62 | } 63 | 64 | // Return rotation as Quaternion 65 | Quaternion q() const { 66 | return pose().rotation().toQuaternion(); 67 | } 68 | 69 | // Return translation as Vector3 70 | Vector3 p() const { 71 | return pose().translation(); 72 | } 73 | 74 | // Return accelerometer bias as Vector3 75 | const Vector3& ba() const { 76 | return b().accelerometer(); 77 | } 78 | 79 | // Return gyroscope bias as Vector3 80 | const Vector3& bg() const { 81 | return b().gyroscope(); 82 | } 83 | 84 | /// How this node gets printed in the ostream 85 | GTSAM_EXPORT 86 | friend std::ostream &operator<<(std::ostream &os, const State& state) { 87 | os << "[STATE]: q = " << std::fixed << state.q().x() << ", " << std::fixed << state.q().y() << ", " 88 | << std::fixed << state.q().z() << ", " << std::fixed << state.q().w() << " | "; 89 | os << "p = " << std::fixed << state.p()(0) << ", " << std::fixed << state.p()(1) << ", " << std::fixed << state.p()(2) << " | "; 90 | os << "v = " << std::fixed << state.v()(0) << ", " << std::fixed << state.v()(1) << ", " << std::fixed << state.v()(2) << " | "; 91 | os << "ba = " << std::fixed << state.ba()(0) << ", " << std::fixed << state.ba()(1) << ", " << std::fixed << state.ba()(2) << " | "; 92 | os << "bg = " << std::fixed << state.bg()(0) << ", " << std::fixed << state.bg()(1) << ", " << std::fixed << state.bg()(2); 93 | return os; 94 | } 95 | 96 | /// Print function for this node 97 | void print(const std::string& s = "") const { 98 | std::cout << s << *this << std::endl; 99 | } 100 | 101 | }; // State class 102 | 103 | } // namespace gtsam 104 | -------------------------------------------------------------------------------- /launch/mvsec_test.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 | [0.0, 0.0, 9.81007] 26 | 27 | 28 | 29 | 30 | [0.9999717314190615, -0.007438121416209933, 0.001100323844221122, 31 | 0.00743200596269379, 0.9999574688631824, 0.005461295826837418, 32 | -0.0011408988276470173, -0.0054529638303831614, 0.9999844816472552] 33 | [-0.03921656415229387, 0.00621263233002485, 0.0012210059575531885] 34 | 35 | 36 | [0.716147, 0.051158, 0.133778, 0.683096] 37 | [0.925493, -6.214668, 0.422872] 38 | [1.128364, -2.280640, 0.213326] 39 | [0.00489771688759235973, 0.00800897351104824969, 0.03020588299505782420] 40 | [-0.00041196751646696610, 0.00992948457018005999, 0.02188282212555122189] 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /config/mvsec_vins_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/visensor/imu" 5 | image_topic: "/visensor/left/image_raw" 6 | output_path: "/home/ganlu/output/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: /visensor/left 11 | image_width: 752 12 | image_height: 480 13 | distortion_parameters: 14 | k1: -0.2886519243 15 | k2: 0.08251010503 16 | p1: -0.0003745841 17 | p2: -9.908178225e-05 18 | projection_parameters: 19 | fx: 465.66479689 20 | fy: 465.53104947 21 | cx: 373.17652925 22 | cy: 232.29333312 23 | 24 | # Extrinsic parameter between IMU and Camera. 25 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 26 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 27 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 28 | #If you choose 0 or 1, you should write down the following matrix. 29 | #Rotation from camera frame to imu frame, imu^R_cam 30 | extrinsicRotation: !!opencv-matrix 31 | rows: 3 32 | cols: 3 33 | dt: d 34 | data: [0.9999717314190615, -0.007438121416209933, 0.001100323844221122, 35 | 0.00743200596269379, 0.9999574688631824, 0.005461295826837418, 36 | -0.0011408988276470173, -0.0054529638303831614, 0.9999844816472552] 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [-0.03921656415229387, 0.00621263233002485, 0.0012210059575531885] 43 | 44 | #feature traker paprameters 45 | max_cnt: 200 # max feature number in feature tracking 46 | min_dist: 10 # min distance between two features 47 | freq: 25 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04 60 | gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 61 | acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.81007 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 1 # start loop closure 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | fast_relocalization: 0 # useful in real-time and large project 69 | pose_graph_save_path: "/home/ganlu/output/pose_graph/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 0 # online estimate time offset between camera and imu 73 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /src/gtsam_backend/src/GraphSolver_IMU.cpp: -------------------------------------------------------------------------------- 1 | #include "GraphSolver.h" 2 | 3 | bool GraphSolver::set_imu_preintegration(const gtsam::State& prior_state) { 4 | 5 | // Create GTSAM preintegration parameters for use with Foster's version 6 | boost::shared_ptr params; 7 | params = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedU(config->gravity(2)); // Z-up navigation frame: gravity points along negative Z-axis !!! 8 | 9 | params->setAccelerometerCovariance(gtsam::I_3x3 * config->sigma_a_sq); // acc white noise in continuous 10 | params->setGyroscopeCovariance(gtsam::I_3x3 * config->sigma_g_sq); // gyro white noise in continuous 11 | params->biasAccCovariance = config->sigma_wa_sq * gtsam::Matrix33::Identity(3,3); // acc bias in continuous 12 | params->biasOmegaCovariance = config->sigma_wg_sq * gtsam::Matrix33::Identity(3,3); // gyro bias in continuous 13 | params->setIntegrationCovariance(gtsam::I_3x3 * 0.1); // error committed in integrating position from velocities 14 | params->biasAccOmegaInt = 1e-5*gtsam::Matrix66::Identity(6,6); // error in the bias used for preintegration 15 | 16 | // Actually create the GTSAM preintegration 17 | preint_gtsam = new gtsam::PreintegratedCombinedMeasurements(params, prior_state.b()); 18 | return true; 19 | } 20 | 21 | /** 22 | * This function will create a discrete IMU factor using the GTSAM preintegrator class 23 | * This will integrate from the current state time up to the new update time 24 | */ 25 | gtsam::CombinedImuFactor GraphSolver::create_imu_factor(double updatetime, gtsam::Values& values_initial) { 26 | 27 | int imucompound = 0; 28 | 29 | // TODO: Clean this code, and use the mutex 30 | while(imu_times.size() > 1 && imu_times.at(1) <= updatetime) { 31 | double dt = imu_times.at(1) - imu_times.at(0); 32 | if (dt >= 0) { 33 | // Our IMU measurement 34 | Eigen::Vector3d meas_angvel; 35 | Eigen::Vector3d meas_linaccs; 36 | meas_angvel = imu_angvel.at(0); 37 | meas_linaccs = imu_linaccs.at(0); 38 | // Preintegrate this measurement! 39 | preint_gtsam->integrateMeasurement(meas_linaccs, meas_angvel, dt); 40 | } 41 | //std::cout << "state time = " << updatetime << " | imu0 = " << imu_times.at(0) << " | imu1 = " << imu_times.at(1) << " | dt = " << dt << std::endl; 42 | //cout << "imu dt = " << dt << " | am = " << imu_linaccs.at(0).transpose() << " | wm = " << imu_angvel.at(0).transpose() << endl; 43 | imu_angvel.erase(imu_angvel.begin()); 44 | imu_linaccs.erase(imu_linaccs.begin()); 45 | imu_times.erase(imu_times.begin()); 46 | imucompound++; 47 | } 48 | 49 | // TODO: Clean this code, and use the mutex 50 | double dt_f = updatetime - imu_times.at(0); 51 | if (dt_f > 0) { 52 | // Our IMU measurement 53 | Eigen::Vector3d meas_angvel; 54 | Eigen::Vector3d meas_linaccs; 55 | meas_angvel = imu_angvel.at(0); 56 | meas_linaccs = imu_linaccs.at(0); 57 | // Preintegrate this measurement! 58 | preint_gtsam->integrateMeasurement(meas_linaccs, meas_angvel, dt_f); 59 | imu_times.at(0) = updatetime; 60 | imucompound++; 61 | } 62 | 63 | return gtsam::CombinedImuFactor(X(ct_state ), V(ct_state), 64 | X(ct_state+1), V(ct_state+1), 65 | B(ct_state ), B(ct_state+1), 66 | *preint_gtsam); 67 | } 68 | 69 | 70 | /** 71 | * This function will get the predicted state based on the IMU measurement 72 | */ 73 | 74 | gtsam::State GraphSolver::get_predicted_state(gtsam::Values& values_initial) { 75 | 76 | // Get the current state (t=k) 77 | gtsam::State stateK = gtsam::State(values_initial.at(X(ct_state)), 78 | values_initial.at(V(ct_state)), 79 | values_initial.at(B(ct_state))); 80 | 81 | // From this we should predict where we will be at the next time (t=K+1) 82 | gtsam::NavState stateK1 = preint_gtsam->predict(gtsam::NavState(stateK.pose(), stateK.v()), stateK.b()); 83 | return gtsam::State(stateK1.pose(), stateK1.v(), stateK.b()); 84 | } 85 | 86 | void GraphSolver::reset_imu_integration() { 87 | 88 | // Use the optimized bias to reset integration 89 | if (values_initial.exists(B(ct_state))) 90 | preint_gtsam->resetIntegrationAndSetBias(values_initial.at(B(ct_state))); 91 | //preint_gtsam_->resetIntegrationAndSetBias(Bias()); 92 | 93 | return; 94 | } 95 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/gtsam_backend/include/gtsam_backend/GraphSolver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // Graphs 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // Factors 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "utils/Config.h" 21 | #include "utils/State.h" 22 | 23 | using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 24 | using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) 25 | using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) 26 | 27 | typedef std::vector> Trajectory; 28 | typedef gtsam::SmartProjectionPoseFactor SmartFactor; 29 | 30 | class GraphSolver { 31 | public: 32 | GraphSolver(Config* config) { 33 | 34 | // Set up config 35 | this->config = config; 36 | this->graph = new gtsam::NonlinearFactorGraph(); 37 | this->graph_new = new gtsam::NonlinearFactorGraph(); 38 | 39 | // ISAM2 solver 40 | gtsam::ISAM2Params isam_params; 41 | isam_params.relinearizeThreshold = 0.01; 42 | isam_params.relinearizeSkip = 1; 43 | isam_params.cacheLinearizedFactors = false; 44 | isam_params.enableDetailedResults = true; 45 | isam_params.print(); 46 | this->isam2 = new gtsam::ISAM2(isam_params); 47 | } 48 | 49 | /// Will return true if the system is initialized 50 | inline bool is_initialized() { return systeminitalized; } 51 | 52 | /// Function that takes in IMU measurements for use in preintegration measurements 53 | void addmeasurement_imu(double timestamp, Eigen::Vector3d linacc, Eigen::Vector3d angvel, Eigen::Vector4d orientation); 54 | 55 | /// Function that takes in UV measurements that will be used as "features" in our graph 56 | void addmeasurement_uv(double timestamp, std::vector leftids, std::vector leftuv); 57 | 58 | /// This function will optimize the graph 59 | void optimize(); 60 | 61 | /// This function returns the current state, return origin if we have not initialized yet 62 | gtsam::State get_state(size_t ct) { 63 | // Ensure valid states 64 | assert (ct <= ct_state); 65 | if (!values_initial.exists(X(ct))) 66 | return gtsam::State(); 67 | return gtsam::State(values_initial.at( X(ct)), 68 | values_initial.at(V(ct)), 69 | values_initial.at( B(ct))); 70 | } 71 | 72 | gtsam::State get_current_state() { 73 | return get_state(ct_state); 74 | } 75 | 76 | 77 | Trajectory get_trajectory() { 78 | // Return if we do not have any nodes yet 79 | if (values_initial.empty()) { 80 | Trajectory traj; 81 | traj.push_back(std::make_pair(0.0, gtsam::State())); 82 | return traj; 83 | } 84 | Trajectory trajectory; 85 | // Else loop through the states and return them 86 | for (size_t i = 1; i <= ct_state; ++i) { 87 | double timestamp = timestamp_lookup[i]; 88 | trajectory.push_back(std::make_pair(timestamp, get_state(i))); 89 | } 90 | return trajectory; 91 | } 92 | 93 | /// Returns the currently tracked features 94 | std::vector get_current_features() { 95 | // Return if we do not have any nodes yet 96 | if(values_initial.empty()) { 97 | return std::vector(); 98 | } 99 | // Our vector of points in the global 100 | std::vector features; 101 | // Else loop through the features and return them 102 | for (auto element : measurement_smart_lookup_left) { 103 | boost::optional point = element.second->point(values_initial); 104 | if (point) 105 | features.push_back(*point); 106 | } 107 | return features; 108 | } 109 | 110 | private: 111 | /// Functions 112 | // Function which will try to initalize our graph using the current IMU measurements 113 | void initialize(double timestamp); 114 | 115 | // ******************************* TODO ********************************* // 116 | bool set_imu_preintegration(const gtsam::State& prior_state); 117 | 118 | // Function that will compound the GTSAM preintegrator to get discrete preintegration measurement 119 | gtsam::CombinedImuFactor create_imu_factor(double updatetime, gtsam::Values& values_initial); 120 | 121 | // Function will get the predicted Navigation State based on this generated measurement 122 | gtsam::State get_predicted_state(gtsam::Values& values_initial); 123 | 124 | // Function that will reset imu preintegration 125 | void reset_imu_integration(); 126 | 127 | // Smart feature measurements 128 | void process_feat_smart(double timestamp, std::vector leftids, std::vector leftuv); 129 | // ******************************* END TODO ********************************* // 130 | 131 | /// Members 132 | /// Config object (has all sensor noise values) 133 | Config* config; 134 | 135 | // New factors that have not been optimized yet 136 | gtsam::NonlinearFactorGraph* graph_new; 137 | 138 | // Master non-linear GTSAM graph, all created factors 139 | gtsam::NonlinearFactorGraph* graph; 140 | 141 | // New nodes that have not been optimized 142 | gtsam::Values values_new; 143 | 144 | // All created nodes 145 | gtsam::Values values_initial; 146 | 147 | // ISAM2 solvers 148 | gtsam::ISAM2* isam2; 149 | 150 | // Current ID of state and features 151 | size_t ct_state = 0; 152 | size_t ct_features = 0; 153 | 154 | /// Boolean that tracks if we have initialized 155 | bool systeminitalized = false; 156 | 157 | std::unordered_map ct_state_lookup; // ct state based on timestamp 158 | std::unordered_map timestamp_lookup; 159 | 160 | // IMU data from the sensor 161 | std::mutex imu_mutex; 162 | std::deque imu_times; 163 | std::deque imu_linaccs; 164 | std::deque imu_angvel; 165 | std::deque imu_orientation; 166 | 167 | // Imu Preintegration 168 | gtsam::PreintegratedCombinedMeasurements* preint_gtsam; 169 | 170 | /// Lookup tables for features 171 | std::mutex features_mutex; 172 | std::unordered_map measurement_smart_lookup_left; 173 | }; 174 | -------------------------------------------------------------------------------- /rviz/mvsec_test.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 389 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: PointCloud2 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Autocompute Intensity Bounds: true 57 | Autocompute Value Bounds: 58 | Max Value: 10 59 | Min Value: -10 60 | Value: true 61 | Axis: Z 62 | Channel Name: intensity 63 | Class: rviz/PointCloud2 64 | Color: 239; 41; 41 65 | Color Transformer: FlatColor 66 | Decay Time: 0 67 | Enabled: true 68 | Invert Rainbow: false 69 | Max Color: 255; 255; 255 70 | Max Intensity: 4096 71 | Min Color: 0; 0; 0 72 | Min Intensity: 0 73 | Name: PointCloud2 74 | Position Transformer: XYZ 75 | Queue Size: 10 76 | Selectable: true 77 | Size (Pixels): 3 78 | Size (m): 0.30000001192092896 79 | Style: Flat Squares 80 | Topic: /vio/feature_cloud 81 | Unreliable: false 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | - Alpha: 1 86 | Buffer Length: 1 87 | Class: rviz/Path 88 | Color: 25; 255; 0 89 | Enabled: true 90 | Head Diameter: 0.30000001192092896 91 | Head Length: 0.20000000298023224 92 | Length: 0.30000001192092896 93 | Line Style: Lines 94 | Line Width: 0.029999999329447746 95 | Name: Path 96 | Offset: 97 | X: 0 98 | Y: 0 99 | Z: 0 100 | Pose Color: 255; 85; 255 101 | Pose Style: None 102 | Radius: 0.029999999329447746 103 | Shaft Diameter: 0.10000000149011612 104 | Shaft Length: 0.10000000149011612 105 | Topic: /vio/path_imu 106 | Unreliable: false 107 | Value: true 108 | - Class: rviz/Image 109 | Enabled: true 110 | Image Topic: /feature_tracker/feature_img 111 | Max Value: 1 112 | Median window: 5 113 | Min Value: 0 114 | Name: Image 115 | Normalize Range: true 116 | Queue Size: 2 117 | Transport Hint: raw 118 | Unreliable: false 119 | Value: true 120 | Enabled: true 121 | Global Options: 122 | Background Color: 48; 48; 48 123 | Default Light: true 124 | Fixed Frame: world 125 | Frame Rate: 30 126 | Name: root 127 | Tools: 128 | - Class: rviz/Interact 129 | Hide Inactive Objects: true 130 | - Class: rviz/MoveCamera 131 | - Class: rviz/Select 132 | - Class: rviz/FocusCamera 133 | - Class: rviz/Measure 134 | - Class: rviz/SetInitialPose 135 | Theta std deviation: 0.2617993950843811 136 | Topic: /initialpose 137 | X std deviation: 0.5 138 | Y std deviation: 0.5 139 | - Class: rviz/SetGoal 140 | Topic: /move_base_simple/goal 141 | - Class: rviz/PublishPoint 142 | Single click: true 143 | Topic: /clicked_point 144 | Value: true 145 | Views: 146 | Current: 147 | Class: rviz/Orbit 148 | Distance: 242.13746643066406 149 | Enable Stereo Rendering: 150 | Stereo Eye Separation: 0.05999999865889549 151 | Stereo Focal Distance: 1 152 | Swap Stereo Eyes: false 153 | Value: false 154 | Focal Point: 155 | X: 33.12284851074219 156 | Y: -58.36363220214844 157 | Z: -7.210040092468262 158 | Focal Shape Fixed Size: true 159 | Focal Shape Size: 0.05000000074505806 160 | Invert Z Axis: false 161 | Name: Current View 162 | Near Clip Distance: 0.009999999776482582 163 | Pitch: 0.6847967505455017 164 | Target Frame: 165 | Value: Orbit (rviz) 166 | Yaw: 1.99539315700531 167 | Saved: ~ 168 | Window Geometry: 169 | Displays: 170 | collapsed: false 171 | Height: 1131 172 | Hide Left Dock: false 173 | Hide Right Dock: false 174 | Image: 175 | collapsed: false 176 | QMainWindow State: 000000ff00000000fd000000040000000000000287000003cdfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065010000003d000001b70000001600fffffffb0000000a0049006d006100670065010000003d000001a30000000000000000fb000000100044006900730070006c00610079007301000001fa00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003cdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003cd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000089b0000003efc0100000002fb0000000800540069006d006501000000000000089b000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004f9000003cd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 177 | Selection: 178 | collapsed: false 179 | Time: 180 | collapsed: false 181 | Tool Properties: 182 | collapsed: false 183 | Views: 184 | collapsed: false 185 | Width: 2203 186 | X: 330 187 | Y: 109 188 | -------------------------------------------------------------------------------- /src/camera_model/include/camodocal/camera_models/PinholeCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef PINHOLECAMERA_H 2 | #define PINHOLECAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | class PinholeCamera: public Camera 14 | { 15 | public: 16 | class Parameters: public Camera::Parameters 17 | { 18 | public: 19 | Parameters(); 20 | Parameters(const std::string& cameraName, 21 | int w, int h, 22 | double k1, double k2, double p1, double p2, 23 | double fx, double fy, double cx, double cy); 24 | 25 | double& k1(void); 26 | double& k2(void); 27 | double& p1(void); 28 | double& p2(void); 29 | double& fx(void); 30 | double& fy(void); 31 | double& cx(void); 32 | double& cy(void); 33 | 34 | double xi(void) const; 35 | double k1(void) const; 36 | double k2(void) const; 37 | double p1(void) const; 38 | double p2(void) const; 39 | double fx(void) const; 40 | double fy(void) const; 41 | double cx(void) const; 42 | double cy(void) const; 43 | 44 | bool readFromYamlFile(const std::string& filename); 45 | void writeToYamlFile(const std::string& filename) const; 46 | 47 | Parameters& operator=(const Parameters& other); 48 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 49 | 50 | private: 51 | double m_k1; 52 | double m_k2; 53 | double m_p1; 54 | double m_p2; 55 | double m_fx; 56 | double m_fy; 57 | double m_cx; 58 | double m_cy; 59 | }; 60 | 61 | PinholeCamera(); 62 | 63 | /** 64 | * \brief Constructor from the projection model parameters 65 | */ 66 | PinholeCamera(const std::string& cameraName, 67 | int imageWidth, int imageHeight, 68 | double k1, double k2, double p1, double p2, 69 | double fx, double fy, double cx, double cy); 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | PinholeCamera(const Parameters& params); 74 | 75 | Camera::ModelType modelType(void) const; 76 | const std::string& cameraName(void) const; 77 | int imageWidth(void) const; 78 | int imageHeight(void) const; 79 | 80 | void estimateIntrinsics(const cv::Size& boardSize, 81 | const std::vector< std::vector >& objectPoints, 82 | const std::vector< std::vector >& imagePoints); 83 | 84 | // Lift points from the image plane to the sphere 85 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 86 | //%output P 87 | 88 | // Lift points from the image plane to the projective space 89 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 90 | //%output P 91 | 92 | // Projects 3D points to the image plane (Pi function) 93 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 94 | //%output p 95 | 96 | // Projects 3D points to the image plane (Pi function) 97 | // and calculates jacobian 98 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 99 | Eigen::Matrix& J) const; 100 | //%output p 101 | //%output J 102 | 103 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 104 | //%output p 105 | 106 | template 107 | static void spaceToPlane(const T* const params, 108 | const T* const q, const T* const t, 109 | const Eigen::Matrix& P, 110 | Eigen::Matrix& p); 111 | 112 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 113 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 114 | Eigen::Matrix2d& J) const; 115 | 116 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 117 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 118 | float fx = -1.0f, float fy = -1.0f, 119 | cv::Size imageSize = cv::Size(0, 0), 120 | float cx = -1.0f, float cy = -1.0f, 121 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 122 | 123 | int parameterCount(void) const; 124 | 125 | const Parameters& getParameters(void) const; 126 | void setParameters(const Parameters& parameters); 127 | 128 | void readParameters(const std::vector& parameterVec); 129 | void writeParameters(std::vector& parameterVec) const; 130 | 131 | void writeParametersToYamlFile(const std::string& filename) const; 132 | 133 | std::string parametersToString(void) const; 134 | 135 | private: 136 | Parameters mParameters; 137 | 138 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 139 | bool m_noDistortion; 140 | }; 141 | 142 | typedef boost::shared_ptr PinholeCameraPtr; 143 | typedef boost::shared_ptr PinholeCameraConstPtr; 144 | 145 | template 146 | void 147 | PinholeCamera::spaceToPlane(const T* const params, 148 | const T* const q, const T* const t, 149 | const Eigen::Matrix& P, 150 | Eigen::Matrix& p) 151 | { 152 | T P_w[3]; 153 | P_w[0] = T(P(0)); 154 | P_w[1] = T(P(1)); 155 | P_w[2] = T(P(2)); 156 | 157 | // Convert quaternion from Eigen convention (x, y, z, w) 158 | // to Ceres convention (w, x, y, z) 159 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 160 | 161 | T P_c[3]; 162 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 163 | 164 | P_c[0] += t[0]; 165 | P_c[1] += t[1]; 166 | P_c[2] += t[2]; 167 | 168 | // project 3D object point to the image plane 169 | T k1 = params[0]; 170 | T k2 = params[1]; 171 | T p1 = params[2]; 172 | T p2 = params[3]; 173 | T fx = params[4]; 174 | T fy = params[5]; 175 | T alpha = T(0); //cameraParams.alpha(); 176 | T cx = params[6]; 177 | T cy = params[7]; 178 | 179 | // Transform to model plane 180 | T u = P_c[0] / P_c[2]; 181 | T v = P_c[1] / P_c[2]; 182 | 183 | T rho_sqr = u * u + v * v; 184 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 185 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 186 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 187 | 188 | u = L * u + du; 189 | v = L * v + dv; 190 | p(0) = fx * (u + alpha * v) + cx; 191 | p(1) = fy * v + cy; 192 | } 193 | 194 | } 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /src/gtsam_backend/src/GraphSolver.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "GraphSolver.h" 5 | 6 | 7 | void GraphSolver::addmeasurement_imu(double timestamp, Eigen::Vector3d linacc, Eigen::Vector3d angvel, Eigen::Vector4d orientation) { 8 | // Request access to the imu measurements 9 | std::unique_lock lock(imu_mutex); 10 | 11 | // Append this new measurement to the array 12 | imu_times.push_back(timestamp); 13 | imu_linaccs.push_back(linacc); 14 | imu_angvel.push_back(angvel); 15 | imu_orientation.push_back(orientation); 16 | 17 | } 18 | 19 | void GraphSolver::addmeasurement_uv(double timestamp, std::vector leftids, std::vector leftuv) { 20 | 21 | // Return if the node already exists in the graph 22 | if (ct_state_lookup.find(timestamp) != ct_state_lookup.end()) 23 | return; 24 | 25 | // Return if we don't actually have any plane measurements 26 | if(leftids.empty()) 27 | return; 28 | 29 | // Return if we don't actually have any IMU measurements 30 | if(imu_times.size() < 2) 31 | return; 32 | 33 | // We should try to initialize now 34 | // Or add the current a new IMU measurement and state! 35 | if(!systeminitalized) { 36 | 37 | initialize(timestamp); 38 | 39 | // Return if we have not initialized the system yet 40 | if(!systeminitalized) 41 | return; 42 | 43 | } else { 44 | 45 | // Forster2 discrete preintegration 46 | gtsam::CombinedImuFactor imuFactor = create_imu_factor(timestamp, values_initial); 47 | graph_new->add(imuFactor); 48 | graph->add(imuFactor); 49 | 50 | // Original models 51 | gtsam::State newstate = get_predicted_state(values_initial); 52 | 53 | // Move node count forward in time 54 | ct_state++; 55 | 56 | // Append to our node vectors 57 | values_new.insert( X(ct_state), newstate.pose()); 58 | values_new.insert( V(ct_state), newstate.v()); 59 | values_new.insert( B(ct_state), newstate.b()); 60 | values_initial.insert(X(ct_state), newstate.pose()); 61 | values_initial.insert(V(ct_state), newstate.v()); 62 | values_initial.insert(B(ct_state), newstate.b()); 63 | 64 | // Add ct state to map 65 | ct_state_lookup[timestamp] = ct_state; 66 | timestamp_lookup[ct_state] = timestamp; 67 | } 68 | 69 | // Assert our vectors are equal (note will need to remove top one eventually) 70 | assert(leftids.size() == leftuv.size()); 71 | 72 | // Request access 73 | std::unique_lock features_lock(features_mutex); 74 | 75 | // If we are using inverse depth, then lets call on it 76 | process_feat_smart(timestamp, leftids, leftuv); 77 | } 78 | 79 | void GraphSolver::optimize() { 80 | 81 | // Return if not initialized 82 | if(!systeminitalized && ct_state < 2) 83 | return; 84 | 85 | // Perform smoothing update 86 | try { 87 | gtsam::ISAM2Result result = isam2->update(*graph_new, values_new); 88 | values_initial = isam2->calculateEstimate(); 89 | } catch(gtsam::IndeterminantLinearSystemException &e) { 90 | ROS_ERROR("FORSTER2 gtsam indeterminate linear system exception!"); 91 | std::cerr << e.what() << std::endl; 92 | exit(EXIT_FAILURE); 93 | } 94 | 95 | // Remove the used up nodes 96 | values_new.clear(); 97 | 98 | // Remove the used up factors 99 | graph_new->resize(0); 100 | 101 | // reset imu preintegration 102 | reset_imu_integration(); 103 | } 104 | 105 | void GraphSolver::initialize(double timestamp) { 106 | 107 | // If we have already initialized, then just return 108 | if (systeminitalized) 109 | return; 110 | 111 | // Wait for enough IMU readings if we initialize from rest 112 | if (imu_times.size() < config->imuWait) 113 | return; 114 | 115 | // Reading from launch file 116 | gtsam::Vector4 q_GtoI = config->prior_qGtoI; 117 | gtsam::Vector3 p_IinG = config->prior_pIinG; 118 | gtsam::Vector3 v_IinG = config->prior_vIinG; 119 | gtsam::Vector3 ba = config->prior_ba; 120 | gtsam::Vector3 bg = config->prior_bg; 121 | 122 | // Create prior factor and add it to the graph 123 | gtsam::State prior_state = gtsam::State(gtsam::Pose3(gtsam::Quaternion(q_GtoI(3), q_GtoI(0), q_GtoI(1), q_GtoI(2)), p_IinG), 124 | v_IinG, gtsam::Bias(ba, bg)); // gtsam::Quaternion(w, x, y, z) 125 | auto pose_noise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << gtsam::Vector3::Constant(config->sigma_prior_rotation), 126 | gtsam::Vector3::Constant(config->sigma_prior_translation)).finished()); 127 | auto v_noise = gtsam::noiseModel::Isotropic::Sigma(3, config->sigma_velocity); 128 | auto b_noise = gtsam::noiseModel::Isotropic::Sigma(6, config->sigma_bias); 129 | 130 | graph_new->add(gtsam::PriorFactor( X(ct_state), prior_state.pose(), pose_noise)); 131 | graph_new->add(gtsam::PriorFactor(V(ct_state), prior_state.v(), v_noise)); 132 | graph_new->add(gtsam::PriorFactor( B(ct_state), prior_state.b(), b_noise)); 133 | graph->add(gtsam::PriorFactor( X(ct_state), prior_state.pose(), pose_noise)); 134 | graph->add(gtsam::PriorFactor( V(ct_state), prior_state.v(), v_noise)); 135 | graph->add(gtsam::PriorFactor( B(ct_state), prior_state.b(), b_noise)); 136 | 137 | // Add initial state to the graph 138 | values_new.insert( X(ct_state), prior_state.pose()); 139 | values_new.insert( V(ct_state), prior_state.v()); 140 | values_new.insert( B(ct_state), prior_state.b()); 141 | values_initial.insert(X(ct_state), prior_state.pose()); 142 | values_initial.insert(V(ct_state), prior_state.v()); 143 | values_initial.insert(B(ct_state), prior_state.b()); 144 | 145 | // Add ct state to map 146 | ct_state_lookup[timestamp] = ct_state; 147 | timestamp_lookup[ct_state] = timestamp; 148 | 149 | // Clear all old imu messages (keep the last two) 150 | imu_times.erase(imu_times.begin(), imu_times.end() - 1); 151 | imu_linaccs.erase(imu_linaccs.begin(), imu_linaccs.end() - 1); 152 | imu_angvel.erase(imu_angvel.begin(), imu_angvel.end() - 1); 153 | imu_orientation.erase(imu_orientation.begin(), imu_orientation.end() - 1); 154 | 155 | if (set_imu_preintegration(prior_state)) 156 | systeminitalized = true; 157 | 158 | // Debug info 159 | ROS_INFO("\033[0;32m[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\033[0m",q_GtoI(0),q_GtoI(1),q_GtoI(2),q_GtoI(3)); 160 | ROS_INFO("\033[0;32m[INIT]: velocity = %.4f, %.4f, %.4f\033[0m",v_IinG(0),v_IinG(1),v_IinG(2)); 161 | ROS_INFO("\033[0;32m[INIT]: position = %.4f, %.4f, %.4f\033[0m",p_IinG(0),p_IinG(1),p_IinG(2)); 162 | ROS_INFO("\033[0;32m[INIT]: bias accel = %.4f, %.4f, %.4f\033[0m",ba(0),ba(1),ba(2)); 163 | ROS_INFO("\033[0;32m[INIT]: bias gyro = %.4f, %.4f, %.4f\033[0m",bg(0),bg(1),bg(2)); 164 | } 165 | -------------------------------------------------------------------------------- /src/camera_model/src/camera_models/Camera.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/camera_models/Camera.h" 2 | #include "camodocal/camera_models/ScaramuzzaCamera.h" 3 | 4 | #include 5 | 6 | namespace camodocal 7 | { 8 | 9 | Camera::Parameters::Parameters(ModelType modelType) 10 | : m_modelType(modelType) 11 | , m_imageWidth(0) 12 | , m_imageHeight(0) 13 | { 14 | switch (modelType) 15 | { 16 | case KANNALA_BRANDT: 17 | m_nIntrinsics = 8; 18 | break; 19 | case PINHOLE: 20 | m_nIntrinsics = 8; 21 | break; 22 | case SCARAMUZZA: 23 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 24 | break; 25 | case MEI: 26 | default: 27 | m_nIntrinsics = 9; 28 | } 29 | } 30 | 31 | Camera::Parameters::Parameters(ModelType modelType, 32 | const std::string& cameraName, 33 | int w, int h) 34 | : m_modelType(modelType) 35 | , m_cameraName(cameraName) 36 | , m_imageWidth(w) 37 | , m_imageHeight(h) 38 | { 39 | switch (modelType) 40 | { 41 | case KANNALA_BRANDT: 42 | m_nIntrinsics = 8; 43 | break; 44 | case PINHOLE: 45 | m_nIntrinsics = 8; 46 | break; 47 | case SCARAMUZZA: 48 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 49 | break; 50 | case MEI: 51 | default: 52 | m_nIntrinsics = 9; 53 | } 54 | } 55 | 56 | Camera::ModelType& 57 | Camera::Parameters::modelType(void) 58 | { 59 | return m_modelType; 60 | } 61 | 62 | std::string& 63 | Camera::Parameters::cameraName(void) 64 | { 65 | return m_cameraName; 66 | } 67 | 68 | int& 69 | Camera::Parameters::imageWidth(void) 70 | { 71 | return m_imageWidth; 72 | } 73 | 74 | int& 75 | Camera::Parameters::imageHeight(void) 76 | { 77 | return m_imageHeight; 78 | } 79 | 80 | Camera::ModelType 81 | Camera::Parameters::modelType(void) const 82 | { 83 | return m_modelType; 84 | } 85 | 86 | const std::string& 87 | Camera::Parameters::cameraName(void) const 88 | { 89 | return m_cameraName; 90 | } 91 | 92 | int 93 | Camera::Parameters::imageWidth(void) const 94 | { 95 | return m_imageWidth; 96 | } 97 | 98 | int 99 | Camera::Parameters::imageHeight(void) const 100 | { 101 | return m_imageHeight; 102 | } 103 | 104 | int 105 | Camera::Parameters::nIntrinsics(void) const 106 | { 107 | return m_nIntrinsics; 108 | } 109 | 110 | cv::Mat& 111 | Camera::mask(void) 112 | { 113 | return m_mask; 114 | } 115 | 116 | const cv::Mat& 117 | Camera::mask(void) const 118 | { 119 | return m_mask; 120 | } 121 | 122 | void 123 | Camera::estimateExtrinsics(const std::vector& objectPoints, 124 | const std::vector& imagePoints, 125 | cv::Mat& rvec, cv::Mat& tvec) const 126 | { 127 | std::vector Ms(imagePoints.size()); 128 | for (size_t i = 0; i < Ms.size(); ++i) 129 | { 130 | Eigen::Vector3d P; 131 | liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); 132 | 133 | P /= P(2); 134 | 135 | Ms.at(i).x = P(0); 136 | Ms.at(i).y = P(1); 137 | } 138 | 139 | // assume unit focal length, zero principal point, and zero distortion 140 | cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); 141 | } 142 | 143 | double 144 | Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const 145 | { 146 | Eigen::Vector2d p1, p2; 147 | 148 | spaceToPlane(P1, p1); 149 | spaceToPlane(P2, p2); 150 | 151 | return (p1 - p2).norm(); 152 | } 153 | 154 | double 155 | Camera::reprojectionError(const std::vector< std::vector >& objectPoints, 156 | const std::vector< std::vector >& imagePoints, 157 | const std::vector& rvecs, 158 | const std::vector& tvecs, 159 | cv::OutputArray _perViewErrors) const 160 | { 161 | int imageCount = objectPoints.size(); 162 | size_t pointsSoFar = 0; 163 | double totalErr = 0.0; 164 | 165 | bool computePerViewErrors = _perViewErrors.needed(); 166 | cv::Mat perViewErrors; 167 | if (computePerViewErrors) 168 | { 169 | _perViewErrors.create(imageCount, 1, CV_64F); 170 | perViewErrors = _perViewErrors.getMat(); 171 | } 172 | 173 | for (int i = 0; i < imageCount; ++i) 174 | { 175 | size_t pointCount = imagePoints.at(i).size(); 176 | 177 | pointsSoFar += pointCount; 178 | 179 | std::vector estImagePoints; 180 | projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), 181 | estImagePoints); 182 | 183 | double err = 0.0; 184 | for (size_t j = 0; j < imagePoints.at(i).size(); ++j) 185 | { 186 | err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); 187 | } 188 | 189 | if (computePerViewErrors) 190 | { 191 | perViewErrors.at(i) = err / pointCount; 192 | } 193 | 194 | totalErr += err; 195 | } 196 | 197 | return totalErr / pointsSoFar; 198 | } 199 | 200 | double 201 | Camera::reprojectionError(const Eigen::Vector3d& P, 202 | const Eigen::Quaterniond& camera_q, 203 | const Eigen::Vector3d& camera_t, 204 | const Eigen::Vector2d& observed_p) const 205 | { 206 | Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; 207 | 208 | Eigen::Vector2d p; 209 | spaceToPlane(P_cam, p); 210 | 211 | return (p - observed_p).norm(); 212 | } 213 | 214 | void 215 | Camera::projectPoints(const std::vector& objectPoints, 216 | const cv::Mat& rvec, 217 | const cv::Mat& tvec, 218 | std::vector& imagePoints) const 219 | { 220 | // project 3D object points to the image plane 221 | imagePoints.reserve(objectPoints.size()); 222 | 223 | //double 224 | cv::Mat R0; 225 | cv::Rodrigues(rvec, R0); 226 | 227 | Eigen::MatrixXd R(3,3); 228 | R << R0.at(0,0), R0.at(0,1), R0.at(0,2), 229 | R0.at(1,0), R0.at(1,1), R0.at(1,2), 230 | R0.at(2,0), R0.at(2,1), R0.at(2,2); 231 | 232 | Eigen::Vector3d t; 233 | t << tvec.at(0), tvec.at(1), tvec.at(2); 234 | 235 | for (size_t i = 0; i < objectPoints.size(); ++i) 236 | { 237 | const cv::Point3f& objectPoint = objectPoints.at(i); 238 | 239 | // Rotate and translate 240 | Eigen::Vector3d P; 241 | P << objectPoint.x, objectPoint.y, objectPoint.z; 242 | 243 | P = R * P + t; 244 | 245 | Eigen::Vector2d p; 246 | spaceToPlane(P, p); 247 | 248 | imagePoints.push_back(cv::Point2f(p(0), p(1))); 249 | } 250 | } 251 | 252 | } 253 | -------------------------------------------------------------------------------- /src/camera_model/include/camodocal/camera_models/CataCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef CATACAMERA_H 2 | #define CATACAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | /** 14 | * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration 15 | * from Planar Grids, ICRA 2007 16 | */ 17 | 18 | class CataCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double xi, 28 | double k1, double k2, double p1, double p2, 29 | double gamma1, double gamma2, double u0, double v0); 30 | 31 | double& xi(void); 32 | double& k1(void); 33 | double& k2(void); 34 | double& p1(void); 35 | double& p2(void); 36 | double& gamma1(void); 37 | double& gamma2(void); 38 | double& u0(void); 39 | double& v0(void); 40 | 41 | double xi(void) const; 42 | double k1(void) const; 43 | double k2(void) const; 44 | double p1(void) const; 45 | double p2(void) const; 46 | double gamma1(void) const; 47 | double gamma2(void) const; 48 | double u0(void) const; 49 | double v0(void) const; 50 | 51 | bool readFromYamlFile(const std::string& filename); 52 | void writeToYamlFile(const std::string& filename) const; 53 | 54 | Parameters& operator=(const Parameters& other); 55 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 56 | 57 | private: 58 | double m_xi; 59 | double m_k1; 60 | double m_k2; 61 | double m_p1; 62 | double m_p2; 63 | double m_gamma1; 64 | double m_gamma2; 65 | double m_u0; 66 | double m_v0; 67 | }; 68 | 69 | CataCamera(); 70 | 71 | /** 72 | * \brief Constructor from the projection model parameters 73 | */ 74 | CataCamera(const std::string& cameraName, 75 | int imageWidth, int imageHeight, 76 | double xi, double k1, double k2, double p1, double p2, 77 | double gamma1, double gamma2, double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | CataCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 121 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 122 | Eigen::Matrix2d& J) const; 123 | 124 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 125 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 126 | float fx = -1.0f, float fy = -1.0f, 127 | cv::Size imageSize = cv::Size(0, 0), 128 | float cx = -1.0f, float cy = -1.0f, 129 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 130 | 131 | int parameterCount(void) const; 132 | 133 | const Parameters& getParameters(void) const; 134 | void setParameters(const Parameters& parameters); 135 | 136 | void readParameters(const std::vector& parameterVec); 137 | void writeParameters(std::vector& parameterVec) const; 138 | 139 | void writeParametersToYamlFile(const std::string& filename) const; 140 | 141 | std::string parametersToString(void) const; 142 | 143 | private: 144 | Parameters mParameters; 145 | 146 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 147 | bool m_noDistortion; 148 | }; 149 | 150 | typedef boost::shared_ptr CataCameraPtr; 151 | typedef boost::shared_ptr CataCameraConstPtr; 152 | 153 | template 154 | void 155 | CataCamera::spaceToPlane(const T* const params, 156 | const T* const q, const T* const t, 157 | const Eigen::Matrix& P, 158 | Eigen::Matrix& p) 159 | { 160 | T P_w[3]; 161 | P_w[0] = T(P(0)); 162 | P_w[1] = T(P(1)); 163 | P_w[2] = T(P(2)); 164 | 165 | // Convert quaternion from Eigen convention (x, y, z, w) 166 | // to Ceres convention (w, x, y, z) 167 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 168 | 169 | T P_c[3]; 170 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 171 | 172 | P_c[0] += t[0]; 173 | P_c[1] += t[1]; 174 | P_c[2] += t[2]; 175 | 176 | // project 3D object point to the image plane 177 | T xi = params[0]; 178 | T k1 = params[1]; 179 | T k2 = params[2]; 180 | T p1 = params[3]; 181 | T p2 = params[4]; 182 | T gamma1 = params[5]; 183 | T gamma2 = params[6]; 184 | T alpha = T(0); //cameraParams.alpha(); 185 | T u0 = params[7]; 186 | T v0 = params[8]; 187 | 188 | // Transform to model plane 189 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 190 | P_c[0] /= len; 191 | P_c[1] /= len; 192 | P_c[2] /= len; 193 | 194 | T u = P_c[0] / (P_c[2] + xi); 195 | T v = P_c[1] / (P_c[2] + xi); 196 | 197 | T rho_sqr = u * u + v * v; 198 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 199 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 200 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 201 | 202 | u = L * u + du; 203 | v = L * v + dv; 204 | p(0) = gamma1 * (u + alpha * v) + u0; 205 | p(1) = gamma2 * v + v0; 206 | } 207 | 208 | } 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /src/camera_model/include/camodocal/camera_models/EquidistantCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef EQUIDISTANTCAMERA_H 2 | #define EQUIDISTANTCAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | /** 14 | * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method 15 | * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 16 | */ 17 | 18 | class EquidistantCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double k2, double k3, double k4, double k5, 28 | double mu, double mv, 29 | double u0, double v0); 30 | 31 | double& k2(void); 32 | double& k3(void); 33 | double& k4(void); 34 | double& k5(void); 35 | double& mu(void); 36 | double& mv(void); 37 | double& u0(void); 38 | double& v0(void); 39 | 40 | double k2(void) const; 41 | double k3(void) const; 42 | double k4(void) const; 43 | double k5(void) const; 44 | double mu(void) const; 45 | double mv(void) const; 46 | double u0(void) const; 47 | double v0(void) const; 48 | 49 | bool readFromYamlFile(const std::string& filename); 50 | void writeToYamlFile(const std::string& filename) const; 51 | 52 | Parameters& operator=(const Parameters& other); 53 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 54 | 55 | private: 56 | // projection 57 | double m_k2; 58 | double m_k3; 59 | double m_k4; 60 | double m_k5; 61 | 62 | double m_mu; 63 | double m_mv; 64 | double m_u0; 65 | double m_v0; 66 | }; 67 | 68 | EquidistantCamera(); 69 | 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | EquidistantCamera(const std::string& cameraName, 74 | int imageWidth, int imageHeight, 75 | double k2, double k3, double k4, double k5, 76 | double mu, double mv, 77 | double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | EquidistantCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 121 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 122 | float fx = -1.0f, float fy = -1.0f, 123 | cv::Size imageSize = cv::Size(0, 0), 124 | float cx = -1.0f, float cy = -1.0f, 125 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 126 | 127 | int parameterCount(void) const; 128 | 129 | const Parameters& getParameters(void) const; 130 | void setParameters(const Parameters& parameters); 131 | 132 | void readParameters(const std::vector& parameterVec); 133 | void writeParameters(std::vector& parameterVec) const; 134 | 135 | void writeParametersToYamlFile(const std::string& filename) const; 136 | 137 | std::string parametersToString(void) const; 138 | 139 | private: 140 | template 141 | static T r(T k2, T k3, T k4, T k5, T theta); 142 | 143 | 144 | void fitOddPoly(const std::vector& x, const std::vector& y, 145 | int n, std::vector& coeffs) const; 146 | 147 | void backprojectSymmetric(const Eigen::Vector2d& p_u, 148 | double& theta, double& phi) const; 149 | 150 | Parameters mParameters; 151 | 152 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 153 | }; 154 | 155 | typedef boost::shared_ptr EquidistantCameraPtr; 156 | typedef boost::shared_ptr EquidistantCameraConstPtr; 157 | 158 | template 159 | T 160 | EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) 161 | { 162 | // k1 = 1 163 | return theta + 164 | k2 * theta * theta * theta + 165 | k3 * theta * theta * theta * theta * theta + 166 | k4 * theta * theta * theta * theta * theta * theta * theta + 167 | k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; 168 | } 169 | 170 | template 171 | void 172 | EquidistantCamera::spaceToPlane(const T* const params, 173 | const T* const q, const T* const t, 174 | const Eigen::Matrix& P, 175 | Eigen::Matrix& p) 176 | { 177 | T P_w[3]; 178 | P_w[0] = T(P(0)); 179 | P_w[1] = T(P(1)); 180 | P_w[2] = T(P(2)); 181 | 182 | // Convert quaternion from Eigen convention (x, y, z, w) 183 | // to Ceres convention (w, x, y, z) 184 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 185 | 186 | T P_c[3]; 187 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 188 | 189 | P_c[0] += t[0]; 190 | P_c[1] += t[1]; 191 | P_c[2] += t[2]; 192 | 193 | // project 3D object point to the image plane; 194 | T k2 = params[0]; 195 | T k3 = params[1]; 196 | T k4 = params[2]; 197 | T k5 = params[3]; 198 | T mu = params[4]; 199 | T mv = params[5]; 200 | T u0 = params[6]; 201 | T v0 = params[7]; 202 | 203 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 204 | T theta = acos(P_c[2] / len); 205 | T phi = atan2(P_c[1], P_c[0]); 206 | 207 | Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); 208 | 209 | p(0) = mu * p_u(0) + u0; 210 | p(1) = mv * p_u(1) + v0; 211 | } 212 | 213 | } 214 | 215 | #endif 216 | -------------------------------------------------------------------------------- /src/camera_model/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /src/feature_tracker/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /src/gtsam_backend/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gtsam_backend) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | ) 14 | 15 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 16 | 17 | find_package(PCL REQUIRED) 18 | find_package(Boost REQUIRED timer) 19 | find_package(GTSAM REQUIRED) 20 | find_package(MKL REQUIRED) 21 | find_package(TBB REQUIRED) 22 | 23 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 24 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall") 25 | SET(EIGEN_INCLUDE_DIR /usr/include/eigen3) 26 | 27 | ## System dependencies are found with CMake's conventions 28 | # find_package(Boost REQUIRED COMPONENTS system) 29 | 30 | 31 | ## Uncomment this if the package has a setup.py. This macro ensures 32 | ## modules and global scripts declared therein get installed 33 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 34 | # catkin_python_setup() 35 | 36 | ################################################ 37 | ## Declare ROS messages, services and actions ## 38 | ################################################ 39 | 40 | ## To declare and build messages, services or actions from within this 41 | ## package, follow these steps: 42 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 43 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 44 | ## * In the file package.xml: 45 | ## * add a build_depend tag for "message_generation" 46 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 47 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 48 | ## but can be declared for certainty nonetheless: 49 | ## * add a exec_depend tag for "message_runtime" 50 | ## * In this file (CMakeLists.txt): 51 | ## * add "message_generation" and every package in MSG_DEP_SET to 52 | ## find_package(catkin REQUIRED COMPONENTS ...) 53 | ## * add "message_runtime" and every package in MSG_DEP_SET to 54 | ## catkin_package(CATKIN_DEPENDS ...) 55 | ## * uncomment the add_*_files sections below as needed 56 | ## and list every .msg/.srv/.action file to be processed 57 | ## * uncomment the generate_messages entry below 58 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 59 | 60 | ## Generate messages in the 'msg' folder 61 | # add_message_files( 62 | # FILES 63 | # Message1.msg 64 | # Message2.msg 65 | # ) 66 | 67 | ## Generate services in the 'srv' folder 68 | # add_service_files( 69 | # FILES 70 | # Service1.srv 71 | # Service2.srv 72 | # ) 73 | 74 | ## Generate actions in the 'action' folder 75 | # add_action_files( 76 | # FILES 77 | # Action1.action 78 | # Action2.action 79 | # ) 80 | 81 | ## Generate added messages and services with any dependencies listed here 82 | # generate_messages( 83 | # DEPENDENCIES 84 | # std_msgs 85 | # ) 86 | 87 | ################################################ 88 | ## Declare ROS dynamic reconfigure parameters ## 89 | ################################################ 90 | 91 | ## To declare and build dynamic reconfigure parameters within this 92 | ## package, follow these steps: 93 | ## * In the file package.xml: 94 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 95 | ## * In this file (CMakeLists.txt): 96 | ## * add "dynamic_reconfigure" to 97 | ## find_package(catkin REQUIRED COMPONENTS ...) 98 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 99 | ## and list every .cfg file to be processed 100 | 101 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 102 | # generate_dynamic_reconfigure_options( 103 | # cfg/DynReconf1.cfg 104 | # cfg/DynReconf2.cfg 105 | # ) 106 | 107 | ################################### 108 | ## catkin specific configuration ## 109 | ################################### 110 | ## The catkin_package macro generates cmake config files for your package 111 | ## Declare things to be passed to dependent projects 112 | ## INCLUDE_DIRS: uncomment this if your package contains header files 113 | ## LIBRARIES: libraries you create in this project that dependent projects also need 114 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 115 | ## DEPENDS: system dependencies of this project that dependent projects also need 116 | catkin_package( 117 | # INCLUDE_DIRS include 118 | # LIBRARIES gtsam_backend 119 | # CATKIN_DEPENDS roscpp std_msgs 120 | # DEPENDS system_lib 121 | ) 122 | 123 | ########### 124 | ## Build ## 125 | ########### 126 | 127 | ## Specify additional locations of header files 128 | ## Your package locations should be listed before other locations 129 | include_directories( 130 | include/gtsam_backend 131 | ${catkin_INCLUDE_DIRS} 132 | ${EIGEN_INCLUDE_DIR} 133 | ${PCL_INCLUDE_DIRS} 134 | ${MKL_INCLUDE_DIRS} 135 | ) 136 | 137 | ## Declare a C++ library 138 | # add_library(${PROJECT_NAME} 139 | # src/${PROJECT_NAME}/gtsam_backend.cpp 140 | # ) 141 | 142 | ## Add cmake target dependencies of the library 143 | ## as an example, code may need to be generated before libraries 144 | ## either from message generation or dynamic reconfigure 145 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Declare a C++ executable 148 | ## With catkin_make all packages are built within a single CMake context 149 | ## The recommended prefix ensures that target names across packages don't collide 150 | add_executable(vio 151 | src/main_estimator.cpp 152 | src/GraphSolver.cpp 153 | src/GraphSolver_IMU.cpp 154 | src/GraphSolver_FEAT.cpp 155 | ) 156 | 157 | ## Rename C++ executable without prefix 158 | ## The above recommended prefix causes long target names, the following renames the 159 | ## target back to the shorter version for ease of user use 160 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 161 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 162 | 163 | ## Add cmake target dependencies of the executable 164 | ## same as for the library above 165 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 166 | 167 | ## Specify libraries to link a library or executable target against 168 | target_link_libraries(vio 169 | ${catkin_LIBRARIES} 170 | ${MKL_LIBRARIES} 171 | tbb 172 | tbbmalloc 173 | gtsam 174 | gtsam_unstable 175 | ) 176 | 177 | ############# 178 | ## Install ## 179 | ############# 180 | 181 | # all install targets should use catkin DESTINATION variables 182 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 183 | 184 | ## Mark executable scripts (Python etc.) for installation 185 | ## in contrast to setup.py, you can choose the destination 186 | # install(PROGRAMS 187 | # scripts/my_python_script 188 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark executables for installation 192 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 193 | # install(TARGETS ${PROJECT_NAME}_node 194 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 195 | # ) 196 | 197 | ## Mark libraries for installation 198 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 199 | # install(TARGETS ${PROJECT_NAME} 200 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 201 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 202 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 203 | # ) 204 | 205 | ## Mark cpp header files for installation 206 | # install(DIRECTORY include/${PROJECT_NAME}/ 207 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 208 | # FILES_MATCHING PATTERN "*.h" 209 | # PATTERN ".svn" EXCLUDE 210 | # ) 211 | 212 | ## Mark other files for installation (e.g. launch and bag files, etc.) 213 | # install(FILES 214 | # # myfile1 215 | # # myfile2 216 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 217 | # ) 218 | 219 | ############# 220 | ## Testing ## 221 | ############# 222 | 223 | ## Add gtest based cpp test target and link libraries 224 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_gtsam_backend.cpp) 225 | # if(TARGET ${PROJECT_NAME}-test) 226 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 227 | # endif() 228 | 229 | ## Add folders to be run by python nosetests 230 | # catkin_add_nosetests(test) 231 | -------------------------------------------------------------------------------- /src/gtsam_backend/cmake/FindMKL.cmake: -------------------------------------------------------------------------------- 1 | # This file is adapted from the one in OpenMEEG: http://www-sop.inria.fr/athena/software/OpenMEEG/ 2 | # - Try to find the Intel Math Kernel Library 3 | # Once done this will define 4 | # 5 | # MKL_FOUND - system has MKL 6 | # MKL_ROOT_DIR - path to the MKL base directory 7 | # MKL_INCLUDE_DIR - the MKL include directory 8 | # MKL_LIBRARIES - MKL libraries 9 | # 10 | # There are few sets of libraries: 11 | # Array indexes modes: 12 | # LP - 32 bit indexes of arrays 13 | # ILP - 64 bit indexes of arrays 14 | # Threading: 15 | # SEQUENTIAL - no threading 16 | # INTEL - Intel threading library 17 | # GNU - GNU threading library 18 | # MPI support 19 | # NOMPI - no MPI support 20 | # INTEL - Intel MPI library 21 | # OPEN - Open MPI library 22 | # SGI - SGI MPT Library 23 | 24 | # linux 25 | IF(UNIX AND NOT APPLE) 26 | IF(${CMAKE_HOST_SYSTEM_PROCESSOR} STREQUAL "x86_64") 27 | SET(MKL_ARCH_DIR "intel64") 28 | ELSE() 29 | SET(MKL_ARCH_DIR "32") 30 | ENDIF() 31 | ENDIF() 32 | # macos 33 | IF(APPLE) 34 | SET(MKL_ARCH_DIR "intel64") 35 | ENDIF() 36 | 37 | IF(FORCE_BUILD_32BITS) 38 | set(MKL_ARCH_DIR "32") 39 | ENDIF() 40 | # windows 41 | IF(WIN32) 42 | IF(${CMAKE_SIZEOF_VOID_P} EQUAL 8) 43 | SET(MKL_ARCH_DIR "intel64") 44 | ELSE() 45 | SET(MKL_ARCH_DIR "ia32") 46 | ENDIF() 47 | ENDIF() 48 | 49 | SET(MKL_THREAD_VARIANTS SEQUENTIAL GNUTHREAD INTELTHREAD) 50 | SET(MKL_MODE_VARIANTS ILP LP) 51 | SET(MKL_MPI_VARIANTS NOMPI INTELMPI OPENMPI SGIMPT) 52 | 53 | FIND_PATH(MKL_ROOT_DIR 54 | include/mkl_cblas.h 55 | PATHS 56 | $ENV{MKLDIR} 57 | /opt/intel/mkl/ 58 | /opt/intel/mkl/*/ 59 | /opt/intel/cmkl/ 60 | /opt/intel/cmkl/*/ 61 | /opt/intel/*/mkl/ 62 | /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal 63 | "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl" 64 | "C:/Program Files (x86)/Intel/Composer XE 2013/mkl" 65 | "C:/Program Files/Intel/MKL/*/" 66 | "C:/Program Files/Intel/ComposerXE-2011/mkl" 67 | "C:/Program Files/Intel/Composer XE 2013/mkl" 68 | ) 69 | 70 | FIND_PATH(MKL_INCLUDE_DIR 71 | mkl_cblas.h 72 | PATHS 73 | ${MKL_ROOT_DIR}/include 74 | ${INCLUDE_INSTALL_DIR} 75 | ) 76 | 77 | FIND_PATH(MKL_FFTW_INCLUDE_DIR 78 | fftw3.h 79 | PATH_SUFFIXES fftw 80 | PATHS 81 | ${MKL_ROOT_DIR}/include 82 | ${INCLUDE_INSTALL_DIR} 83 | NO_DEFAULT_PATH 84 | ) 85 | 86 | IF(WIN32) 87 | SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}") 88 | 89 | IF (MKL_INCLUDE_DIR MATCHES "10.") 90 | IF(CMAKE_CL_64) 91 | SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64) 92 | ELSE() 93 | SET(MKL_LIBS mkl_solver mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libguide mkl_lapack95 mkl_blas95) 94 | ENDIF() 95 | ELSEIF(MKL_INCLUDE_DIR MATCHES "2013") # version 11 ... 96 | IF(CMAKE_CL_64) 97 | SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_intel_thread libiomp5md mkl_lapack95_lp64 mkl_blas95_lp64) 98 | ELSE() 99 | SET(MKL_LIBS mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libiomp5md mkl_lapack95 mkl_blas95) 100 | ENDIF() 101 | ELSE() # old MKL 9 102 | SET(MKL_LIBS mkl_solver mkl_c libguide mkl_lapack mkl_ia32) 103 | ENDIF() 104 | 105 | IF (MKL_INCLUDE_DIR MATCHES "10.3") 106 | SET(MKL_LIBS ${MKL_LIBS} libiomp5md) 107 | ENDIF() 108 | 109 | FOREACH (LIB ${MKL_LIBS}) 110 | FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH) 111 | IF(${LIB}_PATH) 112 | SET(MKL_LIBRARIES ${MKL_LIBRARIES} ${${LIB}_PATH}) 113 | ELSE() 114 | MESSAGE(STATUS "Could not find ${LIB}: disabling MKL") 115 | ENDIF() 116 | ENDFOREACH() 117 | SET(MKL_FOUND ON) 118 | ELSE() # UNIX and macOS 119 | FIND_LIBRARY(MKL_CORE_LIBRARY 120 | mkl_core 121 | PATHS 122 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 123 | ${MKL_ROOT_DIR}/lib/ 124 | ) 125 | 126 | # Threading libraries 127 | FIND_LIBRARY(MKL_SEQUENTIAL_LIBRARY 128 | mkl_sequential 129 | PATHS 130 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 131 | ${MKL_ROOT_DIR}/lib/ 132 | ) 133 | 134 | FIND_LIBRARY(MKL_INTELTHREAD_LIBRARY 135 | mkl_intel_thread 136 | PATHS 137 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 138 | ${MKL_ROOT_DIR}/lib/ 139 | ) 140 | 141 | # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above) 142 | IF(NOT APPLE) 143 | FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY 144 | mkl_gnu_thread 145 | PATHS 146 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 147 | ${MKL_ROOT_DIR}/lib/ 148 | ) 149 | ENDIF() 150 | 151 | # Intel Libraries 152 | IF("${MKL_ARCH_DIR}" STREQUAL "32") 153 | FIND_LIBRARY(MKL_LP_LIBRARY 154 | mkl_intel 155 | PATHS 156 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 157 | ${MKL_ROOT_DIR}/lib/ 158 | ) 159 | 160 | FIND_LIBRARY(MKL_ILP_LIBRARY 161 | mkl_intel 162 | PATHS 163 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 164 | ${MKL_ROOT_DIR}/lib/ 165 | ) 166 | else() 167 | FIND_LIBRARY(MKL_LP_LIBRARY 168 | mkl_intel_lp64 169 | PATHS 170 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 171 | ${MKL_ROOT_DIR}/lib/ 172 | ) 173 | 174 | FIND_LIBRARY(MKL_ILP_LIBRARY 175 | mkl_intel_ilp64 176 | PATHS 177 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 178 | ${MKL_ROOT_DIR}/lib/ 179 | ) 180 | ENDIF() 181 | 182 | # Lapack 183 | FIND_LIBRARY(MKL_LAPACK_LIBRARY 184 | mkl_lapack 185 | PATHS 186 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 187 | ${MKL_ROOT_DIR}/lib/ 188 | ) 189 | 190 | IF(NOT MKL_LAPACK_LIBRARY) 191 | FIND_LIBRARY(MKL_LAPACK_LIBRARY 192 | mkl_lapack95_lp64 193 | PATHS 194 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 195 | ${MKL_ROOT_DIR}/lib/ 196 | ) 197 | ENDIF() 198 | 199 | # iomp5 200 | IF("${MKL_ARCH_DIR}" STREQUAL "32") 201 | IF(UNIX AND NOT APPLE) 202 | FIND_LIBRARY(MKL_IOMP5_LIBRARY 203 | iomp5 204 | PATHS 205 | ${MKL_ROOT_DIR}/../lib/ia32 206 | ) 207 | ELSE() 208 | SET(MKL_IOMP5_LIBRARY "") # no need for mac 209 | ENDIF() 210 | else() 211 | IF(UNIX AND NOT APPLE) 212 | FIND_LIBRARY(MKL_IOMP5_LIBRARY 213 | iomp5 214 | PATHS 215 | ${MKL_ROOT_DIR}/../lib/intel64 216 | ) 217 | ELSE() 218 | SET(MKL_IOMP5_LIBRARY "") # no need for mac 219 | ENDIF() 220 | ENDIF() 221 | 222 | foreach (MODEVAR ${MKL_MODE_VARIANTS}) 223 | foreach (THREADVAR ${MKL_THREAD_VARIANTS}) 224 | if (MKL_CORE_LIBRARY AND MKL_${MODEVAR}_LIBRARY AND MKL_${THREADVAR}_LIBRARY) 225 | set(MKL_${MODEVAR}_${THREADVAR}_LIBRARIES 226 | ${MKL_${MODEVAR}_LIBRARY} ${MKL_${THREADVAR}_LIBRARY} ${MKL_CORE_LIBRARY} 227 | ${MKL_LAPACK_LIBRARY} ${MKL_IOMP5_LIBRARY}) 228 | # message("${MODEVAR} ${THREADVAR} ${MKL_${MODEVAR}_${THREADVAR}_LIBRARIES}") # for debug 229 | endif() 230 | endforeach() 231 | endforeach() 232 | 233 | IF(APPLE) 234 | SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES}) 235 | ELSE() 236 | SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) 237 | ENDIF() 238 | 239 | MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY 240 | MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY) 241 | ENDIF() 242 | 243 | INCLUDE(FindPackageHandleStandardArgs) 244 | find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES) 245 | 246 | #if(MKL_FOUND) 247 | # LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack 248 | #endif() 249 | 250 | MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES) 251 | -------------------------------------------------------------------------------- /src/camera_model/src/intrinsic_calib.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "camodocal/chessboard/Chessboard.h" 12 | #include "camodocal/calib/CameraCalibration.h" 13 | #include "camodocal/gpl/gpl.h" 14 | 15 | int main(int argc, char** argv) 16 | { 17 | cv::Size boardSize; 18 | float squareSize; 19 | std::string inputDir; 20 | std::string cameraModel; 21 | std::string cameraName; 22 | std::string prefix; 23 | std::string fileExtension; 24 | bool useOpenCV; 25 | bool viewResults; 26 | bool verbose; 27 | 28 | //========= Handling Program options ========= 29 | boost::program_options::options_description desc("Allowed options"); 30 | desc.add_options() 31 | ("help", "produce help message") 32 | ("width,w", boost::program_options::value(&boardSize.width)->default_value(8), "Number of inner corners on the chessboard pattern in x direction") 33 | ("height,h", boost::program_options::value(&boardSize.height)->default_value(12), "Number of inner corners on the chessboard pattern in y direction") 34 | ("size,s", boost::program_options::value(&squareSize)->default_value(7.f), "Size of one square in mm") 35 | ("input,i", boost::program_options::value(&inputDir)->default_value("calibrationdata"), "Input directory containing chessboard images") 36 | ("prefix,p", boost::program_options::value(&prefix)->default_value("left-"), "Prefix of images") 37 | ("file-extension,e", boost::program_options::value(&fileExtension)->default_value(".png"), "File extension of images") 38 | ("camera-model", boost::program_options::value(&cameraModel)->default_value("mei"), "Camera model: kannala-brandt | mei | pinhole") 39 | ("camera-name", boost::program_options::value(&cameraName)->default_value("camera"), "Name of camera") 40 | ("opencv", boost::program_options::bool_switch(&useOpenCV)->default_value(true), "Use OpenCV to detect corners") 41 | ("view-results", boost::program_options::bool_switch(&viewResults)->default_value(false), "View results") 42 | ("verbose,v", boost::program_options::bool_switch(&verbose)->default_value(true), "Verbose output") 43 | ; 44 | 45 | boost::program_options::positional_options_description pdesc; 46 | pdesc.add("input", 1); 47 | 48 | boost::program_options::variables_map vm; 49 | boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm); 50 | boost::program_options::notify(vm); 51 | 52 | if (vm.count("help")) 53 | { 54 | std::cout << desc << std::endl; 55 | return 1; 56 | } 57 | 58 | if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir)) 59 | { 60 | std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl; 61 | return 1; 62 | } 63 | 64 | camodocal::Camera::ModelType modelType; 65 | if (boost::iequals(cameraModel, "kannala-brandt")) 66 | { 67 | modelType = camodocal::Camera::KANNALA_BRANDT; 68 | } 69 | else if (boost::iequals(cameraModel, "mei")) 70 | { 71 | modelType = camodocal::Camera::MEI; 72 | } 73 | else if (boost::iequals(cameraModel, "pinhole")) 74 | { 75 | modelType = camodocal::Camera::PINHOLE; 76 | } 77 | else if (boost::iequals(cameraModel, "scaramuzza")) 78 | { 79 | modelType = camodocal::Camera::SCARAMUZZA; 80 | } 81 | else 82 | { 83 | std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl; 84 | return 1; 85 | } 86 | 87 | switch (modelType) 88 | { 89 | case camodocal::Camera::KANNALA_BRANDT: 90 | std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl; 91 | break; 92 | case camodocal::Camera::MEI: 93 | std::cout << "# INFO: Camera model: Mei" << std::endl; 94 | break; 95 | case camodocal::Camera::PINHOLE: 96 | std::cout << "# INFO: Camera model: Pinhole" << std::endl; 97 | break; 98 | case camodocal::Camera::SCARAMUZZA: 99 | std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl; 100 | break; 101 | } 102 | 103 | // look for images in input directory 104 | std::vector imageFilenames; 105 | boost::filesystem::directory_iterator itr; 106 | for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr) 107 | { 108 | if (!boost::filesystem::is_regular_file(itr->status())) 109 | { 110 | continue; 111 | } 112 | 113 | std::string filename = itr->path().filename().string(); 114 | 115 | // check if prefix matches 116 | if (!prefix.empty()) 117 | { 118 | if (filename.compare(0, prefix.length(), prefix) != 0) 119 | { 120 | continue; 121 | } 122 | } 123 | 124 | // check if file extension matches 125 | if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0) 126 | { 127 | continue; 128 | } 129 | 130 | imageFilenames.push_back(itr->path().string()); 131 | 132 | if (verbose) 133 | { 134 | std::cerr << "# INFO: Adding " << imageFilenames.back() << std::endl; 135 | } 136 | } 137 | 138 | if (imageFilenames.empty()) 139 | { 140 | std::cerr << "# ERROR: No chessboard images found." << std::endl; 141 | return 1; 142 | } 143 | 144 | if (verbose) 145 | { 146 | std::cerr << "# INFO: # images: " << imageFilenames.size() << std::endl; 147 | } 148 | 149 | std::sort(imageFilenames.begin(), imageFilenames.end()); 150 | 151 | cv::Mat image = cv::imread(imageFilenames.front(), -1); 152 | const cv::Size frameSize = image.size(); 153 | 154 | camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize); 155 | calibration.setVerbose(verbose); 156 | 157 | std::vector chessboardFound(imageFilenames.size(), false); 158 | for (size_t i = 0; i < imageFilenames.size(); ++i) 159 | { 160 | image = cv::imread(imageFilenames.at(i), -1); 161 | 162 | camodocal::Chessboard chessboard(boardSize, image); 163 | 164 | chessboard.findCorners(useOpenCV); 165 | if (chessboard.cornersFound()) 166 | { 167 | if (verbose) 168 | { 169 | std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", " << imageFilenames.at(i) << std::endl; 170 | } 171 | 172 | calibration.addChessboardData(chessboard.getCorners()); 173 | 174 | cv::Mat sketch; 175 | chessboard.getSketch().copyTo(sketch); 176 | 177 | cv::imshow("Image", sketch); 178 | cv::waitKey(50); 179 | } 180 | else if (verbose) 181 | { 182 | std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl; 183 | } 184 | chessboardFound.at(i) = chessboard.cornersFound(); 185 | } 186 | cv::destroyWindow("Image"); 187 | 188 | if (calibration.sampleCount() < 10) 189 | { 190 | std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl; 191 | return 1; 192 | } 193 | 194 | if (verbose) 195 | { 196 | std::cerr << "# INFO: Calibrating..." << std::endl; 197 | } 198 | 199 | double startTime = camodocal::timeInSeconds(); 200 | 201 | calibration.calibrate(); 202 | calibration.writeParams(cameraName + "_camera_calib.yaml"); 203 | calibration.writeChessboardData(cameraName + "_chessboard_data.dat"); 204 | 205 | if (verbose) 206 | { 207 | std::cout << "# INFO: Calibration took a total time of " 208 | << std::fixed << std::setprecision(3) << camodocal::timeInSeconds() - startTime 209 | << " sec.\n"; 210 | } 211 | 212 | if (verbose) 213 | { 214 | std::cerr << "# INFO: Wrote calibration file to " << cameraName + "_camera_calib.yaml" << std::endl; 215 | } 216 | 217 | if (viewResults) 218 | { 219 | std::vector cbImages; 220 | std::vector cbImageFilenames; 221 | 222 | for (size_t i = 0; i < imageFilenames.size(); ++i) 223 | { 224 | if (!chessboardFound.at(i)) 225 | { 226 | continue; 227 | } 228 | 229 | cbImages.push_back(cv::imread(imageFilenames.at(i), -1)); 230 | cbImageFilenames.push_back(imageFilenames.at(i)); 231 | } 232 | 233 | // visualize observed and reprojected points 234 | calibration.drawResults(cbImages); 235 | 236 | for (size_t i = 0; i < cbImages.size(); ++i) 237 | { 238 | cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20), 239 | cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 240 | 1, CV_AA); 241 | cv::imshow("Image", cbImages.at(i)); 242 | cv::waitKey(0); 243 | } 244 | } 245 | 246 | return 0; 247 | } 248 | -------------------------------------------------------------------------------- /src/feature_tracker/src/feature_tracker_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "feature_tracker.h" 11 | 12 | #define SHOW_UNDISTORTION 0 13 | 14 | vector r_status; 15 | vector r_err; 16 | queue img_buf; 17 | 18 | ros::Publisher pub_img,pub_match; 19 | ros::Publisher pub_restart; 20 | 21 | FeatureTracker trackerData[NUM_OF_CAM]; 22 | double first_image_time; 23 | int pub_count = 1; 24 | bool first_image_flag = true; 25 | double last_image_time = 0; 26 | bool init_pub = 0; 27 | 28 | void img_callback(const sensor_msgs::ImageConstPtr &img_msg) 29 | { 30 | if(first_image_flag) 31 | { 32 | first_image_flag = false; 33 | first_image_time = img_msg->header.stamp.toSec(); 34 | last_image_time = img_msg->header.stamp.toSec(); 35 | return; 36 | } 37 | // detect unstable camera stream 38 | if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time) 39 | { 40 | ROS_WARN("image discontinue! reset the feature tracker!"); 41 | first_image_flag = true; 42 | last_image_time = 0; 43 | pub_count = 1; 44 | std_msgs::Bool restart_flag; 45 | restart_flag.data = true; 46 | pub_restart.publish(restart_flag); 47 | return; 48 | } 49 | last_image_time = img_msg->header.stamp.toSec(); 50 | // frequency control 51 | if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ) 52 | { 53 | PUB_THIS_FRAME = true; 54 | // reset the frequency control 55 | if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ) 56 | { 57 | first_image_time = img_msg->header.stamp.toSec(); 58 | pub_count = 0; 59 | } 60 | } 61 | else 62 | PUB_THIS_FRAME = false; 63 | 64 | cv_bridge::CvImageConstPtr ptr; 65 | if (img_msg->encoding == "8UC1") 66 | { 67 | sensor_msgs::Image img; 68 | img.header = img_msg->header; 69 | img.height = img_msg->height; 70 | img.width = img_msg->width; 71 | img.is_bigendian = img_msg->is_bigendian; 72 | img.step = img_msg->step; 73 | img.data = img_msg->data; 74 | img.encoding = "mono8"; 75 | ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); 76 | } 77 | else 78 | ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); 79 | 80 | cv::Mat show_img = ptr->image; 81 | TicToc t_r; 82 | for (int i = 0; i < NUM_OF_CAM; i++) 83 | { 84 | ROS_DEBUG("processing camera %d", i); 85 | if (i != 1 || !STEREO_TRACK) 86 | trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec()); 87 | else 88 | { 89 | if (EQUALIZE) 90 | { 91 | cv::Ptr clahe = cv::createCLAHE(); 92 | clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img); 93 | } 94 | else 95 | trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1)); 96 | } 97 | 98 | #if SHOW_UNDISTORTION 99 | trackerData[i].showUndistortion("undistrotion_" + std::to_string(i)); 100 | #endif 101 | } 102 | 103 | for (unsigned int i = 0;; i++) 104 | { 105 | bool completed = false; 106 | for (int j = 0; j < NUM_OF_CAM; j++) 107 | if (j != 1 || !STEREO_TRACK) 108 | completed |= trackerData[j].updateID(i); 109 | if (!completed) 110 | break; 111 | } 112 | 113 | if (PUB_THIS_FRAME) 114 | { 115 | pub_count++; 116 | sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud); 117 | sensor_msgs::ChannelFloat32 id_of_point; 118 | sensor_msgs::ChannelFloat32 u_of_point; 119 | sensor_msgs::ChannelFloat32 v_of_point; 120 | sensor_msgs::ChannelFloat32 velocity_x_of_point; 121 | sensor_msgs::ChannelFloat32 velocity_y_of_point; 122 | 123 | feature_points->header = img_msg->header; 124 | feature_points->header.frame_id = "world"; 125 | 126 | vector> hash_ids(NUM_OF_CAM); 127 | for (int i = 0; i < NUM_OF_CAM; i++) 128 | { 129 | auto &un_pts = trackerData[i].cur_un_pts; 130 | auto &cur_pts = trackerData[i].cur_pts; 131 | auto &ids = trackerData[i].ids; 132 | auto &pts_velocity = trackerData[i].pts_velocity; 133 | for (unsigned int j = 0; j < ids.size(); j++) 134 | { 135 | if (trackerData[i].track_cnt[j] > 1) 136 | { 137 | int p_id = ids[j]; 138 | hash_ids[i].insert(p_id); 139 | geometry_msgs::Point32 p; 140 | p.x = un_pts[j].x; 141 | p.y = un_pts[j].y; 142 | p.z = 1; 143 | 144 | feature_points->points.push_back(p); 145 | id_of_point.values.push_back(p_id * NUM_OF_CAM + i); 146 | u_of_point.values.push_back(cur_pts[j].x); 147 | v_of_point.values.push_back(cur_pts[j].y); 148 | velocity_x_of_point.values.push_back(pts_velocity[j].x); 149 | velocity_y_of_point.values.push_back(pts_velocity[j].y); 150 | } 151 | } 152 | } 153 | feature_points->channels.push_back(id_of_point); 154 | feature_points->channels.push_back(u_of_point); 155 | feature_points->channels.push_back(v_of_point); 156 | feature_points->channels.push_back(velocity_x_of_point); 157 | feature_points->channels.push_back(velocity_y_of_point); 158 | ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec()); 159 | // skip the first image; since no optical speed on frist image 160 | if (!init_pub) 161 | { 162 | init_pub = 1; 163 | } 164 | else 165 | pub_img.publish(feature_points); 166 | 167 | if (SHOW_TRACK) 168 | { 169 | ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8); 170 | //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3); 171 | cv::Mat stereo_img = ptr->image; 172 | 173 | for (int i = 0; i < NUM_OF_CAM; i++) 174 | { 175 | cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW); 176 | cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB); 177 | 178 | for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++) 179 | { 180 | double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE); 181 | cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2); 182 | //draw speed line 183 | /* 184 | Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y); 185 | Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y); 186 | Vector3d tmp_prev_un_pts; 187 | tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity; 188 | tmp_prev_un_pts.z() = 1; 189 | Vector2d tmp_prev_uv; 190 | trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv); 191 | cv::line(tmp_img, trackerData[i].cur_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0); 192 | */ 193 | //char name[10]; 194 | //sprintf(name, "%d", trackerData[i].ids[j]); 195 | //cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); 196 | } 197 | } 198 | //cv::imshow("vis", stereo_img); 199 | //cv::waitKey(5); 200 | pub_match.publish(ptr->toImageMsg()); 201 | } 202 | } 203 | ROS_INFO("whole feature tracker processing costs: %f", t_r.toc()); 204 | } 205 | 206 | int main(int argc, char **argv) 207 | { 208 | ros::init(argc, argv, "feature_tracker"); 209 | ros::NodeHandle n("~"); 210 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 211 | readParameters(n); 212 | 213 | for (int i = 0; i < NUM_OF_CAM; i++) 214 | trackerData[i].readIntrinsicParameter(CAM_NAMES[i]); 215 | 216 | if(FISHEYE) 217 | { 218 | for (int i = 0; i < NUM_OF_CAM; i++) 219 | { 220 | trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0); 221 | if(!trackerData[i].fisheye_mask.data) 222 | { 223 | ROS_INFO("load mask fail"); 224 | ROS_BREAK(); 225 | } 226 | else 227 | ROS_INFO("load mask success"); 228 | } 229 | } 230 | 231 | ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback); 232 | 233 | pub_img = n.advertise("feature", 1000); 234 | pub_match = n.advertise("feature_img",1000); 235 | pub_restart = n.advertise("restart",1000); 236 | /* 237 | if (SHOW_TRACK) 238 | cv::namedWindow("vis", cv::WINDOW_NORMAL); 239 | */ 240 | ros::spin(); 241 | return 0; 242 | } 243 | 244 | 245 | // new points velocity is 0, pub or not? 246 | // track cnt > 1 pub? -------------------------------------------------------------------------------- /src/camera_model/include/camodocal/chessboard/Spline.h: -------------------------------------------------------------------------------- 1 | /* dynamo:- Event driven molecular dynamics simulator 2 | http://www.marcusbannerman.co.uk/dynamo 3 | Copyright (C) 2011 Marcus N Campbell Bannerman 4 | 5 | This program is free software: you can redistribute it and/or 6 | modify it under the terms of the GNU General Public License 7 | version 3 as published by the Free Software Foundation. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace ublas = boost::numeric::ublas; 28 | 29 | class Spline : private std::vector > 30 | { 31 | public: 32 | //The boundary conditions available 33 | enum BC_type { 34 | FIXED_1ST_DERIV_BC, 35 | FIXED_2ND_DERIV_BC, 36 | PARABOLIC_RUNOUT_BC 37 | }; 38 | 39 | enum Spline_type { 40 | LINEAR, 41 | CUBIC 42 | }; 43 | 44 | //Constructor takes the boundary conditions as arguments, this 45 | //sets the first derivative (gradient) at the lower and upper 46 | //end points 47 | Spline(): 48 | _valid(false), 49 | _BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC), 50 | _BCLowVal(0), _BCHighVal(0), 51 | _type(CUBIC) 52 | {} 53 | 54 | typedef std::vector > base; 55 | typedef base::const_iterator const_iterator; 56 | 57 | //Standard STL read-only container stuff 58 | const_iterator begin() const { return base::begin(); } 59 | const_iterator end() const { return base::end(); } 60 | void clear() { _valid = false; base::clear(); _data.clear(); } 61 | size_t size() const { return base::size(); } 62 | size_t max_size() const { return base::max_size(); } 63 | size_t capacity() const { return base::capacity(); } 64 | bool empty() const { return base::empty(); } 65 | 66 | //Add a point to the spline, and invalidate it so its 67 | //recalculated on the next access 68 | inline void addPoint(double x, double y) 69 | { 70 | _valid = false; 71 | base::push_back(std::pair(x,y)); 72 | } 73 | 74 | //Reset the boundary conditions 75 | inline void setLowBC(BC_type BC, double val = 0) 76 | { _BCLow = BC; _BCLowVal = val; _valid = false; } 77 | 78 | inline void setHighBC(BC_type BC, double val = 0) 79 | { _BCHigh = BC; _BCHighVal = val; _valid = false; } 80 | 81 | void setType(Spline_type type) { _type = type; _valid = false; } 82 | 83 | //Check if the spline has been calculated, then generate the 84 | //spline interpolated value 85 | double operator()(double xval) 86 | { 87 | if (!_valid) generate(); 88 | 89 | //Special cases when we're outside the range of the spline points 90 | if (xval <= x(0)) return lowCalc(xval); 91 | if (xval >= x(size()-1)) return highCalc(xval); 92 | 93 | //Check all intervals except the last one 94 | for (std::vector::const_iterator iPtr = _data.begin(); 95 | iPtr != _data.end()-1; ++iPtr) 96 | if ((xval >= iPtr->x) && (xval <= (iPtr+1)->x)) 97 | return splineCalc(iPtr, xval); 98 | 99 | return splineCalc(_data.end() - 1, xval); 100 | } 101 | 102 | private: 103 | 104 | ///////PRIVATE DATA MEMBERS 105 | struct SplineData { double x,a,b,c,d; }; 106 | //vector of calculated spline data 107 | std::vector _data; 108 | //Second derivative at each point 109 | ublas::vector _ddy; 110 | //Tracks whether the spline parameters have been calculated for 111 | //the current set of points 112 | bool _valid; 113 | //The boundary conditions 114 | BC_type _BCLow, _BCHigh; 115 | //The values of the boundary conditions 116 | double _BCLowVal, _BCHighVal; 117 | 118 | Spline_type _type; 119 | 120 | ///////PRIVATE FUNCTIONS 121 | //Function to calculate the value of a given spline at a point xval 122 | inline double splineCalc(std::vector::const_iterator i, double xval) 123 | { 124 | const double lx = xval - i->x; 125 | return ((i->a * lx + i->b) * lx + i->c) * lx + i->d; 126 | } 127 | 128 | inline double lowCalc(double xval) 129 | { 130 | const double lx = xval - x(0); 131 | 132 | if (_type == LINEAR) 133 | return lx * _BCHighVal + y(0); 134 | 135 | const double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6; 136 | 137 | switch(_BCLow) 138 | { 139 | case FIXED_1ST_DERIV_BC: 140 | return lx * _BCLowVal + y(0); 141 | case FIXED_2ND_DERIV_BC: 142 | return lx * lx * _BCLowVal + firstDeriv * lx + y(0); 143 | case PARABOLIC_RUNOUT_BC: 144 | return lx * lx * _ddy[0] + lx * firstDeriv + y(0); 145 | } 146 | throw std::runtime_error("Unknown BC"); 147 | } 148 | 149 | inline double highCalc(double xval) 150 | { 151 | const double lx = xval - x(size() - 1); 152 | 153 | if (_type == LINEAR) 154 | return lx * _BCHighVal + y(size() - 1); 155 | 156 | const double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2); 157 | 158 | switch(_BCHigh) 159 | { 160 | case FIXED_1ST_DERIV_BC: 161 | return lx * _BCHighVal + y(size() - 1); 162 | case FIXED_2ND_DERIV_BC: 163 | return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1); 164 | case PARABOLIC_RUNOUT_BC: 165 | return lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1); 166 | } 167 | throw std::runtime_error("Unknown BC"); 168 | } 169 | 170 | //These just provide access to the point data in a clean way 171 | inline double x(size_t i) const { return operator[](i).first; } 172 | inline double y(size_t i) const { return operator[](i).second; } 173 | inline double h(size_t i) const { return x(i+1) - x(i); } 174 | 175 | //Invert a arbitrary matrix using the boost ublas library 176 | template 177 | bool InvertMatrix(ublas::matrix A, 178 | ublas::matrix& inverse) 179 | { 180 | using namespace ublas; 181 | 182 | // create a permutation matrix for the LU-factorization 183 | permutation_matrix pm(A.size1()); 184 | 185 | // perform LU-factorization 186 | int res = lu_factorize(A,pm); 187 | if( res != 0 ) return false; 188 | 189 | // create identity matrix of "inverse" 190 | inverse.assign(ublas::identity_matrix(A.size1())); 191 | 192 | // backsubstitute to get the inverse 193 | lu_substitute(A, pm, inverse); 194 | 195 | return true; 196 | } 197 | 198 | //This function will recalculate the spline parameters and store 199 | //them in _data, ready for spline interpolation 200 | void generate() 201 | { 202 | if (size() < 2) 203 | throw std::runtime_error("Spline requires at least 2 points"); 204 | 205 | //If any spline points are at the same x location, we have to 206 | //just slightly seperate them 207 | { 208 | bool testPassed(false); 209 | while (!testPassed) 210 | { 211 | testPassed = true; 212 | std::sort(base::begin(), base::end()); 213 | 214 | for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr) 215 | if (iPtr->first == (iPtr+1)->first) 216 | { 217 | if ((iPtr+1)->first != 0) 218 | (iPtr+1)->first += (iPtr+1)->first 219 | * std::numeric_limits::epsilon() * 10; 220 | else 221 | (iPtr+1)->first = std::numeric_limits::epsilon() * 10; 222 | testPassed = false; 223 | break; 224 | } 225 | } 226 | } 227 | 228 | const size_t e = size() - 1; 229 | 230 | switch (_type) 231 | { 232 | case LINEAR: 233 | { 234 | _data.resize(e); 235 | for (size_t i(0); i < e; ++i) 236 | { 237 | _data[i].x = x(i); 238 | _data[i].a = 0; 239 | _data[i].b = 0; 240 | _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i)); 241 | _data[i].d = y(i); 242 | } 243 | break; 244 | } 245 | case CUBIC: 246 | { 247 | ublas::matrix A(size(), size()); 248 | for (size_t yv(0); yv <= e; ++yv) 249 | for (size_t xv(0); xv <= e; ++xv) 250 | A(xv,yv) = 0; 251 | 252 | for (size_t i(1); i < e; ++i) 253 | { 254 | A(i-1,i) = h(i-1); 255 | A(i,i) = 2 * (h(i-1) + h(i)); 256 | A(i+1,i) = h(i); 257 | } 258 | 259 | ublas::vector C(size()); 260 | for (size_t xv(0); xv <= e; ++xv) 261 | C(xv) = 0; 262 | 263 | for (size_t i(1); i < e; ++i) 264 | C(i) = 6 * 265 | ((y(i+1) - y(i)) / h(i) 266 | - (y(i) - y(i-1)) / h(i-1)); 267 | 268 | //Boundary conditions 269 | switch(_BCLow) 270 | { 271 | case FIXED_1ST_DERIV_BC: 272 | C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal); 273 | A(0,0) = 2 * h(0); 274 | A(1,0) = h(0); 275 | break; 276 | case FIXED_2ND_DERIV_BC: 277 | C(0) = _BCLowVal; 278 | A(0,0) = 1; 279 | break; 280 | case PARABOLIC_RUNOUT_BC: 281 | C(0) = 0; A(0,0) = 1; A(1,0) = -1; 282 | break; 283 | } 284 | 285 | switch(_BCHigh) 286 | { 287 | case FIXED_1ST_DERIV_BC: 288 | C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1)); 289 | A(e,e) = 2 * h(e - 1); 290 | A(e-1,e) = h(e - 1); 291 | break; 292 | case FIXED_2ND_DERIV_BC: 293 | C(e) = _BCHighVal; 294 | A(e,e) = 1; 295 | break; 296 | case PARABOLIC_RUNOUT_BC: 297 | C(e) = 0; A(e,e) = 1; A(e-1,e) = -1; 298 | break; 299 | } 300 | 301 | ublas::matrix AInv(size(), size()); 302 | InvertMatrix(A,AInv); 303 | 304 | _ddy = ublas::prod(C, AInv); 305 | 306 | _data.resize(size()-1); 307 | for (size_t i(0); i < e; ++i) 308 | { 309 | _data[i].x = x(i); 310 | _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i)); 311 | _data[i].b = _ddy(i) / 2; 312 | _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3; 313 | _data[i].d = y(i); 314 | } 315 | } 316 | } 317 | _valid = true; 318 | } 319 | }; 320 | -------------------------------------------------------------------------------- /src/feature_tracker/src/feature_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_tracker.h" 2 | 3 | int FeatureTracker::n_id = 0; 4 | 5 | bool inBorder(const cv::Point2f &pt) 6 | { 7 | const int BORDER_SIZE = 1; 8 | int img_x = cvRound(pt.x); 9 | int img_y = cvRound(pt.y); 10 | return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE; 11 | } 12 | 13 | void reduceVector(vector &v, vector status) 14 | { 15 | int j = 0; 16 | for (int i = 0; i < int(v.size()); i++) 17 | if (status[i]) 18 | v[j++] = v[i]; 19 | v.resize(j); 20 | } 21 | 22 | void reduceVector(vector &v, vector status) 23 | { 24 | int j = 0; 25 | for (int i = 0; i < int(v.size()); i++) 26 | if (status[i]) 27 | v[j++] = v[i]; 28 | v.resize(j); 29 | } 30 | 31 | 32 | FeatureTracker::FeatureTracker() 33 | { 34 | } 35 | 36 | void FeatureTracker::setMask() 37 | { 38 | if(FISHEYE) 39 | mask = fisheye_mask.clone(); 40 | else 41 | mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255)); 42 | 43 | 44 | // prefer to keep features that are tracked for long time 45 | vector>> cnt_pts_id; 46 | 47 | for (unsigned int i = 0; i < forw_pts.size(); i++) 48 | cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i]))); 49 | 50 | sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, const pair> &b) 51 | { 52 | return a.first > b.first; 53 | }); 54 | 55 | forw_pts.clear(); 56 | ids.clear(); 57 | track_cnt.clear(); 58 | 59 | for (auto &it : cnt_pts_id) 60 | { 61 | if (mask.at(it.second.first) == 255) 62 | { 63 | forw_pts.push_back(it.second.first); 64 | ids.push_back(it.second.second); 65 | track_cnt.push_back(it.first); 66 | cv::circle(mask, it.second.first, MIN_DIST, 0, -1); 67 | } 68 | } 69 | } 70 | 71 | void FeatureTracker::addPoints() 72 | { 73 | for (auto &p : n_pts) 74 | { 75 | forw_pts.push_back(p); 76 | ids.push_back(-1); 77 | track_cnt.push_back(1); 78 | } 79 | } 80 | 81 | void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) 82 | { 83 | cv::Mat img; 84 | TicToc t_r; 85 | cur_time = _cur_time; 86 | 87 | if (EQUALIZE) 88 | { 89 | cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); 90 | TicToc t_c; 91 | clahe->apply(_img, img); 92 | ROS_DEBUG("CLAHE costs: %fms", t_c.toc()); 93 | } 94 | else 95 | img = _img; 96 | 97 | if (forw_img.empty()) 98 | { 99 | prev_img = cur_img = forw_img = img; 100 | } 101 | else 102 | { 103 | forw_img = img; 104 | } 105 | 106 | forw_pts.clear(); 107 | 108 | if (cur_pts.size() > 0) 109 | { 110 | TicToc t_o; 111 | vector status; 112 | vector err; 113 | cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3); 114 | 115 | for (int i = 0; i < int(forw_pts.size()); i++) 116 | if (status[i] && !inBorder(forw_pts[i])) 117 | status[i] = 0; 118 | reduceVector(prev_pts, status); 119 | reduceVector(cur_pts, status); 120 | reduceVector(forw_pts, status); 121 | reduceVector(ids, status); 122 | reduceVector(cur_un_pts, status); 123 | reduceVector(track_cnt, status); 124 | ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc()); 125 | } 126 | 127 | for (auto &n : track_cnt) 128 | n++; 129 | 130 | if (PUB_THIS_FRAME) 131 | { 132 | rejectWithF(); 133 | ROS_DEBUG("set mask begins"); 134 | TicToc t_m; 135 | setMask(); 136 | ROS_DEBUG("set mask costs %fms", t_m.toc()); 137 | 138 | ROS_DEBUG("detect feature begins"); 139 | TicToc t_t; 140 | int n_max_cnt = MAX_CNT - static_cast(forw_pts.size()); 141 | if (n_max_cnt > 0) 142 | { 143 | if(mask.empty()) 144 | cout << "mask is empty " << endl; 145 | if (mask.type() != CV_8UC1) 146 | cout << "mask type wrong " << endl; 147 | if (mask.size() != forw_img.size()) 148 | cout << "wrong size " << endl; 149 | cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask); 150 | } 151 | else 152 | n_pts.clear(); 153 | ROS_DEBUG("detect feature costs: %fms", t_t.toc()); 154 | 155 | ROS_DEBUG("add feature begins"); 156 | TicToc t_a; 157 | addPoints(); 158 | ROS_DEBUG("selectFeature costs: %fms", t_a.toc()); 159 | } 160 | prev_img = cur_img; 161 | prev_pts = cur_pts; 162 | prev_un_pts = cur_un_pts; 163 | cur_img = forw_img; 164 | cur_pts = forw_pts; 165 | undistortedPoints(); 166 | prev_time = cur_time; 167 | } 168 | 169 | void FeatureTracker::rejectWithF() 170 | { 171 | if (forw_pts.size() >= 8) 172 | { 173 | ROS_DEBUG("FM ransac begins"); 174 | TicToc t_f; 175 | vector un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size()); 176 | for (unsigned int i = 0; i < cur_pts.size(); i++) 177 | { 178 | Eigen::Vector3d tmp_p; 179 | m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p); 180 | tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; 181 | tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0; 182 | un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); 183 | 184 | m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p); 185 | tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; 186 | tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0; 187 | un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); 188 | } 189 | 190 | vector status; 191 | cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status); 192 | int size_a = cur_pts.size(); 193 | reduceVector(prev_pts, status); 194 | reduceVector(cur_pts, status); 195 | reduceVector(forw_pts, status); 196 | reduceVector(cur_un_pts, status); 197 | reduceVector(ids, status); 198 | reduceVector(track_cnt, status); 199 | ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a); 200 | ROS_DEBUG("FM ransac costs: %fms", t_f.toc()); 201 | } 202 | } 203 | 204 | bool FeatureTracker::updateID(unsigned int i) 205 | { 206 | if (i < ids.size()) 207 | { 208 | if (ids[i] == -1) 209 | ids[i] = n_id++; 210 | return true; 211 | } 212 | else 213 | return false; 214 | } 215 | 216 | void FeatureTracker::readIntrinsicParameter(const string &calib_file) 217 | { 218 | ROS_INFO("reading paramerter of camera %s", calib_file.c_str()); 219 | m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file); 220 | } 221 | 222 | void FeatureTracker::showUndistortion(const string &name) 223 | { 224 | cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0)); 225 | vector distortedp, undistortedp; 226 | for (int i = 0; i < COL; i++) 227 | for (int j = 0; j < ROW; j++) 228 | { 229 | Eigen::Vector2d a(i, j); 230 | Eigen::Vector3d b; 231 | m_camera->liftProjective(a, b); 232 | distortedp.push_back(a); 233 | undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z())); 234 | //printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z()); 235 | } 236 | for (int i = 0; i < int(undistortedp.size()); i++) 237 | { 238 | cv::Mat pp(3, 1, CV_32FC1); 239 | pp.at(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2; 240 | pp.at(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2; 241 | pp.at(2, 0) = 1.0; 242 | //cout << trackerData[0].K << endl; 243 | //printf("%lf %lf\n", p.at(1, 0), p.at(0, 0)); 244 | //printf("%lf %lf\n", pp.at(1, 0), pp.at(0, 0)); 245 | if (pp.at(1, 0) + 300 >= 0 && pp.at(1, 0) + 300 < ROW + 600 && pp.at(0, 0) + 300 >= 0 && pp.at(0, 0) + 300 < COL + 600) 246 | { 247 | undistortedImg.at(pp.at(1, 0) + 300, pp.at(0, 0) + 300) = cur_img.at(distortedp[i].y(), distortedp[i].x()); 248 | } 249 | else 250 | { 251 | //ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at(1, 0), pp.at(0, 0)); 252 | } 253 | } 254 | cv::imshow(name, undistortedImg); 255 | cv::waitKey(0); 256 | } 257 | 258 | void FeatureTracker::undistortedPoints() 259 | { 260 | cur_un_pts.clear(); 261 | cur_un_pts_map.clear(); 262 | //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat()); 263 | for (unsigned int i = 0; i < cur_pts.size(); i++) 264 | { 265 | Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y); 266 | Eigen::Vector3d b; 267 | m_camera->liftProjective(a, b); 268 | cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); 269 | cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z()))); 270 | //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y); 271 | } 272 | // caculate points velocity 273 | if (!prev_un_pts_map.empty()) 274 | { 275 | double dt = cur_time - prev_time; 276 | pts_velocity.clear(); 277 | for (unsigned int i = 0; i < cur_un_pts.size(); i++) 278 | { 279 | if (ids[i] != -1) 280 | { 281 | std::map::iterator it; 282 | it = prev_un_pts_map.find(ids[i]); 283 | if (it != prev_un_pts_map.end()) 284 | { 285 | double v_x = (cur_un_pts[i].x - it->second.x) / dt; 286 | double v_y = (cur_un_pts[i].y - it->second.y) / dt; 287 | pts_velocity.push_back(cv::Point2f(v_x, v_y)); 288 | } 289 | else 290 | pts_velocity.push_back(cv::Point2f(0, 0)); 291 | } 292 | else 293 | { 294 | pts_velocity.push_back(cv::Point2f(0, 0)); 295 | } 296 | } 297 | } 298 | else 299 | { 300 | for (unsigned int i = 0; i < cur_pts.size(); i++) 301 | { 302 | pts_velocity.push_back(cv::Point2f(0, 0)); 303 | } 304 | } 305 | prev_un_pts_map = cur_un_pts_map; 306 | } 307 | -------------------------------------------------------------------------------- /src/camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef SCARAMUZZACAMERA_H 2 | #define SCARAMUZZACAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | #define SCARAMUZZA_POLY_SIZE 5 14 | #define SCARAMUZZA_INV_POLY_SIZE 20 15 | 16 | #define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/) 17 | 18 | /** 19 | * Scaramuzza Camera (Omnidirectional) 20 | * https://sites.google.com/site/scarabotix/ocamcalib-toolbox 21 | */ 22 | 23 | class OCAMCamera: public Camera 24 | { 25 | public: 26 | class Parameters: public Camera::Parameters 27 | { 28 | public: 29 | Parameters(); 30 | 31 | double& C(void) { return m_C; } 32 | double& D(void) { return m_D; } 33 | double& E(void) { return m_E; } 34 | 35 | double& center_x(void) { return m_center_x; } 36 | double& center_y(void) { return m_center_y; } 37 | 38 | double& poly(int idx) { return m_poly[idx]; } 39 | double& inv_poly(int idx) { return m_inv_poly[idx]; } 40 | 41 | double C(void) const { return m_C; } 42 | double D(void) const { return m_D; } 43 | double E(void) const { return m_E; } 44 | 45 | double center_x(void) const { return m_center_x; } 46 | double center_y(void) const { return m_center_y; } 47 | 48 | double poly(int idx) const { return m_poly[idx]; } 49 | double inv_poly(int idx) const { return m_inv_poly[idx]; } 50 | 51 | bool readFromYamlFile(const std::string& filename); 52 | void writeToYamlFile(const std::string& filename) const; 53 | 54 | Parameters& operator=(const Parameters& other); 55 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 56 | 57 | private: 58 | double m_poly[SCARAMUZZA_POLY_SIZE]; 59 | double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE]; 60 | double m_C; 61 | double m_D; 62 | double m_E; 63 | double m_center_x; 64 | double m_center_y; 65 | }; 66 | 67 | OCAMCamera(); 68 | 69 | /** 70 | * \brief Constructor from the projection model parameters 71 | */ 72 | OCAMCamera(const Parameters& params); 73 | 74 | Camera::ModelType modelType(void) const; 75 | const std::string& cameraName(void) const; 76 | int imageWidth(void) const; 77 | int imageHeight(void) const; 78 | 79 | void estimateIntrinsics(const cv::Size& boardSize, 80 | const std::vector< std::vector >& objectPoints, 81 | const std::vector< std::vector >& imagePoints); 82 | 83 | // Lift points from the image plane to the sphere 84 | void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 85 | //%output P 86 | 87 | // Lift points from the image plane to the projective space 88 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 89 | //%output P 90 | 91 | // Projects 3D points to the image plane (Pi function) 92 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 93 | //%output p 94 | 95 | // Projects 3D points to the image plane (Pi function) 96 | // and calculates jacobian 97 | //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 98 | // Eigen::Matrix& J) const; 99 | //%output p 100 | //%output J 101 | 102 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 103 | //%output p 104 | 105 | template 106 | static void spaceToPlane(const T* const params, 107 | const T* const q, const T* const t, 108 | const Eigen::Matrix& P, 109 | Eigen::Matrix& p); 110 | template 111 | static void spaceToSphere(const T* const params, 112 | const T* const q, const T* const t, 113 | const Eigen::Matrix& P, 114 | Eigen::Matrix& P_s); 115 | template 116 | static void LiftToSphere(const T* const params, 117 | const Eigen::Matrix& p, 118 | Eigen::Matrix& P); 119 | 120 | template 121 | static void SphereToPlane(const T* const params, const Eigen::Matrix& P, 122 | Eigen::Matrix& p); 123 | 124 | 125 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 126 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 127 | float fx = -1.0f, float fy = -1.0f, 128 | cv::Size imageSize = cv::Size(0, 0), 129 | float cx = -1.0f, float cy = -1.0f, 130 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 131 | 132 | int parameterCount(void) const; 133 | 134 | const Parameters& getParameters(void) const; 135 | void setParameters(const Parameters& parameters); 136 | 137 | void readParameters(const std::vector& parameterVec); 138 | void writeParameters(std::vector& parameterVec) const; 139 | 140 | void writeParametersToYamlFile(const std::string& filename) const; 141 | 142 | std::string parametersToString(void) const; 143 | 144 | private: 145 | Parameters mParameters; 146 | 147 | double m_inv_scale; 148 | }; 149 | 150 | typedef boost::shared_ptr OCAMCameraPtr; 151 | typedef boost::shared_ptr OCAMCameraConstPtr; 152 | 153 | template 154 | void 155 | OCAMCamera::spaceToPlane(const T* const params, 156 | const T* const q, const T* const t, 157 | const Eigen::Matrix& P, 158 | Eigen::Matrix& p) 159 | { 160 | T P_c[3]; 161 | { 162 | T P_w[3]; 163 | P_w[0] = T(P(0)); 164 | P_w[1] = T(P(1)); 165 | P_w[2] = T(P(2)); 166 | 167 | // Convert quaternion from Eigen convention (x, y, z, w) 168 | // to Ceres convention (w, x, y, z) 169 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 170 | 171 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 172 | 173 | P_c[0] += t[0]; 174 | P_c[1] += t[1]; 175 | P_c[2] += t[2]; 176 | } 177 | 178 | T c = params[0]; 179 | T d = params[1]; 180 | T e = params[2]; 181 | T xc[2] = { params[3], params[4] }; 182 | 183 | //T poly[SCARAMUZZA_POLY_SIZE]; 184 | //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) 185 | // poly[i] = params[5+i]; 186 | 187 | T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; 188 | for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) 189 | inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; 190 | 191 | T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; 192 | T norm = T(0.0); 193 | if (norm_sqr > T(0.0)) 194 | norm = sqrt(norm_sqr); 195 | 196 | T theta = atan2(-P_c[2], norm); 197 | T rho = T(0.0); 198 | T theta_i = T(1.0); 199 | 200 | for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) 201 | { 202 | rho += theta_i * inv_poly[i]; 203 | theta_i *= theta; 204 | } 205 | 206 | T invNorm = T(1.0) / norm; 207 | T xn[2] = { 208 | P_c[0] * invNorm * rho, 209 | P_c[1] * invNorm * rho 210 | }; 211 | 212 | p(0) = xn[0] * c + xn[1] * d + xc[0]; 213 | p(1) = xn[0] * e + xn[1] + xc[1]; 214 | } 215 | 216 | template 217 | void 218 | OCAMCamera::spaceToSphere(const T* const params, 219 | const T* const q, const T* const t, 220 | const Eigen::Matrix& P, 221 | Eigen::Matrix& P_s) 222 | { 223 | T P_c[3]; 224 | { 225 | T P_w[3]; 226 | P_w[0] = T(P(0)); 227 | P_w[1] = T(P(1)); 228 | P_w[2] = T(P(2)); 229 | 230 | // Convert quaternion from Eigen convention (x, y, z, w) 231 | // to Ceres convention (w, x, y, z) 232 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 233 | 234 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 235 | 236 | P_c[0] += t[0]; 237 | P_c[1] += t[1]; 238 | P_c[2] += t[2]; 239 | } 240 | 241 | //T poly[SCARAMUZZA_POLY_SIZE]; 242 | //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) 243 | // poly[i] = params[5+i]; 244 | 245 | T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]; 246 | T norm = T(0.0); 247 | if (norm_sqr > T(0.0)) 248 | norm = sqrt(norm_sqr); 249 | 250 | P_s(0) = P_c[0] / norm; 251 | P_s(1) = P_c[1] / norm; 252 | P_s(2) = P_c[2] / norm; 253 | } 254 | 255 | template 256 | void 257 | OCAMCamera::LiftToSphere(const T* const params, 258 | const Eigen::Matrix& p, 259 | Eigen::Matrix& P) 260 | { 261 | T c = params[0]; 262 | T d = params[1]; 263 | T e = params[2]; 264 | T cc[2] = { params[3], params[4] }; 265 | T poly[SCARAMUZZA_POLY_SIZE]; 266 | for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) 267 | poly[i] = params[5+i]; 268 | 269 | // Relative to Center 270 | T p_2d[2]; 271 | p_2d[0] = T(p(0)); 272 | p_2d[1] = T(p(1)); 273 | 274 | T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]}; 275 | 276 | T inv_scale = T(1.0) / (c - d * e); 277 | 278 | // Affine Transformation 279 | T xc_a[2]; 280 | 281 | xc_a[0] = inv_scale * (xc[0] - d * xc[1]); 282 | xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]); 283 | 284 | T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]; 285 | T phi = sqrt(norm_sqr); 286 | T phi_i = T(1.0); 287 | T z = T(0.0); 288 | 289 | for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) 290 | { 291 | if (i!=1) { 292 | z += phi_i * poly[i]; 293 | } 294 | phi_i *= phi; 295 | } 296 | 297 | T p_3d[3]; 298 | p_3d[0] = xc[0]; 299 | p_3d[1] = xc[1]; 300 | p_3d[2] = -z; 301 | 302 | T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2]; 303 | T p_3d_norm = sqrt(p_3d_norm_sqr); 304 | 305 | P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm; 306 | } 307 | 308 | template 309 | void OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix& P, 310 | Eigen::Matrix& p) { 311 | T P_c[3]; 312 | { 313 | P_c[0] = T(P(0)); 314 | P_c[1] = T(P(1)); 315 | P_c[2] = T(P(2)); 316 | } 317 | 318 | T c = params[0]; 319 | T d = params[1]; 320 | T e = params[2]; 321 | T xc[2] = {params[3], params[4]}; 322 | 323 | T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; 324 | for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) 325 | inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; 326 | 327 | T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; 328 | T norm = T(0.0); 329 | if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); 330 | 331 | T theta = atan2(-P_c[2], norm); 332 | T rho = T(0.0); 333 | T theta_i = T(1.0); 334 | 335 | for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) { 336 | rho += theta_i * inv_poly[i]; 337 | theta_i *= theta; 338 | } 339 | 340 | T invNorm = T(1.0) / norm; 341 | T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho}; 342 | 343 | p(0) = xn[0] * c + xn[1] * d + xc[0]; 344 | p(1) = xn[0] * e + xn[1] + xc[1]; 345 | } 346 | } 347 | 348 | #endif 349 | -------------------------------------------------------------------------------- /src/gtsam_backend/src/main_estimator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Lu Gan 3 | * ganlu@umich.edu 4 | */ 5 | 6 | #include 7 | 8 | // Ros related 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "GraphSolver.h" 18 | #include "utils/Config.h" 19 | #include "utils/Convert.h" 20 | 21 | // Functions 22 | void setup_config(ros::NodeHandle& nh, Config* config); 23 | void setup_subpub(ros::NodeHandle& nh); 24 | void handle_measurement_imu(const sensor_msgs::ImuConstPtr& msg); 25 | void handle_measurement_uv(const sensor_msgs::PointCloudConstPtr& msg); 26 | void optimize_graph(double timestamp); 27 | void publish_state(double timestamp, gtsam::State& state); 28 | void publish_trajectory(double timestamp, Trajectory& trajectory); 29 | void publish_cloud(double timestamp); 30 | 31 | // Subscribers and publishers 32 | ros::Subscriber subIMUMeas; 33 | ros::Subscriber subUVMeas; 34 | ros::Publisher pubPoseIMU; 35 | ros::Publisher pubPathIMU; 36 | ros::Publisher pubFeatureCloud; 37 | 38 | // Master config and graph object 39 | Config* config; 40 | GraphSolver* graphsolver; 41 | int poses_seq = 0; 42 | int skip = 0; 43 | 44 | int main(int argc, char** argv) { 45 | 46 | ros::init(argc, argv, "vio"); 47 | ros::NodeHandle nh; 48 | ros::NodeHandle nhPrivate("~"); 49 | 50 | config = new Config(); 51 | setup_config(nhPrivate, config); 52 | setup_subpub(nh); 53 | 54 | sleep(2); 55 | 56 | graphsolver = new GraphSolver(config); 57 | 58 | ros::spin(); 59 | return EXIT_SUCCESS; 60 | } 61 | 62 | void setup_config(ros::NodeHandle& nh, Config* config) { 63 | 64 | config->fixedId = "global"; 65 | nh.param("fixedId", config->fixedId, config->fixedId); 66 | // Load global gravity 67 | std::vector gravity = {0, 0, 9.8}; 68 | nh.param>("gravity", gravity, gravity); 69 | for (size_t i = 0; i < 3; ++i) config->gravity(i, 0) = gravity.at(i); 70 | 71 | // Read in number of IMU and FEAT message we should wait to initialize from 72 | nh.param("imuWait", config->imuWait, 300); 73 | nh.param("featWait", config->featWait, 0); 74 | 75 | // Read in Rotation and Translation from camera frame to Imu frame 76 | std::vector R_C0toI = {1, 0, 0, 0, 1, 0, 0, 0, 1}; 77 | nh.param>("R_C0toI", R_C0toI, R_C0toI); 78 | for (size_t i = 0; i < 9; ++i) config->R_C0toI(i) = R_C0toI.at(i); 79 | 80 | std::vector p_IinC0 = {0, 0, 0}; 81 | nh.param>("p_IinC0", p_IinC0, p_IinC0); 82 | for (size_t i = 0; i < 3; ++i) config->p_IinC0(i) = p_IinC0.at(i); 83 | 84 | // Load priors 85 | std::vector prior_qGtoI = {0, 0, 0, 1.0}; 86 | nh.param>("prior_qGtoI", prior_qGtoI, prior_qGtoI); 87 | for (size_t i = 0; i < 4; ++i) config->prior_qGtoI(i) = prior_qGtoI.at(i); 88 | 89 | std::vector prior_pIinG = {0, 0, 0}; 90 | nh.param>("prior_pIinG", prior_pIinG, prior_pIinG); 91 | for (size_t i = 0; i < 3; ++i) config->prior_pIinG(i) = prior_pIinG.at(i); 92 | 93 | std::vector prior_vIinG = {0, 0, 0}; 94 | nh.param>("prior_vIinG", prior_vIinG, prior_vIinG); 95 | for (size_t i = 0; i < 3; ++i) config->prior_vIinG(i) = prior_vIinG.at(i); 96 | 97 | std::vector prior_ba = {0, 0, 0}; 98 | nh.param>("prior_ba", prior_ba, prior_ba); 99 | for (size_t i = 0; i < 3; ++i) config->prior_ba(i) = prior_ba.at(i); 100 | 101 | std::vector prior_bg = {0, 0, 0}; 102 | nh.param>("prior_bg", prior_bg, prior_bg); 103 | for (size_t i = 0; i < 3; ++i) config->prior_bg(i) = prior_bg.at(i); 104 | 105 | // Read in our CAMERA noise values 106 | nh.param("sigma_camera", config->sigma_camera, 1.0/484.1316); 107 | config->sigma_camera_sq = std::pow(config->sigma_camera, 2); 108 | 109 | // Read in our IMU noise values 110 | nh.param("accelerometer_noise_density", config->sigma_a, 0.01); 111 | config->sigma_a_sq = std::pow(config->sigma_a, 2); 112 | nh.param("gyroscope_noise_density", config->sigma_g, 0.005); 113 | config->sigma_g_sq = std::pow(config->sigma_g, 2); 114 | nh.param("accelerometer_random_walk", config->sigma_wa, 0.002); 115 | config->sigma_wa_sq = std::pow(config->sigma_wa, 2); 116 | nh.param("gyroscope_random_walk", config->sigma_wg, 4.0e-06); 117 | config->sigma_wg_sq = std::pow(config->sigma_wg, 2); 118 | 119 | // Read in our Initialization noise values 120 | nh.param("sigma_prior_rotation", config->sigma_prior_rotation, 1.0e-4); 121 | nh.param("sigma_prior_translation", config->sigma_prior_translation, 1.0e-4); 122 | nh.param("sigma_velocity", config->sigma_velocity, 1.0e-4); 123 | nh.param("sigma_bias", config->sigma_bias, 1.0e-4); 124 | nh.param("sigma_pose_rotation", config->sigma_pose_rotation, 1.0e-4); 125 | nh.param("sigma_pose_translation", config->sigma_pose_translation, 1.0e-4); 126 | 127 | // Debug print to screen for the user 128 | Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); 129 | std::cout << "\t- fixed ID: "<< config->fixedId << std::endl; 130 | std::cout << "\t- gravity: " << config->gravity.format(CommaInitFmt) << std::endl; 131 | std::cout << "\t- imuWait: " << config->imuWait << std::endl; 132 | std::cout << "\t- featWait: "<< config->featWait << std::endl; 133 | std::cout << "\t- R_C0toI: " << std::endl << config->R_C0toI << std::endl; 134 | std::cout << "\t- p_IinC0: " << std::endl << config->p_IinC0.transpose() << std::endl; 135 | 136 | std::cout << "Initialization:" << std::endl; 137 | std::cout << "\t- prior_q_GtoI: " << std::endl << config->prior_qGtoI.transpose() << std::endl; 138 | std::cout << "\t- prior_p_IinG: " << std::endl << config->prior_pIinG.transpose() << std::endl; 139 | std::cout << "\t- prior_ba: " << std::endl << config->prior_ba.transpose() << std::endl; 140 | std::cout << "\t- prior_bg: " << std::endl << config->prior_bg.transpose() << std::endl; 141 | 142 | std::cout << "Noise Parameters:" << std::endl; 143 | std::cout << "\t- sigma_camera: " << config->sigma_camera << std::endl; 144 | std::cout << "\t- sigma_a: " << config->sigma_a << std::endl; 145 | std::cout << "\t- sigma_g: " << config->sigma_g << std::endl; 146 | std::cout << "\t- sigma_wa: " << config->sigma_wa << std::endl; 147 | std::cout << "\t- sigma_wg: " << config->sigma_wg << std::endl; 148 | std::cout << "\t- sigma_prior_rotation: " << config->sigma_prior_rotation << std::endl; 149 | std::cout << "\t- sigma_prior_translation: " << config->sigma_prior_translation << std::endl; 150 | std::cout << "\t- sigma_velocity: " << config->sigma_velocity << std::endl; 151 | std::cout << "\t- sigma_bias: " << config->sigma_bias << std::endl; 152 | std::cout << "\t- sigma_pose_rotation: " << config->sigma_pose_rotation << std::endl; 153 | std::cout << "\t- sigma_pose_translation: " << config->sigma_pose_translation << std::endl; 154 | } 155 | 156 | void setup_subpub(ros::NodeHandle& nh) { 157 | 158 | // Subscribe to IMU measurements 159 | subIMUMeas = nh.subscribe("vio/data_imu", 2000, handle_measurement_imu); 160 | ROS_INFO("Subscribing: %s", subIMUMeas.getTopic().c_str()); 161 | 162 | // Subscribe to uv measurements 163 | subUVMeas = nh.subscribe("vio/data_uv", 500, handle_measurement_uv); 164 | ROS_INFO("Subscribing: %s", subUVMeas.getTopic().c_str()); 165 | 166 | // IMU pose visualization 167 | pubPoseIMU = nh.advertise("vio/pose_imu", 2); 168 | ROS_INFO("Publishing: %s", pubPoseIMU.getTopic().c_str()); 169 | 170 | // IMU path visualization 171 | pubPathIMU = nh.advertise("vio/path_imu", 2); 172 | ROS_INFO("Publishing: %s", pubPathIMU.getTopic().c_str()); 173 | 174 | // Feature cloud visualization 175 | pubFeatureCloud = nh.advertise("vio/feature_cloud", 2); 176 | ROS_INFO("Publishing: %s", pubFeatureCloud.getTopic().c_str()); 177 | 178 | } 179 | 180 | void handle_measurement_imu(const sensor_msgs::ImuConstPtr& msg) { 181 | 182 | // Convert to eigen format 183 | Eigen::Vector3d linearacceleration; 184 | linearacceleration << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; 185 | Eigen::Vector3d angularvelocity; 186 | angularvelocity << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; 187 | Eigen::Vector4d orientation; 188 | orientation << msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w; 189 | 190 | // Send to graph solver 191 | graphsolver->addmeasurement_imu(msg->header.stamp.toSec(), linearacceleration, angularvelocity, orientation); 192 | } 193 | 194 | void handle_measurement_uv(const sensor_msgs::PointCloudConstPtr& msg) { 195 | 196 | // Skip first keyframes 197 | if (graphsolver->is_initialized() && skip != config->featWait) { 198 | skip++; 199 | return; 200 | } else 201 | skip = 0; 202 | 203 | // Feature measurements 204 | std::vector leftids; 205 | std::vector leftuv; 206 | 207 | // Loop through LEFT features and append 208 | for(size_t i = 0; i < msg->points.size(); ++i) { 209 | int v = msg->channels[0].values[i] + 0.5; 210 | int id = v / 1; 211 | leftids.push_back((uint)id); 212 | Eigen::Vector2d uv; 213 | uv << msg->points[i].x, msg->points[i].y; 214 | leftuv.push_back(uv); 215 | } 216 | 217 | // We have successfully handled all features, lets send them to the optimizer 218 | graphsolver->addmeasurement_uv(msg->header.stamp.toSec(), leftids, leftuv); 219 | optimize_graph(msg->header.stamp.toSec()); 220 | } 221 | 222 | void optimize_graph(double timestamp) { 223 | // Optimize graph 224 | graphsolver->optimize(); 225 | 226 | // Ros visualization 227 | gtsam::State state = graphsolver->get_current_state(); 228 | publish_state(timestamp, state); 229 | std::vector> trajectory = graphsolver->get_trajectory(); 230 | publish_trajectory(timestamp, trajectory); 231 | publish_cloud(timestamp); 232 | } 233 | 234 | void publish_state(double timestamp, gtsam::State& state) { 235 | 236 | // Return if we have not initialized yet 237 | if(!graphsolver->is_initialized()) 238 | return; 239 | 240 | // Create our stamped pose with covariance 241 | geometry_msgs::PoseWithCovarianceStamped pose; 242 | pose.header.stamp = ros::Time(timestamp); 243 | pose.header.frame_id = config->fixedId; 244 | Eigen::Matrix covariance = Eigen::Matrix::Zero(); 245 | ToPoseWithCovariance(state.pose(), covariance, pose.pose); 246 | 247 | // Publish this pose 248 | pubPoseIMU.publish(pose); 249 | } 250 | 251 | void publish_trajectory(double timestamp, Trajectory& trajectory) { 252 | 253 | // Return if trajectory is empty 254 | if (trajectory.empty()) 255 | return; 256 | 257 | // Create stamped pose for path publishing 258 | std::vector traj_est; 259 | for (auto it = trajectory.begin(); it != trajectory.end(); ++it) { 260 | geometry_msgs::PoseStamped poseStamped; 261 | poseStamped.header.stamp = ros::Time(it->first); 262 | poseStamped.header.frame_id = config->fixedId; 263 | ToPose(it->second.pose(), poseStamped.pose); 264 | traj_est.push_back(poseStamped); 265 | } 266 | 267 | // Create pose arrays and publish 268 | nav_msgs::Path patharr; 269 | patharr.header.frame_id = config->fixedId; 270 | patharr.header.stamp = ros::Time(timestamp); 271 | patharr.header.seq = poses_seq++; 272 | patharr.poses = traj_est; 273 | pubPathIMU.publish(patharr); 274 | } 275 | 276 | void publish_cloud(double timestamp) { 277 | 278 | // Return if we have not initialized yet 279 | if (!graphsolver->is_initialized()) 280 | return; 281 | 282 | // Loop through and create a point cloud 283 | std::vector points = graphsolver->get_current_features(); 284 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud ); 285 | for (size_t i = 0; i < points.size(); ++i) { 286 | pcl::PointXYZ pt; 287 | pt.x = points.at(i)(0); 288 | pt.y = points.at(i)(1); 289 | pt.z = points.at(i)(2); 290 | cloud->push_back(pt); 291 | } 292 | 293 | // publish as a ros msg 294 | sensor_msgs::PointCloud2 msgOut; 295 | pcl::toROSMsg(*cloud, msgOut); 296 | msgOut.header.frame_id = config->fixedId; 297 | msgOut.header.stamp = ros::Time(timestamp); 298 | pubFeatureCloud.publish(msgOut); 299 | } 300 | --------------------------------------------------------------------------------