├── .gitignore ├── README.md ├── src ├── GraphOptimizer.cpp ├── Sensor.cpp ├── main.cpp ├── PinholeModel.cpp ├── KeyFrame.cpp ├── SensorRos.cpp ├── Config.cpp ├── SensorSavedData.cpp ├── Frame.cpp ├── System.cpp ├── Logger.cpp └── Tracker.cpp ├── include ├── GraphOptimizer.h ├── SensorRos.h ├── PinholeModel.h ├── SensorSavedData.h ├── System.h ├── Sensor.h ├── Datatypes.h ├── KeyFrame.h ├── Config.h ├── Logger.h ├── Frame.h └── Tracker.h ├── cmake_modules ├── FindCSparse.cmake ├── FindCholmod.cmake ├── FindSuiteSparse.cmake └── FindG2O.cmake ├── yaml ├── defalut_param.yaml └── defalut_param_kitti.yaml ├── launch └── dvl_mapping.launch ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | cmake-build-debug/ 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DVL_SLAM_ROS 2 | DVL_SLAM_ROS 3 | -------------------------------------------------------------------------------- /src/GraphOptimizer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #include "GraphOptimizer.h" 6 | 7 | GraphOptimizer::GraphOptimizer(Config &config) 8 | { 9 | 10 | 11 | } 12 | 13 | 14 | GraphOptimizer::~GraphOptimizer() 15 | { 16 | 17 | 18 | } 19 | 20 | 21 | void GraphOptimizer::AddEdge() 22 | { 23 | 24 | 25 | } 26 | 27 | 28 | void GraphOptimizer::Optimize() 29 | { 30 | 31 | 32 | } 33 | 34 | -------------------------------------------------------------------------------- /src/Sensor.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #include "Sensor.h" 6 | 7 | Sensor::Sensor(Config& config) 8 | : config_(config) 9 | { 10 | lidarFlag_ = false; 11 | imgFlag_ = false; 12 | 13 | minZ = config_.pointcloudConfig.minZ; 14 | maxZ = config_.pointcloudConfig.maxZ; 15 | 16 | inputCloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); 17 | } 18 | 19 | Sensor::~Sensor(){ 20 | } 21 | 22 | bool Sensor::IsLidarSubscribed(){ return lidarFlag_; } 23 | 24 | bool Sensor::IsVisionSubscribed(){ return imgFlag_; } -------------------------------------------------------------------------------- /include/GraphOptimizer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_GRAPHOPTIMIZER_H 6 | #define DVL_SLAM_MODIFY_GRAPHOPTIMIZER_H 7 | 8 | #include 9 | #include 10 | 11 | #include "Config.h" 12 | 13 | class GraphOptimizer{ 14 | public: 15 | GraphOptimizer(Config &config); 16 | ~GraphOptimizer(); 17 | 18 | void AddEdge(); 19 | void Optimize(); 20 | 21 | private: 22 | g2o::SparseOptimizer graphOptimizer; 23 | 24 | }; 25 | 26 | 27 | #endif //DVL_SLAM_MODIFY_GRAPHOPTIMIZER_H 28 | -------------------------------------------------------------------------------- /include/SensorRos.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 12. 11.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_SENSORROS_H 6 | #define DVL_SLAM_MODIFY_SENSORROS_H 7 | 8 | #include "Sensor.h" 9 | 10 | class SensorRos : public Sensor{ 11 | public: 12 | SensorRos(Config& config); 13 | ~SensorRos(); 14 | void ImgCb(const sensor_msgs::ImageConstPtr& img); 15 | void PcCb(const sensor_msgs::PointCloud2ConstPtr& pc); 16 | 17 | void data2Frame(Frame& frame) override; 18 | private: 19 | ros::NodeHandle nh_; 20 | ros::Subscriber imgSub; 21 | ros::Subscriber pointCloudSub; 22 | }; 23 | 24 | #endif //DVL_SLAM_MODIFY_SENSORROS_H 25 | -------------------------------------------------------------------------------- /include/PinholeModel.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 12. 4.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_PINHOLEMODEL_H 6 | #define DVL_SLAM_MODIFY_PINHOLEMODEL_H 7 | 8 | #include "Config.h" 9 | #include "Datatypes.h" 10 | #include 11 | #include 12 | 13 | class PinholeModel{ 14 | public: 15 | PinholeModel(Config &config); 16 | ~PinholeModel(); 17 | 18 | Eigen::Vector2f PointCloudXyz2Uv(Eigen::Vector3f point); 19 | std::vector PointCloudXyz2UvVec(const pcl::PointCloud& pc, float scale); 20 | 21 | private: 22 | Config &config_; 23 | 24 | }; 25 | 26 | #endif //DVL_SLAM_MODIFY_PINHOLEMODEL_H 27 | -------------------------------------------------------------------------------- /include/SensorSavedData.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 12. 11.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_SENSORSAVEDDATA_H 6 | #define DVL_SLAM_MODIFY_SENSORSAVEDDATA_H 7 | 8 | #include "Sensor.h" 9 | #include "glob.h" 10 | #include "boost/filesystem.hpp" 11 | 12 | class SensorSavedData : public Sensor{ 13 | public: 14 | SensorSavedData(Config& config); 15 | ~SensorSavedData(); 16 | 17 | void loadImg(); 18 | void loadPointCloud(); 19 | 20 | void data2Frame(Frame& frame) override; 21 | 22 | private: 23 | Config& config_; 24 | std::queue imgVec; 25 | std::queue> cloudVec; 26 | }; 27 | 28 | #endif //DVL_SLAM_MODIFY_SENSORSAVEDDATA_H 29 | -------------------------------------------------------------------------------- /cmake_modules/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | ) 14 | 15 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 16 | PATHS 17 | /usr/lib 18 | /usr/local/lib 19 | /opt/local/lib 20 | /sw/lib 21 | ) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 25 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 26 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "Config.h" 5 | #include "System.h" 6 | 7 | using namespace std; 8 | 9 | int main(int argc, char** argv){ 10 | ros::init(argc, argv, "dvl_reimpl"); 11 | ros::NodeHandle nh; 12 | 13 | ros::Rate rate(20.0f); 14 | Config Config; 15 | 16 | System System(Config); 17 | 18 | if(Config.datasetConfig.useRos){ 19 | while(ros::ok()){ 20 | System.Run(); 21 | ros::spinOnce(); 22 | } 23 | } 24 | else{ 25 | while(true){ 26 | // char ch; 27 | // std::cin.get(ch); 28 | // if(ch == '\n'){ 29 | // System.Run(); 30 | // } 31 | System.Run(); 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /include/System.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_SYSTEM_H 6 | #define DVL_SLAM_MODIFY_SYSTEM_H 7 | 8 | #include "Config.h" 9 | #include "Frame.h" 10 | #include "GraphOptimizer.h" 11 | #include "KeyFrame.h" 12 | #include "Sensor.h" 13 | #include "SensorRos.h" 14 | #include "SensorSavedData.h" 15 | #include "Tracker.h" 16 | #include 17 | #include "Logger.h" 18 | #include 19 | #include 20 | 21 | class System{ 22 | public: 23 | System(Config& config_); 24 | ~System(); 25 | 26 | void Run(); 27 | 28 | private: 29 | Config& config_; 30 | GraphOptimizer graphOptimizer_; 31 | Sensor* sensor_; 32 | Tracker tracker_; 33 | Logger logger_; 34 | 35 | bool initialized_; 36 | float num; 37 | Sophus::SE3f Tij_; 38 | Sophus::SE3f Tji_; 39 | Sophus::SE3f dTji_; 40 | 41 | FrameDB::Ptr frameDB_; 42 | KeyFrameDB::Ptr keyFrameDB_; 43 | }; 44 | 45 | 46 | #endif //DVL_SLAM_MODIFY_SYSTEM_H 47 | -------------------------------------------------------------------------------- /yaml/defalut_param.yaml: -------------------------------------------------------------------------------- 1 | Dataset: 2 | isKitti: false 3 | isIndoor: true 4 | useRos: true 5 | visualize: false 6 | 7 | Camera: 8 | fx: 730.617807179727 9 | fy: 731.2502851701612 10 | cx: 335.1539179332822 11 | cy: 256.15500058538953 12 | k1: -0.3370792140385211 13 | k2: 0.12848011803681716 14 | k3: 0.012402628238191207 15 | p1: -0.001618208242720009 16 | p2: 0.0008113530876134809 17 | 18 | Extrinsic: 19 | delX: 0.0 20 | delY: 0.0 21 | delZ: -0.08 22 | r11: 7.027555e-03 23 | r12: -9.999753e-01 24 | r13: 2.599616e-05 25 | r21: -2.254837e-03 26 | r22: -4.184312e-05 27 | r23: -9.999975e-01 28 | r31: 9.999728e-01 29 | r32: 7.027479e-03 30 | r33: -2.255075e-03 31 | 32 | Rectifying: 33 | R11: 1.0 34 | R12: 0.0 35 | R13: 0.0 36 | R21: 0.0 37 | R22: 1.0 38 | R23: 0.0 39 | R31: 0.0 40 | R32: 0.0 41 | R33: 1.0 42 | 43 | Image: 44 | width: 1226 45 | height: 370 46 | 47 | PointCloud: 48 | minZ: 0.15 49 | maxZ: 50.0 50 | 51 | Tracker: 52 | imgPyramidMinLevel: 0 53 | imgPyramidMaxLevel: 1 54 | maxIteration: 10 55 | normXThresForIteration: 0.0005 56 | border: 6 57 | 58 | Logger: 59 | voxelSize: 0.03 60 | 61 | System: 62 | ratioThres: 0.5 -------------------------------------------------------------------------------- /include/Sensor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_SENSOR_H 6 | #define DVL_SLAM_MODIFY_SENSOR_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "Frame.h" 18 | #include 19 | #include 20 | #include 21 | 22 | class Sensor{ 23 | public: 24 | Sensor(Config& config); 25 | ~Sensor(); 26 | 27 | virtual void data2Frame(Frame& frame) = 0; 28 | 29 | void publishImg(cv::Mat image); 30 | void publishTransform(Sophus::SE3f input); 31 | 32 | bool IsLidarSubscribed(); 33 | bool IsVisionSubscribed(); 34 | 35 | protected: 36 | Config& config_; 37 | 38 | pcl::PointCloud::Ptr inputCloud_; 39 | cv::Mat inputImg_; 40 | 41 | bool lidarFlag_; 42 | bool imgFlag_; 43 | 44 | float minZ; 45 | float maxZ; 46 | 47 | }; 48 | 49 | #endif //DVL_SLAM_MODIFY_SENSOR_H 50 | -------------------------------------------------------------------------------- /include/Datatypes.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 11. 19.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_DATATYPES_H 6 | #define DVL_SLAM_MODIFY_DATATYPES_H 7 | 8 | #include 9 | #include 10 | 11 | typedef float NumType; 12 | 13 | typedef Eigen::Matrix Matrix60x60; 14 | typedef Eigen::Matrix Matrix30x30; 15 | typedef Eigen::Matrix Matrix10x10; 16 | typedef Eigen::Matrix Matrix12x12; 17 | typedef Eigen::Matrix Matrix6x6; 18 | 19 | typedef Eigen::Matrix Matrix3x4; 20 | typedef Eigen::Matrix Matrix3x6; 21 | 22 | typedef Eigen::Matrix Matrix2x6; 23 | typedef Eigen::Matrix Matrix2x3; 24 | 25 | typedef Eigen::Matrix Matrix1x12; 26 | typedef Eigen::Matrix Matrix1x6; 27 | typedef Eigen::Matrix Matrix1x2; 28 | typedef Eigen::Matrix Matrix1x3; 29 | 30 | typedef Eigen::Matrix Vector60; 31 | typedef Eigen::Matrix Vector30; 32 | typedef Eigen::Matrix Vector12; 33 | typedef Eigen::Matrix Vector6; 34 | typedef Eigen::Matrix Vector4; 35 | typedef Eigen::Matrix Vector3; 36 | 37 | typedef Eigen::Transform AffineTransform; 38 | 39 | typedef std::vector ImgPyramid; 40 | 41 | 42 | #endif //DVL_SLAM_MODIFY_DATATYPES_H 43 | -------------------------------------------------------------------------------- /yaml/defalut_param_kitti.yaml: -------------------------------------------------------------------------------- 1 | Dataset: 2 | isKitti: true 3 | isIndoor: false 4 | useRos: false 5 | imgDir: "/home/jongsik/temp_work/kitti2bag/2011_09_30/2011_09_30_drive_0020_sync/image_02/data_temp/" 6 | lidarDir: "/home/jongsik/temp_work/kitti2bag/2011_09_30/2011_09_30_drive_0020_sync/velodyne_points/data_temp/" 7 | visualize: false 8 | 9 | Camera: 10 | fx: 707.0912 11 | fy: 707.0912 12 | cx: 601.8873 13 | cy: 183.1104 14 | 15 | k1: -3.792567e-01 16 | k2: 2.121203e-01 17 | k3: -7.605535e-02 18 | p1: 9.182571e-04 19 | p2: 1.911304e-03 20 | 21 | Extrinsic: 22 | delX: -7.137748e-03 23 | delY: -7.482656e-02 24 | delZ: -3.336324e-01 25 | 26 | r11: 7.027555e-03 27 | r12: -9.999753e-01 28 | r13: 2.599616e-05 29 | r21: -2.254837e-03 30 | r22: -4.184312e-05 31 | r23: -9.999975e-01 32 | r31: 9.999728e-01 33 | r32: 7.027479e-03 34 | r33: -2.255075e-03 35 | 36 | Rectifying: 37 | R11: 9.999280e-01 38 | R12: 8.085985e-03 39 | R13: -8.866797e-03 40 | R21: -8.123205e-03 41 | R22: 9.999583e-01 42 | R23: -4.169750e-03 43 | R31: 8.832711e-03 44 | R32: 4.241477e-03 45 | R33: 9.999520e-01 46 | 47 | Image: 48 | width: 1226 49 | height: 370 50 | 51 | PointCloud: 52 | minZ: 0.15 53 | maxZ: 50.0 54 | 55 | Tracker: 56 | imgPyramidMinLevel: 0 57 | imgPyramidMaxLevel: 1 58 | maxIteration: 100 59 | normXThresForIteration: 0.0005 60 | border: 6 61 | 62 | Logger: 63 | voxelSize: 0.02 64 | 65 | System: 66 | ratioThres: 0.5 -------------------------------------------------------------------------------- /launch/dvl_mapping.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 | 27 | 28 | 29 | 30 | launch-prefix="gdb -ex run --args" 31 | 32 | -------------------------------------------------------------------------------- /src/PinholeModel.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 12. 4.. 3 | // 4 | #include "PinholeModel.h" 5 | PinholeModel::PinholeModel(Config& config) 6 | :config_(config) 7 | { 8 | 9 | } 10 | 11 | PinholeModel::~PinholeModel() 12 | { 13 | 14 | } 15 | 16 | Eigen::Vector2f PinholeModel::PointCloudXyz2Uv(Eigen::Vector3f point){ 17 | float U = config_.cameraConfig.fx * (point(0) / point(2)) + config_.cameraConfig.cx; 18 | float V = config_.cameraConfig.fy * (point(1) / point(2)) + config_.cameraConfig.cy; 19 | Eigen::Vector2f uv(U, V); 20 | return uv; 21 | } 22 | 23 | std::vector PinholeModel::PointCloudXyz2UvVec(const pcl::PointCloud& pc, float scale){ 24 | std::vector uvSet; 25 | for(auto point:pc){ 26 | float X = point.x / point.z; 27 | float Y = point.y / point.z; 28 | float U = config_.cameraConfig.fx * X + config_.cameraConfig.cx; 29 | float V = config_.cameraConfig.fy * Y + config_.cameraConfig.cy; 30 | // std::cout << "[PinholeModel] point.x : " << point.x << std::endl; 31 | // std::cout << "[PinholeModel] point.z : " << point.z << std::endl; 32 | // std::cout << "[PinholeModel]point.x / point.z : " << point.x / point.z << std::endl; 33 | // std::cout << "[PinholeModel]X : " << X << std::endl; 34 | // std::cout << "[PinholeModel]config_.cameraConfig.fx" << config_.cameraConfig.fx << std::endl; 35 | // std::cout << "[PinholeModel]config_.cameraConfig.fx * (point.x / point.z)" << config_.cameraConfig.fx * (point.x / point.z) << std::endl; 36 | // std::cout << "[PinholeModel]config_.cameraConfig.fx * X" << config_.cameraConfig.fx * X << std::endl; 37 | // std::cout << "[PinholeModel] U : " << U << std::endl; 38 | Eigen::Vector2f uv(U, V); 39 | uvSet.push_back(uv*scale); 40 | } 41 | return uvSet; 42 | } 43 | -------------------------------------------------------------------------------- /include/KeyFrame.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_KEYFRAME_H 6 | #define DVL_SLAM_MODIFY_KEYFRAME_H 7 | 8 | #include "Config.h" 9 | #include "Frame.h" 10 | #include 11 | #include 12 | #include "PinholeModel.h" 13 | 14 | class KeyFrame{ 15 | public: 16 | typedef std::shared_ptr Ptr; 17 | 18 | KeyFrame(Config &config, Frame::Ptr frame); 19 | ~KeyFrame(); 20 | 21 | float GetVisibleRatio (const KeyFrame::Ptr keyframe); 22 | 23 | 24 | Frame::Ptr frame; 25 | 26 | private: 27 | Config& config_; 28 | 29 | PinholeModel pinholeModel_; 30 | 31 | }; 32 | 33 | class KeyFrameDB { 34 | public: 35 | typedef std::shared_ptr Ptr; 36 | 37 | KeyFrameDB(){} 38 | ~KeyFrameDB(){} 39 | void Add(KeyFrame::Ptr keyframe) { 40 | keyframeDB_.push_back(keyframe); 41 | } 42 | 43 | KeyFrame::Ptr LatestKeyframe() { 44 | return *(keyframeDB_.end()-1); 45 | } 46 | 47 | void LatestKeyframe(std::vector& keyframe_window, int n) { 48 | 49 | for(int i = n; i > 0; --i) { 50 | keyframe_window.push_back( *(keyframeDB_.end() - i) ); 51 | } 52 | 53 | } 54 | 55 | void KeyframeSet(std::vector& keyframe_window, int idx, int n) { 56 | 57 | for(int i = n; i > 0; --i) { 58 | keyframe_window.push_back( keyframeDB_[idx-i] ); 59 | } 60 | 61 | } 62 | 63 | void ConnectedKeyframe(std::vector& connected_keyframe, int n) { 64 | 65 | for(int i = n; i > 1; --i) { 66 | connected_keyframe.push_back( *(keyframeDB_.end() - i) ); 67 | } 68 | 69 | } 70 | 71 | int size() { 72 | return keyframeDB_.size(); 73 | } 74 | 75 | std::vector::iterator begin() { return keyframeDB_.begin(); } 76 | std::vector::iterator end() { return keyframeDB_.end(); } 77 | std::vector& keyframeDB() { return keyframeDB_; } 78 | 79 | private: 80 | std::vector keyframeDB_; 81 | }; 82 | 83 | #endif //DVL_SLAM_MODIFY_KEYFRAME_H 84 | -------------------------------------------------------------------------------- /include/Config.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 28.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_CONFIG_H 6 | #define DVL_SLAM_MODIFY_CONFIG_H 7 | 8 | #include 9 | struct DatasetConfig 10 | { 11 | bool isKitti; 12 | bool isIndoor; 13 | bool useRos; 14 | std::string imgDir; 15 | std::string lidarDir; 16 | bool visualize; 17 | }; 18 | 19 | struct CameraConfig 20 | { 21 | float fx; 22 | float fy; 23 | float cx; 24 | float cy; 25 | float k1; 26 | float k2; 27 | float p1; 28 | float p2; 29 | float k3; 30 | }; 31 | 32 | struct ExtrinsicConfig 33 | { 34 | float delX; 35 | float delY; 36 | float delZ; 37 | 38 | float r11; 39 | float r12; 40 | float r13; 41 | float r21; 42 | float r22; 43 | float r23; 44 | float r31; 45 | float r32; 46 | float r33; 47 | }; 48 | 49 | struct RectifyingConfig 50 | { 51 | float R11; 52 | float R12; 53 | float R13; 54 | float R21; 55 | float R22; 56 | float R23; 57 | float R31; 58 | float R32; 59 | float R33; 60 | }; 61 | 62 | struct ImageConfig 63 | { 64 | int width; 65 | int height; 66 | }; 67 | 68 | struct TrackerConfig 69 | { 70 | int imgPyramidMinLevel; 71 | int imgPyramidMaxLevel; 72 | int maxIteration; 73 | float normXThresForIteration; 74 | int border; 75 | }; 76 | 77 | struct PointCloudConfig 78 | { 79 | float minZ; 80 | float maxZ; 81 | }; 82 | 83 | struct LoggerConfig 84 | { 85 | float voxelSize; 86 | }; 87 | 88 | struct SystemConfig 89 | { 90 | float ratioThres; 91 | }; 92 | class Config{ 93 | public: 94 | Config(); 95 | ~Config(); 96 | 97 | DatasetConfig datasetConfig; 98 | CameraConfig cameraConfig; 99 | ExtrinsicConfig extrinsicConfig; 100 | RectifyingConfig rectifyingConfig; 101 | ImageConfig imageConfig; 102 | TrackerConfig trackerConfig; 103 | PointCloudConfig pointcloudConfig; 104 | LoggerConfig loggerConfig; 105 | SystemConfig systemConfig; 106 | 107 | private: 108 | void ReadEveryParameter(YAML::Node yamlFile); 109 | }; 110 | 111 | #endif //DVL_SLAM_MODIFY_CONFIG_H 112 | -------------------------------------------------------------------------------- /src/KeyFrame.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 11. 11.. 3 | // 4 | #include "KeyFrame.h" 5 | 6 | KeyFrame::KeyFrame(Config &config, Frame::Ptr frame) 7 | :config_(config), 8 | frame(frame), 9 | pinholeModel_(config) 10 | { 11 | } 12 | 13 | KeyFrame::~KeyFrame() 14 | { 15 | 16 | } 17 | 18 | float KeyFrame::GetVisibleRatio (const KeyFrame::Ptr keyframe) 19 | { 20 | 21 | std::cout << "[KeyFrame] Start GetVisibleRatio" << std::endl; 22 | 23 | Sophus::SE3f Tij = frame->GetTwc().inverse() * keyframe->frame->GetTwc(); 24 | 25 | int border = config_.trackerConfig.border; 26 | int currentLevel = 0; 27 | 28 | cv::Mat& current_img = frame->GetPyramidImg(currentLevel); 29 | 30 | const float scale = 1.0f / (1 << currentLevel); 31 | 32 | int visiblePoints = 0; 33 | int inCameraPoints = 0; 34 | 35 | std::cout << "[KeyFrame] Start2 GetVisibleRatio" << std::endl; 36 | 37 | pcl::PointCloud currFramePointCloud = keyframe->frame->GetOriginalCloud(); 38 | 39 | Eigen::Affine3f transformPitch(Tij.matrix()); 40 | pcl::transformPointCloud(currFramePointCloud, currFramePointCloud, transformPitch); 41 | std::vector uvSet = pinholeModel_.PointCloudXyz2UvVec(currFramePointCloud, scale); 42 | auto pointCloudIter = currFramePointCloud.begin(); 43 | 44 | for (auto iter=uvSet.begin(); iter!=uvSet.end(); ++iter, ++pointCloudIter) { 45 | 46 | Eigen::Vector2f uv_prev = *iter; 47 | 48 | const float u_prev_f = uv_prev(0); 49 | const float v_prev_f = uv_prev(1); 50 | const int u_prev_i = static_cast (u_prev_f); 51 | const int v_prev_i = static_cast (v_prev_f); 52 | 53 | if (u_prev_i - border < 0 || u_prev_i + border > config_.imageConfig.width || v_prev_i - border < 0 || v_prev_i + border > config_.imageConfig.height || pointCloudIter->z <= 0) 54 | continue; 55 | 56 | visiblePoints++; 57 | 58 | } 59 | std::cout << "[KeyFrame] visible_points : " << visiblePoints << std::endl; 60 | std::cout << "[KeyFrame] keyframe->frame->GetOriginalCloud().size() : " << keyframe->frame->GetOriginalCloud().size() << std::endl; 61 | 62 | return static_cast (visiblePoints) / static_cast (keyframe->frame->GetOriginalCloud().size()); 63 | } -------------------------------------------------------------------------------- /include/Logger.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 12. 18.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_LOGGER_H 6 | #define DVL_SLAM_MODIFY_LOGGER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "PinholeModel.h" 24 | #include "Config.h" 25 | #include 26 | #include 27 | #include 28 | 29 | class Logger{ 30 | public: 31 | Logger(Config& config); 32 | ~Logger(); 33 | 34 | void PushBackOdometryResult(pcl::PointXYZ odometryPoint); 35 | void PushBackColorMapResult(pcl::PointCloud mapCloud, Sophus::SE3f T); 36 | void PushBackNonColorMapResult(pcl::PointCloud mapCloud, Sophus::SE3f T); 37 | 38 | void SaveOdometryResult(); 39 | void SaveMapResult(); 40 | 41 | void PublishImg(cv::Mat image); 42 | void PublishTransform(Sophus::SE3f input); 43 | 44 | void PublishOdometryPoint(); 45 | void PublishColorMapPointCloud(); 46 | void PublishNonColorMapPointCloud(); 47 | 48 | private: 49 | Config &config_; 50 | 51 | pcl::PointCloud::Ptr odometryPointCloud_; 52 | pcl::PointCloud::Ptr mapColorPointCloud_; 53 | pcl::PointCloud::Ptr mapNonColorPointCloud_; 54 | 55 | octomap::ColorOcTree* colorTree_; 56 | octomap::OcTree* nonColorTree_; 57 | 58 | sensor_msgs::PointCloud2 odometryPC2; 59 | sensor_msgs::PointCloud2 mapPC2; 60 | 61 | ros::NodeHandle nh_; 62 | 63 | ros::Publisher transPub; 64 | ros::Publisher odometryPointPub; 65 | ros::Publisher colorMapPointCloudPub; 66 | ros::Publisher nonColorMapPointCloudPub; 67 | 68 | image_transport::ImageTransport it = image_transport::ImageTransport(ros::NodeHandle()); 69 | image_transport::Publisher imgPub; 70 | 71 | pcl::VoxelGrid voxelFilter_; 72 | double voxelSize_; 73 | PinholeModel pinholeModel_; 74 | }; 75 | 76 | 77 | 78 | #endif //DVL_SLAM_MODIFY_LOGGER_H 79 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(DVL_SLAM_ROS) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | #add_compile_options(-std=c++11) 9 | 10 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 11 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 12 | 13 | LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules ) 14 | 15 | find_package(catkin REQUIRED COMPONENTS 16 | image_transport 17 | laser_geometry 18 | pcl_conversions 19 | pcl_ros 20 | roscpp 21 | sensor_msgs 22 | cv_bridge 23 | nav_msgs 24 | octomap_msgs 25 | ) 26 | 27 | find_package(yaml-cpp REQUIRED) 28 | find_package(OpenCV REQUIRED) 29 | find_package(octomap REQUIRED) 30 | 31 | find_package(PCL REQUIRED) 32 | message("[PCL Version] : ${PCL_VERSION}") 33 | message("[PCL path] : ${PCL_INCLUDE_DIRS}") 34 | 35 | find_package(Eigen3 3.3 REQUIRED) 36 | message("{Eigen path} : ${EIGEN3_INCLUDE_DIR}") 37 | message("[Eigen Version] : ${Eigen3_VERSION}") 38 | 39 | find_package(Sophus REQUIRED) 40 | #find_package(DLib REQUIRED) 41 | #find_package(DBoW2 REQUIRED) 42 | #find_library(cholmod REQUIRED) 43 | find_package(G2O REQUIRED) 44 | 45 | 46 | catkin_package( 47 | ) 48 | 49 | include_directories( 50 | include 51 | ${catkin_INCLUDE_DIRS} 52 | ${EIGEN3_INCLUDE_DIR} 53 | ${Sophus_INCLUDE_DIR} 54 | ${PCL_INCLUDE_DIRS} 55 | # ${CHOLMOD_INCLUDE_DIR} 56 | ${G2O_INCLUDE_DIR} 57 | ${OpenCV_INCLUDE_DIRS} 58 | ${OCTOMAP_INCLUDE_DIRS} 59 | 60 | ) 61 | 62 | add_executable(dvl_slam_modify 63 | src/main.cpp 64 | src/Config.cpp 65 | src/Frame.cpp 66 | src/Tracker.cpp 67 | src/GraphOptimizer.cpp 68 | src/Sensor.cpp 69 | src/System.cpp 70 | src/KeyFrame.cpp 71 | src/PinholeModel.cpp 72 | src/SensorRos.cpp 73 | src/SensorSavedData.cpp 74 | src/Logger.cpp 75 | ) 76 | 77 | target_link_libraries(dvl_slam_modify 78 | ${catkin_LIBRARIES} 79 | yaml-cpp 80 | # ${DBoW2_LIBS} 81 | ${PCL_LIBRARIES} 82 | ${DLib_LIBS} 83 | ${CHOLMOD_LIBRARIES} 84 | ${G2O_STUFF_LIBRARY} 85 | ${G2O_CORE_LIBRARY} 86 | ${G2O_SOLVER_DENSE} 87 | ${G2O_SOLVER_EIGEN} 88 | ${G2O_SOLVER_CHOLMOD} 89 | ${G2O_SOLVER_CSPARSE} 90 | ${G2O_SOLVER_CSPARSE_EXTENSION} 91 | ${G2O_TYPES_SLAM3D} 92 | ${G2O_TYPES_SBA} 93 | ${OpenCV_LIBRARIES} 94 | ${OpenCV_LIBS} 95 | ${OCTOMAP_LIBRARIES} 96 | ) 97 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | DVL_SLAM_ROS 4 | 0.0.0 5 | The dvl_slam_modify package 6 | 7 | 8 | 9 | 10 | jongsik 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /cmake_modules/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (CHOLMOD_SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) 90 | -------------------------------------------------------------------------------- /include/Frame.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_FRAME_H 6 | #define DVL_SLAM_MODIFY_FRAME_H 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "Config.h" 13 | #include "Datatypes.h" 14 | #include "sophus/se3.hpp" 15 | #include 16 | #include "PinholeModel.h" 17 | 18 | void createImagePyramid(const cv::Mat& img_level_0, int n_levels, ImgPyramid& pyr); 19 | 20 | class Frame { 21 | public: 22 | typedef std::shared_ptr Ptr; 23 | 24 | Frame(Config &config); 25 | ~Frame(); 26 | 27 | cv::Mat PointCloud2Img(); 28 | 29 | void setImg(); 30 | void setPointCloud(); 31 | 32 | void showImg(cv::Mat& img); 33 | void saveImg(cv::Mat& img); 34 | 35 | cv::Mat& GetPyramidImg(size_t level); 36 | void SetOriginalImg(cv::Mat originalImg); 37 | cv::Mat GetOriginalImg(); 38 | 39 | pcl::PointCloud GetOriginalCloud(); 40 | void SetOriginalCloud(pcl::PointCloud& originalCloud); 41 | 42 | Sophus::SE3f GetTwc(); 43 | void SetTwc(Sophus::SE3f Twc); 44 | 45 | Eigen::Vector2f PointCloudXyz2Uv(Eigen::Vector3f point); 46 | std::vector PointCloudXyz2UvVec(const pcl::PointCloud& pc); 47 | 48 | cv::Mat pointCloudProjection(); 49 | 50 | void createImagePyramid(); 51 | 52 | template 53 | void pyrDownMeanSmooth(const cv::Mat& in, cv::Mat& out); 54 | 55 | inline static void jacobian_xyz2uv(const Eigen::Vector3f& xyzFloat, Matrix2x6& J) 56 | { 57 | const float x = xyzFloat[0]; 58 | const float y = xyzFloat[1]; 59 | const float zInv = 1./xyzFloat[2]; 60 | const float zInv2 = zInv*zInv; 61 | 62 | J(0,0) = -zInv; // -1/z 63 | J(0,1) = 0.0; // 0 64 | J(0,2) = x*zInv2; // x/z^2 65 | J(0,3) = y*J(0,2); // x*y/z^2 66 | J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2) 67 | J(0,5) = y*zInv; // y/z 68 | 69 | 70 | J(1,0) = 0.0; // 0 71 | J(1,1) = -zInv; // -1/z 72 | J(1,2) = y*zInv2; // y/z^2 73 | J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2 74 | J(1,4) = -J(0,3); // -x*y/z^2 75 | J(1,5) = -x*zInv; // x/z 76 | } 77 | private: 78 | Config &config_; 79 | 80 | cv::Mat originalImg_; 81 | cv::Mat gray_; 82 | pcl::PointCloud originalCloud_; 83 | 84 | Sophus::SE3f Twc_; 85 | 86 | ImgPyramid imgPyramid_; 87 | PinholeModel pinholeModel_; 88 | int numLevel_; 89 | }; 90 | 91 | 92 | class FrameDB 93 | { 94 | public: 95 | typedef std::shared_ptr Ptr; 96 | 97 | FrameDB() {} 98 | ~FrameDB() {} 99 | 100 | void Add(Frame::Ptr frame) { 101 | boost::unique_lock ul{DB_mutex_}; 102 | frameDB_.push_back(frame); 103 | ul.unlock(); 104 | } 105 | 106 | std::vector::iterator begin() { return frameDB_.begin(); } 107 | std::vector::iterator end() { return frameDB_.end(); } 108 | size_t size() { return frameDB_.size(); } 109 | std::vector& frameDB() { return frameDB_; } 110 | private: 111 | std::vector frameDB_; 112 | std::mutex DB_mutex_; 113 | }; 114 | 115 | #endif //DVL_SLAM_MODIFY_FRAME_H 116 | -------------------------------------------------------------------------------- /cmake_modules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 39 | PATHS 40 | ${SUITE_SPARSE_ROOT}/lib 41 | /usr/lib 42 | /usr/local/lib 43 | /opt/local/lib 44 | /sw/lib 45 | ) 46 | 47 | 48 | # Different platforms seemingly require linking against different sets of libraries 49 | IF(CYGWIN) 50 | FIND_PACKAGE(PkgConfig) 51 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 52 | PATHS 53 | /usr/lib 54 | /usr/local/lib 55 | /opt/local/lib 56 | /sw/lib 57 | ) 58 | PKG_CHECK_MODULES(LAPACK lapack) 59 | 60 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 61 | 62 | # MacPorts build of the SparseSuite requires linking against extra libraries 63 | 64 | ELSEIF(APPLE) 65 | 66 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 67 | PATHS 68 | /usr/lib 69 | /usr/local/lib 70 | /opt/local/lib 71 | /sw/lib 72 | ) 73 | 74 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 75 | PATHS 76 | /usr/lib 77 | /usr/local/lib 78 | /opt/local/lib 79 | /sw/lib 80 | ) 81 | 82 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 83 | PATHS 84 | /usr/lib 85 | /usr/local/lib 86 | /opt/local/lib 87 | /sw/lib 88 | ) 89 | 90 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 91 | ELSE(APPLE) 92 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 93 | ENDIF(CYGWIN) 94 | 95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 96 | SET(CHOLMOD_FOUND TRUE) 97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 98 | SET(CHOLMOD_FOUND FALSE) 99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 100 | 101 | # Look for csparse; note the difference in the directory specifications! 102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 103 | PATHS 104 | /usr/include/suitesparse 105 | /usr/include 106 | /opt/local/include 107 | /usr/local/include 108 | /sw/include 109 | /usr/include/ufsparse 110 | /opt/local/include/ufsparse 111 | /usr/local/include/ufsparse 112 | /sw/include/ufsparse 113 | ) 114 | 115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 116 | PATHS 117 | /usr/lib 118 | /usr/local/lib 119 | /opt/local/lib 120 | /sw/lib 121 | ) 122 | 123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 124 | SET(CSPARSE_FOUND TRUE) 125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 126 | SET(CSPARSE_FOUND FALSE) 127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 128 | -------------------------------------------------------------------------------- /src/SensorRos.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 12. 11.. 3 | // 4 | 5 | #include "SensorRos.h" 6 | 7 | SensorRos::SensorRos(Config &config) 8 | : Sensor(config) 9 | { 10 | std::string imgTopic; 11 | std::string pcTopic; 12 | if(config_.datasetConfig.isIndoor) 13 | { 14 | imgTopic = "/left_camera/image"; 15 | pcTopic = "/livox_pcl0"; 16 | } 17 | else if(config_.datasetConfig.isKitti) 18 | { 19 | imgTopic = "/kitti/camera_color_left/image_raw"; 20 | pcTopic = "/kitti/velo/pointcloud"; 21 | } 22 | 23 | imgSub = nh_.subscribe(imgTopic, 1, &SensorRos::ImgCb, this); 24 | pointCloudSub = nh_.subscribe(pcTopic, 1, &SensorRos::PcCb, this); 25 | } 26 | 27 | SensorRos::~SensorRos() 28 | { 29 | } 30 | 31 | void SensorRos::ImgCb(const sensor_msgs::ImageConstPtr& img) 32 | { 33 | cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, "bgr8"); 34 | inputImg_ = cv_ptr->image.clone(); 35 | imgFlag_ = true; 36 | } 37 | 38 | void SensorRos::PcCb(const sensor_msgs::PointCloud2ConstPtr& input) 39 | { 40 | pcl::PCLPointCloud2 pcl_pc2; 41 | pcl_conversions::toPCL(*input, pcl_pc2); 42 | pcl::fromPCLPointCloud2(pcl_pc2, *inputCloud_); 43 | lidarFlag_ = true; 44 | } 45 | 46 | void SensorRos::data2Frame(Frame& frame){ 47 | frame.SetOriginalImg(inputImg_); 48 | if(lidarFlag_) { 49 | pcl::PointCloud::Ptr transformedCloud = pcl::PointCloud::Ptr(new pcl::PointCloud); 50 | pcl::PointCloud::Ptr rectifiedCloud = pcl::PointCloud::Ptr(new pcl::PointCloud); 51 | 52 | 53 | Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); 54 | 55 | transform (0,0) = config_.extrinsicConfig.r11; 56 | transform (0,1) = config_.extrinsicConfig.r12; 57 | transform (0,2) = config_.extrinsicConfig.r13; 58 | 59 | transform (1,0) = config_.extrinsicConfig.r21; 60 | transform (1,1) = config_.extrinsicConfig.r22; 61 | transform (1,2) = config_.extrinsicConfig.r23; 62 | 63 | transform (2,0) = config_.extrinsicConfig.r31; 64 | transform (2,1) = config_.extrinsicConfig.r32; 65 | transform (2,2) = config_.extrinsicConfig.r33; 66 | 67 | transform (0,3) = config_.extrinsicConfig.delX; 68 | transform (1,3) = config_.extrinsicConfig.delY; 69 | transform (2,3) = config_.extrinsicConfig.delZ; 70 | 71 | pcl::transformPointCloud (*inputCloud_, *transformedCloud, transform); 72 | 73 | Eigen::Matrix4f transformRect = Eigen::Matrix4f::Identity(); 74 | 75 | transformRect (0,0) = config_.rectifyingConfig.R11; 76 | transformRect (0,1) = config_.rectifyingConfig.R12; 77 | transformRect (0,2) = config_.rectifyingConfig.R13; 78 | 79 | transformRect (1,0) = config_.rectifyingConfig.R21; 80 | transformRect (1,1) = config_.rectifyingConfig.R22; 81 | transformRect (1,2) = config_.rectifyingConfig.R23; 82 | 83 | transformRect (2,0) = config_.rectifyingConfig.R31; 84 | transformRect (2,1) = config_.rectifyingConfig.R32; 85 | transformRect (2,2) = config_.rectifyingConfig.R33; 86 | 87 | transformRect (0,3) = 0; 88 | transformRect (1,3) = 0; 89 | transformRect (2,3) = 0; 90 | 91 | pcl::transformPointCloud (*transformedCloud, *rectifiedCloud, transformRect); 92 | 93 | pcl::PassThrough pass; 94 | pass.setInputCloud(rectifiedCloud); 95 | pass.setFilterFieldName("z"); 96 | pass.setFilterLimits(minZ, maxZ); 97 | pass.filter(*rectifiedCloud); 98 | 99 | 100 | frame.SetOriginalCloud(*rectifiedCloud); 101 | } 102 | 103 | } -------------------------------------------------------------------------------- /include/Tracker.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #ifndef DVL_SLAM_MODIFY_TRACKER_H 6 | #define DVL_SLAM_MODIFY_TRACKER_H 7 | 8 | #include "Config.h" 9 | #include "Frame.h" 10 | #include "KeyFrame.h" 11 | #include 12 | #include "Datatypes.h" 13 | #include "PinholeModel.h" 14 | 15 | class Tracker{ 16 | 17 | static const int patchHalfsize_ = 2; 18 | static const int patchSize_ = 2*patchHalfsize_; 19 | static const int patternLength_ = 8; 20 | int pattern_[8][2] = { {0, 0}, {2, 0}, {1, 1}, {0, -2}, {-1, -1}, {-2, 0}, {-1, 1}, {0, 2} }; 21 | 22 | public: 23 | 24 | Tracker(Config &config); 25 | ~Tracker(); 26 | 27 | Config& config_; 28 | 29 | bool Solve(); 30 | void UpdatePose(const Sophus::SE3f& old_Tji, Sophus::SE3f& Tji); 31 | void Optimize(Sophus::SE3f& Tji); 32 | float HuberWeight(const float res); 33 | void CheckVisiblePointsInPrevFrame(Frame::Ptr currFrame, Sophus::SE3f& transformation); 34 | cv::Mat PrecomputePatches(cv::Mat& img, pcl::PointCloud& pointcloud, cv::Mat& patch_buf, bool is_derivative); 35 | double ComputeResiduals(Sophus::SE3f& transformation); 36 | void trackFrame2Frame(Frame::Ptr currFrame, KeyFrame::Ptr refFrame, Sophus::SE3f& transformation); 37 | 38 | inline double NormMax(const Vector6& v) 39 | { 40 | double max = -1; 41 | for (int i=0; imax){ 45 | max = abs; 46 | } 47 | } 48 | return max; 49 | } 50 | 51 | inline float compute(cv::Mat& errors) 52 | { 53 | float initial_sigma_ = 5.0f; 54 | float dof_ = 5.0f; 55 | float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); 56 | int num = 0; 57 | float lambda = initial_lamda; 58 | int iterations = 0; 59 | do 60 | { 61 | ++iterations; 62 | initial_lamda = lambda; 63 | num = 0.0f; 64 | lambda = 0.0f; 65 | 66 | const float* data_ptr = errors.ptr(); 67 | 68 | for(size_t idx = 0; idx < errors.size().area(); ++idx, ++data_ptr) 69 | { 70 | const float& data = *data_ptr; 71 | 72 | if(std::isfinite(data)) 73 | { 74 | ++num; 75 | lambda += data * data * ( (dof_ + 1.0f) / (dof_ + initial_lamda * data * data) ); 76 | } 77 | } 78 | 79 | lambda = float(num) / lambda; 80 | } while(std::abs(lambda - initial_lamda) > 1e-3); 81 | 82 | return std::sqrt(1.0f / lambda); 83 | } 84 | 85 | inline float calcWeight(const float &res) 86 | { 87 | float dof_ = 5.0f; 88 | return (dof_ + 1.0) / (dof_ + res*res); 89 | } 90 | private: 91 | Frame::Ptr currentFrame_; 92 | KeyFrame::Ptr referenceFrame_; 93 | PinholeModel pinholeModel_; 94 | 95 | float huberK_; 96 | 97 | Vector6 x_; 98 | std::vector J_; 99 | Vector6 Jres_; 100 | Matrix6x6 H_; 101 | Matrix6x6 prev_H_; 102 | 103 | std::vector weight_; 104 | cv::Mat refPatchBuf_; 105 | cv::Mat currPatchBuf_; 106 | 107 | std::vector errors_; 108 | 109 | int minLevel_; 110 | int maxLevel_; 111 | int currentLevel_; 112 | 113 | Eigen::Matrix dIBuf_; 114 | Eigen::Matrix jacobianBuf_; 115 | 116 | float scale_; 117 | 118 | int maxIteration; 119 | float residual_; 120 | float normXThres_; 121 | bool status_; 122 | 123 | 124 | float affine_a_ = 1; 125 | float affine_b_ = 0; 126 | bool stop_; 127 | bool isPreComputed_; 128 | 129 | cv::Mat refImgClone; 130 | cv::Mat currImgClone; 131 | }; 132 | 133 | 134 | #endif //DVL_SLAM_MODIFY_TRACKER_H 135 | -------------------------------------------------------------------------------- /cmake_modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /src/Config.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 28.. 3 | // 4 | 5 | #include "Config.h" 6 | #include 7 | 8 | Config::Config() 9 | { 10 | 11 | YAML::Node yamlFile = YAML::LoadFile("/home/chunran/dvl_ws/src/DVL_SLAM_ROS/yaml/defalut_param.yaml"); 12 | ReadEveryParameter(yamlFile); 13 | } 14 | 15 | 16 | Config::~Config() 17 | { 18 | 19 | 20 | } 21 | 22 | 23 | void Config::ReadEveryParameter(const YAML::Node yamlFile) 24 | { 25 | std::cout<<"111111111111"<(); 40 | datasetConfig.isIndoor = datasetYaml["isIndoor"].as(); 41 | datasetConfig.useRos = datasetYaml["useRos"].as(); 42 | // datasetConfig.imgDir = datasetYaml["imgDir"].as(); 43 | // datasetConfig.lidarDir = datasetYaml["lidarDir"].as(); 44 | datasetConfig.visualize = datasetYaml["visualize"].as(); 45 | 46 | cameraConfig.fx = cameraYaml["fx"].as(); 47 | cameraConfig.fy = cameraYaml["fy"].as(); 48 | cameraConfig.cx = cameraYaml["cx"].as(); 49 | cameraConfig.cy = cameraYaml["cy"].as(); 50 | cameraConfig.k1 = cameraYaml["k1"].as(); 51 | cameraConfig.k2 = cameraYaml["k2"].as(); 52 | cameraConfig.p1 = cameraYaml["p1"].as(); 53 | cameraConfig.p2 = cameraYaml["p2"].as(); 54 | cameraConfig.k3 = cameraYaml["k3"].as(); 55 | 56 | std::cout<<"5555555555555"<(); 59 | extrinsicConfig.delY = extrinsicYaml["delY"].as(); 60 | extrinsicConfig.delZ = extrinsicYaml["delZ"].as(); 61 | 62 | extrinsicConfig.r11 = extrinsicYaml["r11"].as(); 63 | extrinsicConfig.r12 = extrinsicYaml["r12"].as(); 64 | extrinsicConfig.r13 = extrinsicYaml["r13"].as(); 65 | extrinsicConfig.r21 = extrinsicYaml["r21"].as(); 66 | extrinsicConfig.r22 = extrinsicYaml["r22"].as(); 67 | extrinsicConfig.r23 = extrinsicYaml["r23"].as(); 68 | extrinsicConfig.r31 = extrinsicYaml["r31"].as(); 69 | extrinsicConfig.r32 = extrinsicYaml["r32"].as(); 70 | extrinsicConfig.r33 = extrinsicYaml["r33"].as(); 71 | 72 | rectifyingConfig.R11 = rectifyingYaml["R11"].as(); 73 | rectifyingConfig.R12 = rectifyingYaml["R12"].as(); 74 | rectifyingConfig.R13 = rectifyingYaml["R13"].as(); 75 | rectifyingConfig.R21 = rectifyingYaml["R21"].as(); 76 | rectifyingConfig.R22 = rectifyingYaml["R22"].as(); 77 | rectifyingConfig.R23 = rectifyingYaml["R23"].as(); 78 | rectifyingConfig.R31 = rectifyingYaml["R31"].as(); 79 | rectifyingConfig.R32 = rectifyingYaml["R32"].as(); 80 | rectifyingConfig.R33 = rectifyingYaml["R33"].as(); 81 | 82 | std::cout<<"3333333333333333333"<(); 85 | imageConfig.height = imageYaml["height"].as(); 86 | 87 | trackerConfig.imgPyramidMinLevel = trackerYaml["imgPyramidMinLevel"].as(); 88 | trackerConfig.imgPyramidMaxLevel = trackerYaml["imgPyramidMaxLevel"].as(); 89 | trackerConfig.maxIteration = trackerYaml["maxIteration"].as(); 90 | trackerConfig.normXThresForIteration = trackerYaml["normXThresForIteration"].as(); 91 | trackerConfig.border = trackerYaml["border"].as(); 92 | 93 | pointcloudConfig.minZ = pointcloudYaml["minZ"].as(); 94 | pointcloudConfig.maxZ = pointcloudYaml["maxZ"].as(); 95 | 96 | loggerConfig.voxelSize = loggerYaml["voxelSize"].as(); 97 | 98 | systemConfig.ratioThres = systemYaml["ratioThres"].as(); 99 | std::cout<<"44444444444444444"<(cloudVec.front())); 28 | imgVec.pop(); 29 | cloudVec.pop(); 30 | 31 | frame.SetOriginalImg(inputImg_); 32 | 33 | pcl::PointCloud::Ptr transformedCloud = pcl::PointCloud::Ptr(new pcl::PointCloud); 34 | pcl::PointCloud::Ptr rectifiedCloud = pcl::PointCloud::Ptr(new pcl::PointCloud); 35 | 36 | Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); 37 | 38 | transform(0, 0) = config_.extrinsicConfig.r11; 39 | transform(0, 1) = config_.extrinsicConfig.r12; 40 | transform(0, 2) = config_.extrinsicConfig.r13; 41 | 42 | transform(1, 0) = config_.extrinsicConfig.r21; 43 | transform(1, 1) = config_.extrinsicConfig.r22; 44 | transform(1, 2) = config_.extrinsicConfig.r23; 45 | 46 | transform(2, 0) = config_.extrinsicConfig.r31; 47 | transform(2, 1) = config_.extrinsicConfig.r32; 48 | transform(2, 2) = config_.extrinsicConfig.r33; 49 | 50 | transform(0, 3) = config_.extrinsicConfig.delX; 51 | transform(1, 3) = config_.extrinsicConfig.delY; 52 | transform(2, 3) = config_.extrinsicConfig.delZ; 53 | 54 | pcl::transformPointCloud(*inputCloud_, *transformedCloud, transform); 55 | 56 | Eigen::Matrix4f transformRect = Eigen::Matrix4f::Identity(); 57 | 58 | transformRect(0, 0) = config_.rectifyingConfig.R11; 59 | transformRect(0, 1) = config_.rectifyingConfig.R12; 60 | transformRect(0, 2) = config_.rectifyingConfig.R13; 61 | 62 | transformRect(1, 0) = config_.rectifyingConfig.R21; 63 | transformRect(1, 1) = config_.rectifyingConfig.R22; 64 | transformRect(1, 2) = config_.rectifyingConfig.R23; 65 | 66 | transformRect(2, 0) = config_.rectifyingConfig.R31; 67 | transformRect(2, 1) = config_.rectifyingConfig.R32; 68 | transformRect(2, 2) = config_.rectifyingConfig.R33; 69 | 70 | transformRect(0, 3) = 0; 71 | transformRect(1, 3) = 0; 72 | transformRect(2, 3) = 0; 73 | 74 | pcl::transformPointCloud(*transformedCloud, *rectifiedCloud, transformRect); 75 | 76 | pcl::PassThrough pass; 77 | pass.setInputCloud(rectifiedCloud); 78 | pass.setFilterFieldName("z"); 79 | pass.setFilterLimits(minZ, maxZ); 80 | pass.filter(*rectifiedCloud); 81 | 82 | frame.SetOriginalCloud(*rectifiedCloud); 83 | } 84 | 85 | void SensorSavedData::loadImg() 86 | { 87 | std::vector fn; 88 | std::string filename = config_.datasetConfig.imgDir + "*.png"; 89 | glob(filename.c_str(), fn, false); 90 | 91 | size_t count = fn.size(); 92 | for(size_t i=0; i filenameVec; 102 | copy(boost::filesystem::directory_iterator(lidarDir), boost::filesystem::directory_iterator(), std::back_inserter(filenameVec)); 103 | std::sort(filenameVec.begin(), filenameVec.end()); 104 | 105 | for(auto& boostFilename:filenameVec){ 106 | 107 | pcl::PointCloud occupiedCloud; 108 | pcl::PointXYZRGB occupiedPoint; 109 | std::string str_buf; 110 | float f_buf; 111 | std::fstream fs; 112 | std::string filename = boostFilename.string(); 113 | fs.open(filename.c_str(), std::ios::in); 114 | int i=0; 115 | while (!fs.eof()){ 116 | getline(fs, str_buf, ','); 117 | if(str_buf.size() > 2){ 118 | f_buf = stof(str_buf); 119 | if (i%4 == 0){ 120 | occupiedPoint.x = f_buf; 121 | } 122 | else if(i%4 == 1){ 123 | occupiedPoint.y = f_buf; 124 | } 125 | else if(i%4 == 2){ 126 | occupiedPoint.z = f_buf; 127 | occupiedCloud.points.push_back(occupiedPoint); 128 | } 129 | i++; 130 | } 131 | } 132 | cloudVec.push(occupiedCloud); 133 | } 134 | } -------------------------------------------------------------------------------- /src/Frame.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #include 6 | 7 | Frame::Frame(Config &config) 8 | : config_(config) 9 | , pinholeModel_(config) 10 | { 11 | numLevel_ = config_.trackerConfig.imgPyramidMaxLevel - config.trackerConfig.imgPyramidMinLevel + 1; 12 | } 13 | 14 | Frame::~Frame(){ 15 | imgPyramid_.clear(); 16 | } 17 | template 18 | void Frame::pyrDownMeanSmooth(const cv::Mat& in, cv::Mat& out) 19 | { 20 | for(int y = 0; y < out.rows; ++y) 21 | { 22 | for(int x = 0; x < out.cols; ++x) 23 | { 24 | int x0 = x * 2; 25 | int x1 = x0 + 1; 26 | int y0 = y * 2; 27 | int y1 = y0 + 1; 28 | out.at(y, x) = (T) ( (in.at(y0, x0) + in.at(y0, x1) + in.at(y1, x0) + in.at(y1, x1)) / 4.0f ); 29 | } 30 | } 31 | } 32 | 33 | void Frame::createImagePyramid() 34 | { 35 | imgPyramid_.resize(numLevel_); 36 | imgPyramid_[0] = gray_; 37 | 38 | for(int i=1; i(imgPyramid_[i-1], imgPyramid_[i]); 42 | } 43 | } 44 | 45 | cv::Mat& Frame::GetPyramidImg(size_t level){ return imgPyramid_[level]; } 46 | 47 | cv::Mat Frame::GetOriginalImg(){ return originalImg_; } 48 | 49 | void Frame::SetOriginalImg(cv::Mat originalImg){ 50 | this->originalImg_ = originalImg; 51 | originalImg_.convertTo(originalImg_, CV_32FC3, 1.0/255); 52 | cvtColor(originalImg, gray_, cv::COLOR_BGR2GRAY); 53 | gray_.convertTo(gray_, CV_32FC1, 1.0/255); 54 | 55 | createImagePyramid(); 56 | } 57 | 58 | pcl::PointCloud Frame::GetOriginalCloud(){ return originalCloud_; } 59 | 60 | void Frame::SetOriginalCloud(pcl::PointCloud& originalCloud) 61 | { 62 | originalCloud_.clear(); 63 | int border = config_.trackerConfig.border; 64 | std::vector uvSet = pinholeModel_.PointCloudXyz2UvVec(originalCloud, 1); 65 | auto pointCloudIter = originalCloud.begin(); 66 | pcl::PointXYZRGB temp; 67 | for(auto uvIter=uvSet.begin(); uvIter!=uvSet.end(); ++uvIter, ++pointCloudIter){ 68 | Eigen::Vector2f uv = *uvIter; 69 | 70 | const float uFloat = uv(0); 71 | const float vFloat = uv(1); 72 | const int uInt = static_cast (uFloat); 73 | const int vInt = static_cast (vFloat); 74 | 75 | if(uInt - border < 0 || uInt + border > config_.imageConfig.width || vInt - border < 0 || vInt + border > config_.imageConfig.height || pointCloudIter->z <= 0.0){ 76 | continue; 77 | } 78 | temp.b = 255*originalImg_.at(vInt, uInt)(0); 79 | temp.g = 255*originalImg_.at(vInt, uInt)(1); 80 | temp.r = 255*originalImg_.at(vInt, uInt)(2); 81 | temp.x = pointCloudIter->x; 82 | temp.y = pointCloudIter->y; 83 | temp.z = pointCloudIter->z; 84 | 85 | originalCloud_.push_back(temp); 86 | // std::cout << "pointCloudIter->b = 255*originalImg_.at(vInt, uInt)(0)" << 255*originalImg_.at(vInt, uInt) << std::endl; 87 | // std::cout << "pointCloudIter->b = 255*originalImg_.at(vInt, uInt)(0)" << 255*originalImg_.at(vInt, uInt)(0) << std::endl; 88 | // std::cout << "pointCloudIter->b = 255*originalImg_.at(vInt, uInt)(1)" << 255*originalImg_.at(vInt, uInt)(1) << std::endl; 89 | // std::cout << "pointCloudIter->b = 255*originalImg_.at(vInt, uInt)(2)" << 255*originalImg_.at(vInt, uInt)(2) << std::endl; 90 | 91 | } 92 | 93 | } 94 | 95 | Sophus::SE3f Frame::GetTwc(){ return Twc_; } 96 | 97 | void Frame::SetTwc(Sophus::SE3f Twc){ this->Twc_ = Twc; } 98 | 99 | cv::Mat Frame::pointCloudProjection() 100 | { 101 | 102 | cv::Mat projectedImg = originalImg_.clone(); 103 | 104 | for(int i=0; i v_max) v = v_max; 114 | 115 | if(v < v_min + 0.25*dv) { 116 | r = 0.0; 117 | g = 4*(v - v_min) / dv; 118 | } 119 | else if (v < (v_min + 0.5 * dv)) { 120 | r = 0.0; 121 | b = 1 + 4*(v_min + 0.25 * dv - v) / dv; 122 | } 123 | else if (v < (v_min + 0.75 * dv)) { 124 | r =4 * (v - v_min - 0.5 * dv) / dv; 125 | b = 0.0; 126 | } 127 | else { 128 | g = 1 + 4*(v_min + 0.75 * dv - v) / dv; 129 | b = 0.0; 130 | } 131 | 132 | cv::circle(projectedImg, cv::Point(U, V), 2, cv::Scalar(255*r, 255*g, 255*b), 1); 133 | } 134 | return projectedImg; 135 | } 136 | 137 | -------------------------------------------------------------------------------- /src/System.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | 5 | #include "System.h" 6 | 7 | System::System(Config& config) 8 | : config_(config), 9 | graphOptimizer_(config), 10 | tracker_(config), 11 | logger_(config), 12 | Tij_(Eigen::Matrix3f::Identity(), Eigen::Vector3f(0.0f, 0.0f, 0.0f)), 13 | Tji_(Eigen::Matrix3f::Identity(), Eigen::Vector3f(0.0f, 0.0f, 0.0f)), 14 | dTji_(Eigen::Matrix3f::Identity(), Eigen::Vector3f(0.0f, 0.0f, 0.0f)) 15 | { 16 | initialized_ = false; 17 | num = 0; 18 | 19 | keyFrameDB_.reset(new KeyFrameDB()); 20 | frameDB_.reset(new FrameDB()); 21 | if(config_.datasetConfig.useRos) { sensor_ = new SensorRos(config); } 22 | else { sensor_ = new SensorSavedData(config); } 23 | } 24 | 25 | System::~System() 26 | { 27 | 28 | } 29 | 30 | void System::Run(){ 31 | Frame::Ptr currFrame (new Frame(config_)); 32 | if(!sensor_->IsLidarSubscribed() || !sensor_->IsVisionSubscribed()){ 33 | std::cout << "[System] No sensor data subscribed" << std::endl; 34 | return; 35 | } 36 | sensor_->data2Frame(*currFrame); 37 | 38 | std::cout << "[System] frame class got data from sensor class" << std::endl; 39 | 40 | if(!initialized_){ 41 | Eigen::Matrix3f rot = Eigen::Matrix3f::Identity(); 42 | Eigen::Vector3f twc(0, 0, 0); 43 | 44 | Sophus::SE3f initialTwc(rot, twc); 45 | currFrame->SetTwc(initialTwc); 46 | 47 | KeyFrame::Ptr keyFrame(new KeyFrame(config_, currFrame)); 48 | frameDB_->Add(currFrame); 49 | keyFrameDB_->Add(keyFrame); 50 | 51 | initialized_ = true; 52 | 53 | pcl::PointXYZ odometryLogger; 54 | odometryLogger.x = initialTwc.translation()[0]; 55 | odometryLogger.y = initialTwc.translation()[1]; 56 | odometryLogger.z = initialTwc.translation()[2]; 57 | 58 | logger_.PushBackOdometryResult(odometryLogger); 59 | logger_.PushBackColorMapResult(currFrame->GetOriginalCloud(), initialTwc); 60 | logger_.PushBackNonColorMapResult(currFrame->GetOriginalCloud(), initialTwc); 61 | logger_.PublishOdometryPoint(); 62 | logger_.PublishNonColorMapPointCloud(); 63 | logger_.PublishColorMapPointCloud(); 64 | 65 | std::cout << "[System] Initialized" << std::endl; 66 | return; 67 | } 68 | else{ 69 | 70 | std::cout << "[System] Find KeyFrame from KeyFrame DB" << std::endl; 71 | 72 | KeyFrame::Ptr lastKeyFrame = keyFrameDB_->LatestKeyframe(); 73 | 74 | Sophus::SE3f prevTji = Tji_; 75 | Tji_ = Tji_ * dTji_; 76 | 77 | tracker_.trackFrame2Frame(currFrame, lastKeyFrame, Tji_); 78 | dTji_ = Tji_ * prevTji.inverse(); 79 | Tij_ = Tji_.inverse(); 80 | std::cout << "[System] Tracking Finished" << std::endl; 81 | 82 | Sophus::SE3f Twc = lastKeyFrame->frame->GetTwc(); 83 | Sophus::SE3f T = Twc * Tij_; 84 | 85 | std::ofstream foutC("/home/chunran/CameraTrajectory.txt", std::ios::app|std::ios::out); 86 | foutC.setf(std::ios::fixed, std::ios::floatfield); 87 | foutC.precision(9); //9 timestamp seconds 88 | foutC << num << " "; 89 | num = num + 0.1; 90 | foutC.precision(5); 91 | 92 | Eigen::Matrix R = T.rotationMatrix(); 93 | Eigen::Vector3f t = T.translation(); 94 | 95 | Eigen::Quaternionf q(R); 96 | q.normalized(); 97 | 98 | float x = t(0); 99 | float y = t(1); 100 | float z = t(2); 101 | float qx = q.x(); 102 | float qy = q.y(); 103 | float qz = q.z(); 104 | float qw = q.w(); 105 | 106 | foutC << x <<" " << y << " " << z << " " << qx << " " << qy << " " << qz << " " << qw << std::endl; 107 | 108 | std::cout << "[System] T = " << std::endl; 109 | std::cout << T.matrix() << std::endl; 110 | 111 | currFrame->SetTwc(Twc * Tij_); 112 | 113 | float ratio_threshold = config_.systemConfig.ratioThres; 114 | std::cout << "[System] Find Keyframe" << std::endl; 115 | 116 | KeyFrame::Ptr currentKeyframe(new KeyFrame(config_, currFrame)); 117 | 118 | float visible_ratio1 = lastKeyFrame->GetVisibleRatio(currentKeyframe); 119 | float visible_ratio2 = currentKeyframe->GetVisibleRatio(lastKeyFrame); 120 | 121 | bool is_keyframe = (visible_ratio1 < ratio_threshold ? true : false) || ((visible_ratio2 < ratio_threshold ? true : false)); 122 | 123 | std::cout << "[System] KeyFrame Decision calculated" << std::endl; 124 | 125 | if(is_keyframe) 126 | { 127 | keyFrameDB_->Add(currentKeyframe); 128 | pcl::PointXYZ odometryLogger; 129 | odometryLogger.x = T.translation()[0]; 130 | odometryLogger.y = T.translation()[1]; 131 | odometryLogger.z = T.translation()[2]; 132 | 133 | 134 | logger_.PushBackOdometryResult(odometryLogger); 135 | logger_.PushBackColorMapResult(currFrame->GetOriginalCloud(), T); 136 | logger_.PushBackNonColorMapResult(currFrame->GetOriginalCloud(), T); 137 | logger_.PublishOdometryPoint(); 138 | logger_.PublishNonColorMapPointCloud(); 139 | logger_.PublishColorMapPointCloud(); 140 | 141 | Tji_.rotationMatrix() = Eigen::Matrix3f::Identity(); 142 | Tji_.translation() = Eigen::Vector3f(0.0f, 0.0f, 0.0f); 143 | 144 | std::cout << "[System] Add KeyFrame" << std::endl; 145 | 146 | } 147 | 148 | std::cout << "[System] Finished" << std::endl; 149 | 150 | // sensor_->publishTransform(Twc * Tij_); 151 | } 152 | 153 | } -------------------------------------------------------------------------------- /src/Logger.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 12. 18.. 3 | // 4 | 5 | #include "Logger.h" 6 | 7 | Logger::Logger(Config& config) 8 | : pinholeModel_(config) 9 | , config_(config) 10 | { 11 | mapColorPointCloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); 12 | mapNonColorPointCloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); 13 | 14 | odometryPointCloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); 15 | 16 | imgPub = it.advertise("/camera/image", 1); 17 | transPub = nh_.advertise("/pose_result", 1); 18 | odometryPointPub = nh_.advertise("/odometry_result", 1); 19 | colorMapPointCloudPub = nh_.advertise("/color_map_result", 1); 20 | nonColorMapPointCloudPub = nh_.advertise("/noncolor_octomap_result", 1); 21 | 22 | voxelSize_ = config_.loggerConfig.voxelSize; 23 | 24 | colorTree_ = new octomap::ColorOcTree(voxelSize_); 25 | nonColorTree_ = new octomap::OcTree(voxelSize_); 26 | } 27 | 28 | Logger::~Logger() 29 | { 30 | 31 | } 32 | 33 | void Logger::PushBackOdometryResult(pcl::PointXYZ odometryPoint) 34 | { 35 | this->odometryPointCloud_->push_back(odometryPoint); 36 | } 37 | 38 | void Logger::PushBackColorMapResult(pcl::PointCloud mapCloud, Sophus::SE3f T) 39 | { 40 | mapColorPointCloud_->clear(); 41 | pcl::PointCloud temp; 42 | 43 | int border = config_.trackerConfig.border; 44 | 45 | std::vector uvSet = pinholeModel_.PointCloudXyz2UvVec(mapCloud, 1); 46 | auto pointCloudIter = mapCloud.begin(); 47 | for(auto uvIter=uvSet.begin(); uvIter!=uvSet.end(); ++uvIter, ++pointCloudIter) 48 | { 49 | Eigen::Vector2f uv = *uvIter; 50 | 51 | const float uFloat = uv(0); 52 | const float vFloat = uv(1); 53 | const int uInt = static_cast (uFloat); 54 | const int vInt = static_cast (vFloat); 55 | 56 | if(uInt - border < 0 || uInt + border > config_.imageConfig.width || vInt - border < 0 || vInt + border > config_.imageConfig.height || pointCloudIter->z <= 0.0){ 57 | continue; 58 | } 59 | 60 | pcl::PointXYZRGB point; 61 | point.x = pointCloudIter->x; 62 | point.y = pointCloudIter->y; 63 | point.z = pointCloudIter->z; 64 | 65 | point.r = pointCloudIter->r; 66 | point.g = pointCloudIter->g; 67 | point.b = pointCloudIter->b; 68 | 69 | temp.push_back(point); 70 | } 71 | 72 | pcl::PointCloud transformedTemp; 73 | 74 | pcl::transformPointCloud(temp, transformedTemp, T.matrix()); 75 | 76 | for (auto p:transformedTemp.points) 77 | { 78 | // The point cloud of points into the octomap 79 | colorTree_->updateNode( octomap::point3d(p.x, p.y, p.z), true ); 80 | } 81 | for (auto p:transformedTemp.points) 82 | { 83 | colorTree_->integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b ); 84 | } 85 | 86 | colorTree_->updateInnerOccupancy(); 87 | 88 | pcl::PointXYZRGB Point; 89 | for (octomap::ColorOcTree::leaf_iterator it = colorTree_->begin_leafs(), end = colorTree_->end_leafs(); it != end; ++it) { 90 | if (colorTree_->isNodeOccupied(*it)) { 91 | Point.x = it.getX(); 92 | Point.y = it.getY(); 93 | Point.z = it.getZ(); 94 | Point.r = it->getColor().r; 95 | Point.g = it->getColor().g; 96 | Point.b = it->getColor().b; 97 | 98 | mapColorPointCloud_->push_back(Point); 99 | } 100 | } 101 | } 102 | 103 | void Logger::PushBackNonColorMapResult(pcl::PointCloud mapCloud, Sophus::SE3f T) 104 | { 105 | mapNonColorPointCloud_->clear(); 106 | pcl::PointCloud temp; 107 | pcl::transformPointCloud(mapCloud, temp, T.matrix()); 108 | 109 | for (auto p:temp.points) 110 | { 111 | nonColorTree_->updateNode( octomap::point3d(p.x, p.y, p.z), true ); 112 | } 113 | 114 | nonColorTree_->updateInnerOccupancy(); 115 | 116 | pcl::PointXYZ Point; 117 | 118 | for (octomap::OcTree::leaf_iterator it = nonColorTree_->begin_leafs(), end = nonColorTree_->end_leafs(); it != end; ++it) { 119 | if (nonColorTree_->isNodeOccupied(*it)) { 120 | Point.x = it.getX(); 121 | Point.y = it.getY(); 122 | Point.z = it.getZ(); 123 | mapNonColorPointCloud_->push_back(Point); 124 | } 125 | } 126 | } 127 | 128 | void Logger::SaveMapResult() 129 | { 130 | 131 | } 132 | 133 | void Logger::SaveOdometryResult() 134 | { 135 | 136 | } 137 | 138 | void Logger::PublishImg(cv::Mat image){ 139 | cv_bridge::CvImage img_bridge; 140 | sensor_msgs::Image img_msg; 141 | std_msgs::Header header; 142 | header.stamp = ros::Time::now(); 143 | img_bridge = cv_bridge::CvImage(header, "bgr8", image); 144 | img_bridge.toImageMsg(img_msg); 145 | imgPub.publish(img_msg); 146 | } 147 | 148 | void Logger::PublishTransform(Sophus::SE3f input){ 149 | 150 | geometry_msgs::PoseStamped msg; 151 | msg.pose.position.x = input.translation().x(); 152 | msg.pose.position.y = input.translation().y(); 153 | msg.pose.position.z = input.translation().z(); 154 | msg.pose.orientation.x = input.unit_quaternion().x(); 155 | msg.pose.orientation.y = input.unit_quaternion().y(); 156 | msg.pose.orientation.z = input.unit_quaternion().z(); 157 | msg.pose.orientation.w = input.unit_quaternion().w(); 158 | msg.header.frame_id = "world"; 159 | 160 | // geometry_msgs::Transform msg; 161 | // msg.translation.x = input.translation().x(); 162 | // msg.translation.y = input.translation().y(); 163 | // msg.translation.y = input.translation().y(); 164 | // msg.rotation.x = input.unit_quaternion().x(); 165 | // msg.rotation.x = input.unit_quaternion().y(); 166 | // msg.rotation.x = input.unit_quaternion().z(); 167 | // msg.rotation.x = input.unit_quaternion().w(); 168 | 169 | transPub.publish(msg); 170 | } 171 | 172 | void Logger::PublishOdometryPoint(){ 173 | pcl::toROSMsg(*odometryPointCloud_, odometryPC2); 174 | odometryPC2.header.frame_id = "world"; 175 | odometryPC2.header.stamp = ros::Time::now(); 176 | odometryPointPub.publish(odometryPC2); 177 | } 178 | 179 | void Logger::PublishNonColorMapPointCloud(){ 180 | pcl::toROSMsg(*mapNonColorPointCloud_, mapPC2); 181 | mapPC2.header.frame_id = "world"; 182 | mapPC2.header.stamp = ros::Time::now(); 183 | nonColorMapPointCloudPub.publish(mapPC2); 184 | } 185 | 186 | void Logger::PublishColorMapPointCloud(){ 187 | pcl::toROSMsg(*mapColorPointCloud_, mapPC2); 188 | mapPC2.header.frame_id = "world"; 189 | mapPC2.header.stamp = ros::Time::now(); 190 | colorMapPointCloudPub.publish(mapPC2); 191 | } 192 | 193 | -------------------------------------------------------------------------------- /src/Tracker.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jongsik on 20. 10. 30.. 3 | // 4 | #include "Tracker.h" 5 | 6 | Tracker::Tracker(Config &config) 7 | : config_(config) 8 | , pinholeModel_(config) 9 | { 10 | huberK_ = 1.345; 11 | residual_ = 0; 12 | 13 | normXThres_ = config_.trackerConfig.normXThresForIteration; 14 | minLevel_ = config_.trackerConfig.imgPyramidMinLevel; 15 | maxLevel_ = config_.trackerConfig.imgPyramidMaxLevel; 16 | maxIteration = config_.trackerConfig.maxIteration; 17 | 18 | } 19 | 20 | Tracker::~Tracker(){ 21 | 22 | } 23 | 24 | bool Tracker::Solve(){ 25 | x_ = H_.ldlt().solve(Jres_); 26 | // std::cout << "[Optimize] Jres_ = " << Jres_ << std::endl; 27 | // std::cout << "[Optimize] H_ = " << H_ << std::endl; 28 | // std::cout << "[Optimize] x_ = " << x_ << std::endl; 29 | 30 | 31 | if ( ((x_ - x_).array() == (x_ - x_).array()).all() ) 32 | return true; 33 | return false; 34 | } 35 | 36 | void Tracker::UpdatePose(const Sophus::SE3f& old_Tji, Sophus::SE3f& Tji){ 37 | Tji = old_Tji * Sophus::SE3f::exp(-x_); 38 | // std::cout << "[Tracker] old_Tji = " << old_Tji.matrix() << std::endl; 39 | // std::cout << "[Tracker] Sophus::SE3f::exp(-x_) = " << Sophus::SE3f::exp(-x_).matrix() << std::endl; 40 | // std::cout << "[Tracker] Tji = " << Tji.matrix() << std::endl; 41 | } 42 | 43 | void Tracker::Optimize(Sophus::SE3f& Tji){ 44 | Sophus::SE3f oldTji = Tji; 45 | Matrix6x6 oldH = Matrix6x6::Identity(); 46 | 47 | stop_ = false; 48 | for (int i=0; i 0 && newResidual > residual_ || stop_){ 75 | // Tji = oldTji; 76 | // H_ = oldH; 77 | // std::cout << "[Optimize] Stop Sign From Res" << std::endl; 78 | // break; 79 | // } 80 | Sophus::SE3f newTji; 81 | UpdatePose(Tji, newTji); 82 | oldTji = Tji; 83 | oldH = H_; 84 | Tji = newTji; 85 | 86 | residual_ = newResidual; 87 | // std::cout << "[Optimize] NormMax(x_) = " << NormMax(x_) << std::endl; 88 | 89 | if(NormMax(x_) < normXThres_){ 90 | status_ = true; 91 | 92 | if ( ((x_ - x_).array() != (x_ - x_).array()).all() ) 93 | status_ = false; 94 | // std::cout << "[Optimize] break from normmax" << std::endl; 95 | 96 | break; 97 | } 98 | } 99 | // std::cout << "[Optimize] Iteration Finished" << std::endl; 100 | 101 | } 102 | 103 | float Tracker::HuberWeight(const float res){ 104 | float tAbs = fabsf(res); 105 | float sig = 5.0; 106 | if(tAbs < huberK_ * sig) 107 | return 1.0; 108 | else 109 | return sig*huberK_ / tAbs; 110 | } 111 | 112 | //void Tracker::CheckVisiblePointsInPrevFrame(Frame::Ptr currFrame, Sophus::SE3f& transformation){ 113 | // const int border = patch_halfsize_+2; 114 | // cv::Mat& current_img = currFrame->GetOriginalImg(); 115 | // 116 | // const float scale = 1.0f; 117 | // std::vector::iterator visibilityIterPrev = visiblePointsPrev_.begin(); 118 | // 119 | // for (auto iter=currFrame->GetOriginalCloud().begin(); iter!=currFrame->GetOriginalCloud().end(); ++iter, ++visibilityIterPrev) { 120 | // Eigen::Vector3f xyz_cur (iter->x, iter->y, iter->z); 121 | // Eigen::Vector3f xyz_prev = transformation.inverse()*xyz_cur; 122 | // Eigen::Vector2f uv_prev; 123 | // uv_prev.noalias() = currFrame->PointCloudXyz2Uv(xyz_prev) * scale; 124 | // 125 | // const float u_prev_f = uv_prev(0); 126 | // const float v_prev_f = uv_prev(1); 127 | // const int u_prev_i = static_cast (u_prev_f); 128 | // const int v_prev_i = static_cast (v_prev_f); 129 | // 130 | // if (u_prev_i - border < 0 || u_prev_i + border > current_img.cols || v_prev_i - border < 0 || v_prev_i + border > current_img.rows || xyz_prev(2) <= 0) 131 | // continue; 132 | // 133 | // *visibilityIterPrev = true; 134 | // } 135 | //} 136 | 137 | cv::Mat Tracker::PrecomputePatches(cv::Mat& img, pcl::PointCloud& pointcloud, cv::Mat& patchBuf, bool isDerivative){ 138 | int border = config_.trackerConfig.border; 139 | int stride = img.cols; 140 | float scale = 1.0f/(1< uvSet = pinholeModel_.PointCloudXyz2UvVec(pointcloud, scale); 143 | patchBuf = cv::Mat(pointcloud.size(), patternLength_, CV_32F); 144 | if(isDerivative){ 145 | jacobianBuf_.resize(Eigen::NoChange, patchBuf.rows * patternLength_); 146 | jacobianBuf_.setZero(); 147 | } 148 | 149 | auto pointCloudIter = pointcloud.begin(); 150 | size_t pointCounter = 0; 151 | 152 | int pointNum = pointcloud.points.size(); 153 | cv::Mat imgClone = cv::Mat(img.rows, img.cols, CV_32FC3, cv::Scalar(0)); 154 | 155 | // cv::cvtColor(imgClone, imgClone, cv::COLOR_GRAY2BGR); 156 | // for(int i=0; i(uInt, vInt)0 = " << imgClone.at(i, j) << std::endl; 159 | // std::cout << "[Optimize] img.at(uInt, vInt)0 = " << img.at(i, j) << std::endl; 160 | // } 161 | // } 162 | // std::cout << "[PrecomputePatches] DEBUG1" << std::endl; 163 | // std::cout << "[PrecomputePatches] pointNum = " << pointNum << std::endl; 164 | 165 | float temp; 166 | float debug = 0; 167 | for(auto uvIter=uvSet.begin(); uvIter!=uvSet.end(); ++uvIter, ++pointCloudIter, ++pointCounter){ 168 | debug++; 169 | temp = 0; 170 | Eigen::Vector2f uv = *uvIter; 171 | const float uFloat = uv(0); 172 | const float vFloat = uv(1); 173 | const long long int uInt = static_cast (uFloat); 174 | const long long int vInt = static_cast (vFloat); 175 | // std::cout << "[PrecomputePatches] uv(0) : " << uv(0) << std::endl; 176 | // std::cout << "[PrecomputePatches] uv(1) : " << uv(1) << std::endl; 177 | // std::cout << "[PrecomputePatches] vFloat : " << vFloat << std::endl; 178 | // std::cout << "[PrecomputePatches] uFloat : " << uFloat << std::endl; 179 | // std::cout << "[PrecomputePatches] vInt : " << vInt << std::endl; 180 | // std::cout << "[PrecomputePatches] uInt : " << uInt << std::endl; 181 | // std::cout << "[PrecomputePatches] uInt - border < 0 : " << (uInt - border < 0) << std::endl; 182 | // std::cout << "[PrecomputePatches] vInt - border < 0 : " << (vInt - border < 0) << std::endl; 183 | // std::cout << "[PrecomputePatches] uInt + border > img.cols : " << (uInt + border > img.cols) << std::endl; 184 | // std::cout << "[PrecomputePatches] vInt + border > img.rows : " << (vInt + border > img.rows) << std::endl; 185 | 186 | if(uInt - border < 0 || uInt + border > img.cols || vInt - border < 0 || vInt + border > img.rows || pointCloudIter->z <= 0.0){ 187 | float* patchBufPtr = reinterpret_cast (patchBuf.data) + patternLength_ * pointCounter; 188 | for(int i=0; i::quiet_NaN(); 190 | continue; 191 | } 192 | 193 | const float subpixURef = uFloat - uInt; 194 | const float subpixVRef = vFloat - vInt; 195 | const float wTL = (1.0-subpixURef) * (1.0-subpixURef); 196 | const float wTR = subpixURef * (1.0-subpixVRef); 197 | const float wBL = (1.0-subpixURef) * subpixVRef; 198 | const float wBR = subpixURef * subpixVRef; 199 | 200 | size_t pixelCounter = 0; 201 | float* patchBufPtr = reinterpret_cast (patchBuf.data) + patternLength_ * pointCounter; 202 | // std::cout << "[PrecomputePatches] debug3 : " << debug << std::endl; 203 | 204 | for (int i=0; ix, pointCloudIter->y, pointCloudIter->z); 227 | Frame::jacobian_xyz2uv(xyz, frameJacobian); 228 | 229 | jacobianBuf_.col(pointCounter*patternLength_ + pixelCounter) = (dx*config_.cameraConfig.fx * frameJacobian.row(0) + dy*config_.cameraConfig.fy*frameJacobian.row(1)) / (1 << currentLevel_); 230 | } 231 | } 232 | // std::cout << "[PrecomputePatches] debug : " << debug << std::endl; 233 | 234 | temp /= patternLength_; 235 | cv::Scalar_ color = cv::Scalar_(temp, temp, temp); 236 | cv::circle(imgClone, cv::Point(uInt, vInt), 2, color, 1); 237 | } 238 | // std::cout << "[Tracker] patchBuf.rows = " << patchBuf.rows << std::endl; 239 | // std::cout << "[Tracker] pointcloud.size() = " << pointcloud.size() << std::endl; 240 | // std::cout << "[Tracker] jacobianBuf_.size() = " << jacobianBuf_.size() << std::endl; 241 | 242 | 243 | return imgClone; 244 | } 245 | 246 | double Tracker::ComputeResiduals(Sophus::SE3f &transformation) 247 | { 248 | errors_.clear(); 249 | J_.clear(); 250 | weight_.clear(); 251 | 252 | if (!isPreComputed_) 253 | { 254 | cv::Mat &referenceImg = referenceFrame_->frame->GetPyramidImg(currentLevel_); 255 | pcl::PointCloud referencePointCloud = referenceFrame_->frame->GetOriginalCloud(); 256 | refImgClone = PrecomputePatches(referenceImg, referencePointCloud, refPatchBuf_, true); 257 | isPreComputed_ = true; 258 | } 259 | // std::cout << "[ComputeResiduals] transformation.matrix()" << std::endl; 260 | std::cout << transformation.matrix() << std::endl; 261 | cv::Mat &currImg = currentFrame_->GetPyramidImg(currentLevel_); 262 | // std::cout << "[ComputeResiduals] currentLevel_" << std::endl; 263 | // std::cout << currentLevel_ << std::endl; 264 | pcl::PointCloud currPointCloud; 265 | pcl::transformPointCloud(referenceFrame_->frame->GetOriginalCloud(), currPointCloud, transformation.matrix()); 266 | currImgClone = PrecomputePatches(currImg, currPointCloud, currPatchBuf_, false); 267 | cv::Mat errors = cv::Mat(currPointCloud.size(), patternLength_, CV_32F); 268 | errors = currPatchBuf_ - refPatchBuf_; 269 | 270 | if(config_.datasetConfig.visualize){ 271 | cv::Mat diff = currImgClone - refImgClone; 272 | cv::Mat temp1, temp2, temp3; 273 | temp1 = refImgClone.clone(); 274 | temp2 = currImgClone.clone(); 275 | temp3 = diff.clone(); 276 | temp1.convertTo(temp1, CV_8U, 255); 277 | temp2.convertTo(temp2, CV_8U, 255); 278 | temp3.convertTo(temp3, CV_8U, 255); 279 | cv::imshow("refImgClone", temp1); 280 | cv::imshow("currImgClone", temp2); 281 | cv::imshow("diff", diff); 282 | cv::moveWindow("refImgClone", 40, 30); 283 | cv::moveWindow("currImgClone", 40, 500); 284 | cv::moveWindow("diff", 40, 900); 285 | 286 | while(1){ 287 | int key = cv::waitKey(0); 288 | if(key == 27) break; 289 | } 290 | } 291 | // time_t rawtime = time(nullptr); 292 | // struct tm* time_info = localtime(&rawtime); 293 | // 294 | // char logTime[13]; 295 | // sprintf(logTime, "%02d%02d%02d_%02d%02d%02d", (time_info->tm_year) - 100, (time_info->tm_mon) + 1, time_info->tm_mday, 296 | // time_info->tm_hour, time_info->tm_min, time_info->tm_sec); 297 | // std::string curPath = "/home/jongsik/modu_ws/img_result/"; 298 | // std::string mappingLogFilenameRef = curPath + "Ref/" + logTime + ".jpg"; 299 | // std::string mappingLogFilenameCurr = curPath + "Curr/" + logTime + ".jpg"; 300 | // std::string mappingLogFilenameDiff = curPath + "Diff/" + logTime + ".jpg"; 301 | 302 | 303 | // cv::Mat diff = currImgClone - refImgClone; 304 | // cv::Mat temp1, temp2, temp3; 305 | // temp1 = refImgClone.clone(); 306 | // temp2 = currImgClone.clone(); 307 | // temp3 = diff.clone(); 308 | // temp1.convertTo(temp1, CV_8U, 255); 309 | // temp2.convertTo(temp2, CV_8U, 255); 310 | // temp3.convertTo(temp3, CV_8U, 255); 311 | // cv::imwrite(mappingLogFilenameRef, temp1); 312 | // cv::imwrite(mappingLogFilenameCurr, temp2); 313 | // cv::imwrite(mappingLogFilenameDiff, temp3); 314 | 315 | scale_ = compute(errors); 316 | 317 | float chi2 = 0.0; 318 | size_t nMeasurement = 0; 319 | 320 | float *errorsPtr = errors.ptr(); 321 | float *refPatchBufPtr = refPatchBuf_.ptr(); 322 | float *currPatchBufPtr = currPatchBuf_.ptr(); 323 | 324 | float IiIj = 0.0f; 325 | float IiIi = 0.0f; 326 | float sumIi = 0.0f; 327 | float sumIj = 0.0f; 328 | 329 | for (int i = 0; i < errors.size().area(); ++i, ++errorsPtr, ++refPatchBufPtr, ++currPatchBufPtr) 330 | { 331 | float &res = *errorsPtr; 332 | float &Ii = *refPatchBufPtr; 333 | float &Ij = *currPatchBufPtr; 334 | 335 | // std::cout << "[Tracker] i = " << i << std::endl; 336 | // std::cout << "[Tracker] res = " << res << std::endl; 337 | // std::cout << "[Tracker] Ii = " << Ii << std::endl; 338 | // std::cout << "[Tracker] Ij = " << Ij << std::endl; 339 | 340 | if (std::isfinite(res)) 341 | { 342 | nMeasurement++; 343 | Vector6 J(jacobianBuf_.col(i)); 344 | errors_.push_back(res); 345 | J_.push_back(J); 346 | IiIj += Ii * Ij; 347 | IiIi += Ii * Ii; 348 | sumIi += Ii; 349 | sumIj += Ij; 350 | } 351 | } 352 | 353 | affine_a_ = IiIj / IiIi; 354 | affine_b_ = (sumIj - affine_a_ * sumIi) / nMeasurement; 355 | 356 | std::vector sortedErrors; 357 | sortedErrors.resize(errors_.size()); 358 | std::copy(errors_.begin(), errors_.end(), sortedErrors.begin()); 359 | std::sort(sortedErrors.begin(), sortedErrors.end()); 360 | 361 | float medianMu = sortedErrors[sortedErrors.size() / 2]; 362 | std::vector absoluteResError; 363 | for (auto error:errors_) 364 | { 365 | absoluteResError.push_back(fabs(error - medianMu)); 366 | } 367 | sort(absoluteResError.begin(), absoluteResError.end()); 368 | float medianAbsDeviation = 1.4826 * absoluteResError[absoluteResError.size() / 2]; 369 | for (auto error:errors_) 370 | { 371 | float weight = 1.0; 372 | // weight = calcWeight((error - medianMu) / medianAbsDeviation); 373 | weight_.push_back(weight); 374 | chi2 += error * error * weight; 375 | // std::cout << "[Tracker] weight = " << weight << std::endl; 376 | // std::cout << "[Tracker] error = " << error << std::endl; 377 | } 378 | 379 | return chi2 / nMeasurement; 380 | } 381 | 382 | void Tracker::trackFrame2Frame(Frame::Ptr currFrame, KeyFrame::Ptr refFrame, Sophus::SE3f& transformation) 383 | { 384 | currentFrame_ = currFrame; 385 | referenceFrame_ = refFrame; 386 | 387 | affine_a_ = 1.0f; 388 | affine_b_ = 0.0f; 389 | 390 | // std::cout << "[Tracker] Try to Optimize" << std::endl; 391 | 392 | for(currentLevel_ = maxLevel_; currentLevel_ >= minLevel_; currentLevel_--){ 393 | isPreComputed_ = false; 394 | stop_ = false; 395 | Optimize(transformation); 396 | 397 | } 398 | // std::cout << "[Tracker] Optimization finished" << std::endl; 399 | 400 | } 401 | --------------------------------------------------------------------------------