diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..2c3671b5 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hdl_graph_slam) + +# -mavx causes a lot of errors!! +add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) +set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") + +# pcl 1.7 causes a segfault when it is built with debug mode +set(CMAKE_BUILD_TYPE "RELEASE") + +find_package(catkin REQUIRED COMPONENTS + roscpp + pcl_ros + geodesy + nmea_msgs + sensor_msgs + message_generation + ndt_omp +) + +find_package(PCL 1.7 REQUIRED) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) +message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) +message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) + +find_package(G2O REQUIRED) +include_directories(SYSTEM ${G2O_INCLUDE_DIR} ${G2O_INCLUDE_DIRS}) +link_directories(${G2O_LIBRARY_DIRS}) +# link_libraries(${G2O_LIBRARIES}) + +find_package(OpenMP) +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + +######################## +## message generation ## +######################## +add_message_files(FILES + FloorCoeffs.msg +) + +generate_messages(DEPENDENCIES std_msgs) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS include +# LIBRARIES hdl_scan_matching_odometry +# CATKIN_DEPENDS pcl_ros roscpp sensor_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### +include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +# nodelets +add_library(prefiltering_nodelet apps/prefiltering_nodelet.cpp) +target_link_libraries(prefiltering_nodelet + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} +) + + +add_library(floor_detection_nodelet apps/floor_detection_nodelet.cpp) +target_link_libraries(floor_detection_nodelet + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} +) +add_dependencies(floor_detection_nodelet ${PROJECT_NAME}_gencpp) + + +add_library(scan_matching_odometry_nodelet + apps/scan_matching_odometry_nodelet.cpp + src/hdl_graph_slam/registrations.cpp +) +target_link_libraries(scan_matching_odometry_nodelet + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} +) + + +add_library(hdl_graph_slam_nodelet + apps/hdl_graph_slam_nodelet.cpp + src/hdl_graph_slam/graph_slam.cpp + src/hdl_graph_slam/keyframe.cpp + src/hdl_graph_slam/map_cloud_generator.cpp + src/hdl_graph_slam/registrations.cpp + src/hdl_graph_slam/information_matrix_calculator.cpp +) +target_link_libraries(hdl_graph_slam_nodelet + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${G2O_TYPES_DATA} + ${G2O_CORE_LIBRARY} + ${G2O_STUFF_LIBRARY} + ${G2O_SOLVER_PCG} + ${G2O_SOLVER_CSPARSE} + ${G2O_TYPES_SLAM3D} + ${G2O_TYPES_SLAM3D_ADDONS} +) +add_dependencies(hdl_graph_slam_nodelet ${PROJECT_NAME}_gencpp) diff --git a/README.md b/README.md new file mode 100644 index 00000000..bd7d43f7 --- /dev/null +++ b/README.md @@ -0,0 +1,105 @@ +# hdl_graph_slam +***hdl_graph_slam*** is an open source ROS package for real-time 3D slam using a 3D LIDAR. It is based on 3D Graph SLAM with NDT scan matching-based odometry estimation and loop detection. It also utilizes floor plane detection to generate an environmental map with a completely flat floor. This package also contains an implementation of multi-threaded NDT (10 times faster than the normal NDT in PCL!). We have tested this packaged mainly in indoor environments, but it can be applied to outdoor environment mapping as well. + + + +video (TODO: upload this video to youtube!) + +## Nodelets +***hdl_graph_slam*** consists of four nodelets. +- *prefiltering_nodelet* +- *scan_matching_odometry_nodelet* +- *floor_detection_nodelet* +- *hdl_graph_slam_nodelet* + +The input point cloud is first downsampled by *prefiltering_nodelet*, and then passed to the next nodelets. While *scan_matching_odometry_nodelet* estimates the sensor pose by iteratively applying a scan matching between consecutive frames (i.e., odometry estimation), *floor_detection_nodelet* detects floor planes by RANSAC. The estimated odometry and the detected floor planes are sent to *hdl_graph_slam*. To compensate the accumulated error of the scan matching, it performs loop detection and optimizes a pose graph which takes odometry, loops, and floor planes into account.
+ +**[Sep/1/2017]** A GPS-based graph optimization has been implemented. *hdl_graph_slam_nodelet* receives GPS data (i.e., latitude, longitude) from */gps/geopoint* and converts them into the UTM coordinate, and then it adds them into the pose graph. It effectively compensate the scan matching error in outdoor environments. + +
+ + +### Parameters +All the parameters are listed in *launch/hdl_graph_slam.launch* as ros params. + +### Services +- */hdl_graph_slam/dump* (std_srvs/Empty) + - save all data (point clouds, floor coeffs, odoms, and pose graph) to the current directory. + +## Multi-threaded Scan Matching Algorithms +This package contains implementations of multi-threaded and SSE friendly NDT and GICP algorithms. They are derived from the implementations in PCL. With recent multi-core processors, they may show ten times faster processing speed than the NDT and the GICP in PCL. See *include/pclomp* and *src/pclomp* to use the boosted scan matching algorithms. + +## Requirements +***hdl_graph_slam*** requires the following libraries: +- OpenMP +- PCL 1.7 +- g2o + +Note that ***hdl_graph_slam*** cannot be built with older g2o libraries (such as ros-indigo-libg2o). Install the latest g2o: +```bash +git clone https://github.com/RainerKuemmerle/g2o.git +cd g2o +mkdir build && cd build +cmake .. -DCMAKE_BUILD_TYPE=RELEASE +make -j8 +sudo make install +``` + +The following ros packages are required: +- geodesy +- pcl_ros +```bash +sudo apt-get install ros-indigo-geodesy ros-indigo-pcl_ros +``` + +## Example + +Example bag files (recorded in a small room): +- [hdl_501.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_501.bag.tar.gz) (raw data, 344MB) +- [hdl_501_filtered.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_501_filtered.bag.tar.gz) (downsampled data, 57MB, **Recommended!**) + +```bash +rosparam set use_sim_time true +roslaunch hdl_graph_slam hdl_graph_slam_501.launch +``` + +```bash +roscd hdl_graph_slam/rviz +rviz -d hdl_graph_slam.rviz +``` + +```bash +rosbag play --clock hdl_501_filtered.bag +``` + +We also provide bag_player.py which adjusts the playback speed according to the processing speed of your PC. It allows processing data as fast as possible for your PC. + +```bash +rosrun hdl_graph_slam bag_player.py hdl_501_filtered.bag +``` + +You'll see a generated point cloud like: + + + + +## Example with GPS feature +Ford Campus Vision and Lidar Data Set [URL]. + +The following script converts the Ford Lidar Dataset to a rosbag and plays it. In this example, ***hdl_graph_slam*** utilized the GPS data to correct the pose graph. + +```bash +cd IJRR-Dataset-2 +rosrun hdl_graph_slam ford2bag.py dataset-2.bag +rosrun hdl_graph_slam bag_player.py dataset-2.bag +``` + + + +## Papers +Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Robotics and Autonomous Systems (under review) [PDF]. + +## Contact +Kenji Koide, Active Intelligent Systems Laboratory, Toyohashi University of Technology [URL]
+koide@aisl.cs.tut.ac.jp + diff --git a/apps/floor_detection_nodelet.cpp b/apps/floor_detection_nodelet.cpp new file mode 100644 index 00000000..607a9c56 --- /dev/null +++ b/apps/floor_detection_nodelet.cpp @@ -0,0 +1,263 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace hdl_graph_slam { + +class FloorDetectionNodelet : public nodelet::Nodelet { +public: + typedef pcl::PointXYZI PointT; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FloorDetectionNodelet() {} + virtual ~FloorDetectionNodelet() {} + + virtual void onInit() { + NODELET_DEBUG("initializing floor_detection_nodelet..."); + nh = getNodeHandle(); + private_nh = getPrivateNodeHandle(); + + initialize_params(); + + points_sub = nh.subscribe("/filtered_points", 256, &FloorDetectionNodelet::cloud_callback, this); + floor_pub = nh.advertise("/floor_detection/floor_coeffs", 32); + + read_until_pub = nh.advertise("/floor_detection/read_until", 32); + floor_filtered_pub = nh.advertise("/floor_detection/floor_filtered_points", 32); + floor_points_pub = nh.advertise("/floor_detection/floor_points", 32); + } + + +private: + /** + * @brief initialize parameters + */ + void initialize_params() { + tilt_deg = private_nh.param("tilt_deg", 0.0); // approximate sensor tilt angle [deg] + sensor_height = private_nh.param("sensor_height", 2.0); // approximate sensor height [m] + height_clip_range= private_nh.param("height_clip_range", 1.0); // points with heights in [sensor_height - height_clip_range, sensor_height + height_clip_range] will be used for floor detection + floor_pts_thresh = private_nh.param("floor_pts_thresh", 512); // minimum number of support points of RANSAC to accept a detected floor plane + floor_normal_thresh = private_nh.param("floor_normal_thresh", 10.0); // verticality check thresold for the detected floor plane [deg] + use_normal_filtering = private_nh.param("use_normal_filtering", true); // if true, points with "non-"vertical normals will be filtered before RANSAC + normal_filter_thresh = private_nh.param("normal_filter_thresh", 20.0); // "non-"verticality check threshold [deg] + } + + /** + * @brief callback for point clouds + * @param cloud_msg point cloud msg + */ + void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*cloud_msg, *cloud); + + if(cloud->empty()) { + return; + } + + // floor detection + boost::optional floor = detect(cloud); + + // publish the detected floor coefficients + hdl_graph_slam::FloorCoeffs coeffs; + coeffs.header = cloud_msg->header; + if(floor) { + coeffs.coeffs.resize(4); + for(int i=0; i<4; i++) { + coeffs.coeffs[i] = (*floor)[i]; + } + } + + floor_pub.publish(coeffs); + + // for offline estimation + std_msgs::HeaderPtr read_until(new std_msgs::Header()); + read_until->frame_id = "/velodyne_points"; + read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0); + read_until_pub.publish(read_until); + + read_until->frame_id = "/filtered_points"; + read_until_pub.publish(read_until); + } + + /** + * @brief detect the floor plane from a point cloud + * @param cloud input cloud + * @return detected floor plane coefficients + */ + boost::optional detect(const pcl::PointCloud::Ptr& cloud) const { + // compensate the tilt rotation + Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity(); + tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix(); + + // filtering before RANSAC (height and normal filtering) + pcl::PointCloud::Ptr filtered(new pcl::PointCloud); + pcl::transformPointCloud(*cloud, *filtered, tilt_matrix); + filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false); + filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true); + + if(use_normal_filtering) { + filtered = normal_filtering(filtered); + } + + pcl::transformPointCloud(*filtered, *filtered, static_cast(tilt_matrix.inverse())); + + if(floor_filtered_pub.getNumSubscribers()) { + filtered->header = cloud->header; + floor_filtered_pub.publish(filtered); + } + + // too few points for RANSAC + if(filtered->size() < floor_pts_thresh) { + return boost::none; + } + + // RANSAC + pcl::SampleConsensusModelPlane::Ptr model_p(new pcl::SampleConsensusModelPlane(filtered)); + pcl::RandomSampleConsensus ransac(model_p); + ransac.setDistanceThreshold(0.1); + ransac.computeModel(); + + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + ransac.getInliers(inliers->indices); + + // too few inliers + if(inliers->indices.size() < floor_pts_thresh) { + return boost::none; + } + + // verticality check of the detected floor's normal + Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ(); + + Eigen::VectorXf coeffs; + ransac.getModelCoefficients(coeffs); + + double dot = coeffs.head<3>().dot(reference.head<3>()); + if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) { + // the normal is not vertical + return boost::none; + } + + // make the normal upward + if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) { + coeffs *= -1.0f; + } + + if(floor_points_pub.getNumSubscribers()) { + pcl::PointCloud::Ptr inlier_cloud(new pcl::PointCloud); + pcl::ExtractIndices extract; + extract.setInputCloud(filtered); + extract.setIndices(inliers); + extract.filter(*inlier_cloud); + inlier_cloud->header = cloud->header; + + floor_points_pub.publish(inlier_cloud); + } + + return Eigen::Vector4f(coeffs); + } + + /** + * @brief plane_clip + * @param src_cloud + * @param plane + * @param negative + * @return + */ + pcl::PointCloud::Ptr plane_clip(const pcl::PointCloud::Ptr& src_cloud, const Eigen::Vector4f& plane, bool negative) const { + pcl::PlaneClipper3D clipper(plane); + pcl::PointIndices::Ptr indices(new pcl::PointIndices); + + clipper.clipPointCloud3D(*src_cloud, indices->indices); + + pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud); + + pcl::ExtractIndices extract; + extract.setInputCloud(src_cloud); + extract.setIndices(indices); + extract.setNegative(negative); + extract.filter(*dst_cloud); + + return dst_cloud; + } + + /** + * @brief filter points with non-vertical normals + * @param cloud input cloud + * @return filtered cloud + */ + pcl::PointCloud::Ptr normal_filtering(const pcl::PointCloud::Ptr& cloud) const { + pcl::NormalEstimation ne; + ne.setInputCloud(cloud); + + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + ne.setSearchMethod(tree); + + pcl::PointCloud::Ptr normals(new pcl::PointCloud); + ne.setKSearch(10); + ne.setViewPoint(0.0f, 0.0f, sensor_height); + ne.compute(*normals); + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud); + filtered->reserve(cloud->size()); + + for (int i = 0; i < cloud->size(); i++) { + float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ()); + if (std::abs(dot) > std::cos(normal_filter_thresh * M_PI / 180.0)) { + filtered->push_back(cloud->at(i)); + } + } + + filtered->width = filtered->size(); + filtered->height = 1; + filtered->is_dense = false; + + return filtered; + } + + +private: + ros::NodeHandle nh; + ros::NodeHandle private_nh; + + // ROS topics + ros::Subscriber points_sub; + + ros::Publisher floor_pub; + ros::Publisher floor_points_pub; + ros::Publisher floor_filtered_pub; + + ros::Publisher read_until_pub; + + // floor detection parameters + // see initialize_params() for the details + double tilt_deg; + double sensor_height; + double height_clip_range; + + int floor_pts_thresh; + double floor_normal_thresh; + + bool use_normal_filtering; + double normal_filter_thresh; +}; + +} + +PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::FloorDetectionNodelet, nodelet::Nodelet) diff --git a/apps/hdl_graph_slam_nodelet.cpp b/apps/hdl_graph_slam_nodelet.cpp new file mode 100644 index 00000000..b0c39064 --- /dev/null +++ b/apps/hdl_graph_slam_nodelet.cpp @@ -0,0 +1,697 @@ +#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 + + +namespace hdl_graph_slam { + +class HdlGraphSlamNodelet : public nodelet::Nodelet { +public: + typedef pcl::PointXYZI PointT; + + HdlGraphSlamNodelet() { + } + virtual ~HdlGraphSlamNodelet() { + + } + + virtual void onInit() { + nh = getNodeHandle(); + mt_nh = getMTNodeHandle(); + private_nh = getPrivateNodeHandle(); + + // init parameters + map_frame_id = private_nh.param("map_frame_id", "map"); + odom_frame_id = private_nh.param("odom_frame_id", "odom"); + map_cloud_resolution = private_nh.param("map_cloud_resolution", 0.05); + trans_odom2map.setIdentity(); + + max_keyframes_per_update = private_nh.param("max_keyframes_per_update", 10); + + // + graph_slam.reset(new GraphSLAM()); + keyframe_updater.reset(new KeyframeUpdater(private_nh)); + loop_detector.reset(new LoopDetector(private_nh)); + map_cloud_generator.reset(new MapCloudGenerator()); + inf_calclator.reset(new InformationMatrixCalculator(private_nh)); + nmea_parser.reset(new NmeaSentenceParser()); + + gps_edge_stddev = private_nh.param("gps_edge_stddev", 10000.0); + floor_edge_stddev = private_nh.param("floor_edge_stddev", 10.0); + last_gps_edge_stamp = ros::Time(0); + + // subscribers + odom_sub.reset(new message_filters::Subscriber(nh, "/odom", 32)); + cloud_sub.reset(new message_filters::Subscriber(nh, "/filtered_points", 32)); + sync.reset(new message_filters::TimeSynchronizer(*odom_sub, *cloud_sub, 32)); + sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2)); + floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 32, &HdlGraphSlamNodelet::floor_coeffs_callback, this); + gps_sub = nh.subscribe("/gps/geopoint", 32, &HdlGraphSlamNodelet::gps_callback, this); + nmea_sub = nh.subscribe("/gpsimu_driver/nmea_sentence", 32, &HdlGraphSlamNodelet::nmea_callback, this); + + // publishers + markers_pub = nh.advertise("/hdl_graph_slam/markers", 16); + odom2map_pub = nh.advertise("/hdl_graph_slam/odom2pub", 16); + map_points_pub = mt_nh.advertise("/hdl_graph_slam/map_points", 1); + read_until_pub = nh.advertise("/hdl_graph_slam/read_until", 32); + + dump_service_server = nh.advertiseService("/hdl_graph_slam/dump", &HdlGraphSlamNodelet::dump_service, this); + + double graph_update_interval = private_nh.param("graph_update_interval", 3.0); + double map_cloud_update_interval = private_nh.param("map_cloud_update_interval", 10.0); + optimization_timer = mt_nh.createTimer(ros::Duration(graph_update_interval), &HdlGraphSlamNodelet::optimization_timer_callback, this); + map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &HdlGraphSlamNodelet::map_points_publish_timer_callback, this); + } + +private: + /** + * @brief received point clouds are pushed to #keyframe_queue + * @param odom_msg + * @param cloud_msg + */ + void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) { + const ros::Time& stamp = odom_msg->header.stamp; + Eigen::Isometry3d odom = odom2isometry(odom_msg); + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*cloud_msg, *cloud); + + if(!keyframe_updater->update(odom)) { + std::lock_guard lock(keyframe_queue_mutex); + if(keyframe_queue.empty()) { + std_msgs::Header read_until; + read_until.stamp = stamp + ros::Duration(30, 0); + read_until.frame_id = "/velodyne_points"; + read_until_pub.publish(read_until); + read_until.frame_id = "/filtered_points"; + read_until_pub.publish(read_until); + } + + return; + } + + double accum_d = keyframe_updater->get_accum_distance(); + KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, cloud)); + + std::lock_guard lock(keyframe_queue_mutex); + keyframe_queue.push_back(keyframe); + } + + /** + * @brief this method adds all the keyframes in #keyframe_queue to the pose graph + * @return if true, at least one keyframe is added to the pose graph + */ + bool flush_keyframe_queue() { + std::lock_guard lock(keyframe_queue_mutex); + + if(keyframe_queue.empty()) { + return false; + } + + trans_odom2map_mutex.lock(); + Eigen::Isometry3d odom2map(trans_odom2map.cast()); + trans_odom2map_mutex.unlock(); + + int num_processed = 0; + for(int i=0; i(keyframe_queue.size(), max_keyframes_per_update); i++) { + num_processed = i; + + const auto& keyframe = keyframe_queue[i]; + new_keyframes.push_back(keyframe); + + Eigen::Isometry3d odom = odom2map * keyframe->odom; + keyframe->node = graph_slam->add_se3_node(odom); + keyframe_hash[keyframe->stamp] = keyframe; + + if(i==0 && keyframes.empty()) { + continue; + } + + // add edge between keyframes + const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1]; + + Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom; + Eigen::MatrixXd information = inf_calclator->calc_information_matrix(prev_keyframe->cloud, keyframe->cloud, relative_pose); + graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information); + } + + std_msgs::Header read_until; + read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(30, 0); + read_until.frame_id = "/velodyne_points"; + read_until_pub.publish(read_until); + read_until.frame_id = "/filtered_points"; + read_until_pub.publish(read_until); + + keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1); + + return true; + } + + void nmea_callback(const nmea_msgs::SentenceConstPtr& nmea_msg) { + GPRMC grmc = nmea_parser->parse(nmea_msg->sentence); + + if(grmc.status != 'A') { + return; + } + + geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped()); + gps_msg->header = nmea_msg->header; + gps_msg->position.latitude = grmc.latitude; + gps_msg->position.longitude = grmc.longitude; + gps_msg->position.altitude = NAN; + + gps_callback(gps_msg); + } + + /** + * @brief received gps data is added to #gps_queue + * @param gps_msg + */ + void gps_callback(const geographic_msgs::GeoPointStampedConstPtr& gps_msg) { + std::lock_guard lock(gps_queue_mutex); + gps_queue.push_back(gps_msg); + } + + std::ofstream ofs; + + /** + * @brief + * @return + */ + bool flush_gps_queue() { + std::lock_guard lock(gps_queue_mutex); + + if(keyframes.empty()) { + return false; + } + + const auto& latest_keyframe_stamp = keyframes.back()->stamp; + + bool updated = false; + auto seek = keyframes.begin(); + for(const auto& gps_msg : gps_queue) { + seek = std::lower_bound(seek, keyframes.end(), gps_msg->header.stamp, [&](const KeyFrame::Ptr& key, const ros::Time& stamp) { return key->stamp < stamp; }); + if(seek == keyframes.end()) { + break; + } + + double residual = ((*seek)->stamp - gps_msg->header.stamp).toSec(); + if(std::abs(residual) > 0.1 || (*seek)->utm_coord) { + continue; + } + + if(gps_msg->header.stamp - last_gps_edge_stamp < ros::Duration(30.0)) { + continue; + } + + geodesy::UTMPoint utm; + geodesy::fromMsg(gps_msg->position, utm); + + if(!ofs.is_open()) { + ofs.open("latlon"); + } + + ofs << boost::format("%.9f %.9f %.9f %.9f") % gps_msg->position.latitude % gps_msg->position.longitude % utm.easting % utm.northing << std::endl; + + Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude); + + if(!zero_utm) { + zero_utm = xyz; + } + xyz -= (*zero_utm); + + (*seek)->utm_coord = xyz; + + Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / gps_edge_stddev; + graph_slam->add_se3_prior_xy_edge((*seek)->node, xyz.head<2>(), information_matrix); + + last_gps_edge_stamp = gps_msg->header.stamp; + + updated = true; + } + + auto remove_loc = std::upper_bound(gps_queue.begin(), gps_queue.end(), latest_keyframe_stamp, + [=](const ros::Time& stamp, const geographic_msgs::GeoPointStampedConstPtr& geopoint) { + return stamp < geopoint->header.stamp; + } + ); + gps_queue.erase(gps_queue.begin(), remove_loc); + + return updated; + } + + + /** + * @brief received floor coefficients are added to #floor_coeffs_queue + * @param floor_coeffs_msg + */ + void floor_coeffs_callback(const hdl_graph_slam::FloorCoeffsConstPtr& floor_coeffs_msg) { + if(floor_coeffs_msg->coeffs.empty()) { + return; + } + + std::lock_guard lock(floor_coeffs_queue_mutex); + floor_coeffs_queue.push_back(floor_coeffs_msg); + } + + /** + * @brief this methods associates floor coefficients messages with registered keyframes, and then adds the associated coeffs to the pose graph + * @return if true, at least one floor plane edge is added to the pose graph + */ + bool flush_floor_queue() { + std::lock_guard lock(floor_coeffs_queue_mutex); + + if(keyframes.empty()) { + return false; + } + + const auto& latest_keyframe_stamp = keyframes.back()->stamp; + + bool updated = false; + for(const auto& floor_coeffs : floor_coeffs_queue) { + if(floor_coeffs->header.stamp > latest_keyframe_stamp) { + break; + } + + auto found = keyframe_hash.find(floor_coeffs->header.stamp); + if(found == keyframe_hash.end()) { + continue; + } + + const auto& keyframe = found->second; + + Eigen::Vector4d coeffs(floor_coeffs->coeffs[0], floor_coeffs->coeffs[1], floor_coeffs->coeffs[2], floor_coeffs->coeffs[3]); + Eigen::Matrix3d information = Eigen::Matrix3d::Identity() * (1.0 / floor_edge_stddev); + graph_slam->add_se3_plane_edge(keyframe->node, graph_slam->floor_plane_node, coeffs, information); + + keyframe->floor_coeffs = coeffs; + + updated = true; + } + + auto remove_loc = std::upper_bound(floor_coeffs_queue.begin(), floor_coeffs_queue.end(), latest_keyframe_stamp, + [=](const ros::Time& stamp, const hdl_graph_slam::FloorCoeffsConstPtr& coeffs) { + return stamp < coeffs->header.stamp; + } + ); + floor_coeffs_queue.erase(floor_coeffs_queue.begin(), remove_loc); + + return updated; + } + + /** + * @brief generate a map point cloud and publish it + * @param event + */ + void map_points_publish_timer_callback(const ros::WallTimerEvent& event) { + if(!map_points_pub.getNumSubscribers()) { + return; + } + + std::vector snapshot; + + keyframes_snapshot_mutex.lock(); + snapshot = keyframes_snapshot; + keyframes_snapshot_mutex.unlock(); + + auto cloud = map_cloud_generator->generate(snapshot, 0.05); + if(!cloud) { + return; + } + + cloud->header.frame_id = map_frame_id; + cloud->header.stamp = snapshot.back()->cloud->header.stamp; + + sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2()); + pcl::toROSMsg(*cloud, *cloud_msg); + + map_points_pub.publish(cloud_msg); + } + + /** + * @brief this methods adds all the data in the queues to the pose graph, and then optimizes the pose graph + * @param event + */ + void optimization_timer_callback(const ros::TimerEvent& event) { + std::lock_guard lock(main_thread_mutex); + + // add keyframes and floor coeffs in the queues to the pose graph + if(!flush_keyframe_queue() & !flush_floor_queue() & !flush_gps_queue()) { + std_msgs::Header read_until; + read_until.stamp = event.current_real + ros::Duration(30, 0); + read_until.frame_id = "/velodyne_points"; + read_until_pub.publish(read_until); + read_until.frame_id = "/filtered_points"; + read_until_pub.publish(read_until); + + return; + } + + // loop detection + std::vector loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam); + for(const auto& loop : loops) { + Eigen::Isometry3d relpose(loop->relative_pose.cast()); + Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose); + graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix); + } + + std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes)); + new_keyframes.clear(); + + // optimize the pose graph + graph_slam->optimize(); + + // publish tf + const auto& keyframe = keyframes.back(); + Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse(); + trans_odom2map_mutex.lock(); + trans_odom2map = trans.matrix().cast(); + trans_odom2map_mutex.unlock(); + + if(map_points_pub.getNumSubscribers()) { + std::vector snapshot(keyframes.size()); + std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), + [=](const KeyFrame::Ptr& k) { + return std::make_shared(k); + }); + + std::lock_guard lock(keyframes_snapshot_mutex); + keyframes_snapshot.swap(snapshot); + } + + if(odom2map_pub.getNumSubscribers()) { + geometry_msgs::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix().cast(), map_frame_id, odom_frame_id); + odom2map_pub.publish(ts); + } + + if(markers_pub.getNumSubscribers()) { + auto markers = create_marker_array(event.current_real); + markers_pub.publish(markers); + } + } + + /** + * @brief create visualization marker + * @param stamp + * @return + */ + visualization_msgs::MarkerArray create_marker_array(const ros::Time& stamp) const { + visualization_msgs::MarkerArray markers; + markers.markers.resize(4); + + // node markers + visualization_msgs::Marker& traj_marker = markers.markers[0]; + traj_marker.header.frame_id = "map"; + traj_marker.header.stamp = stamp; + traj_marker.ns = "nodes"; + traj_marker.id = 0; + traj_marker.type = visualization_msgs::Marker::SPHERE_LIST; + + traj_marker.pose.orientation.w = 1.0; + traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 0.5; + + traj_marker.points.resize(keyframes.size()); + traj_marker.colors.resize(keyframes.size()); + for(int i=0; inode->estimate().translation(); + traj_marker.points[i].x = pos.x(); + traj_marker.points[i].y = pos.y(); + traj_marker.points[i].z = pos.z(); + + double p = static_cast(i) / keyframes.size(); + traj_marker.colors[i].r = 1.0 - p; + traj_marker.colors[i].g = p; + traj_marker.colors[i].b = 0.0; + traj_marker.colors[i].a = 1.0; + } + + // edge markers + visualization_msgs::Marker& edge_marker = markers.markers[1]; + edge_marker.header.frame_id = "map"; + edge_marker.header.stamp = stamp; + edge_marker.ns = "edges"; + edge_marker.id = 1; + edge_marker.type = visualization_msgs::Marker::LINE_LIST; + + edge_marker.pose.orientation.w = 1.0; + edge_marker.scale.x = 0.05; + + edge_marker.points.resize(graph_slam->graph->edges().size() * 2); + edge_marker.colors.resize(graph_slam->graph->edges().size() * 2); + + auto edge_itr = graph_slam->graph->edges().begin(); + for(int i=0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) { + g2o::HyperGraph::Edge* edge = *edge_itr; + g2o::EdgeSE3* edge_se3 = dynamic_cast(edge); + if(edge_se3) { + g2o::VertexSE3* v1 = dynamic_cast(edge_se3->vertices()[0]); + g2o::VertexSE3* v2 = dynamic_cast(edge_se3->vertices()[1]); + Eigen::Vector3d pt1 = v1->estimate().translation(); + Eigen::Vector3d pt2 = v2->estimate().translation(); + + edge_marker.points[i*2].x = pt1.x(); + edge_marker.points[i*2].y = pt1.y(); + edge_marker.points[i*2].z = pt1.z(); + edge_marker.points[i*2 + 1].x = pt2.x(); + edge_marker.points[i*2 + 1].y = pt2.y(); + edge_marker.points[i*2 + 1].z = pt2.z(); + + double p1 = static_cast(v1->id()) / graph_slam->graph->vertices().size(); + double p2 = static_cast(v2->id()) / graph_slam->graph->vertices().size(); + edge_marker.colors[i*2].r = 1.0 - p1; + edge_marker.colors[i*2].g = p1; + edge_marker.colors[i*2].a = 1.0; + edge_marker.colors[i*2 + 1].r = 1.0 - p2; + edge_marker.colors[i*2 + 1].g = p2; + edge_marker.colors[i*2 + 1].a = 1.0; + + if(std::abs(v1->id() - v2->id()) > 2) { + edge_marker.points[i*2].z += 0.5; + edge_marker.points[i*2 + 1].z += 0.5; + } + + continue; + } + + g2o::EdgeSE3Plane* edge_plane = dynamic_cast(edge); + if(edge_plane) { + g2o::VertexSE3* v1 = dynamic_cast(edge_plane->vertices()[0]); + Eigen::Vector3d pt1 = v1->estimate().translation(); + Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0); + + edge_marker.points[i*2].x = pt1.x(); + edge_marker.points[i*2].y = pt1.y(); + edge_marker.points[i*2].z = pt1.z(); + edge_marker.points[i*2 + 1].x = pt2.x(); + edge_marker.points[i*2 + 1].y = pt2.y(); + edge_marker.points[i*2 + 1].z = pt2.z(); + + edge_marker.colors[i*2].b = 1.0; + edge_marker.colors[i*2].a = 1.0; + edge_marker.colors[i*2 + 1].b = 1.0; + edge_marker.colors[i*2 + 1].a = 1.0; + + continue; + } + + g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast(edge); + if(edge_priori_xy) { + g2o::VertexSE3* v1 = dynamic_cast(edge_priori_xy->vertices()[0]); + Eigen::Vector3d pt1 = v1->estimate().translation(); + Eigen::Vector3d pt2 = Eigen::Vector3d::Zero(); + pt2.head<2>() = edge_priori_xy->measurement(); + + edge_marker.points[i*2].x = pt1.x(); + edge_marker.points[i*2].y = pt1.y(); + edge_marker.points[i*2].z = pt1.z(); + edge_marker.points[i*2 + 1].x = pt2.x(); + edge_marker.points[i*2 + 1].y = pt2.y(); + edge_marker.points[i*2 + 1].z = pt2.z(); + + edge_marker.colors[i*2].r = 1.0; + edge_marker.colors[i*2].a = 1.0; + edge_marker.colors[i*2 + 1].r = 1.0; + edge_marker.colors[i*2 + 1].a = 1.0; + + continue; + } + } + + // sphere + visualization_msgs::Marker& sphere_marker = markers.markers[3]; + sphere_marker.header.frame_id = "map"; + sphere_marker.header.stamp = stamp; + sphere_marker.ns = "loop_close_radius"; + sphere_marker.id = 0; + sphere_marker.type = visualization_msgs::Marker::SPHERE; + + if(!keyframes.empty()) { + Eigen::Vector3d pos = keyframes.back()->node->estimate().translation(); + sphere_marker.pose.position.x = pos.x(); + sphere_marker.pose.position.y = pos.y(); + sphere_marker.pose.position.z = pos.z(); + } + sphere_marker.pose.orientation.w = 1.0; + sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0; + + sphere_marker.color.r = 1.0; + sphere_marker.color.a = 0.3; + + return markers; + } + + /** + * @brief dump all data to the current directory + * @param req + * @param res + * @return + */ + bool dump_service(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) { + std::lock_guard lock(main_thread_mutex); + + std::array buffer; + buffer.fill(0); + time_t rawtime; + time(&rawtime); + const auto timeinfo = localtime(&rawtime); + strftime(buffer.data(), sizeof(buffer), "%d-%m-%Y %H:%M:%S", timeinfo); + std::string directory(buffer.data()); + + if(!boost::filesystem::is_directory(directory)) { + boost::filesystem::create_directory(directory); + } + + std::cout << "all data dumped to:" << std::flush; + system("pwd"); + + graph_slam->save(directory + "/graph.g2o"); + for(int i=0; idump(sst.str()); + } + + return true; + } + +private: + // ROS + ros::NodeHandle nh; + ros::NodeHandle mt_nh; + ros::NodeHandle private_nh; + ros::Timer optimization_timer; + ros::WallTimer map_publish_timer; + + std::unique_ptr> odom_sub; + std::unique_ptr> cloud_sub; + std::unique_ptr> sync; + + ros::Subscriber gps_sub; + ros::Subscriber nmea_sub; + ros::Subscriber floor_sub; + + ros::Publisher markers_pub; + + std::string map_frame_id; + std::string odom_frame_id; + + std::mutex trans_odom2map_mutex; + Eigen::Matrix4f trans_odom2map; + ros::Publisher odom2map_pub; + + ros::Publisher read_until_pub; + ros::Publisher map_points_pub; + + std::atomic_bool dump_request; + ros::ServiceServer dump_service_server; + + // keyframe queue + std::mutex keyframe_queue_mutex; + std::deque keyframe_queue; + + // gps queue + double gps_edge_stddev; + boost::optional zero_utm; + ros::Time last_gps_edge_stamp; + std::mutex gps_queue_mutex; + std::deque gps_queue; + + // floor_coeffs queue + double floor_edge_stddev; + std::mutex floor_coeffs_queue_mutex; + std::deque floor_coeffs_queue; + + // for map cloud generation + double map_cloud_resolution; + std::mutex keyframes_snapshot_mutex; + std::vector keyframes_snapshot; + std::unique_ptr map_cloud_generator; + + // graph slam + // all the below members must be accessed after locking main_thread_mutex + std::mutex main_thread_mutex; + + int max_keyframes_per_update; + std::deque new_keyframes; + + std::vector keyframes; + std::unordered_map keyframe_hash; + + std::unique_ptr graph_slam; + std::unique_ptr loop_detector; + std::unique_ptr keyframe_updater; + std::unique_ptr nmea_parser; + + std::unique_ptr inf_calclator; +}; + +} + +PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::HdlGraphSlamNodelet, nodelet::Nodelet) diff --git a/apps/prefiltering_nodelet.cpp b/apps/prefiltering_nodelet.cpp new file mode 100644 index 00000000..4aadd0b4 --- /dev/null +++ b/apps/prefiltering_nodelet.cpp @@ -0,0 +1,162 @@ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace hdl_graph_slam { + +class PrefilteringNodelet : public nodelet::Nodelet { +public: + typedef pcl::PointXYZI PointT; + + PrefilteringNodelet() {} + virtual ~PrefilteringNodelet() {} + + virtual void onInit() { + nh = getNodeHandle(); + private_nh = getPrivateNodeHandle(); + + initialize_params(); + + points_sub = nh.subscribe("/velodyne_points", 64, &PrefilteringNodelet::cloud_callback, this); + points_pub = nh.advertise("/filtered_points", 32); + } + +private: + void initialize_params() { + std::string downsample_method = private_nh.param("downsample_method", "VOXELGRID"); + double downsample_resolution = private_nh.param("downsample_resolution", 0.1); + + if(downsample_method == "VOXELGRID") { + std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; + boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); + voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); + downsample_filter = voxelgrid; + } else if(downsample_method == "APPROX_VOXELGRID") { + std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; + boost::shared_ptr> approx_voxelgrid(new pcl::ApproximateVoxelGrid()); + approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); + downsample_filter = approx_voxelgrid; + } else { + if(downsample_method != "NONE") { + std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl; + std::cerr << " : use passthrough filter" <("outlier_removal_method", "STATISTICAL"); + if(outlier_removal_method == "STATISTICAL") { + int mean_k = private_nh.param("statistical_mean_k", 20); + double stddev_mul_thresh = private_nh.param("statistical_stddev", 1.0); + std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl; + + pcl::StatisticalOutlierRemoval::Ptr sor(new pcl::StatisticalOutlierRemoval()); + sor->setMeanK(mean_k); + sor->setStddevMulThresh(stddev_mul_thresh); + outlier_removal_filter = sor; + } else if(outlier_removal_method == "RADIUS") { + double radius = private_nh.param("radius_radius", 0.8); + int min_neighbors = private_nh.param("radus_min_neighbors", 2); + std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl; + + pcl::RadiusOutlierRemoval::Ptr rad(new pcl::RadiusOutlierRemoval()); + rad->setRadiusSearch(radius); + rad->setMinNeighborsInRadius(min_neighbors); + } else { + std::cout << "outlier_removal: NONE" << std::endl; + } + + use_distance_filter = private_nh.param("use_distance_filter", true); + distance_near_thresh = private_nh.param("distance_near_thresh", 1.0); + distance_far_thresh = private_nh.param("distance_far_thresh", 100.0); + } + + void cloud_callback(const pcl::PointCloud::ConstPtr& src_cloud) { + if(src_cloud->empty()) { + return; + } + + pcl::PointCloud::ConstPtr filtered = distance_filter(src_cloud); + filtered = downsample(filtered); + filtered = outlier_removal(filtered); + + points_pub.publish(filtered); + } + + pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { + if(!downsample_filter) { + return cloud; + } + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + downsample_filter->setInputCloud(cloud); + downsample_filter->filter(*filtered); + filtered->header = cloud->header; + + return filtered; + } + + pcl::PointCloud::ConstPtr outlier_removal(const pcl::PointCloud::ConstPtr& cloud) const { + if(!outlier_removal_filter) { + return cloud; + } + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + outlier_removal_filter->setInputCloud(cloud); + outlier_removal_filter->filter(*filtered); + filtered->header = cloud->header; + + return filtered; + } + + pcl::PointCloud::ConstPtr distance_filter(const pcl::PointCloud::ConstPtr& cloud) const { + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + filtered->reserve(cloud->size()); + + std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points), + [&](const PointT& p) { + double d = p.getVector3fMap().norm(); + return d > distance_near_thresh && d < distance_far_thresh; + } + ); + + filtered->width = filtered->size(); + filtered->height = 1; + filtered->is_dense = false; + + filtered->header = cloud->header; + + return filtered; + } + +private: + ros::NodeHandle nh; + ros::NodeHandle private_nh; + + ros::Subscriber points_sub; + ros::Publisher points_pub; + + bool use_distance_filter; + double distance_near_thresh; + double distance_far_thresh; + + pcl::Filter::Ptr downsample_filter; + pcl::Filter::Ptr outlier_removal_filter; + +}; + +} + +PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::PrefilteringNodelet, nodelet::Nodelet) diff --git a/apps/scan_matching_odometry_nodelet.cpp b/apps/scan_matching_odometry_nodelet.cpp new file mode 100644 index 00000000..a3a7914e --- /dev/null +++ b/apps/scan_matching_odometry_nodelet.cpp @@ -0,0 +1,266 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace hdl_graph_slam { + +class ScanMatchingOdometryNodelet : public nodelet::Nodelet { +public: + typedef pcl::PointXYZI PointT; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ScanMatchingOdometryNodelet() {} + virtual ~ScanMatchingOdometryNodelet() {} + + virtual void onInit() { + NODELET_DEBUG("initializing scan_matching_odometry_nodelet..."); + nh = getNodeHandle(); + private_nh = getPrivateNodeHandle(); + + initialize_params(); + + points_sub = nh.subscribe("/filtered_points", 256, &ScanMatchingOdometryNodelet::cloud_callback, this); + read_until_pub = nh.advertise("/scan_matching_odometry/read_until", 32); + odom_pub = nh.advertise("/odom", 32); + } + +private: + /** + * @brief initialize parameters + */ + void initialize_params() { + auto& pnh = private_nh; + odom_frame_id = pnh.param("odom_frame_id", "odom"); + base_frame_id = pnh.param("base_frame_id", "velodyne"); + + // The minimum tranlational distance and rotation angle between keyframes. + // If this value is zero, frames are always compared with the previous frame + keyframe_delta_trans = pnh.param("keyframe_delta_trans", 0.25); + keyframe_delta_angle = pnh.param("keyframe_delta_angle", 0.15); + keyframe_delta_time = pnh.param("keyframe_delta_time", 1.0); + + // Registration validation by thresholding + transform_thresholding = pnh.param("transform_thresholding", false); + max_acceptable_trans = pnh.param("max_acceptable_trans", 1.0); + max_acceptable_angle = pnh.param("max_acceptable_angle", 1.0); + + // select a downsample method (VOXELGRID, APPROX_VOXELGRID, NONE) + std::string downsample_method = pnh.param("downsample_method", "VOXELGRID"); + double downsample_resolution = pnh.param("downsample_resolution", 0.1); + if(downsample_method == "VOXELGRID") { + std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; + boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); + voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); + downsample_filter = voxelgrid; + } else if(downsample_method == "APPROX_VOXELGRID") { + std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; + boost::shared_ptr> approx_voxelgrid(new pcl::ApproximateVoxelGrid()); + approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); + downsample_filter = approx_voxelgrid; + } else { + if(downsample_method != "NONE") { + std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl; + std::cerr << " : use passthrough filter" <> passthrough(new pcl::PassThrough()); + downsample_filter = passthrough; + } + + registration = select_registration_method(pnh); + } + + /** + * @brief callback for point clouds + * @param cloud_msg point cloud msg + */ + void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { + if(!ros::ok()) { + return; + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*cloud_msg, *cloud); + + Eigen::Matrix4f pose = matching(cloud_msg->header.stamp, cloud); + publish_odometry(cloud_msg->header.stamp, pose); + + // In offline estimation, point clouds until the published time will be supplied + std_msgs::HeaderPtr read_until(new std_msgs::Header()); + read_until->frame_id = "/velodyne_points"; + read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0); + read_until_pub.publish(read_until); + + read_until->frame_id = "/filtered_points"; + read_until_pub.publish(read_until); + + } + + /** + * @brief downsample a point cloud + * @param cloud input cloud + * @return downsampled point cloud + */ + pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { + if(!downsample_filter) { + return cloud; + } + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + downsample_filter->setInputCloud(cloud); + downsample_filter->filter(*filtered); + + return filtered; + } + + /** + * @brief estimate the relative pose between an input cloud and a keyframe cloud + * @param stamp the timestamp of the input cloud + * @param cloud the input cloud + * @return the relative pose between the input cloud and the keyframe cloud + */ + Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud) { + if(!keyframe) { + prev_trans.setIdentity(); + keyframe_pose.setIdentity(); + keyframe_stamp = stamp; + keyframe = downsample(cloud); + registration->setInputTarget(keyframe); + return Eigen::Matrix4f::Identity(); + } + + auto filtered = downsample(cloud); + registration->setInputSource(filtered); + + pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); + registration->align(*aligned, prev_trans); + + if(!registration->hasConverged()) { + NODELET_INFO_STREAM("scan matching has not converged!!"); + NODELET_INFO_STREAM("ignore this frame(" << stamp << ")"); + return keyframe_pose * prev_trans; + } + + Eigen::Matrix4f trans = registration->getFinalTransformation(); + Eigen::Matrix4f odom = keyframe_pose * trans; + + if(transform_thresholding) { + Eigen::Matrix4f delta = prev_trans.inverse() * trans; + double dx = delta.block<3, 1>(0, 3).norm(); + double da = std::acos(Eigen::Quaternionf(delta.block<3, 3>(0, 0)).w()); + + if(dx > max_acceptable_trans || da > max_acceptable_angle) { + NODELET_INFO_STREAM("too large transform!! " << dx << "[m] " << da << "[rad]"); + NODELET_INFO_STREAM("ignore this frame(" << stamp << ")"); + return keyframe_pose * prev_trans; + } + } + + prev_trans = trans; + + auto keyframe_trans = matrix2transform(stamp, keyframe_pose, odom_frame_id, "keyframe"); + keyframe_broadcaster.sendTransform(keyframe_trans); + + double delta_trans = trans.block<3, 1>(0, 3).norm(); + double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w()); + double delta_time = (stamp - keyframe_stamp).toSec(); + if(delta_trans > keyframe_delta_trans || delta_angle > keyframe_delta_angle || delta_time > keyframe_delta_time) { + keyframe = filtered; + registration->setInputTarget(keyframe); + + keyframe_pose = odom; + keyframe_stamp = stamp; + prev_trans.setIdentity(); + } + + return odom; + } + + /** + * @brief publish odometry + * @param stamp timestamp + * @param pose odometry pose to be published + */ + void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) { + // broadcast the transform over tf + geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id); + odom_broadcaster.sendTransform(odom_trans); + + // publish the transform + nav_msgs::Odometry odom; + odom.header.stamp = stamp; + odom.header.frame_id = odom_frame_id; + + odom.pose.pose.position.x = pose(0, 3); + odom.pose.pose.position.y = pose(1, 3); + odom.pose.pose.position.z = pose(2, 3); + odom.pose.pose.orientation = odom_trans.transform.rotation; + + odom.child_frame_id = base_frame_id; + odom.twist.twist.linear.x = 0.0; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.angular.z = 0.0; + + odom_pub.publish(odom); + } + + +private: + // ROS topics + ros::NodeHandle nh; + ros::NodeHandle private_nh; + + ros::Subscriber points_sub; + + ros::Publisher odom_pub; + tf::TransformBroadcaster odom_broadcaster; + tf::TransformBroadcaster keyframe_broadcaster; + + std::string odom_frame_id; + std::string base_frame_id; + ros::Publisher read_until_pub; + + // keyframe parameters + double keyframe_delta_trans; // minimum distance between keyframes + double keyframe_delta_angle; // + double keyframe_delta_time; // + + // registration validation by thresholding + bool transform_thresholding; // + double max_acceptable_trans; // + double max_acceptable_angle; + + // odometry calculation + Eigen::Matrix4f prev_trans; // previous estimated transform from keyframe + Eigen::Matrix4f keyframe_pose; // keyframe pose + ros::Time keyframe_stamp; // keyframe time + pcl::PointCloud::ConstPtr keyframe; // keyframe point cloud + + // + pcl::Filter::Ptr downsample_filter; + pcl::Registration::Ptr registration; +}; + +} + +PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::ScanMatchingOdometryNodelet, nodelet::Nodelet) diff --git a/cmake/FindG2O.cmake b/cmake/FindG2O.cmake new file mode 100644 index 00000000..e1a37b17 --- /dev/null +++ b/cmake/FindG2O.cmake @@ -0,0 +1,114 @@ +# Find the header files + +FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h + ${G2O_ROOT}/include + $ENV{G2O_ROOT}/include + $ENV{G2O_ROOT} + /usr/local/include + /usr/include + /opt/local/include + /sw/local/include + /sw/include + NO_DEFAULT_PATH + ) + +# Macro to unify finding both the debug and release versions of the +# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY +# macro. + +MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ${G2O_ROOT}/lib/Debug + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Debug + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ${G2O_ROOT}/lib/Release + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Release + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + IF(NOT ${MYLIBRARY}_DEBUG) + IF(MYLIBRARY) + SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) + ENDIF(MYLIBRARY) + ENDIF( NOT ${MYLIBRARY}_DEBUG) + +ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) + +# Find the core elements +FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) +FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) + +# Find the CLI library +FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) + +# Find the pluggable solvers +FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) +FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) +FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) +FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) +FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) +FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) + +# Find the predefined types +FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) +FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) +FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) +FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons) + +# G2O solvers declared found if we found at least one solver +SET(G2O_SOLVERS_FOUND "NO") +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) + SET(G2O_SOLVERS_FOUND "YES") +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) + +# G2O itself declared found if we found the core libraries and at least one solver +SET(G2O_FOUND "NO") +IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) + SET(G2O_FOUND "YES") +ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) diff --git a/imgs/birds.png b/imgs/birds.png new file mode 100644 index 00000000..83583903 Binary files /dev/null and b/imgs/birds.png differ diff --git a/imgs/ford1.png b/imgs/ford1.png new file mode 100644 index 00000000..b5e4c79b Binary files /dev/null and b/imgs/ford1.png differ diff --git a/imgs/ford2.png b/imgs/ford2.png new file mode 100644 index 00000000..275df016 Binary files /dev/null and b/imgs/ford2.png differ diff --git a/imgs/ford3.png b/imgs/ford3.png new file mode 100644 index 00000000..016b09e2 Binary files /dev/null and b/imgs/ford3.png differ diff --git a/imgs/hdl_graph_slam.png b/imgs/hdl_graph_slam.png new file mode 100644 index 00000000..04c226c9 Binary files /dev/null and b/imgs/hdl_graph_slam.png differ diff --git a/imgs/nodelets.png b/imgs/nodelets.png new file mode 100644 index 00000000..0d68c2c2 Binary files /dev/null and b/imgs/nodelets.png differ diff --git a/imgs/nodelets.vsdx b/imgs/nodelets.vsdx new file mode 100644 index 00000000..3311c05b Binary files /dev/null and b/imgs/nodelets.vsdx differ diff --git a/imgs/top.png b/imgs/top.png new file mode 100644 index 00000000..df207a8c Binary files /dev/null and b/imgs/top.png differ diff --git a/include/g2o/edge_se3_plane.hpp b/include/g2o/edge_se3_plane.hpp new file mode 100644 index 00000000..c07812df --- /dev/null +++ b/include/g2o/edge_se3_plane.hpp @@ -0,0 +1,52 @@ +#ifndef KKL_G2O_EDGE_SE3_PLANE_HPP +#define KKL_G2O_EDGE_SE3_PLANE_HPP + +#include +#include +#include + +namespace g2o { + class EdgeSE3Plane : public g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane> { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSE3Plane() + : BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane>() + {} + + void computeError() override { + const g2o::VertexSE3* v1 = static_cast(_vertices[0]); + const g2o::VertexPlane* v2 = static_cast(_vertices[1]); + + Eigen::Isometry3d w2n = v1->estimate().inverse(); + Plane3D local_plane = w2n * v2->estimate(); + _error = local_plane.ominus(_measurement); + } + + void setMeasurement(const g2o::Plane3D& m) override { + _measurement = m; + } + + virtual bool read(std::istream& is) override { + Vector4D v; + is >> v(0) >> v(1) >> v(2) >> v(3); + setMeasurement(Plane3D(v)); + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; + } + virtual bool write(std::ostream& os) const override { + Vector4D v = _measurement.toVector(); + os << v(0) << " " << v(1) << " " << v(2) << " " << v(3) << " "; + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) + os << " " << information()(i, j); + return os.good(); + } + }; +} + +#endif diff --git a/include/g2o/edge_se3_priorxy.hpp b/include/g2o/edge_se3_priorxy.hpp new file mode 100644 index 00000000..464687e2 --- /dev/null +++ b/include/g2o/edge_se3_priorxy.hpp @@ -0,0 +1,49 @@ +#ifndef EDGE_SE3_PRIORXY_HPP +#define EDGE_SE3_PRIORXY_HPP + +#include +#include + +namespace g2o { + class EdgeSE3PriorXY : public g2o::BaseUnaryEdge<2, g2o::Vector2D, g2o::VertexSE3> { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSE3PriorXY() + : g2o::BaseUnaryEdge<2, g2o::Vector2D, g2o::VertexSE3>() + {} + + void computeError() override { + const g2o::VertexSE3* v1 = static_cast(_vertices[0]); + + Eigen::Vector2d estimate = v1->estimate().translation().head<2>(); + _error = estimate - _measurement; + } + + void setMeasurement(const g2o::Vector2D& m) override { + _measurement = m; + } + + virtual bool read(std::istream& is) override { + Vector2D v; + is >> v(0) >> v(1); + setMeasurement(Vector2D(v)); + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; + } + virtual bool write(std::ostream& os) const override { + Vector2D v = _measurement; + os << v(0) << " " << v(1) << " "; + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) + os << " " << information()(i, j); + return os.good(); + } + }; +} + +#endif // EDGE_SE3_PRIORXY_HPP diff --git a/include/g2o/edge_se3_priorxyz.hpp b/include/g2o/edge_se3_priorxyz.hpp new file mode 100644 index 00000000..ed599930 --- /dev/null +++ b/include/g2o/edge_se3_priorxyz.hpp @@ -0,0 +1,49 @@ +#ifndef KKL_G2O_EDGE_SE3_PRIORXYZ_HPP +#define KKL_G2O_EDGE_SE3_PRIORXYZ_HPP + +#include +#include + +namespace g2o { + class EdgeSE3PriorXYZ : public g2o::BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3> { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSE3PriorXYZ() + : g2o::BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>() + {} + + void computeError() override { + const g2o::VertexSE3* v1 = static_cast(_vertices[0]); + + Eigen::Vector3d estimate = v1->estimate().translation(); + _error = estimate - _measurement; + } + + void setMeasurement(const g2o::Vector3D& m) override { + _measurement = m; + } + + virtual bool read(std::istream& is) override { + Vector3D v; + is >> v(0) >> v(1) >> v(2); + setMeasurement(Vector3D(v)); + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; + } + virtual bool write(std::ostream& os) const override { + Vector3D v = _measurement; + os << v(0) << " " << v(1) << " " << v(2) << " "; + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) + os << " " << information()(i, j); + return os.good(); + } + }; +} + +#endif \ No newline at end of file diff --git a/include/hdl_graph_slam/graph_slam.hpp b/include/hdl_graph_slam/graph_slam.hpp new file mode 100644 index 00000000..2b40a12e --- /dev/null +++ b/include/hdl_graph_slam/graph_slam.hpp @@ -0,0 +1,107 @@ +#ifndef GRAPH_SLAM_HPP +#define GRAPH_SLAM_HPP + +#include +#include + +#include + +namespace g2o { + class VertexSE3; + class VertexPlane; + class VertexPointXYZ; + class EdgeSE3; + class EdgeSE3Plane; + class EdgeSE3PointXYZ; + class EdgeSE3PriorXY; + class EdgeSE3PriorXYZ; +} + +namespace hdl_graph_slam { + +class GraphSLAM { +public: + GraphSLAM(); + ~GraphSLAM(); + + /** + * @brief add a SE3 node to the graph + * @param pose + * @return registered node + */ + g2o::VertexSE3* add_se3_node(const Eigen::Isometry3d& pose); + + /** + * @brief add a plane node to the graph + * @param plane_coeffs + * @return registered node + */ + g2o::VertexPlane* add_plane_node(const Eigen::Vector4d& plane_coeffs); + + /** + * @brief add a point_xyz node to the graph + * @param xyz + * @return registered node + */ + g2o::VertexPointXYZ* add_point_xyz_node(const Eigen::Vector3d& xyz); + + /** + * @brief add an edge between SE3 nodes + * @param v1 node1 + * @param v2 node2 + * @param relative_pose relative pose between node1 and node2 + * @param information_matrix information matrix (it must be 6x6) + * @return registered edge + */ + g2o::EdgeSE3* add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix); + + /** + * @brief add an edge between an SE3 node and a plane node + * @param v_se3 SE3 node + * @param v_plane plane node + * @param plane_coeffs plane coefficients w.r.t. v_se3 + * @param information_matrix information matrix (it must be 3x3) + * @return registered edge + */ + g2o::EdgeSE3Plane* add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix); + + /** + * @brief add an edge between an SE3 node and a point_xyz node + * @param v_se3 SE3 node + * @param v_xyz point_xyz node + * @param xyz xyz coordinate + * @param information information_matrix (it must be 3x3) + * @return registered edge + */ + g2o::EdgeSE3PointXYZ* add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); + + /** + * @brief add a prior edge to an SE3 node + * @param v_se3 + * @param xy + * @param information_matrix + * @return + */ + g2o::EdgeSE3PriorXY* add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix); + + g2o::EdgeSE3PriorXYZ* add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); + + /** + * @brief perform graph optimization + */ + void optimize(); + + /** + * @brief save the pose graph + * @param filename output filename + */ + void save(const std::string& filename); + +public: + std::unique_ptr graph; // g2o graph + g2o::VertexPlane* floor_plane_node; // ground floor plane node +}; + +} + +#endif // GRAPH_SLAM_HPP diff --git a/include/hdl_graph_slam/information_matrix_calculator.hpp b/include/hdl_graph_slam/information_matrix_calculator.hpp new file mode 100644 index 00000000..f19ab118 --- /dev/null +++ b/include/hdl_graph_slam/information_matrix_calculator.hpp @@ -0,0 +1,42 @@ +#ifndef INFORMATION_MATRIX_CALCULATOR_HPP +#define INFORMATION_MATRIX_CALCULATOR_HPP + +#include +#include +#include + +namespace hdl_graph_slam { + +class InformationMatrixCalculator { +public: + using PointT = pcl::PointXYZI; + + InformationMatrixCalculator(ros::NodeHandle& nh); + ~InformationMatrixCalculator(); + + Eigen::MatrixXd calc_information_matrix(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const; +private: + double weight(double a, double max_x, double min_y, double max_y, double x) const { + double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x)); + return min_y + (max_y - min_y) * y; + } + + double calc_fitness_score(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range = std::numeric_limits::max()) const; + +private: + bool use_const_inf_matrix; + double const_stddev_x; + double const_stddev_q; + + double var_gain_a; + double min_stddev_x; + double max_stddev_x; + double min_stddev_q; + double max_stddev_q; + double fitness_score_thresh; + +}; + +} + +#endif // INFORMATION_MATRIX_CALCULATOR_HPP diff --git a/include/hdl_graph_slam/keyframe.hpp b/include/hdl_graph_slam/keyframe.hpp new file mode 100644 index 00000000..3ad3b00a --- /dev/null +++ b/include/hdl_graph_slam/keyframe.hpp @@ -0,0 +1,62 @@ +#ifndef KEYFRAME_HPP +#define KEYFRAME_HPP + +#include +#include +#include +#include + +namespace g2o { + class VertexSE3; +} + +namespace hdl_graph_slam { + +/** + * @brief KeyFrame (pose node) + */ +struct KeyFrame { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using PointT = pcl::PointXYZI; + using Ptr = std::shared_ptr; + + KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud::ConstPtr& cloud); + ~KeyFrame(); + + void dump(const std::string& directory); + +public: + ros::Time stamp; // timestamp + Eigen::Isometry3d odom; // odometry (estimated by scan_matching_odometry) + double accum_distance; // accumulated distance from the first node (by scan_matching_odometry) + pcl::PointCloud::ConstPtr cloud; // point cloud + boost::optional floor_coeffs; // detected floor's coefficients + boost::optional utm_coord; // UTM coord obtained by GPS + + g2o::VertexSE3* node; // node instance +}; + +/** + * @brief KeyFramesnapshot for map cloud generation + */ +struct KeyFrameSnapshot { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using PointT = KeyFrame::PointT; + using Ptr = std::shared_ptr; + + KeyFrameSnapshot(const KeyFrame::Ptr& key); + KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud::ConstPtr& cloud); + + ~KeyFrameSnapshot(); + +public: + Eigen::Isometry3d pose; // pose estimated by graph optimization + pcl::PointCloud::ConstPtr cloud; // point cloud +}; + +} + +#endif // KEYFRAME_HPP diff --git a/include/hdl_graph_slam/keyframe_updater.hpp b/include/hdl_graph_slam/keyframe_updater.hpp new file mode 100644 index 00000000..9b63f60f --- /dev/null +++ b/include/hdl_graph_slam/keyframe_updater.hpp @@ -0,0 +1,78 @@ +#ifndef KEYFRAME_UPDATER_HPP +#define KEYFRAME_UPDATER_HPP + +#include +#include + +namespace hdl_graph_slam { + +/** + * @brief this class decides if a new frame should be registered to the pose graph as a keyframe + */ +class KeyframeUpdater { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * @brief constructor + * @param pnh + */ + KeyframeUpdater(ros::NodeHandle& pnh) + : is_first(true), + prev_keypose(Eigen::Isometry3d::Identity()) + { + keyframe_delta_trans = pnh.param("keyframe_delta_trans", 2.0); + keyframe_delta_angle = pnh.param("keyframe_delta_angle", 2.0); + + accum_distance = 0.0; + } + + /** + * @brief decide if a new frame should be registered to the graph + * @param pose pose of the frame + * @return if true, the frame should be registered + */ + bool update(const Eigen::Isometry3d& pose) { + // first frame is always registered to the graph + if(is_first) { + is_first = false; + prev_keypose = pose; + return true; + } + + // calculate the delta transformation from the previous keyframe + Eigen::Isometry3d delta = prev_keypose.inverse() * pose; + double dx = delta.translation().norm(); + double da = std::acos(Eigen::Quaterniond(delta.linear()).w()); + + // too close to the previous frame + if(dx < keyframe_delta_trans && da < keyframe_delta_angle) { + return false; + } + + accum_distance += dx; + prev_keypose = pose; + return true; + } + + /** + * @brief the last keyframe's accumulated distance from the first keyframe + * @return accumulated distance + */ + double get_accum_distance() const { + return accum_distance; + } + +private: + // parameters + double keyframe_delta_trans; // + double keyframe_delta_angle; // + + bool is_first; + double accum_distance; + Eigen::Isometry3d prev_keypose; +}; + +} + +#endif // KEYFRAME_UPDATOR_HPP diff --git a/include/hdl_graph_slam/loop_detector.hpp b/include/hdl_graph_slam/loop_detector.hpp new file mode 100644 index 00000000..cfcfe4ee --- /dev/null +++ b/include/hdl_graph_slam/loop_detector.hpp @@ -0,0 +1,183 @@ +#ifndef LOOP_DETECTOR_HPP +#define LOOP_DETECTOR_HPP + +#include +#include +#include + +#include + +namespace hdl_graph_slam { + +struct Loop { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using Ptr = std::shared_ptr; + + Loop(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Matrix4f& relpose) + : key1(key1), + key2(key2), + relative_pose(relpose) + {} + +public: + KeyFrame::Ptr key1; + KeyFrame::Ptr key2; + Eigen::Matrix4f relative_pose; +}; + +/** + * @brief this class finds loops by scam matching and adds them to the pose graph + */ +class LoopDetector { +public: + typedef pcl::PointXYZI PointT; + + /** + * @brief constructor + * @param pnh + */ + LoopDetector(ros::NodeHandle& pnh) { + distance_thresh = pnh.param("distance_thresh", 5.0); + accum_distance_thresh = pnh.param("accum_distance_thresh", 8.0); + distance_from_last_edge_thresh = pnh.param("min_edge_interval", 5.0); + + fitness_score_thresh = pnh.param("fitness_score_thresh", 0.5); + + registration = select_registration_method(pnh); + last_edge_accum_distance = 0.0; + } + + /** + * @brief detect loops and add them to the pose graph + * @param keyframes keyframes + * @param new_keyframes newly registered keyframes + * @param graph_slam pose graph + */ + std::vector detect(const std::vector& keyframes, const std::deque& new_keyframes, hdl_graph_slam::GraphSLAM& graph_slam) { + std::vector detected_loops; + for(const auto& new_keyframe : new_keyframes) { + auto candidates = find_candidates(keyframes, new_keyframe); + auto loop = matching(candidates, new_keyframe, graph_slam); + if(loop) { + detected_loops.push_back(loop); + } + } + + return detected_loops; + } + + double get_distance_thresh() const { + return distance_thresh; + } + +private: + /** + * @brief find loop candidates. A detected loop begins at one of #keyframes and ends at #new_keyframe + * @param keyframes candidate keyframes of loop start + * @param new_keyframe loop end keyframe + * @return loop candidates + */ + std::vector find_candidates(const std::vector& keyframes, const KeyFrame::Ptr& new_keyframe) const { + // too close to the last registered loop edge + if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) { + return std::vector(); + } + + std::vector candidates; + candidates.reserve(32); + + for(const auto& k : keyframes) { + // traveled distance between keyframes is too small + if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) { + continue; + } + + const auto& pos1 = k->node->estimate().translation(); + const auto& pos2 = new_keyframe->node->estimate().translation(); + + // estimated distance between keyframes is too small + double dist = (pos1.head<2>() - pos2.head<2>()).norm(); + if(dist > distance_thresh) { + continue; + } + + candidates.push_back(k); + } + + return candidates; + } + + /** + * @brief To validate a loop candidate this function applies a scan matching between keyframes consisting the loop. If they are matched well, the loop is added to the pose graph + * @param candidate_keyframes candidate keyframes of loop start + * @param new_keyframe loop end keyframe + * @param graph_slam graph slam + */ + Loop::Ptr matching(const std::vector& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam) { + if(candidate_keyframes.empty()) { + return nullptr; + } + + registration->setInputTarget(new_keyframe->cloud); + + double best_score = std::numeric_limits::max(); + KeyFrame::Ptr best_matched; + Eigen::Matrix4f relative_pose; + + std::cout << std::endl; + std::cout << "--- loop detection ---" << std::endl; + std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl; + std::cout << "matching" << std::flush; + auto t1 = ros::Time::now(); + + pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); + for(const auto& candidate : candidate_keyframes) { + registration->setInputSource(candidate->cloud); + Eigen::Matrix4f guess = (new_keyframe->node->estimate().inverse() * candidate->node->estimate()).matrix().cast(); + guess(2, 3) = 0.0; + registration->align(*aligned, guess); + std::cout << "." << std::flush; + + double score = registration->getFitnessScore(); + if(!registration->hasConverged() || score > best_score) { + continue; + } + + best_score = score; + best_matched = candidate; + relative_pose = registration->getFinalTransformation(); + } + + auto t2 = ros::Time::now(); + std::cout << " done" << std::endl; + std::cout << "best_score: " << boost::format("%.3f") % best_score << " time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl; + + if(best_score > fitness_score_thresh) { + std::cout << "loop not found..." << std::endl; + return nullptr; + } + + std::cout << "loop found!!" << std::endl; + std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl; + + last_edge_accum_distance = new_keyframe->accum_distance; + + return std::make_shared(new_keyframe, best_matched, relative_pose); + } + +private: + double distance_thresh; // estimated distance between keyframes consisting a loop must be less than this distance + double accum_distance_thresh; // traveled distance between ... + double distance_from_last_edge_thresh; // a new loop edge must far from the last one at least this distance + + double fitness_score_thresh; // threshold for scan matching + + double last_edge_accum_distance; + + pcl::Registration::Ptr registration; +}; + +} + +#endif // LOOP_DETECTOR_HPP diff --git a/include/hdl_graph_slam/map_cloud_generator.hpp b/include/hdl_graph_slam/map_cloud_generator.hpp new file mode 100644 index 00000000..f0154b3d --- /dev/null +++ b/include/hdl_graph_slam/map_cloud_generator.hpp @@ -0,0 +1,33 @@ +#ifndef MAP_CLOUD_GENERATOR_HPP +#define MAP_CLOUD_GENERATOR_HPP + +#include +#include +#include +#include + +namespace hdl_graph_slam { + +/** + * @brief this class generates a map point cloud from registered keyframes + */ +class MapCloudGenerator { +public: + using PointT = pcl::PointXYZI; + + MapCloudGenerator(); + ~MapCloudGenerator(); + + /** + * @brief generates a map point cloud + * @param keyframes snapshots of keyframes + * @param resolution resolution of generated map + * @return generated map point cloud + */ + pcl::PointCloud::Ptr generate(const std::vector& keyframes, double resolution) const; + +}; + +} + +#endif // MAP_POINTCLOUD_GENERATOR_HPP diff --git a/include/hdl_graph_slam/nmea_sentence_parser.hpp b/include/hdl_graph_slam/nmea_sentence_parser.hpp new file mode 100644 index 00000000..215e6430 --- /dev/null +++ b/include/hdl_graph_slam/nmea_sentence_parser.hpp @@ -0,0 +1,107 @@ +#ifndef NMEA_SENTENCE_PARSER_HPP +#define NMEA_SENTENCE_PARSER_HPP + +#include +#include +#include +#include +#include + +namespace hdl_graph_slam { + +struct GPRMC { +public: + GPRMC() { + status = 'V'; + } + + GPRMC(const std::vector& tokens) { + if(tokens[0] != "$GPRMC" || tokens.size() < 12) { + status = 'V'; + return; + } + + long time = std::stol(tokens[1]); + hour = time / 10000; + minute = (time % 10000) / 100; + second = time % 100; + + status = tokens[2][0]; + + latitude = degmin2deg(std::stod(tokens[3])); + latitude = tokens[4] == "N" ? latitude : -latitude; + + longitude = degmin2deg(std::stod(tokens[5])); + longitude = tokens[6] == "E" ? longitude : -longitude; + + speed_knots = std::stod(tokens[7]); + track_angle_degree = std::stod(tokens[8]); + + long date = std::stol(tokens[9]); + year = date % 100; + month = (date / 100) % 100; + day = (date / 10000) % 100; + + magnetic_variation = std::stod(tokens[10]); + magnetic_variation = tokens[11][0] == 'E' ? magnetic_variation : -magnetic_variation; + } + + double degmin2deg(double degmin) { + double d = std::floor(degmin / 100.0); + double m = (degmin - d * 100.0) / 60.0; + return d + m; + } + +public: + char status; // Status A=active or V=Void. + + int hour; // Fix taken at 12:35:19 UTC + int minute; + int second; + + double latitude; // + double longitude; + + double speed_knots; // Speed over the ground in knots + double track_angle_degree; // Track angle in degrees True + + int year; + int month; + int day; + + double magnetic_variation; +}; + +class NmeaSentenceParser { +public: + NmeaSentenceParser() {} + ~NmeaSentenceParser() {} + + GPRMC parse(const std::string& sentence) const { + int checksum_loc = sentence.find('*'); + if(checksum_loc == std::string::npos) { + return GPRMC(); + } + + int checksum = std::stoul(sentence.substr(checksum_loc + 1), nullptr, 16); + + std::string substr = sentence.substr(1, checksum_loc - 1); + int sum = std::accumulate(substr.begin(), substr.end(), static_cast(0), [=](unsigned char n, unsigned char c) { return n ^ c; }); + + if(checksum != (sum & 0xf)) { + std::cerr << "checksum doesn't match!!" << std::endl; + std::cerr << sentence << " " << sum << std::endl; + return GPRMC(); + } + + std::vector tokens; + boost::split(tokens, sentence, boost::is_any_of(",")); + + return GPRMC(tokens); + } +}; + +} + +#endif // NMEA_SENTENCE_PARSER_HPP + diff --git a/include/hdl_graph_slam/registrations.hpp b/include/hdl_graph_slam/registrations.hpp new file mode 100644 index 00000000..575417c1 --- /dev/null +++ b/include/hdl_graph_slam/registrations.hpp @@ -0,0 +1,19 @@ +#ifndef HDL_GRAPH_SLAM_REGISTRATIONS_HPP +#define HDL_GRAPH_SLAM_REGISTRATIONS_HPP + +#include + +#include + +namespace hdl_graph_slam { + +/** + * @brief select a scan matching algorithm according to rosparams + * @param pnh + * @return selected scan matching + */ +boost::shared_ptr> select_registration_method(ros::NodeHandle& pnh); + +} + +#endif // diff --git a/include/hdl_graph_slam/ros_time_hash.hpp b/include/hdl_graph_slam/ros_time_hash.hpp new file mode 100644 index 00000000..d0276819 --- /dev/null +++ b/include/hdl_graph_slam/ros_time_hash.hpp @@ -0,0 +1,22 @@ +#ifndef ROS_TIME_HASH_HPP +#define ROS_TIME_HASH_HPP + +#include +#include + +#include + +/** + * @brief Hash calculation for ros::Time + */ +class RosTimeHash { +public: + size_t operator() (const ros::Time& val) const { + size_t seed = 0; + boost::hash_combine(seed, val.sec); + boost::hash_combine(seed, val.nsec); + return seed; + } +}; + +#endif // ROS_TIME_HASHER_HPP diff --git a/include/hdl_graph_slam/ros_utils.hpp b/include/hdl_graph_slam/ros_utils.hpp new file mode 100644 index 00000000..5c421dd0 --- /dev/null +++ b/include/hdl_graph_slam/ros_utils.hpp @@ -0,0 +1,60 @@ +#ifndef ROS_UTILS_HPP +#define ROS_UTILS_HPP + +#include + +#include +#include +#include + +namespace hdl_graph_slam { + +/** + * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped + * @param stamp timestamp + * @param pose Eigen::Matrix to be converted + * @param frame_id tf frame_id + * @param child_frame_id tf child frame_id + * @return converted TransformStamped + */ +static geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) { + Eigen::Quaternionf quat(pose.block<3, 3>(0, 0)); + quat.normalize(); + geometry_msgs::Quaternion odom_quat; + odom_quat.w = quat.w(); + odom_quat.x = quat.x(); + odom_quat.y = quat.y(); + odom_quat.z = quat.z(); + + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = stamp; + odom_trans.header.frame_id = frame_id; + odom_trans.child_frame_id = child_frame_id; + + odom_trans.transform.translation.x = pose(0, 3); + odom_trans.transform.translation.y = pose(1, 3); + odom_trans.transform.translation.z = pose(2, 3); + odom_trans.transform.rotation = odom_quat; + + return odom_trans; +} + +static Eigen::Isometry3d odom2isometry(const nav_msgs::OdometryConstPtr& odom_msg) { + const auto& orientation = odom_msg->pose.pose.orientation; + const auto& position = odom_msg->pose.pose.position; + + Eigen::Quaterniond quat; + quat.w() = orientation.w; + quat.x() = orientation.x; + quat.y() = orientation.y; + quat.z() = orientation.z; + + Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity(); + isometry.linear() = quat.toRotationMatrix(); + isometry.translation() = Eigen::Vector3d(position.x, position.y, position.z); + return isometry; +} + +} + +#endif // ROS_UTILS_HPP diff --git a/launch/hdl_graph_slam.launch b/launch/hdl_graph_slam.launch new file mode 100644 index 00000000..7bcf618d --- /dev/null +++ b/launch/hdl_graph_slam.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/hdl_graph_slam_400.launch b/launch/hdl_graph_slam_400.launch new file mode 100644 index 00000000..14a1f50f --- /dev/null +++ b/launch/hdl_graph_slam_400.launch @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/hdl_graph_slam_501.launch b/launch/hdl_graph_slam_501.launch new file mode 100644 index 00000000..7f266796 --- /dev/null +++ b/launch/hdl_graph_slam_501.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/hdl_graph_slam_ford.launch b/launch/hdl_graph_slam_ford.launch new file mode 100644 index 00000000..a8641f7f --- /dev/null +++ b/launch/hdl_graph_slam_ford.launch @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/msg/FloorCoeffs.msg b/msg/FloorCoeffs.msg new file mode 100644 index 00000000..514d53b8 --- /dev/null +++ b/msg/FloorCoeffs.msg @@ -0,0 +1,3 @@ +Header header + +float32[] coeffs diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 00000000..f457ce53 --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..4294caf1 --- /dev/null +++ b/package.xml @@ -0,0 +1,30 @@ + + + hdl_graph_slam + 0.0.0 + The hdl_scan_matching_odometry package + + koide + + TODO + + catkin + pcl_ros + roscpp + nodelet + geodesy + nmea_msgs + sensor_msgs + message_generation + + pcl_ros + roscpp + geodesy + nodelet + nmea_msgs + sensor_msgs + + + + + diff --git a/rviz/hdl_graph_slam.rviz b/rviz/hdl_graph_slam.rviz new file mode 100644 index 00000000..3f9ba95c --- /dev/null +++ b/rviz/hdl_graph_slam.rviz @@ -0,0 +1,264 @@ +Panels: + - Class: rviz/Displays + Help Height: 125 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /MarkerArray1/Namespaces1 + Splitter Ratio: 0.5 + Tree Height: 728 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 10 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 50 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 50 + Frames: + All Enabled: true + keyframe: + Value: true + map: + Value: true + odom: + Value: true + velodyne: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + {} + Update Interval: 0 + 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: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 121 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /velodyne_points + 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: 59; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 104 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.15 + Style: Flat Squares + Topic: /floor_detection/floor_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: -1 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.02 + Style: Flat Squares + Topic: /hdl_graph_slam/map_points + 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: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /filtered_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /hdl_graph_slam/markers + Name: MarkerArray + Namespaces: + edges: true + loop_close_radius: true + nodes: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + 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 + Topic: /initialpose + - 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: 48.8711 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.845398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.775398 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000019d00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004870000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/scripts/bag_player.py b/scripts/bag_player.py new file mode 100755 index 00000000..d1942679 --- /dev/null +++ b/scripts/bag_player.py @@ -0,0 +1,187 @@ +#!/usr/bin/python +import sys +import yaml +import time +import curses +import StringIO + +import rospy +import rosbag +import roslib +from std_msgs.msg import * +from sensor_msgs.msg import * +from rosgraph_msgs.msg import * +from progressbar import ProgressBar + + +class BagPlayer: + def __init__(self, bagfile): + print 'opening...', + self.bag = rosbag.Bag(bagfile, 'r') + print 'done' + + self.message_generator = self.bag.read_messages() + + info_dict = yaml.load(self.bag._get_yaml_info()) + self.duration = float(info_dict['duration']) + self.endtime = float(info_dict['end']) + + self.progress = ProgressBar(0, self.duration, term_width=80, fd=StringIO.StringIO()) + + self.publishers = {} + for con in self.bag._get_connections(): + msg_class = roslib.message.get_message_class(con.datatype) + self.publishers[con.topic] = rospy.Publisher(con.topic, msg_class, queue_size=256) + self.clock_pub = rospy.Publisher('/clock', Clock, queue_size=256) + + self.time_subs = {} + self.target_times = {} + self.latest_stamps = {} + + self.play() + + def update_time_subs(self): + for topic_name, msg_type in rospy.get_published_topics(): + if 'read_until' in topic_name and 'std_msgs/Header' in msg_type: + if topic_name not in self.time_subs: + print 'ADD', topic_name + self.time_subs[topic_name] = rospy.Subscriber(topic_name, Header, self.time_callback, topic_name) + + def time_callback(self, header_msg, topic_name): + if header_msg.frame_id not in self.target_times: + self.target_times[header_msg.frame_id] = {} + self.target_times[header_msg.frame_id][topic_name] = header_msg.stamp + + def play_realtime(self, duration): + topic, msg, stamp = self.message_generator.next() + stamp_wall = time.time() + + start_stamp = stamp + start_stamp_wall = stamp_wall + + self.start_stamp = start_stamp + + while not rospy.is_shutdown() and (stamp - start_stamp) < duration: + stamp_wall = time.time() + elapsed_stamp = stamp - start_stamp + if (stamp_wall - start_stamp_wall) < (elapsed_stamp.secs + elapsed_stamp.nsecs * 1e-9): + time.sleep(1e-6) + self.update_time_subs() + continue + + clock_msg = Clock() + clock_msg.clock = stamp + + self.clock_pub.publish(clock_msg) + self.publishers[topic].publish(msg) + + topic, msg, stamp = self.message_generator.next() + + return topic, msg, stamp + + def print_progress(self, stamp): + self.stdscr.clear() + self.stdscr.addstr(0, 0, 'topic') + self.stdscr.addstr(0, 25, 'time') + + line = 1 + for target in self.target_times: + if target not in self.publishers: + continue + + for sub_name in self.target_times[target]: + target_time = self.target_times[target][sub_name] + self.stdscr.addstr(line, 0, sub_name[:-11]) + self.stdscr.addstr(line, 25, '%.6f' % (target_time.secs + target_time.nsecs * 1e-9)) + + residual = target_time - self.latest_stamps[target].stamp + + color = 1 if residual.to_sec() > 0.0 else 2 + self.stdscr.addstr(line, 50, '%.5f' % residual.to_sec(), curses.color_pair(color)) + line += 1 + + if not hasattr(self, 'prev_stamp'): + self.prev_stamp = stamp + self.prev_stamp_wall = time.time() + self.processing_speed = 1.0 + elif time.time() - self.prev_stamp_wall > 1.0: + sim_duration = (stamp - self.prev_stamp).to_sec() + wall_duration = time.time() - self.prev_stamp_wall + self.processing_speed = sim_duration / wall_duration + + self.stdscr.addstr(line, 0, 'current_stamp') + self.stdscr.addstr(line, 25, '%.6f' % stamp.to_sec()) + self.stdscr.addstr(line, 50, '(x%.2f)' % self.processing_speed) + + elapsed = (stamp - self.start_stamp).to_sec() + self.progress.fd = StringIO.StringIO() + try: + self.progress.update(elapsed) + except: + pass + self.stdscr.addstr(line + 1, 0, '----------') + self.stdscr.addstr(line + 2, 0, self.progress.fd.getvalue()) + + self.stdscr.refresh() + + def check_stamp(self, topic, msg): + if topic not in self.target_times: + return True + + target_time_map = self.target_times[topic] + for sub_name in target_time_map: + self.latest_stamps[topic] = msg.header + if msg.header.stamp > target_time_map[sub_name]: + return False + + return True + + def play(self): + print 'play realtime for 3.0[sec]' + topic, msg, stamp = self.play_realtime(rospy.Duration(15.0)) + self.update_time_subs() + + print 'play as fast as possible' + self.stdscr = curses.initscr() + curses.start_color() + curses.noecho() + + curses.init_pair(1, curses.COLOR_BLUE, curses.COLOR_WHITE) + curses.init_pair(2, curses.COLOR_RED, curses.COLOR_WHITE) + + try: + while not rospy.is_shutdown(): + if not self.check_stamp(topic, msg): + self.update_time_subs() + self.print_progress(stamp) + time.sleep(0.1) + continue + + clock_msg = Clock() + clock_msg.clock = stamp + + self.clock_pub.publish(clock_msg) + self.publishers[topic].publish(msg) + topic, msg, stamp = self.message_generator.next() + except: + clock_msg = Clock() + clock_msg.clock = stamp + rospy.Duration(30.0) + self.clock_pub.publish(clock_msg) + time.sleep(0.5) + + curses.echo() + curses.endwin() + + +def main(): + if len(sys.argv) < 2: + print 'usage bag_player src_bagname' + return + + rospy.init_node('bag_player') + for argv in sys.argv[1:]: + print argv + bp = BagPlayer(argv) + +if __name__ == '__main__': + main() diff --git a/scripts/ford2bag.py b/scripts/ford2bag.py new file mode 100755 index 00000000..8b6beb85 --- /dev/null +++ b/scripts/ford2bag.py @@ -0,0 +1,95 @@ +#!/usr/bin/python +import re +import os +import sys +import struct +import numpy +import scipy.io + +import rospy +import rosbag +import progressbar +import sensor_msgs.point_cloud2 as pc2 + +from sensor_msgs.msg import NavSatFix, NavSatStatus, PointCloud2 +from geographic_msgs.msg import GeoPointStamped + + +def gps2navsat(filename, bag): + with open(filename, 'r') as file: + try: + while True: + data = struct.unpack('qddd', file.read(8*4)) + time = data[0] + local = data[1:] + lat_lon_el_theta = struct.unpack('dddd', file.read(8*4)) + gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16)) + + if abs(lat_lon_el_theta[0]) < 1e-1: + continue + + navsat = NavSatFix() + navsat.header.frame_id = 'gps' + navsat.header.stamp = rospy.Time.from_sec(time * 1e-6) + navsat.status.status = NavSatStatus.STATUS_FIX + navsat.status.service = NavSatStatus.SERVICE_GPS + + navsat.latitude = lat_lon_el_theta[0] + navsat.longitude = lat_lon_el_theta[1] + navsat.altitude = lat_lon_el_theta[2] + + navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist() + navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN + + bag.write('/gps/fix', navsat, navsat.header.stamp) + + geopoint = GeoPointStamped() + geopoint.header = navsat.header + geopoint.position.latitude = lat_lon_el_theta[0] + geopoint.position.longitude = lat_lon_el_theta[1] + geopoint.position.altitude = lat_lon_el_theta[2] + + bag.write('/gps/geopoint', geopoint, geopoint.header.stamp) + + except: + print 'done' + + +def mat2pointcloud(filename): + m = scipy.io.loadmat(filename) + scan = numpy.transpose(m['SCAN']['XYZ'][0][0]).astype(numpy.float32) + stamp = m['SCAN']['timestamp_laser'][0][0][0][0] * 1e-6 + + cloud = PointCloud2() + cloud.header.stamp = rospy.Time.from_sec(stamp) + cloud.header.frame_id = 'velodyne' + cloud = pc2.create_cloud_xyz32(cloud.header, scan) + return cloud + + +def main(): + if len(sys.argv) < 2: + print 'usage: ford2bag.py src_dirname output_filename' + + output_filename = sys.argv[1] + + rospy.init_node('bag') + filenames = sorted(['SCANS/' + x for x in os.listdir('SCANS') if re.match('Scan[0-9]*\.mat', x)]) + print filenames + + progress = progressbar.ProgressBar(max_value=len(filenames)) + pub = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=32) + with rosbag.Bag(output_filename, 'w') as bag: + gps2navsat('GPS.log', bag) + for i, filename in enumerate(filenames): + if rospy.is_shutdown(): + break + progress.update(i) + cloud = mat2pointcloud(filename) + if pub.get_num_connections(): + pub.publish(cloud) + bag.write('/velodyne_points', cloud, cloud.header.stamp) + + +if __name__ == '__main__': + main() diff --git a/scripts/map2odom_publisher.py b/scripts/map2odom_publisher.py new file mode 100755 index 00000000..f6f10d86 --- /dev/null +++ b/scripts/map2odom_publisher.py @@ -0,0 +1,44 @@ +#!/usr/bin/python +import tf +import rospy +from geometry_msgs.msg import * + + +class Map2OdomPublisher: + def __init__(self): + self.broadcaster = tf.TransformBroadcaster() + self.subscriber = rospy.Subscriber('/hdl_graph_slam/odom2pub', TransformStamped, self.callback) + + def callback(self, odom_msg): + self.odom_msg = odom_msg + + def spin(self): + if not hasattr(self, 'odom_msg'): + self.broadcaster.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), 'odom', 'map') + return + + pose = self.odom_msg.transform + pos = (pose.translation.x, pose.translation.y, pose.translation.z) + quat = (pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w) + + map_frame_id = self.odom_msg.header.frame_id + odom_frame_id = self.odom_msg.child_frame_id + + self.broadcaster.sendTransform(pos, quat, rospy.Time.now(), odom_frame_id, map_frame_id) + + +def main(): + rospy.init_node('map2odom_publisher') + node = Map2OdomPublisher() + + rate = rospy.Rate(100.0) + while not rospy.is_shutdown(): + node.spin() + rate.sleep() + +if __name__ == '__main__': + try: + main() + except: + print 'shutdown' + diff --git a/src/hdl_graph_slam/graph_slam.cpp b/src/hdl_graph_slam/graph_slam.cpp new file mode 100644 index 00000000..0938ca51 --- /dev/null +++ b/src/hdl_graph_slam/graph_slam.cpp @@ -0,0 +1,173 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +G2O_USE_OPTIMIZATION_LIBRARY(csparse) + +namespace g2o { + G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane) + G2O_REGISTER_TYPE(EDGE_SE3_PRIORXY, EdgeSE3PriorXY) + G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ) +} + +namespace hdl_graph_slam { + +/** + * @brief constructor + */ +GraphSLAM::GraphSLAM() { + graph.reset(new g2o::SparseOptimizer()); + + std::string g2o_solver_name = "lm_var"; + std::cout << "construct solver... " << std::endl; + g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance(); + g2o::OptimizationAlgorithmProperty solver_property; + g2o::OptimizationAlgorithm* solver = solver_factory->construct(g2o_solver_name, solver_property); + graph->setAlgorithm(solver); + + if (!graph->solver()) { + std::cerr << std::endl; + std::cerr << "error : failed to allocate solver!!" << std::endl; + solver_factory->listSolvers(std::cerr); + std::cerr << "-------------" << std::endl; + std::cin.ignore(1); + return; + } + std::cout << "done" << std::endl; + + floor_plane_node = add_plane_node(Eigen::Vector4d(0.0, 0.0, 1.0, 0.0)); + floor_plane_node->setFixed(true); +} + +/** + * @brief destructor + */ +GraphSLAM::~GraphSLAM() { + graph.reset(); +} + + +g2o::VertexSE3* GraphSLAM::add_se3_node(const Eigen::Isometry3d& pose) { + g2o::VertexSE3* vertex(new g2o::VertexSE3()); + vertex->setId(graph->vertices().size()); + vertex->setEstimate(pose); + graph->addVertex(vertex); + + return vertex; +} + +g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& plane_coeffs) { + g2o::VertexPlane* vertex(new g2o::VertexPlane()); + vertex->setId(graph->vertices().size()); + vertex->setEstimate(plane_coeffs); + graph->addVertex(vertex); + + return vertex; +} + +g2o::VertexPointXYZ* GraphSLAM::add_point_xyz_node(const Eigen::Vector3d& xyz) { + g2o::VertexPointXYZ* vertex(new g2o::VertexPointXYZ()); + vertex->setId(graph->vertices().size()); + vertex->setEstimate(xyz); + graph->addVertex(vertex); + + return vertex; +} + +g2o::EdgeSE3* GraphSLAM::add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) { + g2o::EdgeSE3* edge(new g2o::EdgeSE3()); + edge->setMeasurement(relative_pose); + edge->setInformation(information_matrix); + edge->vertices()[0] = v1; + edge->vertices()[1] = v2; + graph->addEdge(edge); + + return edge; +} + +g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) { + g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane()); + edge->setMeasurement(plane_coeffs); + edge->setInformation(information_matrix); + edge->vertices()[0] = v_se3; + edge->vertices()[1] = v_plane; + graph->addEdge(edge); + + return edge; +} + +g2o::EdgeSE3PointXYZ* GraphSLAM::add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) { + g2o::EdgeSE3PointXYZ* edge(new g2o::EdgeSE3PointXYZ()); + edge->setMeasurement(xyz); + edge->setInformation(information_matrix); + edge->vertices()[0] = v_se3; + edge->vertices()[1] = v_xyz; + graph->addEdge(edge); + + return edge; +} + +g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) { + g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY()); + edge->setMeasurement(xy); + edge->setInformation(information_matrix); + edge->vertices()[0] = v_se3; + graph->addEdge(edge); + + return edge; +} + +g2o::EdgeSE3PriorXYZ* GraphSLAM::add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) { + g2o::EdgeSE3PriorXYZ* edge(new g2o::EdgeSE3PriorXYZ()); + edge->setMeasurement(xyz); + edge->setInformation(information_matrix); + edge->vertices()[0] = v_se3; + graph->addEdge(edge); + + return edge; +} + + +void GraphSLAM::optimize() { + if(graph->edges().size() < 10) { + return; + } + + std::cout << std::endl; + std::cout << "--- pose graph optimization ---" << std::endl; + std::cout << "nodes: " << graph->vertices().size() << " edges: " << graph->edges().size() << std::endl; + std::cout << "optimizing... " << std::flush; + + graph->initializeOptimization(); + graph->setVerbose(false); + + double chi2 = graph->chi2(); + + auto t1 = ros::Time::now(); + int iterations = graph->optimize(1024); + + auto t2 = ros::Time::now(); + std::cout << "done" << std::endl; + std::cout << "iterations: " << iterations << std::endl; + std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl; + std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl; +} + +void GraphSLAM::save(const std::string& filename) { + std::ofstream ofs(filename); + graph->save(ofs); +} + +} diff --git a/src/hdl_graph_slam/information_matrix_calculator.cpp b/src/hdl_graph_slam/information_matrix_calculator.cpp new file mode 100644 index 00000000..02c5ca38 --- /dev/null +++ b/src/hdl_graph_slam/information_matrix_calculator.cpp @@ -0,0 +1,85 @@ +#include + +#include +#include + +namespace hdl_graph_slam { + +InformationMatrixCalculator::InformationMatrixCalculator(ros::NodeHandle& nh) { + use_const_inf_matrix = nh.param("use_const_inf_matrix", false); + const_stddev_x = nh.param("const_stddev_x", 0.5); + const_stddev_q = nh.param("const_stddev_q", 0.1); + + var_gain_a = nh.param("var_gain_a", 20.0); + min_stddev_x = nh.param("min_stddev_x", 0.1); + max_stddev_x = nh.param("max_stddev_x", 5.0); + min_stddev_q = nh.param("min_stddev_q", 0.05); + max_stddev_q = nh.param("max_stddev_q", 0.2); + fitness_score_thresh = nh.param("fitness_score_thresh", 0.5); +} + +InformationMatrixCalculator::~InformationMatrixCalculator() { + +} + +Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const { + if(use_const_inf_matrix) { + Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); + inf.topLeftCorner(3, 3).array() /= const_stddev_x; + inf.bottomRightCorner(3, 3).array() /= const_stddev_q; + return inf; + } + + double fitness_score = calc_fitness_score(cloud1, cloud2, relpose); + + double min_var_x = std::pow(min_stddev_x, 2); + double max_var_x = std::pow(max_stddev_x, 2); + double min_var_q = std::pow(min_stddev_q, 2); + double max_var_q = std::pow(max_stddev_q, 2); + + float w_x = weight(var_gain_a, fitness_score_thresh, min_var_x, max_var_x, fitness_score); + float w_q = weight(var_gain_a, fitness_score_thresh, min_var_q, max_var_q, fitness_score); + + Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); + inf.topLeftCorner(3, 3).array() /= w_x; + inf.bottomRightCorner(3, 3).array() /= w_q; + return inf; +} + +double InformationMatrixCalculator::calc_fitness_score(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range) const { + pcl::search::KdTree::Ptr tree_(new pcl::search::KdTree()); + tree_->setInputCloud(cloud1); + + double fitness_score = 0.0; + + // Transform the input dataset using the final transformation + pcl::PointCloud input_transformed; + pcl::transformPointCloud (*cloud2, input_transformed, relpose.cast()); + + std::vector nn_indices (1); + std::vector nn_dists (1); + + // For each point in the source dataset + int nr = 0; + for (size_t i = 0; i < input_transformed.points.size (); ++i) + { + // Find its nearest neighbor in the target + tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); + + // Deal with occlusions (incomplete targets) + if (nn_dists[0] <= max_range) + { + // Add to the fitness score + fitness_score += nn_dists[0]; + nr++; + } + } + + if (nr > 0) + return (fitness_score / nr); + else + return (std::numeric_limits::max ()); +} + +} + diff --git a/src/hdl_graph_slam/keyframe.cpp b/src/hdl_graph_slam/keyframe.cpp new file mode 100644 index 00000000..8380fa3a --- /dev/null +++ b/src/hdl_graph_slam/keyframe.cpp @@ -0,0 +1,62 @@ +#include + +#include + +#include +#include + +namespace hdl_graph_slam { + +KeyFrame::KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud::ConstPtr& cloud) + : stamp(stamp), + odom(odom), + accum_distance(accum_distance), + cloud(cloud), + node(nullptr) +{} + +KeyFrame::~KeyFrame() { + +} + +void KeyFrame::dump(const std::string& directory) { + if(!boost::filesystem::is_directory(directory)) { + boost::filesystem::create_directory(directory); + } + + std::ofstream ofs(directory + "/data"); + ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n"; + + ofs << "odom\n"; + ofs << odom.matrix() << "\n"; + + ofs << "accum_distance " << accum_distance << "\n"; + + if(floor_coeffs) { + ofs << "floor_coeffs " << floor_coeffs->transpose() << "\n"; + } + + if(node) { + ofs << "id " << node->id() << "\n"; + } + + pcl::io::savePCDFileBinary(directory + "/cloud.pcd", *cloud); + +} + +KeyFrameSnapshot::KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud::ConstPtr& cloud) + : pose(pose), + cloud(cloud) +{} + +KeyFrameSnapshot::KeyFrameSnapshot(const KeyFrame::Ptr& key) + : pose(key->node->estimate()), + cloud(key->cloud) +{} + + +KeyFrameSnapshot::~KeyFrameSnapshot() { + +} + +} diff --git a/src/hdl_graph_slam/map_cloud_generator.cpp b/src/hdl_graph_slam/map_cloud_generator.cpp new file mode 100644 index 00000000..bd717693 --- /dev/null +++ b/src/hdl_graph_slam/map_cloud_generator.cpp @@ -0,0 +1,50 @@ +#include + +#include + +namespace hdl_graph_slam { + +MapCloudGenerator::MapCloudGenerator() { +} + +MapCloudGenerator::~MapCloudGenerator() { + +} + +pcl::PointCloud::Ptr MapCloudGenerator::generate(const std::vector& keyframes, double resolution) const { + if(keyframes.empty()) { + return nullptr; + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + cloud->reserve(keyframes.front()->cloud->size() * keyframes.size()); + + for(const auto& keyframe : keyframes) { + Eigen::Matrix4f pose = keyframe->pose.matrix().cast(); + for(const auto& src_pt : keyframe->cloud->points) { + PointT dst_pt; + dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap(); + dst_pt.intensity = src_pt.intensity; + cloud->push_back(dst_pt); + } + } + + cloud->width = cloud->size(); + cloud->height = 1; + cloud->is_dense = false; + + pcl::octree::OctreePointCloud octree(resolution); + octree.setInputCloud(cloud); + octree.addPointsFromInputCloud(); + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + octree.getOccupiedVoxelCenters(filtered->points); + + filtered->width = filtered->size(); + filtered->height = 1; + filtered->is_dense = false; + + return filtered; +} + +} diff --git a/src/hdl_graph_slam/registrations.cpp b/src/hdl_graph_slam/registrations.cpp new file mode 100644 index 00000000..6670bb96 --- /dev/null +++ b/src/hdl_graph_slam/registrations.cpp @@ -0,0 +1,71 @@ +#include + +#include + +#include +#include +#include + +#include +#include + +namespace hdl_graph_slam { + +boost::shared_ptr> select_registration_method(ros::NodeHandle& pnh) { + using PointT = pcl::PointXYZI; + + // select a registration method (ICP, GICP, NDT) + std::string registration_method = pnh.param("registration_method", "NDT_OMP"); + if(registration_method == "ICP") { + std::cout << "registration: ICP" << std::endl; + boost::shared_ptr> icp(new pcl::IterativeClosestPoint()); + return icp; + } else if(registration_method.find("GICP") != std::string::npos) { + if(registration_method.find("OMP") == std::string::npos) { + std::cout << "registration: GICP" << std::endl; + boost::shared_ptr> gicp(new pcl::GeneralizedIterativeClosestPoint()); + return gicp; + } else { + std::cout << "registration: GICP_OMP" << std::endl; + boost::shared_ptr> gicp(new pclomp::GeneralizedIterativeClosestPoint()); + return gicp; + } + } else { + if(registration_method.find("NDT") == std::string::npos ) { + std::cerr << "warning: unknown registration type(" << registration_method << ")" << std::endl; + std::cerr << " : use NDT" << std::endl; + } + + double ndt_resolution = pnh.param("ndt_resolution", 0.5); + if(registration_method.find("OMP") == std::string::npos) { + std::cout << "registration: NDT " << ndt_resolution << std::endl; + boost::shared_ptr> ndt(new pcl::NormalDistributionsTransform()); + ndt->setTransformationEpsilon(0.01); + ndt->setResolution(ndt_resolution); + return ndt; + } else { + int num_threads = pnh.param("ndt_num_threads", 0); + std::string nn_search_method = pnh.param("ndt_nn_search_method", "DIRECT7"); + std::cout << "registration: NDT_OMP " << nn_search_method << " " << ndt_resolution << " (" << num_threads << " threads)" << std::endl; + boost::shared_ptr> ndt(new pclomp::NormalDistributionsTransform()); + if(num_threads > 0) { + ndt->setNumThreads(num_threads); + } + ndt->setTransformationEpsilon(0.01); + ndt->setMaximumIterations(64); + ndt->setResolution(ndt_resolution); + if(nn_search_method == "KDTREE") { + ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); + } else if (nn_search_method == "DIRECT1") { + ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1); + } else { + ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); + } + return ndt; + } + } + + return nullptr; +} + +}