diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..50f129c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,95 @@ +cmake_minimum_required(VERSION 2.8.3) +project(removert) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) # C++17 to use stdc++fs +# set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") + +find_package(catkin REQUIRED COMPONENTS + tf + roscpp + rospy + cv_bridge + # pcl library + pcl_conversions + # msgs + std_msgs + sensor_msgs + geometry_msgs + nav_msgs + message_generation + image_transport +) + +find_package(OpenMP REQUIRED) +find_package(PCL REQUIRED QUIET) +find_package(OpenCV REQUIRED QUIET) +# find_package(GTSAM REQUIRED QUIET) + +# add_message_files( +# DIRECTORY msg +# FILES +# # cloud_info.msg +# ) + +# generate_messages( +# DEPENDENCIES +# geometry_msgs +# std_msgs +# nav_msgs +# sensor_msgs +# ) + +catkin_package( + INCLUDE_DIRS include + DEPENDS PCL + # GTSAM + + CATKIN_DEPENDS + std_msgs + nav_msgs + geometry_msgs + sensor_msgs + message_runtime + message_generation +) + +# include directories +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + # ${GTSAM_INCLUDE_DIR} +) + +# link directories +link_directories( + include + ${PCL_LIBRARY_DIRS} + ${OpenCV_LIBRARY_DIRS} + # ${GTSAM_LIBRARY_DIRS} +) + +########### +## Build ## +########### + +# Range Image Projection +add_executable( ${PROJECT_NAME}_removert src/removert_main.cpp + src/utility.cpp + src/RosParamServer.cpp + src/Removerter.cpp +) + +add_dependencies(${PROJECT_NAME}_removert ${catkin_EXPORTED_TARGETS}) +target_compile_options(${PROJECT_NAME}_removert PRIVATE ${OpenMP_CXX_FLAGS}) +target_link_libraries(${PROJECT_NAME}_removert + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${OpenCV_LIBRARIES} + ${OpenMP_CXX_FLAGS} + stdc++fs +) + + diff --git a/config/params.yaml b/config/params.yaml new file mode 100644 index 0000000..d1e6bee --- /dev/null +++ b/config/params.yaml @@ -0,0 +1,81 @@ +removert: + + # @ NOTE: Recommended test regions and corresponding parameters + # - KITTI 09 [downsample_voxel_size: 0.05m, skips: 5, start idx: 1300, end idx: 1600] + # - KITTI 03 [downsample_voxel_size: 0.05m, skips: 5, start idx: 0, end idx: 150] + + + # @ save option + saveMapPCD: true + saveCleanScansPCD: true + save_pcd_directory: "/home/user/removert/results/" # replace to your path (please use an absolute path) + + + # @ sequence info (replace to your paths) + # please follow the KITTI odometry dataset's scan and pose formats (i.e., make them .bin and line-by-line corresponding se3 poses composed of 12 numbers) + sequence_scan_dir: "/media/user/GS1TB/KITTI/dataset/sequences/09/velodyne/" + sequence_pose_path: "/media/user/GS1TB/KITTI/dataset/sequences/09/poses.txt" + + sequence_vfov: 50 # including upper and lower fovs, for example, KITTI's HDL64E is 2 + 24.9 ~ 27 deg. (so 25 + 25 = 50 is recommended because it is enough) + sequence_hfov: 360 # generally for targetting scanning LiDAR but a lidar having restricted hfov also just can use the 360 (because no point regions are considered in the algorithm) + + + # @ Sequence's BaseToLidar info + ExtrinsicLiDARtoPoseBase: # this is for the KITTI (in the removert example, using SuMa poses written in the camera coord) + [-1.857e-03, -9.999e-01, -8.039e-03, -4.784e-03, + -6.481e-03, 8.0518e-03, -9.999e-01, -7.337e-02, + 9.999e-01, -1.805e-03, -6.496e-03, -3.339e-01, + 0.0, 0.0, 0.0, 1.0] + # @ If you use the lidar-itself odometry (e.g., from A-LOAM or using odometry saver https://github.com/SMRT-AIST/interactive_slam/wiki/Example2), + # use the below identity matrix, not the above KITTI pose setting (w.r.t the camera). + # ExtrinsicLiDARtoPoseBase: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1] + + + # @ Sampling nodes + use_keyframe_gap: true + keyframe_gap: 5 # i.e., 5 means every 5 frames // 5 is recommended for fast removing while preserving the static map's quality # if you make all scans clean, use this value = 1 + + # use_keyframe_meter: false # TODO + # keyframe_meter: 2 # TODO, e.g., every 2 meters (using the given pose relative translations) + + + # @ target region (scan idx range) when clean_for_all_scan is false + # means the removeter runs on all scans in a specified regions of a sequence + # it means we parse scans every or meter within the idx range [start_idx, end_idx] + # but for a single batch, < 100 scans are recommended for the computation speed. + start_idx: 1300 # change this + end_idx: 1500 # change this + + + # @ auto repeat + # means the removeter runs on all scans in a sequence + clean_for_all_scan: false # default is false + batch_size: 150 + valid_ratio_to_save: 0.75 # i.e., no save cleaned scans located at the boundaries of a batch + + + # @ Range image resolution + # the below is actually magnifier ratio (i.e., 5 means x5 resolution, the x1 means 1 deg x 1 deg per pixel) + # - recommend to use the first removing resolution's magnifier ratio should meet the seonsor vertical fov / number of rays + # - e.g., HDL 64E of KITTI dataset -> appx 25 deg / 64 ray ~ 0.4 deg per pixel -> the magnifier ratio = 1/0.4 = 2.5 + # - recommend to use the first reverting resolution's magnifier ratio should lied in 1.0 to 1.5 + # remove_resolution_list: [2.5, 2.0, 1.5] + remove_resolution_list: [2.5, 1.5] + revert_resolution_list: [1.0, 0.9, 0.8, 0.7] # TODO + + + # @ Removert params + # about density + downsample_voxel_size: 0.05 # user parameter but recommend to use 0.05 to make sure an enough density (this value is related to the removing resolution's expected performance) + # about Static sensitivity + num_nn_points_within: 2 # how many - using higher, more strict static + dist_nn_points_within: 0.1 # meter - using smaller, more strict static + + + # @ For faster + num_omp_cores: 16 # for faster map points projection (to make a map range image) + + + # @ For visualization of range images (rviz -d removert_visualization.rviz) + rimg_color_min: 0.0 # meter + rimg_color_max: 20.0 # meter \ No newline at end of file diff --git a/include/removert/Removerter.h b/include/removert/Removerter.h new file mode 100644 index 0000000..ae183c3 --- /dev/null +++ b/include/removert/Removerter.h @@ -0,0 +1,132 @@ +#pragma once + +#include "removert/RosParamServer.h" + +class Removerter : public RosParamServer +{ +private: + + ros::Subscriber subLaserCloud; // TODO, for real-time integration (later) + + const float kFlagNoPOINT = 10000.0; // no point constant, 10000 has no meaning, but must be larger than the maximum scan range (e.g., 200 meters) + const float kValidDiffUpperBound = 200.0; // must smaller than kFlagNoPOINT + + // Static sensitivity + int kNumKnnPointsToCompare;// static sensitivity (increase this value, less static structure will be removed at the scan-side removal stage) + float kScanKnnAndMapKnnAvgDiffThreshold; // static sensitivity (decrease this value, less static structure will be removed at the scan-side removal stage) + + std::vector sequence_valid_scan_names_; + std::vector sequence_valid_scan_paths_; + std::vector::Ptr> scans_; + std::vector::Ptr> scans_static_; + std::vector::Ptr> scans_dynamic_; + + std::string scan_static_save_dir_; + std::string scan_dynamic_save_dir_; + std::string map_static_save_dir_; + std::string map_dynamic_save_dir_; + + std::vector scan_poses_; + std::vector scan_inverse_poses_; + + pcl::KdTreeFLANN::Ptr kdtree_map_global_curr_; + pcl::KdTreeFLANN::Ptr kdtree_scan_global_curr_; + + pcl::PointCloud::Ptr map_global_orig_; + + pcl::PointCloud::Ptr map_global_curr_; // the M_i. i.e., removert is: M1 -> S1 + D1, D1 -> M2 , M2 -> S2 + D2 ... repeat ... + pcl::PointCloud::Ptr map_local_curr_; + + pcl::PointCloud::Ptr map_subset_global_curr_; + + pcl::PointCloud::Ptr map_global_curr_static_; // the S_i + pcl::PointCloud::Ptr map_global_curr_dynamic_; // the D_i + + pcl::PointCloud::Ptr map_global_accumulated_static_; // TODO, the S_i after reverted + pcl::PointCloud::Ptr map_global_accumulated_dynamic_; // TODO, the D_i after reverted + + std::vector::Ptr> static_map_global_history_; // TODO + std::vector::Ptr> dynamic_map_global_history_; // TODO + + float curr_res_alpha_; // just for tracking current status + + const int base_node_idx_ = 0; + + unsigned long kPauseTimeForClearStaticScanVisualization = 10000; // microsec + + // NOT recommend to use for under 5 million points map input (becausing not-using is just faster) + const bool kUseSubsetMapCloud = false; + const float kBallSize = 80.0; // meter + +public: + Removerter(); + ~Removerter(); + + void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); // TODO (if removert run online) + + void allocateMemory(); + + void parseValidScanInfo(); + void readValidScans(); + + void mergeScansWithinGlobalCoord( + const std::vector::Ptr>& _scans, + const std::vector& _scans_poses, + pcl::PointCloud::Ptr& _ptcloud_to_save ); + void octreeDownsampling(const pcl::PointCloud::Ptr& _src, pcl::PointCloud::Ptr& _to_save); + void makeGlobalMap(); + + void run(void); + + void removeOnce(float _res); + void revertOnce(float _res); + + void saveCurrentStaticMapHistory(void); // the 0th element is a noisy (original input) (actually not static) map. + void saveCurrentDynamicMapHistory(void); + + void takeGlobalMapSubsetWithinBall( int _center_scan_idx ); + void transformGlobalMapSubsetToLocal(int _base_scan_idx); + + void transformGlobalMapToLocal(int _base_scan_idx); + void transformGlobalMapToLocal(int _base_scan_idx, pcl::PointCloud::Ptr& _map_local); + void transformGlobalMapToLocal(const pcl::PointCloud::Ptr& _map_global, int _base_scan_idx, pcl::PointCloud::Ptr& _map_local); + + cv::Mat scan2RangeImg(const pcl::PointCloud::Ptr& _scan, + const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ + const std::pair _rimg_size); + std::pair map2RangeImg(const pcl::PointCloud::Ptr& _scan, + const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ + const std::pair _rimg_size); + + std::vector calcDescrepancyAndParseDynamicPointIdx (const cv::Mat& _scan_rimg, const cv::Mat& _diff_rimg, const cv::Mat& _map_rimg_ptidx); + std::vector calcDescrepancyAndParseDynamicPointIdxForEachScan( std::pair _rimg_shape ); + + std::vector getStaticIdxFromDynamicIdx(const std::vector& _dynamic_point_indexes, int _num_all_points); + std::vector getGlobalMapStaticIdxFromDynamicIdx(const std::vector& _dynamic_point_indexes); + + void parsePointcloudSubsetUsingPtIdx( const pcl::PointCloud::Ptr& _ptcloud_orig, + std::vector& _point_indexes, pcl::PointCloud::Ptr& _ptcloud_to_save ); + void parseMapPointcloudSubsetUsingPtIdx( std::vector& _point_indexes, pcl::PointCloud::Ptr& _ptcloud_to_save ); + void parseStaticMapPointcloudUsingPtIdx( std::vector& _point_indexes ); + void parseDynamicMapPointcloudUsingPtIdx( std::vector& _point_indexes ); + + void saveCurrentStaticAndDynamicPointCloudGlobal( void ); + void saveCurrentStaticAndDynamicPointCloudLocal( int _base_pose_idx = 0); + + // void local2global(const pcl::PointCloud::Ptr& _ptcloud_global, pcl::PointCloud::Ptr& _ptcloud_local_to_save ); + pcl::PointCloud::Ptr local2global(const pcl::PointCloud::Ptr& _scan_local, int _scan_idx); + pcl::PointCloud::Ptr global2local(const pcl::PointCloud::Ptr& _scan_global, int _scan_idx); + + // scan-side removal + std::pair::Ptr, pcl::PointCloud::Ptr> removeDynamicPointsOfScanByKnn ( int _scan_idx ); + void removeDynamicPointsAndSaveStaticScanForEachScan( void ); + + void scansideRemovalForEachScan(void); + void saveCleanedScans(void); + void saveMapPointcloudByMergingCleanedScans(void); + void scansideRemovalForEachScanAndSaveThem(void); + + void saveStaticScan( int _scan_idx, const pcl::PointCloud::Ptr& _ptcloud ); + void saveDynamicScan( int _scan_idx, const pcl::PointCloud::Ptr& _ptcloud ); + +}; // Removerter \ No newline at end of file diff --git a/include/removert/RosParamServer.h b/include/removert/RosParamServer.h new file mode 100644 index 0000000..c6ad4df --- /dev/null +++ b/include/removert/RosParamServer.h @@ -0,0 +1,105 @@ +#pragma once + +#include "removert/utility.h" + +class RosNodeHandle +{ +public: + ros::NodeHandle nh_super; +}; // class: RosNodeHandle + + +class RosParamServer: public RosNodeHandle +{ +public: + // + ros::NodeHandle & nh; + + // + std::string pointcloud_topic; + + // removert params + float kVFOV; + float kHFOV; + std::pair kFOV; + + // sequence info + std::vector kVecExtrinsicLiDARtoPoseBase; + Eigen::Matrix4d kSE3MatExtrinsicLiDARtoPoseBase; + // Base is where of the pose writtened (e.g., for KITTI, poses is usually in camera) + // if the pose file is obtained via lidar odometry itself, then kMatExtrinsicLiDARtoBase is eye(4) + Eigen::Matrix4d kSE3MatExtrinsicPoseBasetoLiDAR; + + // sequence bin files + std::string sequence_scan_dir_; + std::vector sequence_scan_names_; + std::vector sequence_scan_paths_; + int num_total_scans_of_sequence_; + float kDownsampleVoxelSize; + + // sequence pose file + std::string sequence_pose_path_; + std::vector sequence_scan_poses_; + std::vector sequence_scan_inverse_poses_; // used for global to local + + // target region to removerting + int start_idx_; + int end_idx_; + + bool use_keyframe_gap_; + bool use_keyframe_meter_; + int keyframe_gap_; + float keyframe_gap_meter_; + + // + std::vector remove_resolution_list_; + std::vector revert_resolution_list_; + + // + int kNumOmpCores; + + // + pcl::PointCloud::Ptr single_scan; + pcl::PointCloud::Ptr projected_scan; + + // ros pub + ros::Publisher scan_publisher_; + ros::Publisher global_scan_publisher_; + + ros::Publisher original_map_local_publisher_; + + ros::Publisher curr_map_local_publisher_; + ros::Publisher static_map_local_publisher_; + ros::Publisher dynamic_map_local_publisher_; + + ros::Publisher static_curr_scan_publisher_; + ros::Publisher dynamic_curr_scan_publisher_; + + float rimg_color_min_; + float rimg_color_max_; + std::pair kRangeColorAxis; // meter + std::pair kRangeColorAxisForDiff; // meter + + image_transport::ImageTransport ROSimg_transporter_; + + sensor_msgs::ImagePtr scan_rimg_msg_; + image_transport::Publisher scan_rimg_msg_publisher_; + + sensor_msgs::ImagePtr map_rimg_msg_; + image_transport::Publisher map_rimg_msg_publisher_; + + sensor_msgs::ImagePtr diff_rimg_msg_; + image_transport::Publisher diff_rimg_msg_publisher_; + + sensor_msgs::ImagePtr map_rimg_ptidx_msg_; + image_transport::Publisher map_rimg_ptidx_msg_publisher_; + + // + bool kFlagSaveMapPointcloud; + bool kFlagSaveCleanScans; + std::string save_pcd_directory_; + +public: + RosParamServer(); + +}; // class: RosParamServer diff --git a/include/removert/utility.h b/include/removert/utility.h new file mode 100644 index 0000000..938c6c0 --- /dev/null +++ b/include/removert/utility.h @@ -0,0 +1,153 @@ +#pragma once +#ifndef _UTILITY_REMOVERT_H_ +#define _UTILITY_REMOVERT_H_ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include // requires gcc version >= 8 + +namespace fs = std::filesystem; +using std::ios; +using std::cout; +using std::cerr; +using std::endl; + +// struct PointXYZIS +// { +// PCL_ADD_POINT4D +// float intensity; +// float score; +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW +// } EIGEN_ALIGN16; + +// POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIS, +// (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, score, score) +// ) + +using PointType = pcl::PointXYZI; + +struct SphericalPoint +{ + float az; // azimuth + float el; // elevation + float r; // radius +}; + +inline float rad2deg(float radians); +inline float deg2rad(float degrees); + +void readBin(std::string _bin_path, pcl::PointCloud::Ptr _pcd_ptr); + +std::vector splitPoseLine(std::string _str_line, char _delimiter); + +SphericalPoint cart2sph(const PointType & _cp); + +std::pair resetRimgSize(const std::pair _fov, const float _resize_ratio); + +template +cv::Mat convertColorMappedImg (const cv::Mat &_src, std::pair _caxis) +{ + T min_color_val = _caxis.first; + T max_color_val = _caxis.second; + + cv::Mat image_dst; + image_dst = 255 * (_src - min_color_val) / (max_color_val - min_color_val); + image_dst.convertTo(image_dst, CV_8UC1); + + cv::applyColorMap(image_dst, image_dst, cv::COLORMAP_JET); + + return image_dst; +} + +std::set convertIntVecToSet(const std::vector & v); + +template +std::vector linspace(T a, T b, size_t N) { + T h = (b - a) / static_cast(N-1); + std::vector xs(N); + typename std::vector::iterator x; + T val; + for (x = xs.begin(), val = a; x != xs.end(); ++x, val += h) + *x = val; + return xs; +} + +sensor_msgs::ImagePtr cvmat2msg(const cv::Mat &_img); + +void pubRangeImg(cv::Mat& _rimg, sensor_msgs::ImagePtr& _msg, image_transport::Publisher& _publiser, std::pair _caxis); +void publishPointcloud2FromPCLptr(const ros::Publisher& _scan_publisher, const pcl::PointCloud::Ptr _scan); +sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame); + +template +double ROS_TIME(T msg) +{ + return msg->header.stamp.toSec(); +} + +float pointDistance(PointType p); +float pointDistance(PointType p1, PointType p2); + +#endif \ No newline at end of file diff --git a/launch/removert_visualization.rviz b/launch/removert_visualization.rviz new file mode 100644 index 0000000..e814197 --- /dev/null +++ b/launch/removert_visualization.rviz @@ -0,0 +1,201 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Image1 + - /Image2 + - /Image3 + Splitter Ratio: 0.4148681163787842 + Tree Height: 572 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.6600000262260437 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Spheres + Topic: /removert/scan_single_local_dynamic + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 138; 226; 52 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.9900000095367432 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 1 + Size (m): 0.10000000149011612 + Style: Points + Topic: /removert/scan_single_local_static + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /scan_rimg_single + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /map_rimg_single + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /diff_rimg_single + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: removert + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 101.88595581054688 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.6624842882156372 + Y: 3.6606950759887695 + Z: 0.8095508813858032 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9153987169265747 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.498590469360352 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a1000002c7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000751000000d1fc0100000006fb0000000a0049006d0061006700650100000000000002610000005e00fffffffb0000000800540069006d006500000002b4000002eb000002eb00fffffffb0000000a0049006d0061006700650100000267000002800000005e00fffffffb0000000a0049006d00610067006501000004ed000002640000005e00fffffffb0000000a0049006d00610067006501000004ef000002620000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005aa000002c700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1873 + X: 1967 + Y: 27 diff --git a/launch/run.launch b/launch/run.launch new file mode 100644 index 0000000..3ed7b1a --- /dev/null +++ b/launch/run.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..653492a --- /dev/null +++ b/package.xml @@ -0,0 +1,51 @@ + + + removert + 1.0.0 + Remove-then-revert-based static point cloud map construction + + + Giseop Kim + + TODO + + + Giseop Kim + + catkin + + roscpp + roscpp + rospy + rospy + + tf + tf + + cv_bridge + cv_bridge + + pcl_conversions + pcl_conversions + + std_msgs + std_msgs + sensor_msgs + sensor_msgs + geometry_msgs + geometry_msgs + nav_msgs + nav_msgs + + message_generation + message_generation + message_runtime + message_runtime + + image_transport + image_transport + + + + diff --git a/src/Removerter.cpp b/src/Removerter.cpp new file mode 100644 index 0000000..b559000 --- /dev/null +++ b/src/Removerter.cpp @@ -0,0 +1,887 @@ +#include "removert/Removerter.h" + +inline float rad2deg(float radians) +{ + return radians * 180.0 / M_PI; +} + +inline float deg2rad(float degrees) +{ + return degrees * M_PI / 180.0; +} + +void Removerter::cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) +{ + cout << "TODO" << endl; +} + +void fsmkdir(std::string _path) +{ + if (!fs::is_directory(_path) || !fs::exists(_path)) + fs::create_directories(_path); // create src folder +} //fsmkdir + +Removerter::Removerter() +{ + subLaserCloud = nh.subscribe("/os1_points", 5, &Removerter::cloudHandler, this, ros::TransportHints().tcpNoDelay()); + + // voxelgrid generates warnings frequently, so verbose off + ps. recommend to use octree (see makeGlobalMap) + pcl::console::setVerbosityLevel(pcl::console::L_ERROR); + // pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + + nh.param("removert/num_nn_points_within", kNumKnnPointsToCompare, 3); // using higher, more strict static + nh.param("removert/dist_nn_points_within", kScanKnnAndMapKnnAvgDiffThreshold, 0.1); // using smaller, more strict static + + if( save_pcd_directory_.substr(save_pcd_directory_.size()-1, 1) != std::string("/") ) + save_pcd_directory_ = save_pcd_directory_ + "/"; + fsmkdir(save_pcd_directory_); + scan_static_save_dir_ = save_pcd_directory_ + "scan_static"; fsmkdir(scan_static_save_dir_); + scan_dynamic_save_dir_ = save_pcd_directory_ + "scan_dynamic"; fsmkdir(scan_dynamic_save_dir_); + map_static_save_dir_ = save_pcd_directory_ + "map_static"; fsmkdir(map_static_save_dir_); + map_dynamic_save_dir_ = save_pcd_directory_ + "map_dynamic"; fsmkdir(map_dynamic_save_dir_); + + allocateMemory(); + + // ros pub for visual debug + scan_publisher_ = nh.advertise ("removert/scan_single_local", 1); + global_scan_publisher_ = nh.advertise ("removert/scan_single_global", 1); + + original_map_local_publisher_ = nh.advertise ("removert/original_map", 10); + + curr_map_local_publisher_ = nh.advertise ("removert/curr_map", 1); + static_map_local_publisher_ = nh.advertise ("removert/static_map", 1); + dynamic_map_local_publisher_ = nh.advertise ("removert/dynamic_map", 1); + + static_curr_scan_publisher_ = nh.advertise ("removert/scan_single_local_static", 1); + dynamic_curr_scan_publisher_ = nh.advertise ("removert/scan_single_local_dynamic", 1); + +} // ctor + + +void Removerter::allocateMemory() +{ + map_global_orig_.reset(new pcl::PointCloud()); + + map_global_curr_.reset(new pcl::PointCloud()); + map_local_curr_.reset(new pcl::PointCloud()); + + map_global_curr_static_.reset(new pcl::PointCloud()); + map_global_curr_dynamic_.reset(new pcl::PointCloud()); + + map_subset_global_curr_.reset(new pcl::PointCloud()); + + kdtree_map_global_curr_.reset(new pcl::KdTreeFLANN()); + kdtree_scan_global_curr_.reset(new pcl::KdTreeFLANN()); + + // + map_global_orig_->clear(); + map_global_curr_->clear(); + map_local_curr_->clear(); + map_global_curr_static_->clear(); + map_global_curr_dynamic_->clear(); + map_subset_global_curr_->clear(); + +} // allocateMemory + + +Removerter::~Removerter(){} + + +void Removerter::parseValidScanInfo( void ) +{ + int num_valid_parsed {0}; + float movement_counter {0.0}; + + for(int curr_idx=0; curr_idx < int(sequence_scan_paths_.size()); curr_idx++) + { + // check the scan idx within the target idx range + if(curr_idx > end_idx_ || curr_idx < start_idx_) { + curr_idx++; + continue; + } + + // check enough movement occured (e.g., parse every 2m) + if(use_keyframe_gap_) { + if( remainder(num_valid_parsed, keyframe_gap_) != 0 ) { + num_valid_parsed++; + continue; + } + } + if(use_keyframe_meter_) { + if( 0 /*TODO*/ ) { + // TODO using movement_counter + } + } + + // save the info (reading scan bin is in makeGlobalMap) + sequence_valid_scan_paths_.emplace_back(sequence_scan_paths_.at(curr_idx)); + sequence_valid_scan_names_.emplace_back(sequence_scan_names_.at(curr_idx)); + + scan_poses_.emplace_back(sequence_scan_poses_.at(curr_idx)); // used for local2global + scan_inverse_poses_.emplace_back(sequence_scan_inverse_poses_.at(curr_idx)); // used for global2local + + // + num_valid_parsed++; + } + + if(use_keyframe_gap_) { + ROS_INFO_STREAM("\033[1;32m Total " << sequence_valid_scan_paths_.size() + << " nodes are used from the index range [" << start_idx_ << ", " << end_idx_ << "]" + << " (every " << keyframe_gap_ << " frames parsed)\033[0m"); + } +} // parseValidScanInfo + + +void Removerter::readValidScans( void ) +// for target range of scan idx +{ + const int cout_interval {100}; + int cout_counter {0}; + + for(auto& _scan_path : sequence_valid_scan_paths_) + { + // read bin files and save + pcl::PointCloud::Ptr points (new pcl::PointCloud); // pcl::PointCloud Ptr is a shared ptr so this points will be automatically destroyed after this function block (because no others ref it). + readBin(_scan_path, points); + + // pcdown + pcl::VoxelGrid downsize_filter; + downsize_filter.setLeafSize(kDownsampleVoxelSize, kDownsampleVoxelSize, kDownsampleVoxelSize); + downsize_filter.setInputCloud(points); + + pcl::PointCloud::Ptr downsampled_points (new pcl::PointCloud); + downsize_filter.filter(*downsampled_points); + + // save downsampled pointcloud + scans_.emplace_back(downsampled_points); + + // cout for debug + cout_counter++; + if (remainder(cout_counter, cout_interval) == 0) { + cout << _scan_path << endl; + cout << "Read KTTI point cloud with " << points->points.size() << " points." << endl; + cout << "downsampled KTTI point cloud with " << downsampled_points->points.size() << " points." << endl; + cout << " ... (display every " << cout_interval << " readings) ..." << endl; + } + } + cout << endl; +} // readValidScans + + +std::pair Removerter::map2RangeImg(const pcl::PointCloud::Ptr& _scan, + const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ + const std::pair _rimg_size) +{ + const float kVFOV = _fov.first; + const float kHFOV = _fov.second; + + const int kNumRimgRow = _rimg_size.first; + const int kNumRimgCol = _rimg_size.second; + + // @ range image initizliation + cv::Mat rimg = cv::Mat(kNumRimgRow, kNumRimgCol, CV_32FC1, cv::Scalar::all(kFlagNoPOINT)); // float matrix, save range value + cv::Mat rimg_ptidx = cv::Mat(kNumRimgRow, kNumRimgCol, CV_32SC1, cv::Scalar::all(0)); // int matrix, save point (of global map) index + + // @ points to range img + int num_points = _scan->points.size(); + #pragma omp parallel for num_threads(kNumOmpCores) + for (int pt_idx = 0; pt_idx < num_points; ++pt_idx) + { + PointType this_point = _scan->points[pt_idx]; + SphericalPoint sph_point = cart2sph(this_point); + + // @ note about vfov: e.g., (+ V_FOV/2) to adjust [-15, 15] to [0, 30] + // @ min and max is just for the easier (naive) boundary checks. + int lower_bound_row_idx {0}; + int lower_bound_col_idx {0}; + int upper_bound_row_idx {kNumRimgRow - 1}; + int upper_bound_col_idx {kNumRimgCol - 1}; + int pixel_idx_row = int(std::min(std::max(std::round(kNumRimgRow * (1 - (rad2deg(sph_point.el) + (kVFOV/float(2.0))) / (kVFOV - float(0.0)))), float(lower_bound_row_idx)), float(upper_bound_row_idx))); + int pixel_idx_col = int(std::min(std::max(std::round(kNumRimgCol * ((rad2deg(sph_point.az) + (kHFOV/float(2.0))) / (kHFOV - float(0.0)))), float(lower_bound_col_idx)), float(upper_bound_col_idx))); + + float curr_range = sph_point.r; + + // @ Theoretically, this if-block would have race condition (i.e., this is a critical section), + // @ But, the resulting range image is acceptable (watching via Rviz), + // @ so I just naively applied omp pragma for this whole for-block (2020.10.28) + // @ Reason: because this for loop is splited by the omp, points in a single splited for range do not race among them, + // @ also, a point A and B lied in different for-segments do not tend to correspond to the same pixel, + // # so we can assume practically there are few race conditions. + // @ P.S. some explicit mutexing directive makes the code even slower ref: https://stackoverflow.com/questions/2396430/how-to-use-lock-in-openmp + if ( curr_range < rimg.at(pixel_idx_row, pixel_idx_col) ) { + rimg.at(pixel_idx_row, pixel_idx_col) = curr_range; + rimg_ptidx.at(pixel_idx_row, pixel_idx_col) = pt_idx; + } + } + + return std::pair(rimg, rimg_ptidx); +} // map2RangeImg + + +cv::Mat Removerter::scan2RangeImg(const pcl::PointCloud::Ptr& _scan, + const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ + const std::pair _rimg_size) +{ + const float kVFOV = _fov.first; + const float kHFOV = _fov.second; + + const int kNumRimgRow = _rimg_size.first; + const int kNumRimgCol = _rimg_size.second; + // cout << "rimg size is: [" << _rimg_size.first << ", " << _rimg_size.second << "]." << endl; + + // @ range image initizliation + cv::Mat rimg = cv::Mat(kNumRimgRow, kNumRimgCol, CV_32FC1, cv::Scalar::all(kFlagNoPOINT)); // float matrix + + // @ points to range img + int num_points = _scan->points.size(); + #pragma omp parallel for num_threads(kNumOmpCores) + for (int pt_idx = 0; pt_idx < num_points; ++pt_idx) + { + PointType this_point = _scan->points[pt_idx]; + SphericalPoint sph_point = cart2sph(this_point); + + // @ note about vfov: e.g., (+ V_FOV/2) to adjust [-15, 15] to [0, 30] + // @ min and max is just for the easier (naive) boundary checks. + int lower_bound_row_idx {0}; + int lower_bound_col_idx {0}; + int upper_bound_row_idx {kNumRimgRow - 1}; + int upper_bound_col_idx {kNumRimgCol - 1}; + int pixel_idx_row = int(std::min(std::max(std::round(kNumRimgRow * (1 - (rad2deg(sph_point.el) + (kVFOV/float(2.0))) / (kVFOV - float(0.0)))), float(lower_bound_row_idx)), float(upper_bound_row_idx))); + int pixel_idx_col = int(std::min(std::max(std::round(kNumRimgCol * ((rad2deg(sph_point.az) + (kHFOV/float(2.0))) / (kHFOV - float(0.0)))), float(lower_bound_col_idx)), float(upper_bound_col_idx))); + + float curr_range = sph_point.r; + + // @ Theoretically, this if-block would have race condition (i.e., this is a critical section), + // @ But, the resulting range image is acceptable (watching via Rviz), + // @ so I just naively applied omp pragma for this whole for-block (2020.10.28) + // @ Reason: because this for loop is splited by the omp, points in a single splited for range do not race among them, + // @ also, a point A and B lied in different for-segments do not tend to correspond to the same pixel, + // # so we can assume practically there are few race conditions. + // @ P.S. some explicit mutexing directive makes the code even slower ref: https://stackoverflow.com/questions/2396430/how-to-use-lock-in-openmp + if ( curr_range < rimg.at(pixel_idx_row, pixel_idx_col) ) { + rimg.at(pixel_idx_row, pixel_idx_col) = curr_range; + } + } + + return rimg; +} // scan2RangeImg + + +void Removerter::mergeScansWithinGlobalCoord( + const std::vector::Ptr>& _scans, + const std::vector& _scans_poses, + pcl::PointCloud::Ptr& _ptcloud_to_save ) +{ + // NOTE: _scans must be in local coord + for(std::size_t scan_idx = 0 ; scan_idx < _scans.size(); scan_idx++) + { + auto ii_scan = _scans.at(scan_idx); // pcl::PointCloud::Ptr + auto ii_pose = _scans_poses.at(scan_idx); // Eigen::Matrix4d + + // local to global (local2global) + pcl::PointCloud::Ptr scan_global_coord(new pcl::PointCloud()); + pcl::transformPointCloud(*ii_scan, *scan_global_coord, kSE3MatExtrinsicLiDARtoPoseBase); + pcl::transformPointCloud(*scan_global_coord, *scan_global_coord, ii_pose); + + // merge the scan into the global map + *_ptcloud_to_save += *scan_global_coord; + } +} // mergeScansWithinGlobalCoord + + +void Removerter::octreeDownsampling(const pcl::PointCloud::Ptr& _src, pcl::PointCloud::Ptr& _to_save) +{ + pcl::octree::OctreePointCloudVoxelCentroid octree( kDownsampleVoxelSize ); + octree.setInputCloud(_src); + octree.defineBoundingBox(); + octree.addPointsFromInputCloud(); + pcl::octree::OctreePointCloudVoxelCentroid::AlignedPointTVector centroids; + octree.getVoxelCentroids(centroids); + + // init current map with the downsampled full cloud + _to_save->points.assign(centroids.begin(), centroids.end()); + _to_save->width = 1; + _to_save->height = _to_save->points.size(); // make sure again the format of the downsampled point cloud + ROS_INFO_STREAM("\033[1;32m Downsampled pointcloud have: " << _to_save->points.size() << " points.\033[0m"); + cout << endl; +} // octreeDownsampling + + +void Removerter::makeGlobalMap( void ) +{ + // transform local to global and merging the scans + map_global_orig_->clear(); + map_global_curr_->clear(); + + mergeScansWithinGlobalCoord(scans_, scan_poses_, map_global_orig_); + ROS_INFO_STREAM("\033[1;32m Map pointcloud (having redundant points) have: " << map_global_orig_->points.size() << " points.\033[0m"); + ROS_INFO_STREAM("\033[1;32m Downsampling leaf size is " << kDownsampleVoxelSize << " m.\033[0m"); + + // remove repeated (redundant) points + // - using OctreePointCloudVoxelCentroid for downsampling + // - For a large-size point cloud should use OctreePointCloudVoxelCentroid rather VoxelGrid + octreeDownsampling(map_global_orig_, map_global_curr_); + + // save the original cloud + if( kFlagSaveMapPointcloud ) { + // in global coord + std::string static_global_file_name = save_pcd_directory_ + "OriginalNoisyMapGlobal.pcd"; + pcl::io::savePCDFileASCII(static_global_file_name, *map_global_curr_); + ROS_INFO_STREAM("\033[1;32m The original pointcloud is saved (global coord): " << static_global_file_name << "\033[0m"); + + // in local coord (i.e., base_node_idx == 0 means a start idx is the identity pose) + int base_node_idx = base_node_idx_; + pcl::PointCloud::Ptr map_local_curr (new pcl::PointCloud); + transformGlobalMapToLocal(map_global_curr_, base_node_idx, map_local_curr); + std::string static_local_file_name = save_pcd_directory_ + "OriginalNoisyMapLocal.pcd"; + pcl::io::savePCDFileASCII(static_local_file_name, *map_local_curr); + ROS_INFO_STREAM("\033[1;32m The original pointcloud is saved (local coord): " << static_local_file_name << "\033[0m"); + } + // make tree (for fast ball search for the projection to make a map range image later) + // if(kUseSubsetMapCloud) // NOT recommend to use for under 5 million points map input + // kdtree_map_global_curr_->setInputCloud(map_global_curr_); + + // save current map into history + // TODO + // if(save_history_on_memory_) + // saveCurrentStaticMapHistory(); + +} // makeGlobalMap + + +void Removerter::transformGlobalMapSubsetToLocal(int _base_scan_idx) +{ + Eigen::Matrix4d base_pose_inverse = scan_inverse_poses_.at(_base_scan_idx); + + // global to local (global2local) + map_local_curr_->clear(); + pcl::transformPointCloud(*map_subset_global_curr_, *map_local_curr_, base_pose_inverse); + pcl::transformPointCloud(*map_local_curr_, *map_local_curr_, kSE3MatExtrinsicPoseBasetoLiDAR); + +} // transformGlobalMapSubsetToLocal + + +void Removerter::transformGlobalMapToLocal(int _base_scan_idx) +{ + Eigen::Matrix4d base_pose_inverse = scan_inverse_poses_.at(_base_scan_idx); + + // global to local (global2local) + map_local_curr_->clear(); + pcl::transformPointCloud(*map_global_curr_, *map_local_curr_, base_pose_inverse); + pcl::transformPointCloud(*map_local_curr_, *map_local_curr_, kSE3MatExtrinsicPoseBasetoLiDAR); + +} // transformGlobalMapToLocal + + +void Removerter::transformGlobalMapToLocal(int _base_scan_idx, pcl::PointCloud::Ptr& _map_local) +{ + Eigen::Matrix4d base_pose_inverse = scan_inverse_poses_.at(_base_scan_idx); + + // global to local (global2local) + _map_local->clear(); + pcl::transformPointCloud(*map_global_curr_, *_map_local, base_pose_inverse); + pcl::transformPointCloud(*_map_local, *_map_local, kSE3MatExtrinsicPoseBasetoLiDAR); + +} // transformGlobalMapToLocal + + +void Removerter::transformGlobalMapToLocal( + const pcl::PointCloud::Ptr& _map_global, + int _base_scan_idx, pcl::PointCloud::Ptr& _map_local) +{ + Eigen::Matrix4d base_pose_inverse = scan_inverse_poses_.at(_base_scan_idx); + + // global to local (global2local) + _map_local->clear(); + pcl::transformPointCloud(*_map_global, *_map_local, base_pose_inverse); + pcl::transformPointCloud(*_map_local, *_map_local, kSE3MatExtrinsicPoseBasetoLiDAR); + +} // transformGlobalMapToLocal + + +void Removerter::parseMapPointcloudSubsetUsingPtIdx( std::vector& _point_indexes, pcl::PointCloud::Ptr& _ptcloud_to_save ) +{ + // extractor + pcl::ExtractIndices extractor; + boost::shared_ptr> index_ptr = boost::make_shared>(_point_indexes); + extractor.setInputCloud(map_global_curr_); + extractor.setIndices(index_ptr); + extractor.setNegative(false); // If set to true, you can extract point clouds outside the specified index + + // parse + _ptcloud_to_save->clear(); + extractor.filter(*_ptcloud_to_save); +} // parseMapPointcloudSubsetUsingPtIdx + + +void Removerter::parseStaticMapPointcloudUsingPtIdx( std::vector& _point_indexes ) +{ + // extractor + pcl::ExtractIndices extractor; + boost::shared_ptr> index_ptr = boost::make_shared>(_point_indexes); + extractor.setInputCloud(map_global_curr_); + extractor.setIndices(index_ptr); + extractor.setNegative(false); // If set to true, you can extract point clouds outside the specified index + + // parse + map_global_curr_static_->clear(); + extractor.filter(*map_global_curr_static_); +} // parseStaticMapPointcloudUsingPtIdx + + +void Removerter::parseDynamicMapPointcloudUsingPtIdx( std::vector& _point_indexes ) +{ + // extractor + pcl::ExtractIndices extractor; + boost::shared_ptr> index_ptr = boost::make_shared>(_point_indexes); + extractor.setInputCloud(map_global_curr_); + extractor.setIndices(index_ptr); + extractor.setNegative(false); // If set to true, you can extract point clouds outside the specified index + + // parse + map_global_curr_dynamic_->clear(); + extractor.filter(*map_global_curr_dynamic_); +} // parseDynamicMapPointcloudUsingPtIdx + + +void Removerter::saveCurrentStaticAndDynamicPointCloudGlobal( void ) +{ + if( ! kFlagSaveMapPointcloud ) + return; + + std::string curr_res_alpha_str = std::to_string(curr_res_alpha_); + + // dynamic + std::string dyna_file_name = map_dynamic_save_dir_ + "/DynamicMapMapsideGlobalResX" + curr_res_alpha_str + ".pcd"; + pcl::io::savePCDFileASCII(dyna_file_name, *map_global_curr_dynamic_); + ROS_INFO_STREAM("\033[1;32m -- a pointcloud is saved: " << dyna_file_name << "\033[0m"); + + // static + std::string static_file_name = map_static_save_dir_ + "/StaticMapMapsideGlobalResX" + curr_res_alpha_str + ".pcd"; + pcl::io::savePCDFileASCII(static_file_name, *map_global_curr_static_); + ROS_INFO_STREAM("\033[1;32m -- a pointcloud is saved: " << static_file_name << "\033[0m"); +} // saveCurrentStaticAndDynamicPointCloudGlobal + + +void Removerter::saveCurrentStaticAndDynamicPointCloudLocal( int _base_node_idx ) +{ + if( ! kFlagSaveMapPointcloud ) + return; + + std::string curr_res_alpha_str = std::to_string(curr_res_alpha_); + + // dynamic + pcl::PointCloud::Ptr map_local_curr_dynamic (new pcl::PointCloud); + transformGlobalMapToLocal(map_global_curr_dynamic_, _base_node_idx, map_local_curr_dynamic); + std::string dyna_file_name = map_dynamic_save_dir_ + "/DynamicMapMapsideLocalResX" + curr_res_alpha_str + ".pcd"; + pcl::io::savePCDFileASCII(dyna_file_name, *map_local_curr_dynamic); + ROS_INFO_STREAM("\033[1;32m -- a pointcloud is saved: " << dyna_file_name << "\033[0m"); + + // static + pcl::PointCloud::Ptr map_local_curr_static (new pcl::PointCloud); + transformGlobalMapToLocal(map_global_curr_static_, _base_node_idx, map_local_curr_static); + std::string static_file_name = map_static_save_dir_ + "/StaticMapMapsideLocalResX" + curr_res_alpha_str + ".pcd"; + pcl::io::savePCDFileASCII(static_file_name, *map_local_curr_static); + ROS_INFO_STREAM("\033[1;32m -- a pointcloud is saved: " << static_file_name << "\033[0m"); + +} // saveCurrentStaticAndDynamicPointCloudLocal + + +std::vector Removerter::calcDescrepancyAndParseDynamicPointIdx + (const cv::Mat& _scan_rimg, const cv::Mat& _diff_rimg, const cv::Mat& _map_rimg_ptidx) +{ + int num_dyna_points {0}; // TODO: tracking the number of dynamic-assigned points and decide when to stop removing (currently just fixed iteration e.g., [2.5, 2.0, 1.5]) + + std::vector dynamic_point_indexes; + for (int row_idx = 0; row_idx < _diff_rimg.rows; row_idx++) { + for (int col_idx = 0; col_idx < _diff_rimg.cols; col_idx++) { + float this_diff = _diff_rimg.at(row_idx, col_idx); + float this_range = _scan_rimg.at(row_idx, col_idx); + + float adaptive_coeff = 0.05; // meter, // i.e., if 4m apart point, it should be 0.4m be diff (nearer) wrt the query + float adaptive_dynamic_descrepancy_threshold = adaptive_coeff * this_range; // adaptive descrepancy threshold + // float adaptive_dynamic_descrepancy_threshold = 0.1; + + if( this_diff < kValidDiffUpperBound // exclude no-point pixels either on scan img or map img (100 is roughly 100 meter) + && this_diff > adaptive_dynamic_descrepancy_threshold /* dynamic */) + { // dynamic + int this_point_idx_in_global_map = _map_rimg_ptidx.at(row_idx, col_idx); + dynamic_point_indexes.emplace_back(this_point_idx_in_global_map); + + // num_dyna_points++; // TODO + } + } + } + + return dynamic_point_indexes; +} // calcDescrepancyAndParseDynamicPointIdx + + +void Removerter::takeGlobalMapSubsetWithinBall( int _center_scan_idx ) +{ + Eigen::Matrix4d center_pose_se3 = scan_poses_.at(_center_scan_idx); + PointType center_pose; + center_pose.x = float(center_pose_se3(0, 3)); + center_pose.y = float(center_pose_se3(1, 3)); + center_pose.z = float(center_pose_se3(2, 3)); + + std::vector subset_indexes; + std::vector pointSearchSqDisGlobalMap; + kdtree_map_global_curr_->radiusSearch(center_pose, kBallSize, subset_indexes, pointSearchSqDisGlobalMap, 0); + parseMapPointcloudSubsetUsingPtIdx(subset_indexes, map_subset_global_curr_); +} // takeMapSubsetWithinBall + + +std::vector Removerter::calcDescrepancyAndParseDynamicPointIdxForEachScan( std::pair _rimg_shape ) +{ + std::vector dynamic_point_indexes; + // dynamic_point_indexes.reserve(100000); + for(std::size_t idx_scan=0; idx_scan < scans_.size(); ++idx_scan) { + // curr scan + pcl::PointCloud::Ptr _scan = scans_.at(idx_scan); + + // scan's pointcloud to range img + cv::Mat scan_rimg = scan2RangeImg(_scan, kFOV, _rimg_shape); // openMP inside + + // map's pointcloud to range img + if( kUseSubsetMapCloud ) { + takeGlobalMapSubsetWithinBall(idx_scan); + transformGlobalMapSubsetToLocal(idx_scan); // the most time comsuming part 1 + } else { + // if the input map size (of a batch) is short, just using this line is more fast. + // - e.g., 100-1000m or ~5 million points are ok, empirically more than 10Hz + transformGlobalMapToLocal(idx_scan); + } + auto [map_rimg, map_rimg_ptidx] = map2RangeImg(map_local_curr_, kFOV, _rimg_shape); // the most time comsuming part 2 -> so openMP applied inside + + // diff range img + const int kNumRimgRow = _rimg_shape.first; + const int kNumRimgCol = _rimg_shape.second; + cv::Mat diff_rimg = cv::Mat(kNumRimgRow, kNumRimgCol, CV_32FC1, cv::Scalar::all(0.0)); // float matrix, save range value + cv::absdiff(scan_rimg, map_rimg, diff_rimg); + + // parse dynamic points' indexes: rule: If a pixel value of diff_rimg is larger, scan is the further - means that pixel of submap is likely dynamic. + std::vector this_scan_dynamic_point_indexes = calcDescrepancyAndParseDynamicPointIdx(scan_rimg, diff_rimg, map_rimg_ptidx); + dynamic_point_indexes.insert(dynamic_point_indexes.end(), this_scan_dynamic_point_indexes.begin(), this_scan_dynamic_point_indexes.end()); + + // visualization + pubRangeImg(scan_rimg, scan_rimg_msg_, scan_rimg_msg_publisher_, kRangeColorAxis); + pubRangeImg(map_rimg, map_rimg_msg_, map_rimg_msg_publisher_, kRangeColorAxis); + pubRangeImg(diff_rimg, diff_rimg_msg_, diff_rimg_msg_publisher_, kRangeColorAxisForDiff); + + std::pair kRangeColorAxisForPtIdx {0.0, float(map_global_curr_->points.size())}; + pubRangeImg(map_rimg_ptidx, map_rimg_ptidx_msg_, map_rimg_ptidx_msg_publisher_, kRangeColorAxisForPtIdx); + + publishPointcloud2FromPCLptr(scan_publisher_, _scan); + } // for_each scan Done + + // remove repeated indexes + std::set dynamic_point_indexes_set (dynamic_point_indexes.begin(), dynamic_point_indexes.end()); + std::vector dynamic_point_indexes_unique (dynamic_point_indexes_set.begin(), dynamic_point_indexes_set.end()); + + return dynamic_point_indexes_unique; +} // calcDescrepancyForEachScan + + +std::vector Removerter::getStaticIdxFromDynamicIdx(const std::vector& _dynamic_point_indexes, int _num_all_points) +{ + std::vector pt_idx_all = linspace(0, _num_all_points, _num_all_points); + + std::set pt_idx_all_set (pt_idx_all.begin(), pt_idx_all.end()); + for(auto& _dyna_pt_idx: _dynamic_point_indexes) { + pt_idx_all_set.erase(_dyna_pt_idx); + } + + std::vector static_point_indexes (pt_idx_all_set.begin(), pt_idx_all_set.end()); + return static_point_indexes; +} // getStaticIdxFromDynamicIdx + + +std::vector Removerter::getGlobalMapStaticIdxFromDynamicIdx(const std::vector& _dynamic_point_indexes) +{ + int num_all_points = map_global_curr_->points.size(); + return getStaticIdxFromDynamicIdx(_dynamic_point_indexes, num_all_points); +} // getGlobalMapStaticIdxFromDynamicIdx + + + +void Removerter::saveCurrentStaticMapHistory(void) +{ + // deep copy + pcl::PointCloud::Ptr map_global_curr_static (new pcl::PointCloud); + *map_global_curr_static = *map_global_curr_; + + // save + static_map_global_history_.emplace_back(map_global_curr_static); +} // saveCurrentStaticMapHistory + + +void Removerter::removeOnce( float _res_alpha ) +{ + // filter spec (i.e., a shape of the range image) + curr_res_alpha_ = _res_alpha; + + std::pair rimg_shape = resetRimgSize(kFOV, _res_alpha); + float deg_per_pixel = 1.0 / _res_alpha; + ROS_INFO_STREAM("\033[1;32m Removing starts with resolution: x" << _res_alpha << " (" << deg_per_pixel << " deg/pixel)\033[0m"); + ROS_INFO_STREAM("\033[1;32m -- The range image size is: [" << rimg_shape.first << ", " << rimg_shape.second << "].\033[0m"); + ROS_INFO_STREAM("\033[1;32m -- The number of map points: " << map_global_curr_->points.size() << "\033[0m"); + ROS_INFO_STREAM("\033[1;32m -- ... starts cleaning ... " << "\033[0m"); + + // map-side removal: remove and get dynamic (will be removed) points' index set + std::vector dynamic_point_indexes = calcDescrepancyAndParseDynamicPointIdxForEachScan( rimg_shape ); + ROS_INFO_STREAM("\033[1;32m -- The number of dynamic points: " << dynamic_point_indexes.size() << "\033[0m"); + parseDynamicMapPointcloudUsingPtIdx(dynamic_point_indexes); + + // static_point_indexes == complemently indexing dynamic_point_indexes + std::vector static_point_indexes = getGlobalMapStaticIdxFromDynamicIdx(dynamic_point_indexes); + ROS_INFO_STREAM("\033[1;32m -- The number of static points: " << static_point_indexes.size() << "\033[0m"); + parseStaticMapPointcloudUsingPtIdx(static_point_indexes); + + // Update the current map and reset the tree + map_global_curr_->clear(); + *map_global_curr_ = *map_global_curr_static_; + + // if(kUseSubsetMapCloud) // NOT recommend to use for under 5 million points map input + // kdtree_map_global_curr_->setInputCloud(map_global_curr_); + +} // removeOnce + + +void Removerter::revertOnce( float _res_alpha ) +{ + std::pair rimg_shape = resetRimgSize(kFOV, _res_alpha); + float deg_per_pixel = 1.0 / _res_alpha; + ROS_INFO_STREAM("\033[1;32m Reverting starts with resolution: x" << _res_alpha << " (" << deg_per_pixel << " deg/pixel)\033[0m"); + ROS_INFO_STREAM("\033[1;32m -- The range image size is: [" << rimg_shape.first << ", " << rimg_shape.second << "].\033[0m"); + ROS_INFO_STREAM("\033[1;32m -- ... TODO ... \033[0m"); + + // TODO + +} // revertOnce + + +void Removerter::parsePointcloudSubsetUsingPtIdx( const pcl::PointCloud::Ptr& _ptcloud_orig, + std::vector& _point_indexes, pcl::PointCloud::Ptr& _ptcloud_to_save ) +{ + // extractor + pcl::ExtractIndices extractor; + boost::shared_ptr> index_ptr = boost::make_shared>(_point_indexes); + extractor.setInputCloud(_ptcloud_orig); + extractor.setIndices(index_ptr); + extractor.setNegative(false); // If set to true, you can extract point clouds outside the specified index + + // parse + _ptcloud_to_save->clear(); + extractor.filter(*_ptcloud_to_save); +} // parsePointcloudSubsetUsingPtIdx + + +pcl::PointCloud::Ptr Removerter::local2global(const pcl::PointCloud::Ptr& _scan_local, int _scan_idx) +{ + Eigen::Matrix4d scan_pose = scan_poses_.at(_scan_idx); + + pcl::PointCloud::Ptr scan_global(new pcl::PointCloud()); + pcl::transformPointCloud(*_scan_local, *scan_global, kSE3MatExtrinsicLiDARtoPoseBase); + pcl::transformPointCloud(*scan_global, *scan_global, scan_pose); + + return scan_global; +} + +pcl::PointCloud::Ptr Removerter::global2local(const pcl::PointCloud::Ptr& _scan_global, int _scan_idx) +{ + Eigen::Matrix4d base_pose_inverse = scan_inverse_poses_.at(_scan_idx); + + pcl::PointCloud::Ptr scan_local(new pcl::PointCloud()); + pcl::transformPointCloud(*_scan_global, *scan_local, base_pose_inverse); + pcl::transformPointCloud(*scan_local, *scan_local, kSE3MatExtrinsicPoseBasetoLiDAR); + + return scan_local; +} + +std::pair::Ptr, pcl::PointCloud::Ptr> + Removerter::removeDynamicPointsOfScanByKnn ( int _scan_idx ) +{ + // curr scan (in local coord) + pcl::PointCloud::Ptr scan_orig = scans_.at(_scan_idx); + auto scan_pose = scan_poses_.at(_scan_idx); + + // curr scan (in global coord) + pcl::PointCloud::Ptr scan_orig_global = local2global(scan_orig, _scan_idx); + kdtree_scan_global_curr_->setInputCloud(scan_orig_global); + int num_points_of_a_scan = scan_orig_global->points.size(); + + // + pcl::PointCloud::Ptr scan_static_global (new pcl::PointCloud); + pcl::PointCloud::Ptr scan_dynamic_global (new pcl::PointCloud); + for (std::size_t pt_idx = 0; pt_idx < num_points_of_a_scan; pt_idx++) + { + std::vector topk_indexes_scan; + std::vector topk_L2dists_scan; + kdtree_scan_global_curr_->nearestKSearch(scan_orig_global->points[pt_idx], kNumKnnPointsToCompare, topk_indexes_scan, topk_L2dists_scan); + float sum_topknn_dists_in_scan = accumulate( topk_L2dists_scan.begin(), topk_L2dists_scan.end(), 0.0); + float avg_topknn_dists_in_scan = sum_topknn_dists_in_scan / float(kNumKnnPointsToCompare); + + std::vector topk_indexes_map; + std::vector topk_L2dists_map; + kdtree_map_global_curr_->nearestKSearch(scan_orig_global->points[pt_idx], kNumKnnPointsToCompare, topk_indexes_map, topk_L2dists_map); + float sum_topknn_dists_in_map = accumulate( topk_L2dists_map.begin(), topk_L2dists_map.end(), 0.0); + float avg_topknn_dists_in_map = sum_topknn_dists_in_map / float(kNumKnnPointsToCompare); + + // + if ( std::abs(avg_topknn_dists_in_scan - avg_topknn_dists_in_map) < kScanKnnAndMapKnnAvgDiffThreshold) { + scan_static_global->push_back(scan_orig_global->points[pt_idx]); + } else { + scan_dynamic_global->push_back(scan_orig_global->points[pt_idx]); + } + } + + // again global2local because later in the merging global map function, which requires scans within each local coord. + pcl::PointCloud::Ptr scan_static_local = global2local(scan_static_global, _scan_idx); + pcl::PointCloud::Ptr scan_dynamic_local = global2local(scan_dynamic_global, _scan_idx); + + ROS_INFO_STREAM("\033[1;32m The scan " << sequence_valid_scan_paths_.at(_scan_idx) << "\033[0m"); + ROS_INFO_STREAM("\033[1;32m -- The number of static points in a scan: " << scan_static_local->points.size() << "\033[0m"); + ROS_INFO_STREAM("\033[1;32m -- The number of dynamic points in a scan: " << num_points_of_a_scan - scan_static_local->points.size() << "\033[0m"); + + publishPointcloud2FromPCLptr(static_curr_scan_publisher_, scan_static_local); + publishPointcloud2FromPCLptr(dynamic_curr_scan_publisher_, scan_dynamic_local); + usleep( kPauseTimeForClearStaticScanVisualization ); + + return std::pair::Ptr, pcl::PointCloud::Ptr> (scan_static_local, scan_dynamic_local); + +} // removeDynamicPointsOfScanByKnn + + +void Removerter::saveStaticScan( int _scan_idx, const pcl::PointCloud::Ptr& _ptcloud ) +{ + + std::string file_name_orig = sequence_valid_scan_names_.at(_scan_idx); + std::string file_name = scan_static_save_dir_ + "/" + file_name_orig + ".pcd"; + ROS_INFO_STREAM("\033[1;32m Scan " << _scan_idx << "'s static points is saved (" << file_name << ")\033[0m"); + pcl::io::savePCDFileASCII(file_name, *_ptcloud); +} // saveStaticScan + + +void Removerter::saveDynamicScan( int _scan_idx, const pcl::PointCloud::Ptr& _ptcloud ) +{ + std::string file_name_orig = sequence_valid_scan_names_.at(_scan_idx); + std::string file_name = scan_dynamic_save_dir_ + "/" + file_name_orig + ".pcd"; + ROS_INFO_STREAM("\033[1;32m Scan " << _scan_idx << "'s static points is saved (" << file_name << ")\033[0m"); + pcl::io::savePCDFileASCII(file_name, *_ptcloud); +} // saveDynamicScan + + +void Removerter::saveCleanedScans(void) +{ + if( ! kFlagSaveCleanScans ) + return; + + for(std::size_t idx_scan=0; idx_scan < scans_static_.size(); idx_scan++) { + saveStaticScan(idx_scan, scans_static_.at(idx_scan)); + saveDynamicScan(idx_scan, scans_dynamic_.at(idx_scan)); + } +} // saveCleanedScans + + +void Removerter::saveMapPointcloudByMergingCleanedScans(void) +{ + // merge for verification + if( ! kFlagSaveMapPointcloud ) + return; + + // static map + { + pcl::PointCloud::Ptr map_global_static_scans_merged_to_verify_full (new pcl::PointCloud); + pcl::PointCloud::Ptr map_global_static_scans_merged_to_verify (new pcl::PointCloud); + mergeScansWithinGlobalCoord(scans_static_, scan_poses_, map_global_static_scans_merged_to_verify_full); + octreeDownsampling(map_global_static_scans_merged_to_verify_full, map_global_static_scans_merged_to_verify); + + // global + std::string local_file_name = map_static_save_dir_ + "/StaticMapScansideMapGlobal.pcd"; + pcl::io::savePCDFileASCII(local_file_name, *map_global_static_scans_merged_to_verify); + ROS_INFO_STREAM("\033[1;32m [For verification] A static pointcloud (cleaned scans merged) is saved (global coord): " << local_file_name << "\033[0m"); + + // local + pcl::PointCloud::Ptr map_local_static_scans_merged_to_verify (new pcl::PointCloud); + int base_node_idx = base_node_idx_; + transformGlobalMapToLocal(map_global_static_scans_merged_to_verify, base_node_idx, map_local_static_scans_merged_to_verify); + std::string global_file_name = map_static_save_dir_ + "/StaticMapScansideMapLocal.pcd"; + pcl::io::savePCDFileASCII(global_file_name, *map_local_static_scans_merged_to_verify); + ROS_INFO_STREAM("\033[1;32m [For verification] A static pointcloud (cleaned scans merged) is saved (local coord): " << global_file_name << "\033[0m"); + } + + // dynamic map + { + pcl::PointCloud::Ptr map_global_dynamic_scans_merged_to_verify_full (new pcl::PointCloud); + pcl::PointCloud::Ptr map_global_dynamic_scans_merged_to_verify (new pcl::PointCloud); + mergeScansWithinGlobalCoord(scans_dynamic_, scan_poses_, map_global_dynamic_scans_merged_to_verify_full); + octreeDownsampling(map_global_dynamic_scans_merged_to_verify_full, map_global_dynamic_scans_merged_to_verify); + + // global + std::string local_file_name = map_dynamic_save_dir_ + "/DynamicMapScansideMapGlobal.pcd"; + pcl::io::savePCDFileASCII(local_file_name, *map_global_dynamic_scans_merged_to_verify); + ROS_INFO_STREAM("\033[1;32m [For verification] A dynamic pointcloud (cleaned scans merged) is saved (global coord): " << local_file_name << "\033[0m"); + + // local + pcl::PointCloud::Ptr map_local_dynamic_scans_merged_to_verify (new pcl::PointCloud); + int base_node_idx = base_node_idx_; + transformGlobalMapToLocal(map_global_dynamic_scans_merged_to_verify, base_node_idx, map_local_dynamic_scans_merged_to_verify); + std::string global_file_name = map_dynamic_save_dir_ + "/DynamicMapScansideMapLocal.pcd"; + pcl::io::savePCDFileASCII(global_file_name, *map_local_dynamic_scans_merged_to_verify); + ROS_INFO_STREAM("\033[1;32m [For verification] A dynamic pointcloud (cleaned scans merged) is saved (local coord): " << global_file_name << "\033[0m"); + } +} // saveMapPointcloudByMergingCleanedScans + + +void Removerter::scansideRemovalForEachScan( void ) +{ + // for fast scan-side neighbor search + kdtree_map_global_curr_->setInputCloud(map_global_curr_); + + // for each scan + for(std::size_t idx_scan=0; idx_scan < scans_.size(); idx_scan++) { + auto [this_scan_static, this_scan_dynamic] = removeDynamicPointsOfScanByKnn(idx_scan); + scans_static_.emplace_back(this_scan_static); + scans_dynamic_.emplace_back(this_scan_dynamic); + } +} // scansideRemovalForEachScan + + +void Removerter::scansideRemovalForEachScanAndSaveThem( void ) +{ + scansideRemovalForEachScan(); + saveCleanedScans(); + saveMapPointcloudByMergingCleanedScans(); +} // scansideRemovalForEachScanAndSaveThem + + +void Removerter::run( void ) +{ + // load scan and poses + parseValidScanInfo(); + readValidScans(); + + // construct initial map using the scans and the corresponding poses + makeGlobalMap(); + + // map-side removals + for(float _rm_res: remove_resolution_list_) { + removeOnce( _rm_res ); + } + + // if you want to every iteration's map data, place below two lines to inside of the above for loop + saveCurrentStaticAndDynamicPointCloudGlobal(); // if you want to save within the global points uncomment this line + saveCurrentStaticAndDynamicPointCloudLocal(base_node_idx_); // w.r.t specific node's coord. 0 means w.r.t the start node, as an Identity. + + // TODO + // map-side reverts + // if you want to remove as much as possible, you can use omit this steps + for(float _rv_res: revert_resolution_list_) { + revertOnce( _rv_res ); + } + + // scan-side removals + scansideRemovalForEachScanAndSaveThem(); + +} \ No newline at end of file diff --git a/src/RosParamServer.cpp b/src/RosParamServer.cpp new file mode 100644 index 0000000..8f1521f --- /dev/null +++ b/src/RosParamServer.cpp @@ -0,0 +1,99 @@ +#include "removert/RosParamServer.h" + + +RosParamServer::RosParamServer() +: nh(nh_super), ROSimg_transporter_(nh) +{ + // for visualization + + scan_rimg_msg_publisher_ = ROSimg_transporter_.advertise("/scan_rimg_single", 10); + map_rimg_msg_publisher_ = ROSimg_transporter_.advertise("/map_rimg_single", 10); + diff_rimg_msg_publisher_ = ROSimg_transporter_.advertise("/diff_rimg_single", 10); + map_rimg_ptidx_msg_publisher_ = ROSimg_transporter_.advertise("/map_rimg_ptidx_single", 10); + + nh.param("removert/rimg_color_min", rimg_color_min_, 0.0); + nh.param("removert/rimg_color_max", rimg_color_max_, 10.0); + kRangeColorAxis = std::pair {rimg_color_min_, rimg_color_max_}; // meter + kRangeColorAxisForDiff = std::pair{0.0, 0.5}; // meter + + // fov + nh.param("removert/sequence_vfov", kVFOV, 50.0); + nh.param("removert/sequence_hfov", kHFOV, 360.0); + kFOV = std::pair(kVFOV, kHFOV); + + // resolution + nh.param>("removert/remove_resolution_list", remove_resolution_list_, std::vector()); + nh.param>("removert/revert_resolution_list", revert_resolution_list_, std::vector()); + + // sequcne system info + nh.param>("removert/ExtrinsicLiDARtoPoseBase", kVecExtrinsicLiDARtoPoseBase, std::vector()); + kSE3MatExtrinsicLiDARtoPoseBase = Eigen::Map>(kVecExtrinsicLiDARtoPoseBase.data(), 4, 4); + kSE3MatExtrinsicPoseBasetoLiDAR = kSE3MatExtrinsicLiDARtoPoseBase.inverse(); + + // parsing bin file paths + nh.param("removert/sequence_scan_dir", sequence_scan_dir_, "/use/your/directory/having/*.bin"); + for(auto& _entry : fs::directory_iterator(sequence_scan_dir_)) { + sequence_scan_names_.emplace_back(_entry.path().filename()); + sequence_scan_paths_.emplace_back(_entry.path()); + } + std::sort(sequence_scan_names_.begin(), sequence_scan_names_.end()); + std::sort(sequence_scan_paths_.begin(), sequence_scan_paths_.end()); + + num_total_scans_of_sequence_ = sequence_scan_paths_.size(); + ROS_INFO_STREAM("\033[1;32m Total : " << num_total_scans_of_sequence_ << " scans in the directory.\033[0m"); + + // point cloud pre-processing + nh.param("removert/downsample_voxel_size", kDownsampleVoxelSize, 0.05); + + // parsing pose info + nh.param("removert/sequence_pose_path", sequence_pose_path_, "/use/your/path/having/pose.txt"); + std::ifstream pose_file_handle (sequence_pose_path_); + int num_poses {0}; + std::string strOneLine; + while (getline(pose_file_handle, strOneLine)) + { + // str to vec + std::vector ith_pose_vec = splitPoseLine(strOneLine, ' '); + if(ith_pose_vec.size() == 12) { + ith_pose_vec.emplace_back(double(0.0)); + ith_pose_vec.emplace_back(double(0.0)); + ith_pose_vec.emplace_back(double(0.0)); + ith_pose_vec.emplace_back(double(1.0)); + } + + // vec to eig + Eigen::Matrix4d ith_pose = Eigen::Map>(ith_pose_vec.data(), 4, 4); + Eigen::Matrix4d ith_pose_inverse = ith_pose.inverse(); + + // save (move) + // cout << "Pose of scan: " << sequence_scan_names_.at(num_poses) << endl; + // cout << ith_pose << endl; + sequence_scan_poses_.emplace_back(ith_pose); + sequence_scan_inverse_poses_.emplace_back(ith_pose_inverse); + + num_poses++; + } + // check the number of scans and the number of poses are equivalent + assert(sequence_scan_paths_.size() == sequence_scan_poses_.size()); + + // target scan index range (used in Removert.cpp) + nh.param("removert/start_idx", start_idx_, 1); + nh.param("removert/end_idx", end_idx_, 100); + + nh.param("removert/use_keyframe_gap", use_keyframe_gap_, true); + nh.param("removert/use_keyframe_meter", use_keyframe_meter_, false); + nh.param("removert/keyframe_gap", keyframe_gap_, 10); + nh.param("removert/keyframe_meter", keyframe_gap_meter_, 2.0); + + // faster + nh.param("removert/num_omp_cores", kNumOmpCores, 4); + + // save info + nh.param("removert/saveMapPCD", kFlagSaveMapPointcloud, false); + nh.param("removert/saveCleanScansPCD", kFlagSaveCleanScans, false); + nh.param("removert/save_pcd_directory", save_pcd_directory_, "/"); + + + usleep(100); +} // ctor RosParamServer + diff --git a/src/removert_main.cpp b/src/removert_main.cpp new file mode 100644 index 0000000..f667412 --- /dev/null +++ b/src/removert_main.cpp @@ -0,0 +1,14 @@ +#include "removert/Removerter.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "removert"); + ROS_INFO("\033[1;32m----> Removert Main Started.\033[0m"); + + Removerter RMV; + RMV.run(); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/utility.cpp b/src/utility.cpp new file mode 100644 index 0000000..0d7865b --- /dev/null +++ b/src/utility.cpp @@ -0,0 +1,128 @@ +#include "removert/utility.h" + +bool cout_debug = false; + + +void readBin(std::string _bin_path, pcl::PointCloud::Ptr _pcd_ptr) +{ + std::fstream input(_bin_path.c_str(), ios::in | ios::binary); + if(!input.good()){ + cerr << "Could not read file: " << _bin_path << endl; + exit(EXIT_FAILURE); + } + input.seekg(0, ios::beg); + + for (int ii=0; input.good() && !input.eof(); ii++) { + PointType point; + + input.read((char *) &point.x, sizeof(float)); + input.read((char *) &point.y, sizeof(float)); + input.read((char *) &point.z, sizeof(float)); + input.read((char *) &point.intensity, sizeof(float)); + + _pcd_ptr->push_back(point); + } + input.close(); +} + +std::vector splitPoseLine(std::string _str_line, char _delimiter) { + std::vector parsed; + std::stringstream ss(_str_line); + std::string temp; + while (getline(ss, temp, _delimiter)) { + parsed.push_back(std::stod(temp)); // convert string to "double" + } + return parsed; +} + +SphericalPoint cart2sph(const PointType & _cp) +{ // _cp means cartesian point + + if(cout_debug){ + cout << "Cartesian Point [x, y, z]: [" << _cp.x << ", " << _cp.y << ", " << _cp.z << endl; + } + + SphericalPoint sph_point { + std::atan2(_cp.y, _cp.x), + std::atan2(_cp.z, std::sqrt(_cp.x*_cp.x + _cp.y*_cp.y)), + std::sqrt(_cp.x*_cp.x + _cp.y*_cp.y + _cp.z*_cp.z) + }; + return sph_point; +} + + +std::pair resetRimgSize(const std::pair _fov, const float _resize_ratio) +{ + // default is 1 deg x 1 deg + float alpha_vfov = _resize_ratio; + float alpha_hfov = _resize_ratio; + + float V_FOV = _fov.first; + float H_FOV = _fov.second; + + int NUM_RANGE_IMG_ROW = std::round(V_FOV*alpha_vfov); + int NUM_RANGE_IMG_COL = std::round(H_FOV*alpha_hfov); + + std::pair rimg {NUM_RANGE_IMG_ROW, NUM_RANGE_IMG_COL}; + return rimg; +} + + +std::set convertIntVecToSet(const std::vector & v) +{ + std::set s; + for (int x : v) { + s.insert(x); + } + return s; +} + +void pubRangeImg(cv::Mat& _rimg, + sensor_msgs::ImagePtr& _msg, + image_transport::Publisher& _publiser, + std::pair _caxis) +{ + cv::Mat scan_rimg_viz = convertColorMappedImg(_rimg, _caxis); + _msg = cvmat2msg(scan_rimg_viz); + _publiser.publish(_msg); +} // pubRangeImg + + +sensor_msgs::ImagePtr cvmat2msg(const cv::Mat &_img) +{ + sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", _img).toImageMsg(); + return msg; +} + + +void publishPointcloud2FromPCLptr(const ros::Publisher& _scan_publisher, const pcl::PointCloud::Ptr _scan) +{ + sensor_msgs::PointCloud2 tempCloud; + pcl::toROSMsg(*_scan, tempCloud); + tempCloud.header.stamp = ros::Time::now(); + tempCloud.header.frame_id = "removert"; + _scan_publisher.publish(tempCloud); +} // publishPointcloud2FromPCLptr + + +sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame) +{ + sensor_msgs::PointCloud2 tempCloud; + pcl::toROSMsg(*thisCloud, tempCloud); + tempCloud.header.stamp = thisStamp; + tempCloud.header.frame_id = thisFrame; + if (thisPub->getNumSubscribers() != 0) + thisPub->publish(tempCloud); + return tempCloud; +} + +float pointDistance(PointType p) +{ + return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); +} + +float pointDistance(PointType p1, PointType p2) +{ + return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); +} + diff --git a/tools/matlab/viewer_static_dynamic_overlap.m b/tools/matlab/viewer_static_dynamic_overlap.m new file mode 100644 index 0000000..ace1cb8 --- /dev/null +++ b/tools/matlab/viewer_static_dynamic_overlap.m @@ -0,0 +1,32 @@ + +result_dir= '/home/user/removert/results/'; + +%% +pc_original = pcread(fullfile(result_dir, 'OriginalNoisyMapLocal.pcd')); + +pc_static_scanside = pcread(fullfile(result_dir, 'map_static', 'StaticMapScansideMapLocal.pcd')); +pc_dynamic_scanside = pcread(fullfile(result_dir, 'map_dynamic', 'DynamicMapScansideMapLocal.pcd')); + +%% +static_pt_size = 2; +dynamic_pt_size = 100; +color_axis = [0, 3]; % meter +ylim_for_better_view = [-60, 40]; +xlim_for_better_view = [-50, 250]; + +figure(1); clf; +pcshow(pc_original.Location, 'MarkerSize', static_pt_size ); +colormap jet; caxis(color_axis); +xlim(xlim_for_better_view); +ylim(ylim_for_better_view); +view(0, 90); +title("original map"); + +figure(2); clf; +pcshow(pc_static_scanside.Location, [0.98, 0.98, 0.98], 'MarkerSize', static_pt_size ); hold on; +pcshow(pc_dynamic_scanside.Location, 'r', 'MarkerSize', dynamic_pt_size ); +xlim(xlim_for_better_view); +ylim(ylim_for_better_view); +view(0, 90); +title("static map from scan-side removal"); + diff --git a/tools/matlab/viewer_staticmap_only.m b/tools/matlab/viewer_staticmap_only.m new file mode 100644 index 0000000..d5fe2cd --- /dev/null +++ b/tools/matlab/viewer_staticmap_only.m @@ -0,0 +1,42 @@ + +result_dir= '/home/user/removert/results/'; + +%% +pc_original = pcread(fullfile(result_dir, 'OriginalNoisyMapLocal.pcd')); + +pc_static_mapside = pcread(fullfile(result_dir, 'map_static', 'StaticMapMapsideLocalResX1.500000.pcd')); +pc_dynamic_mapside = pcread(fullfile(result_dir, 'map_dynamic', 'DynamicMapMapsideLocalResX1.500000.pcd')); + +pc_static_scanside = pcread(fullfile(result_dir, 'map_static', 'StaticMapScansideMapLocal.pcd')); +pc_dynamic_scanside = pcread(fullfile(result_dir, 'map_dynamic', 'DynamicMapScansideMapLocal.pcd')); + +%% +pt_size = 2; +color_axis = [0, 3]; % meter +ylim_for_better_view = [-60, 40]; +xlim_for_better_view = [-50, 250]; + +figure(1); clf; +pcshow(pc_original.Location, 'MarkerSize', pt_size ); +colormap jet; caxis(color_axis); +xlim(xlim_for_better_view); +ylim(ylim_for_better_view); +view(0, 90); +title("original map"); + +figure(2); clf; +pcshow(pc_static_mapside.Location, 'MarkerSize', pt_size ); +colormap jet; caxis(color_axis); +xlim(xlim_for_better_view); +ylim(ylim_for_better_view); +view(0, 90); +title("static map from map-side removal"); + +figure(3); clf; +pcshow(pc_static_scanside.Location, 'MarkerSize', pt_size ); +colormap jet; caxis(color_axis); +xlim(xlim_for_better_view); +ylim(ylim_for_better_view); +view(0, 90); +title("static map from scan-side removal"); +