├── README.md ├── config ├── mapfusion.yaml └── mapfusion_k.yaml ├── doc ├── pipeline.jpeg └── pipeline.pdf ├── mapFusion.cpp └── third_parties ├── fast_max-clique_finder ├── COPYRIGHT ├── README.md └── src │ ├── findClique.cpp │ ├── findClique.h │ ├── findCliqueHeu.cpp │ ├── graphIO.cpp │ ├── graphIO.h │ └── utils.cpp ├── nanoflann.hpp ├── nanoflann_pcl.h └── scanContext ├── .Rhistory ├── scanContext.cpp └── scanContext.h /README.md: -------------------------------------------------------------------------------- 1 | # DiSCo-SLAM 2 | 3 | **DiSCo-SLAM is a novel framework for distributed, multi-robot SLAM intended for use with 3D LiDAR observations. The framework is the first to use the lightweight Scan Context descriptor for multi-robot SLAM, permitting a data-efficient exchange of LiDAR observations among robots. Additionally, our framework includes a two-stage global and local optimization framework for distributed multi- robot SLAM which provides stable localization results that are resilient to the unknown initial conditions that typify the search for inter-robot loop closures.** 4 | 5 |

6 | 7 | ## 8 | - Here we provide a distributed multi-robot SLAM example for 3 robots, intended for use with the two datasets provided below. 9 | - The local SLAM used in our project is [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM), please download [the modified version of LIO-SAM](https://github.com/yeweihuang/LIO-SAM.git), and add the DiSCo-SLAM folder into ```LIO-SAM\src```. 10 | ``` 11 | ├──LIO-SAM 12 | ├── ... 13 | ├── src 14 | │ ├── ... 15 | │ └── DiSCo-SLAM # Folder for multi-robot SLAM 16 | └── ... 17 | ``` 18 | ``` 19 | git clone https://github.com/yeweihuang/LIO-SAM.git 20 | cd LIO-SAM/src 21 | git clone git@github.com:RobustFieldAutonomyLab/DiSCo-SLAM.git 22 | ``` 23 | - Code from [Scan Context](https://github.com/irapkaist/scancontext) is used for feature description. 24 | - We use code from [PCM](https://github.com/lajoiepy/robust_distributed_mapper/tree/d609f59658956e1b7fe06c786ed7d07776ecb426/cpp/src/pairwise_consistency_maximization) 25 | for outlier detection. 26 | 27 | 28 | ## Dependencies 29 | - Same dependencies as [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM): 30 | - [ROS Melodic](http://wiki.ros.org/melodic#Installation) 31 | - [gtsam 4.0.2](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library) 32 | - Dependency for [Scan Context](https://github.com/irapkaist/scancontext): 33 | - [libnabo 1.0.7](https://github.com/ethz-asl/libnabo/releases) 34 | 35 | 36 | ## Datasets 37 | 38 | - [The Park Dataset](https://huggingface.co/datasets/yeweihuang/DiSCo-SLAM-Example-Dataset) 39 | - [KITTI 08 Dataset](https://huggingface.co/datasets/yeweihuang/DiSCo-SLAM-Example-Dataset) 40 | 41 | To run the KITTI08 dataset, change line 9 & 10 in launch/run.launch from 42 | ``` 43 | 44 | 45 | ``` 46 | to 47 | ``` 48 | 49 | 50 | ``` 51 | 52 | ## How to use 53 | 54 | ``` 55 | cd ~/catkin_ws/src 56 | git clone 57 | cd .. 58 | catkin_make 59 | ``` 60 | 61 | ``` 62 | roslaunch lio_sam run.launch 63 | rosbag play your_bag_name.bag 64 | ``` 65 | 66 | -------------------------------------------------------------------------------- /config/mapfusion.yaml: -------------------------------------------------------------------------------- 1 | mapfusion: 2 | scancontext: 3 | knn_feature_dim: 16 4 | max_range: 30 5 | num_sector: 60 6 | num_nearest_matches: 50 7 | num_match_candidates: 1 8 | #vertial_axis: #should be x/y/z 9 | 10 | interRobot: 11 | #topic 12 | sc_topic: "context" 13 | sc_frame: "base_link" 14 | local_topic: "lio_sam/mapping/cloud_info" 15 | 16 | #thres 17 | loop_threshold: 0.2 18 | pcm_threshold: 20 19 | icp_threshold: 3 20 | loop_frame_threshold: 10 #frames 21 | pcm_start_threshold: 5 # 5 for best result, 2 for work in harsh environment 22 | 23 | robot_initial: jackal0 24 | use_position_search: false #use position search may introduce more global loop closure, but spend more time for candidate checking 25 | -------------------------------------------------------------------------------- /config/mapfusion_k.yaml: -------------------------------------------------------------------------------- 1 | mapfusion: 2 | scancontext: 3 | knn_feature_dim: 64 4 | max_range: 30 5 | num_sector: 60 6 | num_nearest_matches: 50 7 | num_match_candidates: 1 8 | #vertial_axis: #should be x/y/z 9 | 10 | interRobot: 11 | #topic 12 | sc_topic: "context" 13 | sc_frame: "base_link" 14 | local_topic: "lio_sam/mapping/cloud_info" 15 | 16 | #thres 17 | loop_threshold: 0.2 18 | pcm_threshold: 100 19 | icp_threshold: 5 20 | pcm_start_threshold: 5 # 5 for best result, 2 for work in harsh environment 21 | 22 | robot_initial: jackal0 23 | use_position_search: false #use position search may introduce more global loop closure, but spend more time for candidate checking 24 | -------------------------------------------------------------------------------- /doc/pipeline.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/DiSCo-SLAM/772755702840a029f4fd0eaedd11bab82ac1e3fd/doc/pipeline.jpeg -------------------------------------------------------------------------------- /doc/pipeline.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/DiSCo-SLAM/772755702840a029f4fd0eaedd11bab82ac1e3fd/doc/pipeline.pdf -------------------------------------------------------------------------------- /mapFusion.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yewei on 8/31/20. 3 | // 4 | 5 | //msg 6 | #include "lio_sam/cloud_info.h" 7 | #include "lio_sam/context_info.h" 8 | 9 | //third party 10 | #include "scanContext/scanContext.h" 11 | #include "fast_max-clique_finder/src/findClique.h" 12 | 13 | #include "nabo/nabo.h" 14 | 15 | //ros 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | //gtsam 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | //expression graph 32 | #include 33 | #include 34 | #include 35 | 36 | //factor graph 37 | #include 38 | #include 39 | #include 40 | 41 | //pcl 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | 50 | inline gtsam::Pose3_ transformTo(const gtsam::Pose3_& x, const gtsam::Pose3_& p) { 51 | return gtsam::Pose3_(x, >sam::Pose3::transform_pose_to, p); 52 | } 53 | 54 | sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame) 55 | { 56 | sensor_msgs::PointCloud2 tempCloud; 57 | pcl::toROSMsg(*thisCloud, tempCloud); 58 | tempCloud.header.stamp = thisStamp;// 59 | tempCloud.header.frame_id = thisFrame; 60 | if (thisPub->getNumSubscribers() != 0) 61 | thisPub->publish(tempCloud); 62 | return tempCloud; 63 | } 64 | 65 | class MapFusion{ 66 | 67 | private: 68 | ros::NodeHandle nh;//global node handler for publishing everthing 69 | 70 | ros::Subscriber _sub_laser_cloud_info; 71 | ros::Subscriber _sub_scan_context_info; 72 | ros::Subscriber _sub_odom_trans; 73 | ros::Subscriber _sub_loop_info_global; 74 | 75 | ros::Subscriber _sub_communication_signal; 76 | ros::Subscriber _sub_signal_1; 77 | ros::Subscriber _sub_signal_2; 78 | 79 | 80 | std::string _signal_id_1; 81 | std::string _signal_id_2; 82 | 83 | ros::Publisher _pub_context_info; 84 | ros::Publisher _pub_loop_info; 85 | ros::Publisher _pub_cloud; 86 | 87 | ros::Publisher _pub_trans_odom2map; 88 | ros::Publisher _pub_trans_odom2odom; 89 | 90 | ros::Publisher _pub_loop_info_global; 91 | 92 | //parameters 93 | std::string _robot_id; 94 | std::string _robot_this;//robot id which the thread is now processing 95 | std::string _sc_topic; 96 | std::string _sc_frame; 97 | 98 | std::string _local_topic; 99 | 100 | bool _communication_signal; 101 | bool _signal_1; 102 | bool _signal_2; 103 | bool _use_position_search; 104 | 105 | int _num_bin; 106 | int _robot_id_th; 107 | int _robot_this_th; 108 | 109 | int _max_range; 110 | int _num_sectors; 111 | int _knn_feature_dim; 112 | 113 | int _num_nearest_matches; 114 | int _num_match_candidates; 115 | 116 | int _pcm_start_threshold; 117 | 118 | float _loop_thres; 119 | float _pcm_thres; 120 | float _icp_thres; 121 | int _loop_frame_thres; 122 | 123 | std::mutex mtx_publish_1; 124 | std::mutex mtx_publish_2; 125 | std::mutex mtx; 126 | 127 | std::string _robot_initial; 128 | std::string _pcm_matrix_folder; 129 | 130 | lio_sam::cloud_info _cloud_info; 131 | // lio_sam::context_info _context_info; 132 | std::vector _context_list_to_publish_1; 133 | std::vector _context_list_to_publish_2; 134 | 135 | pcl::KdTreeFLANN::Ptr _kdtree_pose_to_publish; 136 | pcl::PointCloud::Ptr _cloud_pose_to_publish; 137 | 138 | pcl::KdTreeFLANN::Ptr _kdtree_pose_to_search; 139 | pcl::PointCloud::Ptr _cloud_pose_to_search_this;//3D 140 | pcl::PointCloud::Ptr _cloud_pose_to_search_other;//3D 141 | 142 | pcl::KdTreeFLANN::Ptr _kdtree_loop_to_search; 143 | pcl::PointCloud::Ptr _cloud_loop_to_search; 144 | 145 | pcl::VoxelGrid _downsize_filter_icp; 146 | 147 | std::pair _initial_loop; 148 | int _id_bin_last; 149 | 150 | lio_sam::context_info _loop_info; 151 | std_msgs::Header _cloud_header; 152 | 153 | pcl::PointCloud::Ptr _laser_cloud_sum; 154 | pcl::PointCloud::Ptr _laser_cloud_feature; 155 | pcl::PointCloud::Ptr _laser_cloud_corner; 156 | pcl::PointCloud::Ptr _laser_cloud_surface; 157 | 158 | //global variables for sc 159 | Nabo::NNSearchF* _nns = NULL; //KDtree 160 | Eigen::MatrixXf _target_matrix; 161 | ScanContext *_scan_context_factory; 162 | 163 | std::vector _robot_received_list; 164 | std::vector> _idx_nearest_list; 165 | std::unordered_map _bin_with_id; 166 | 167 | //for pcm & graph 168 | //first: source pose; second: source pose in target; third: icp fitness score; 169 | std::unordered_map< int, std::vector< std::tuple > > _pose_queue; 170 | //first: target, second: source, third: relative transform; 171 | std::unordered_map< int, std::vector< std::tuple > > _loop_queue; 172 | 173 | std::unordered_map< std::string, std::vector > _global_odom_trans; 174 | 175 | //first: robot pair id, second: effective loop id in _loop_queue 176 | std::unordered_map< int, std::vector > _loop_accept_queue; 177 | 178 | std::unordered_map< int, std::vector > _global_map_trans; 179 | std::unordered_map< int, PointTypePose> _global_map_trans_optimized; 180 | 181 | int number_print; 182 | 183 | PointTypePose _trans_to_publish; 184 | 185 | std::vector> _processing_time_list; 186 | public: 187 | 188 | MapFusion(){ 189 | ParamLoader(); 190 | initialization(); 191 | 192 | _sub_communication_signal = nh.subscribe(_robot_id + "/lio_sam/signal", 193 | 100, &MapFusion::communicationSignalHandler, this, ros::TransportHints().tcpNoDelay()); 194 | 195 | _sub_signal_1 = nh.subscribe(_signal_id_1 + "/lio_sam/signal", 196 | 100, &MapFusion::signalHandler1, this, ros::TransportHints().tcpNoDelay()); 197 | _sub_signal_2 = nh.subscribe(_signal_id_2 + "/lio_sam/signal", 198 | 100, &MapFusion::signalHandler2, this, ros::TransportHints().tcpNoDelay()); 199 | 200 | _sub_laser_cloud_info = nh.subscribe(_robot_id + "/" + _local_topic, 201 | 1, &MapFusion::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay()); 202 | 203 | _sub_loop_info_global = nh.subscribe(_sc_topic + "/loop_info_global", 204 | 100, &MapFusion::globalLoopInfoHandler, this, ros::TransportHints().tcpNoDelay()); 205 | 206 | 207 | if(_robot_id != _robot_initial){ 208 | _sub_scan_context_info = nh.subscribe(_sc_topic + "/context_info", 209 | 20, &MapFusion::scanContextInfoHandler, this, ros::TransportHints().tcpNoDelay());//number of buffer may differs for different robot numbers 210 | _sub_odom_trans = nh.subscribe(_sc_topic + "/trans_odom", 211 | 20, &MapFusion::OdomTransHandler, this, ros::TransportHints().tcpNoDelay()); 212 | 213 | } 214 | 215 | 216 | 217 | _pub_context_info = nh.advertise (_sc_topic + "/context_info", 1); 218 | _pub_loop_info = nh.advertise (_robot_id + "/" + _sc_topic + "/loop_info", 1); 219 | _pub_cloud = nh.advertise (_robot_id + "/" + _sc_topic + "/cloud", 1); 220 | _pub_trans_odom2map = nh.advertise ( _robot_id + "/" + _sc_topic + "/trans_map", 1); 221 | _pub_trans_odom2odom = nh.advertise ( _sc_topic + "/trans_odom", 1); 222 | _pub_loop_info_global = nh.advertise(_sc_topic + "/loop_info_global", 1); 223 | 224 | } 225 | 226 | void publishContextInfoThread(){ 227 | int signal_id_th_1 = robotID2Number(_signal_id_1); 228 | int signal_id_th_2 = robotID2Number(_signal_id_2); 229 | while (ros::ok()) 230 | { 231 | if (_communication_signal && _signal_1 && _robot_id_th < signal_id_th_1){ 232 | if (_context_list_to_publish_1.empty()) 233 | continue; 234 | //publish scan context info to other robots 235 | mtx_publish_1.lock(); 236 | ScanContextBin bin = _context_list_to_publish_1.back(); 237 | _context_list_to_publish_1.pop_back(); 238 | mtx_publish_1.unlock(); 239 | publishContextInfo(bin, _signal_id_1); 240 | } 241 | if (_communication_signal && _signal_2 && _robot_id_th < signal_id_th_2){ 242 | if (_context_list_to_publish_2.empty()) 243 | continue; 244 | //publish scan context info to other robots 245 | mtx_publish_2.lock(); 246 | ScanContextBin bin = _context_list_to_publish_2.back(); 247 | _context_list_to_publish_2.pop_back(); 248 | mtx_publish_2.unlock(); 249 | publishContextInfo(bin, _signal_id_2); 250 | 251 | } 252 | } 253 | } 254 | 255 | private: 256 | void ParamLoader(){ 257 | ros::NodeHandle n("~");//local node handler 258 | n.param("robot_id", _robot_id, "jackal0"); 259 | n.param("id_1", _signal_id_1, "jackal1"); 260 | n.param("id_2", _signal_id_2, "jackal2"); 261 | n.param("no", number_print, 100); 262 | n.param("pcm_matrix_folder", _pcm_matrix_folder, "aaa"); 263 | 264 | nh.getParam("/mapfusion/scancontext/knn_feature_dim", _knn_feature_dim); 265 | nh.getParam("/mapfusion/scancontext/max_range", _max_range); 266 | nh.getParam("/mapfusion/scancontext/num_sector", _num_sectors); 267 | nh.getParam("/mapfusion/scancontext/num_nearest_matches", _num_nearest_matches); 268 | nh.getParam("/mapfusion/scancontext/num_match_candidates", _num_match_candidates); 269 | 270 | nh.getParam("/mapfusion/interRobot/loop_threshold", _loop_thres); 271 | nh.getParam("/mapfusion/interRobot/pcm_threshold",_pcm_thres); 272 | nh.getParam("/mapfusion/interRobot/icp_threshold",_icp_thres); 273 | nh.getParam("/mapfusion/interRobot/robot_initial",_robot_initial); 274 | nh.getParam("/mapfusion/interRobot/loop_frame_threshold", _loop_frame_thres); 275 | 276 | nh.getParam("/mapfusion/interRobot/sc_topic", _sc_topic); 277 | nh.getParam("/mapfusion/interRobot/sc_frame", _sc_frame); 278 | nh.getParam("/mapfusion/interRobot/local_topic", _local_topic); 279 | nh.getParam("/mapfusion/interRobot/pcm_start_threshold", _pcm_start_threshold); 280 | nh.getParam("/mapfusion/interRobot/use_position_search", _use_position_search); 281 | 282 | } 283 | 284 | void initialization(){ 285 | _laser_cloud_sum.reset(new pcl::PointCloud()); 286 | _laser_cloud_feature.reset(new pcl::PointCloud()); 287 | _laser_cloud_corner.reset(new pcl::PointCloud()); 288 | _laser_cloud_surface.reset(new pcl::PointCloud()); 289 | 290 | _scan_context_factory = new ScanContext(_max_range, _knn_feature_dim, _num_sectors); 291 | 292 | _kdtree_pose_to_publish.reset(new pcl::KdTreeFLANN()); 293 | _cloud_pose_to_publish.reset(new pcl::PointCloud()); 294 | 295 | _kdtree_pose_to_search.reset(new pcl::KdTreeFLANN()); 296 | _cloud_pose_to_search_this.reset(new pcl::PointCloud()); 297 | _cloud_pose_to_search_other.reset(new pcl::PointCloud()); 298 | 299 | _kdtree_loop_to_search.reset(new pcl::KdTreeFLANN()); 300 | _cloud_loop_to_search.reset(new pcl::PointCloud()); 301 | 302 | _downsize_filter_icp.setLeafSize(0.4, 0.4, 0.4); 303 | 304 | _initial_loop.first = -1; 305 | 306 | _robot_id_th = robotID2Number(_robot_id); 307 | 308 | _trans_to_publish.intensity = 0; 309 | 310 | _num_bin = 0;// 311 | 312 | _communication_signal = true; 313 | _signal_1 = true; 314 | _signal_2 = true; 315 | 316 | 317 | } 318 | 319 | int robotID2Number(std::string robo){ 320 | return robo.back() - '0'; 321 | } 322 | 323 | void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn) 324 | { 325 | _laser_cloud_sum->clear(); 326 | _laser_cloud_feature->clear(); 327 | _laser_cloud_corner->clear(); 328 | _laser_cloud_surface->clear(); 329 | 330 | //load newest data 331 | _cloud_info = *msgIn; // new cloud info 332 | _cloud_header = msgIn->header; // new cloud header 333 | pcl::fromROSMsg(msgIn->cloud_deskewed, *_laser_cloud_sum); 334 | pcl::fromROSMsg(msgIn->cloud_corner, *_laser_cloud_corner); 335 | pcl::fromROSMsg(msgIn->cloud_surface, *_laser_cloud_surface); 336 | *_laser_cloud_feature += *_laser_cloud_corner; 337 | *_laser_cloud_feature += *_laser_cloud_surface; 338 | 339 | //do scancontext 340 | ScanContextBin bin = _scan_context_factory->ptcloud2bin(_laser_cloud_sum); 341 | bin.robotname = _robot_id; 342 | bin.time = _cloud_header.stamp.toSec(); 343 | bin.pose.x = _cloud_info.initialGuessX; 344 | bin.pose.y = _cloud_info.initialGuessY; 345 | bin.pose.z = _cloud_info.initialGuessZ; 346 | bin.pose.roll = _cloud_info.initialGuessRoll; 347 | bin.pose.pitch = _cloud_info.initialGuessPitch; 348 | bin.pose.yaw = _cloud_info.initialGuessYaw; 349 | bin.pose.intensity = _cloud_info.imuAvailable; 350 | 351 | bin.cloud->clear(); 352 | pcl::copyPointCloud(*_laser_cloud_feature, *bin.cloud); 353 | 354 | //push the bin info into the wait list 355 | mtx_publish_1.lock(); 356 | _context_list_to_publish_1.push_back(bin); 357 | mtx_publish_1.unlock(); 358 | mtx_publish_2.lock(); 359 | _context_list_to_publish_2.push_back(bin); 360 | mtx_publish_2.unlock(); 361 | 362 | 363 | publishContextInfo(bin, _robot_id); 364 | 365 | } 366 | 367 | void communicationSignalHandler(const std_msgs::Bool::ConstPtr& msg){ 368 | _communication_signal = msg->data; 369 | } 370 | 371 | void signalHandler1(const std_msgs::Bool::ConstPtr& msg){ 372 | _signal_1 = msg->data; 373 | } 374 | 375 | void signalHandler2(const std_msgs::Bool::ConstPtr& msg){ 376 | _signal_2 = msg->data; 377 | } 378 | 379 | void publishContextInfo( ScanContextBin bin , std::string robot_to){ 380 | lio_sam::context_info context_info; 381 | context_info.robotID = _robot_id; 382 | 383 | context_info.numRing = _knn_feature_dim; 384 | context_info.numSector = _num_sectors; 385 | 386 | context_info.scanContextBin.assign(_knn_feature_dim * _num_sectors,0); 387 | context_info.ringKey.assign(_knn_feature_dim, 0); 388 | context_info.header = _cloud_header; 389 | 390 | int cnt = 0; 391 | for (int i = 0; i < _knn_feature_dim; i++){ 392 | context_info.ringKey[i] = bin.ringkey(i); 393 | for (int j = 0; j < _num_sectors; j++){ 394 | context_info.scanContextBin[cnt] = bin.bin(i,j); 395 | ++cnt; 396 | } 397 | } 398 | 399 | context_info.robotIDReceive = robot_to; 400 | context_info.poseX = bin.pose.x; 401 | context_info.poseY = bin.pose.y; 402 | context_info.poseZ = bin.pose.z; 403 | context_info.poseRoll = bin.pose.roll; 404 | context_info.posePitch = bin.pose.pitch; 405 | context_info.poseYaw = bin.pose.yaw; 406 | context_info.poseIntensity = bin.pose.intensity; 407 | 408 | context_info.scanCloud = publishCloud(&_pub_cloud, bin.cloud, ros::Time(bin.time), _robot_id + "/" + _sc_frame); 409 | mtx.lock(); 410 | _pub_context_info.publish(context_info); 411 | mtx.unlock(); 412 | // context_info.scanContextBin. 413 | } 414 | 415 | void OdomTransHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){ 416 | std::string robot_publish = odomMsg->header.frame_id; 417 | if( robot_publish == _robot_id) 418 | return;//skip info publish by the node itself 419 | std::string robot_child = odomMsg->child_frame_id; 420 | std::string index = robot_child + robot_publish; 421 | PointTypePose pose; 422 | pose.x = odomMsg->pose.pose.position.x; 423 | pose.y = odomMsg->pose.pose.position.y; 424 | pose.z = odomMsg->pose.pose.position.z; 425 | tf::Quaternion orientation; 426 | tf::quaternionMsgToTF(odomMsg->pose.pose.orientation, orientation); 427 | double roll, pitch, yaw; 428 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 429 | pose.roll = roll; pose.pitch = pitch; pose.yaw = yaw; 430 | 431 | //intensity also serves as a index 432 | pose.intensity = robotID2Number(robot_child); 433 | 434 | auto ite = _global_odom_trans.find(index); 435 | if(ite == _global_odom_trans.end())//receive a new trans 436 | { 437 | std::vector tmp_pose_list; 438 | tmp_pose_list.push_back(pose); 439 | _global_odom_trans.emplace(std::make_pair(index, tmp_pose_list)); 440 | } 441 | else//else add a new association; 442 | _global_odom_trans[index].push_back(pose); 443 | 444 | gtsamFactorGraph(); 445 | sendMapOutputMessage(); 446 | 447 | } 448 | 449 | void globalLoopInfoHandler(const lio_sam::context_infoConstPtr& msgIn){ 450 | // return; 451 | if (msgIn->robotID != _robot_id) 452 | return; 453 | _pub_loop_info.publish(*msgIn); 454 | sendMapOutputMessage(); 455 | 456 | } 457 | 458 | void gtsamFactorGraph(){ 459 | if (_global_map_trans.size() == 0 && _global_odom_trans.size() == 0) 460 | return; 461 | gtsam::Vector Vector6(6); 462 | gtsam::NonlinearFactorGraph graph; 463 | gtsam::Values initialEstimate; 464 | 465 | std::vector id_received = _robot_received_list; 466 | std::vector> trans_list; 467 | 468 | if (_global_map_trans.size() != 0) 469 | id_received.push_back(_robot_id_th); 470 | 471 | //set initial 472 | Vector6 << 1e-8, 1e-8, 1e-8, 1e-8, 1e-8, 1e-8; 473 | auto odometryNoise0 = gtsam::noiseModel::Diagonal::Variances(Vector6); 474 | graph.add( gtsam::PriorFactor(gtsam::PriorFactor(0, gtsam::Pose3( gtsam::Rot3::RzRyRx(0, 0, 0), gtsam::Point3(0, 0, 0) ) , odometryNoise0) ) ); 475 | initialEstimate.insert(0, gtsam::Pose3( gtsam::Rot3::RzRyRx(0, 0, 0), gtsam::Point3(0, 0, 0))); 476 | 477 | bool ill_posed = true; 478 | //add local constraints 479 | for(auto ite : _global_map_trans){ 480 | int id_0 = std::min(ite.first, _robot_id_th); 481 | int id_1 = std::max(ite.first, _robot_id_th); 482 | 483 | for(auto ite_measure : ite.second){ 484 | PointTypePose pclpose = ite_measure; 485 | gtsam::Pose3 measurement = gtsam::Pose3 (gtsam::Rot3::RzRyRx(pclpose.roll, pclpose.pitch, pclpose.yaw), 486 | gtsam::Point3(pclpose.x, pclpose.y, pclpose.z) ); 487 | Vector6 << 1, 1, 1, 1, 1, 1; 488 | auto odometryNoise = gtsam::noiseModel::Diagonal::Variances(Vector6); 489 | graph.add( gtsam::BetweenFactor(id_0, id_1, measurement, odometryNoise) ); 490 | } 491 | 492 | PointTypePose pclpose = ite.second[ite.second.size() - 1]; 493 | gtsam::Pose3 measurement_latest = gtsam::Pose3 (gtsam::Rot3::RzRyRx(pclpose.roll, pclpose.pitch, pclpose.yaw), 494 | gtsam::Point3(pclpose.x, pclpose.y, pclpose.z)); 495 | if(id_0 == 0){ 496 | initialEstimate.insert(id_1, measurement_latest); 497 | ill_posed = false; 498 | } 499 | else 500 | trans_list.emplace_back(std::make_tuple(id_0, id_1, measurement_latest)); 501 | } 502 | 503 | for(auto ite: _global_odom_trans){ 504 | int id_publish = robotID2Number(ite.first); 505 | int id_child = ite.second[0].intensity; 506 | int id_0 = std::min(id_publish, id_child); 507 | int id_1 = std::max(id_publish, id_child); 508 | 509 | for(auto ite_measure: ite.second){ 510 | 511 | PointTypePose pclpose = ite_measure; 512 | gtsam::Pose3 measurement = gtsam::Pose3 (gtsam::Rot3::RzRyRx(pclpose.roll, pclpose.pitch, pclpose.yaw), 513 | gtsam::Point3(pclpose.x, pclpose.y, pclpose.z)); 514 | Vector6 << 1, 1, 1, 1, 1, 1; 515 | auto odometryNoise = gtsam::noiseModel::Diagonal::Variances(Vector6); 516 | graph.add( gtsam::BetweenFactor(id_0, id_1, measurement, odometryNoise) ); 517 | } 518 | 519 | PointTypePose pclpose = ite.second[ite.second.size() - 1]; 520 | gtsam::Pose3 measurement_latest = gtsam::Pose3 (gtsam::Rot3::RzRyRx(pclpose.roll, pclpose.pitch, pclpose.yaw), 521 | gtsam::Point3(pclpose.x, pclpose.y, pclpose.z)); 522 | 523 | if(id_0 == 0){ 524 | initialEstimate.insert(id_1, measurement_latest); 525 | ill_posed = false; 526 | } 527 | else 528 | trans_list.emplace_back(std::make_tuple(id_0, id_1, measurement_latest)); 529 | 530 | if(find(id_received.begin(), id_received.end(), id_0) == id_received.end()) 531 | id_received.push_back(id_0); 532 | 533 | if(find(id_received.begin(), id_received.end(), id_1) == id_received.end()) 534 | id_received.push_back(id_1); 535 | 536 | } 537 | 538 | if (find(id_received.begin(), id_received.end(), _robot_id_th) == id_received.end()){ 539 | return; 540 | } 541 | if (ill_posed) 542 | return; 543 | 544 | bool terminate_signal = false; 545 | while (!terminate_signal){ 546 | if (id_received.size() == 0) 547 | break; 548 | terminate_signal = true; 549 | for(auto id = id_received.begin(); id != id_received.end();) 550 | { 551 | int id_this = *id; 552 | if(initialEstimate.exists(id_this)){ 553 | id = id_received.erase(id); 554 | continue; 555 | } 556 | else 557 | ++id; 558 | 559 | auto it = std::find_if(trans_list.begin(), trans_list.end(), 560 | [id_this](auto& e) 561 | {return std::get<0>(e) == id_this || std::get<1>(e) == id_this;}); 562 | 563 | if(it == trans_list.end()) 564 | continue; 565 | 566 | int id_t = get<0>(*it) + get<1>(*it) - id_this; 567 | 568 | if(!initialEstimate.exists(id_t)) 569 | continue; 570 | gtsam::Pose3 pose_t = initialEstimate.at(id_t); 571 | if ( id_this == get<1>(*it)){ 572 | gtsam::Pose3 pose_f = pose_t * get<2>(*it); 573 | initialEstimate.insert(id_this, pose_f); 574 | terminate_signal = false; 575 | 576 | } 577 | if ( id_this == get<0>(*it)){ 578 | gtsam::Pose3 pose_f = pose_t * get<2>(*it).inverse(); 579 | initialEstimate.insert(id_this, pose_f); 580 | terminate_signal = false; 581 | } 582 | } 583 | } 584 | 585 | for (auto it : id_received){ 586 | initialEstimate.insert(it, gtsam::Pose3( gtsam::Rot3::RzRyRx(0, 0, 0), gtsam::Point3(0, 0, 0) )); 587 | } 588 | 589 | id_received.clear(); 590 | trans_list.clear(); 591 | 592 | gtsam::Values result = gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); 593 | 594 | initialEstimate.clear(); 595 | graph.resize(0); 596 | 597 | gtsam::Pose3 est = result.at(_robot_id_th); 598 | 599 | _trans_to_publish.x = est.translation().x(); 600 | _trans_to_publish.y = est.translation().y(); 601 | _trans_to_publish.z = est.translation().z(); 602 | _trans_to_publish.roll = est.rotation().roll(); 603 | _trans_to_publish.pitch = est.rotation().pitch(); 604 | _trans_to_publish.yaw = est.rotation().yaw(); 605 | if (_trans_to_publish.x != 0 || _trans_to_publish.y != 0 || _trans_to_publish.z != 0) 606 | _trans_to_publish.intensity = 1; 607 | 608 | if (_trans_to_publish.intensity == 1){ 609 | int robot_id_initial = robotID2Number(_robot_initial); 610 | if (_global_map_trans_optimized.find(robot_id_initial) == _global_map_trans_optimized.end()){ 611 | _global_map_trans_optimized.emplace(std::make_pair(robot_id_initial, _trans_to_publish)); 612 | } 613 | 614 | 615 | else{ 616 | _global_map_trans[robot_id_initial].push_back(_trans_to_publish); 617 | _global_map_trans_optimized[robot_id_initial] = _trans_to_publish; 618 | } 619 | } 620 | } 621 | 622 | void scanContextInfoHandler(const lio_sam::context_infoConstPtr& msgIn){ 623 | lio_sam::context_info context_info_input = *msgIn; 624 | //load the data received 625 | if (!_communication_signal) 626 | return; 627 | if (msgIn->robotIDReceive != _robot_id) 628 | return; 629 | 630 | ScanContextBin bin; 631 | bin.robotname = msgIn->robotID; 632 | bin.time = msgIn->header.stamp.toSec(); 633 | 634 | bin.pose.x = msgIn->poseX; 635 | bin.pose.y = msgIn->poseY; 636 | bin.pose.z = msgIn->poseZ; 637 | bin.pose.roll = msgIn->poseRoll; 638 | bin.pose.pitch = msgIn->posePitch; 639 | bin.pose.yaw = msgIn->poseYaw; 640 | bin.pose.intensity = msgIn->poseIntensity; 641 | 642 | bin.cloud.reset(new pcl::PointCloud()); 643 | pcl::fromROSMsg(msgIn->scanCloud, *bin.cloud); 644 | 645 | bin.bin = Eigen::MatrixXf::Zero(_knn_feature_dim, _num_sectors); 646 | bin.ringkey = Eigen::VectorXf::Zero(_knn_feature_dim); 647 | int cnt = 0; 648 | for (int i=0; inumRing; i++){ 649 | bin.ringkey(i) = msgIn->ringKey[i]; 650 | for (int j=0; jnumSector; j++){ 651 | bin.bin(i,j) = msgIn->scanContextBin[cnt]; 652 | ++cnt; 653 | } 654 | } 655 | 656 | run(bin); 657 | 658 | } 659 | 660 | void run(ScanContextBin bin){ 661 | //build 662 | 663 | buildKDTree(bin); 664 | KNNSearch(bin); 665 | 666 | if(_idx_nearest_list.empty() && _use_position_search) 667 | distanceSearch(bin); 668 | 669 | if(!getInitialGuesses(bin)){ 670 | return; 671 | } 672 | 673 | if(!incrementalPCM()){ 674 | return; 675 | } 676 | 677 | //perform optimization 678 | gtsamExpressionGraph(); 679 | 680 | //send out transform 681 | sendOdomOutputMessage(); 682 | } 683 | 684 | void distanceSearch(ScanContextBin bin){ 685 | int id_this = robotID2Number(bin.robotname); 686 | if(bin.robotname != _robot_id){ 687 | if(_global_map_trans_optimized.find(id_this) == _global_map_trans_optimized.end() ) 688 | return; 689 | PointType pt_query; 690 | std::vector idx_list; 691 | std::vector dist_list; 692 | PointTypePose pose_this = _global_map_trans_optimized[id_this]; 693 | gtsam::Pose3 T_pose_this = gtsam::Pose3(gtsam::Rot3::RzRyRx(pose_this.roll, pose_this.pitch, pose_this.yaw), 694 | gtsam::Point3(pose_this.x, pose_this.y, pose_this.z)); 695 | auto T_query = gtsam::Pose3(gtsam::Rot3::RzRyRx(bin.pose.roll, bin.pose.pitch, bin.pose.yaw), 696 | gtsam::Point3(bin.pose.x, bin.pose.y, bin.pose.z)); 697 | T_query = T_pose_this.inverse() * T_query; 698 | 699 | pt_query.x = T_query.translation().x(); pt_query.y = T_query.translation().y(); pt_query.z = T_query.translation().z(); 700 | 701 | _kdtree_pose_to_search->setInputCloud(_cloud_pose_to_search_this); 702 | _kdtree_pose_to_search->radiusSearch(pt_query, 5, idx_list, dist_list, 0); 703 | if (!idx_list.empty()){ 704 | int tmp_id = _cloud_pose_to_search_this->points[idx_list[0]].intensity; 705 | _idx_nearest_list.emplace_back(std::make_pair(tmp_id, 0)); 706 | } 707 | } 708 | else{ 709 | PointType pt_query; 710 | std::vector idx_list; 711 | std::vector dist_list; 712 | pcl::PointCloud::Ptr cloud_pose_to_search_other_copy(new pcl::PointCloud()); 713 | for (unsigned int i = 0; i < _cloud_pose_to_search_other->size(); i++){ 714 | PointType tmp = _cloud_pose_to_search_other->points[i]; 715 | int id_this = robotID2Number(_bin_with_id[tmp.intensity].robotname); 716 | if(_global_map_trans_optimized.find(id_this) == _global_map_trans_optimized.end() ) 717 | continue; 718 | PointTypePose pose_this = _global_map_trans_optimized[id_this]; 719 | gtsam::Pose3 T_pose_this = gtsam::Pose3(gtsam::Rot3::RzRyRx(pose_this.roll, pose_this.pitch, pose_this.yaw), 720 | gtsam::Point3(pose_this.x, pose_this.y, pose_this.z)); 721 | auto T_this = gtsam::Point3(tmp.x,tmp.y,tmp.z); 722 | T_this = T_pose_this.inverse() * T_this; 723 | tmp.x = T_this.x(); 724 | tmp.y = T_this.y(); 725 | tmp.z = T_this.z(); 726 | cloud_pose_to_search_other_copy->push_back(tmp); 727 | } 728 | pt_query.x = bin.pose.x; 729 | pt_query.y = bin.pose.y; 730 | pt_query.z = bin.pose.z; 731 | if (cloud_pose_to_search_other_copy->empty()) 732 | return; 733 | 734 | _kdtree_pose_to_search->setInputCloud(cloud_pose_to_search_other_copy); 735 | _kdtree_pose_to_search->radiusSearch(pt_query, 10, idx_list, dist_list, 0); 736 | if (!idx_list.empty()){ 737 | for (unsigned int i = 0; i< cloud_pose_to_search_other_copy->size(); i++){ 738 | int tmp_id = cloud_pose_to_search_other_copy->points[idx_list[i]].intensity; 739 | if(tmp_id == _num_bin) 740 | continue; 741 | _idx_nearest_list.emplace_back(std::make_pair(tmp_id, 0)); 742 | break; 743 | } 744 | } 745 | cloud_pose_to_search_other_copy->clear(); 746 | 747 | }// 748 | 749 | 750 | } 751 | 752 | void buildKDTree(ScanContextBin bin){ 753 | _num_bin++; 754 | //store data received 755 | _bin_with_id.emplace( std::make_pair(_num_bin-1, bin) ); 756 | 757 | PointType tmp_pose; 758 | tmp_pose.x = bin.pose.x; tmp_pose.y = bin.pose.y; tmp_pose.z = bin.pose.z; 759 | tmp_pose.intensity = _num_bin - 1; 760 | 761 | if (bin.robotname == _robot_id) 762 | _cloud_pose_to_search_this->push_back(tmp_pose); 763 | else 764 | _cloud_pose_to_search_other->push_back(tmp_pose); 765 | 766 | //add the latest ringkey 767 | _target_matrix.conservativeResize(_knn_feature_dim, _num_bin); 768 | _target_matrix.block(0, _num_bin-1, _knn_feature_dim, 1) = 769 | bin.ringkey.block(0, 0, _knn_feature_dim, 1); 770 | //add the target matrix to nns 771 | _nns = Nabo::NNSearchF::createKDTreeLinearHeap(_target_matrix); 772 | } 773 | 774 | void KNNSearch(ScanContextBin bin){ 775 | if (_num_nearest_matches >= _num_bin){ 776 | return;//if not enough candidates, return 777 | } 778 | 779 | int num_neighbors = _num_nearest_matches; 780 | 781 | //search n nearest neighbors 782 | Eigen::VectorXi indices(num_neighbors); 783 | Eigen::VectorXf dists2(num_neighbors); 784 | 785 | _nns->knn(bin.ringkey, indices, dists2, num_neighbors); 786 | 787 | int idx_candidate, rot_idx; 788 | float distance_to_query; 789 | //first: dist, second: idx in bin, third: rot_idx 790 | std::vector> idx_list; 791 | for (int i = 0; i < std::min( num_neighbors, int(indices.size()) ); ++i){ 792 | //check if the searching work normally 793 | if ( indices.sum() == 0) 794 | continue; 795 | 796 | idx_candidate = indices[i]; 797 | if ( idx_candidate >= _num_bin) 798 | continue; 799 | 800 | // if the candidate & source belong to same robot, skip 801 | if ( bin.robotname == _bin_with_id.at(idx_candidate).robotname) 802 | continue; 803 | 804 | // if the matching pair have nothing to do with the present robot, skip 805 | if ( bin.robotname != _robot_id && _bin_with_id.at(idx_candidate).robotname != _robot_id) 806 | continue; 807 | 808 | if( robotID2Number(bin.robotname) >= _robot_id_th && robotID2Number(_bin_with_id.at(idx_candidate).robotname) >= _robot_id_th) 809 | continue; 810 | 811 | //compute the dist with full scancontext info 812 | distance_to_query = distBtnScanContexts(bin.bin, _bin_with_id.at(idx_candidate).bin, rot_idx); 813 | 814 | if( distance_to_query > _loop_thres) 815 | continue; 816 | 817 | //add to idx list 818 | idx_list.emplace_back( std::make_tuple(distance_to_query, idx_candidate, rot_idx) ); 819 | } 820 | 821 | _idx_nearest_list.clear(); 822 | 823 | if (idx_list.size() == 0) 824 | return; 825 | 826 | //find nearest scan contexts 827 | std::sort(idx_list.begin(), idx_list.end()); 828 | for (int i = 0; i < std::min( _num_match_candidates, int(idx_list.size()) ); i++){ 829 | std::tie(distance_to_query, idx_candidate, rot_idx) = idx_list[i]; 830 | _idx_nearest_list.emplace_back(std::make_pair(idx_candidate, rot_idx)); 831 | } 832 | idx_list.clear(); 833 | } 834 | 835 | bool getInitialGuesses(ScanContextBin bin){ 836 | if(_idx_nearest_list.size() == 0){ 837 | return false; 838 | } 839 | bool new_candidate_signal = false; 840 | for (auto it: _idx_nearest_list){ 841 | new_candidate_signal = getInitialGuess(bin, it.first, it.second); 842 | } 843 | return new_candidate_signal; 844 | } 845 | 846 | bool getInitialGuess(ScanContextBin bin, int idx_nearest, int min_idx){ 847 | 848 | int id0 = idx_nearest, id1 = _num_bin - 1; 849 | 850 | ScanContextBin bin_nearest; 851 | PointTypePose source_pose_initial, target_pose; 852 | 853 | float sc_pitch = (min_idx+1) * M_PI * 2 /_num_sectors; 854 | if (sc_pitch > M_PI) 855 | sc_pitch -= (M_PI * 2); 856 | 857 | int robot_id_this = robotID2Number(bin.robotname); 858 | 859 | auto robot_id_this_ite = std::find(_robot_received_list.begin(), _robot_received_list.end(), robot_id_this); 860 | 861 | //record all received robot id from other robots 862 | if (robot_id_this_ite == _robot_received_list.end() && robot_id_this != _robot_id_th) 863 | _robot_received_list.push_back(robot_id_this); 864 | 865 | // exchange if source has a prior robot id (the last character of the robot name is smaller) (first > second) 866 | if (robot_id_this < _robot_id_th){ 867 | bin_nearest = bin; 868 | bin = _bin_with_id.at(idx_nearest); 869 | 870 | id0 = _num_bin - 1; 871 | id1 = idx_nearest; 872 | 873 | sc_pitch = -sc_pitch; 874 | } 875 | else 876 | bin_nearest = _bin_with_id.at(idx_nearest); 877 | 878 | _robot_this = bin_nearest.robotname; 879 | _robot_this_th = robotID2Number(_robot_this); 880 | 881 | //get initial guess from scancontext 882 | target_pose = bin_nearest.pose; 883 | 884 | //find the pose constrain 885 | if (_global_map_trans_optimized.find(_robot_this_th) != _global_map_trans_optimized.end()){ 886 | PointTypePose trans_to_that = _global_map_trans_optimized[_robot_this_th]; 887 | Eigen::Affine3f t_source2target = pcl::getTransformation(trans_to_that.x, trans_to_that.y, trans_to_that.z, 888 | trans_to_that.roll, trans_to_that.pitch, trans_to_that.yaw); 889 | Eigen::Affine3f t_source = pcl::getTransformation(bin.pose.x, bin.pose.y, bin.pose.z, bin.pose.roll, bin.pose.pitch, bin.pose.yaw); 890 | Eigen::Affine3f t_initial_source = t_source2target * t_source; 891 | pcl::getTranslationAndEulerAngles(t_initial_source, source_pose_initial.x, source_pose_initial.y, source_pose_initial.z, 892 | source_pose_initial.roll, source_pose_initial.pitch, source_pose_initial.yaw); 893 | //if too far away, return false 894 | 895 | } 896 | else if(abs(sc_pitch) < 0.3){ 897 | source_pose_initial = target_pose; 898 | } 899 | else{ 900 | Eigen::Affine3f sc_initial = pcl::getTransformation(0, 0, 0, 901 | 0, 0, sc_pitch); 902 | Eigen::Affine3f t_target = pcl::getTransformation(target_pose.x, target_pose.y, target_pose.z, 903 | target_pose.roll, target_pose.pitch, target_pose.yaw); 904 | Eigen::Affine3f t_initial_source = sc_initial * t_target; 905 | // pre-multiplying -> successive rotation about a fixed frame 906 | 907 | pcl::getTranslationAndEulerAngles(t_initial_source, source_pose_initial.x, source_pose_initial.y, source_pose_initial.z, 908 | source_pose_initial.roll, source_pose_initial.pitch, source_pose_initial.yaw); 909 | source_pose_initial.x = target_pose.x; 910 | source_pose_initial.y = target_pose.y; 911 | source_pose_initial.z = target_pose.z; 912 | } 913 | 914 | PointTypePose pose_source_lidar = icpRelativeMotion(transformPointCloud(bin.cloud, &source_pose_initial), 915 | transformPointCloud(bin_nearest.cloud, &target_pose), source_pose_initial); 916 | 917 | if (pose_source_lidar.intensity == -1 || pose_source_lidar.intensity > _icp_thres) 918 | return false; 919 | 920 | //1: jackal0, 2: jackal1 921 | gtsam::Pose3 pose_from = 922 | gtsam::Pose3(gtsam::Rot3::RzRyRx(bin.pose.roll, bin.pose.pitch, bin.pose.yaw), 923 | gtsam::Point3(bin.pose.x, bin.pose.y, bin.pose.z)); 924 | 925 | gtsam::Pose3 pose_to = 926 | gtsam::Pose3(gtsam::Rot3::RzRyRx(pose_source_lidar.roll, pose_source_lidar.pitch, pose_source_lidar.yaw), 927 | gtsam::Point3(pose_source_lidar.x, pose_source_lidar.y, pose_source_lidar.z)); 928 | 929 | gtsam::Pose3 pose_target = 930 | gtsam::Pose3(gtsam::Rot3::RzRyRx(target_pose.roll, target_pose.pitch, target_pose.yaw), 931 | gtsam::Point3(target_pose.x, target_pose.y, target_pose.z)); 932 | 933 | auto ite = _pose_queue.find(_robot_this_th); 934 | if(ite == _pose_queue.end()){ 935 | std::vector< std::tuple > new_pose_queue; 936 | std::vector< std::tuple > new_loop_queue; 937 | _pose_queue.emplace( std::make_pair(_robot_this_th, new_pose_queue) ); 938 | _loop_queue.emplace( std::make_pair(_robot_this_th, new_loop_queue) ); 939 | } 940 | 941 | 942 | _pose_queue[_robot_this_th].emplace_back(std::make_tuple(pose_from, pose_to, pose_source_lidar.intensity)); 943 | _loop_queue[_robot_this_th].emplace_back(std::make_tuple(id0, id1, pose_to.between(pose_target))); 944 | 945 | return true; 946 | } 947 | 948 | float distBtnScanContexts(Eigen::MatrixXf bin1, Eigen::MatrixXf bin2, int & idx){ 949 | Eigen::VectorXf sim_for_each_cols(_num_sectors); 950 | for (int i = 0; i<_num_sectors; i++) { 951 | 952 | //shift 953 | int one_step = 1; 954 | Eigen::MatrixXf bint = circShift(bin1, one_step); 955 | bin1 = bint; 956 | 957 | //compare 958 | float sum_of_cos_sim = 0; 959 | int num_col_engaged = 0; 960 | 961 | for (int j = 0; j < _num_sectors; j++) { 962 | Eigen::VectorXf col_j_1(_knn_feature_dim); 963 | Eigen::VectorXf col_j_2(_knn_feature_dim); 964 | col_j_1.block(0, 0, _knn_feature_dim, 1) = bin1.block(0, j, _knn_feature_dim, 1); 965 | col_j_2.block(0, 0, _knn_feature_dim, 1) = bin2.block(0, j, _knn_feature_dim, 1); 966 | 967 | if(col_j_1.isZero() || col_j_2.isZero()) 968 | continue; 969 | 970 | //calc sim 971 | float cos_similarity = col_j_1.dot(col_j_2) / col_j_1.norm() / col_j_2.norm(); 972 | sum_of_cos_sim += cos_similarity; 973 | 974 | num_col_engaged++; 975 | } 976 | //devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. 977 | sim_for_each_cols(i) = sum_of_cos_sim / float(num_col_engaged); 978 | 979 | } 980 | Eigen::VectorXf::Index idx_max; 981 | float sim = sim_for_each_cols.maxCoeff(& idx_max); 982 | idx = idx_max; 983 | //get the corresponding angle of the maxcoeff 984 | float dist = 1-sim; 985 | return dist; 986 | 987 | } 988 | 989 | pcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, PointTypePose* transformIn) 990 | { 991 | pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); 992 | 993 | PointType *pointFrom; 994 | 995 | int cloudSize = cloudIn->size(); 996 | cloudOut->resize(cloudSize); 997 | 998 | Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw); 999 | 1000 | #pragma omp parallel for num_threads(numberOfCores) 1001 | for (int i = 0; i < cloudSize; ++i) 1002 | { 1003 | pointFrom = &cloudIn->points[i]; 1004 | cloudOut->points[i].x = transCur(0,0) * pointFrom->x + transCur(0,1) * pointFrom->y + transCur(0,2) * pointFrom->z + transCur(0,3); 1005 | cloudOut->points[i].y = transCur(1,0) * pointFrom->x + transCur(1,1) * pointFrom->y + transCur(1,2) * pointFrom->z + transCur(1,3); 1006 | cloudOut->points[i].z = transCur(2,0) * pointFrom->x + transCur(2,1) * pointFrom->y + transCur(2,2) * pointFrom->z + transCur(2,3); 1007 | cloudOut->points[i].intensity = pointFrom->intensity; 1008 | } 1009 | return cloudOut; 1010 | } 1011 | 1012 | PointTypePose icpRelativeMotion(pcl::PointCloud::Ptr source, 1013 | pcl::PointCloud::Ptr target, 1014 | PointTypePose pose_source) 1015 | { 1016 | // ICP Settings 1017 | pcl::IterativeClosestPoint icp; 1018 | icp.setMaxCorrespondenceDistance(100); 1019 | icp.setMaximumIterations(100); 1020 | icp.setTransformationEpsilon(1e-6); 1021 | icp.setEuclideanFitnessEpsilon(1e-6); 1022 | icp.setRANSACIterations(0); 1023 | 1024 | pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud()); 1025 | _downsize_filter_icp.setInputCloud(source); 1026 | _downsize_filter_icp.filter(*cloud_temp); 1027 | *source = *cloud_temp; 1028 | 1029 | _downsize_filter_icp.setInputCloud(target); 1030 | _downsize_filter_icp.filter(*cloud_temp); 1031 | *target = *cloud_temp; 1032 | 1033 | //Align clouds 1034 | icp.setInputSource(source); 1035 | icp.setInputTarget(target); 1036 | pcl::PointCloud::Ptr unused_result( 1037 | new pcl::PointCloud()); 1038 | icp.align(*unused_result); 1039 | PointTypePose pose_from; 1040 | 1041 | if (icp.hasConverged() == false){ 1042 | pose_from.intensity = -1; 1043 | return pose_from; 1044 | } 1045 | 1046 | float x, y, z, roll, pitch, yaw; 1047 | Eigen::Affine3f correctionLidarFrame; 1048 | 1049 | correctionLidarFrame = icp.getFinalTransformation(); // get transformation in camera frame 1050 | 1051 | pcl::getTranslationAndEulerAngles(correctionLidarFrame, x, y, z, roll, pitch,yaw); 1052 | 1053 | // if(std::min(_robot_id_th, _robot_this_th) == 1 && std::max(_robot_id_th, _robot_this_th) == 2){ 1054 | // publishCloud(&_pub_target_cloud, target, _cloud_header.stamp, "/jackal1/odom"); 1055 | // 1056 | // PointTypePose ptp; 1057 | // ptp.x = x; ptp.y = y; ptp.z = z; ptp.roll = roll; ptp.yaw = yaw; ptp.pitch = pitch; 1058 | // publishCloud(&_pub_match_cloud, transformPointCloud(source, &ptp), _cloud_header.stamp, "/jackal1/odom"); 1059 | // } 1060 | 1061 | 1062 | // transform from world origin to wrong pose 1063 | Eigen::Affine3f tWrong = pcl::getTransformation(pose_source.x, pose_source.y, pose_source.z, 1064 | pose_source.roll, pose_source.pitch, pose_source.yaw); 1065 | // transform from world origin to corrected pose 1066 | Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong; 1067 | 1068 | // pre-multiplying -> successive rotation about a fixed frame 1069 | pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw); 1070 | 1071 | pose_from.x = x; 1072 | pose_from.y = y; 1073 | pose_from.z = z; 1074 | 1075 | pose_from.yaw = yaw; 1076 | pose_from.roll = roll; 1077 | pose_from.pitch = pitch; 1078 | 1079 | pose_from.intensity = icp.getFitnessScore(); 1080 | 1081 | return pose_from; 1082 | 1083 | } 1084 | 1085 | bool incrementalPCM() { 1086 | if (_pose_queue[_robot_this_th].size() < _pcm_start_threshold) 1087 | return false; 1088 | 1089 | //perform pcm for all robot matches 1090 | 1091 | Eigen::MatrixXi consistency_matrix = computePCMMatrix(_loop_queue[_robot_this_th]);//, _pose_queue[_robot_this_th]); 1092 | std::string consistency_matrix_file = _pcm_matrix_folder + "/consistency_matrix" + _robot_id + ".clq.mtx"; 1093 | printPCMGraph(consistency_matrix, consistency_matrix_file); 1094 | // Compute maximum clique 1095 | FMC::CGraphIO gio; 1096 | gio.readGraph(consistency_matrix_file); 1097 | int max_clique_size = 0; 1098 | std::vector max_clique_data; 1099 | 1100 | max_clique_size = FMC::maxCliqueHeu(gio, max_clique_data); 1101 | 1102 | std::sort(max_clique_data.begin(), max_clique_data.end()); 1103 | 1104 | auto loop_accept_queue_this = _loop_accept_queue.find(_robot_this_th); 1105 | if (loop_accept_queue_this == _loop_accept_queue.end()){ 1106 | _loop_accept_queue.emplace(std::make_pair(_robot_this_th, max_clique_data)); 1107 | return true; 1108 | } 1109 | 1110 | if(max_clique_data == loop_accept_queue_this->second) 1111 | return false; 1112 | 1113 | _loop_accept_queue[_robot_this_th].clear(); 1114 | _loop_accept_queue[_robot_this_th] = max_clique_data; 1115 | return true; 1116 | } 1117 | 1118 | Eigen::MatrixXi computePCMMatrix( std::vector< std::tuple > loop_queue_this){ 1119 | Eigen::MatrixXi PCMMat; 1120 | PCMMat.setZero(loop_queue_this.size(), loop_queue_this.size()); 1121 | int id_0, id_1; 1122 | gtsam::Pose3 z_aj_bk, z_ai_bl; 1123 | gtsam::Pose3 z_ai_aj, z_bk_bl; 1124 | gtsam::Pose3 t_ai, t_aj, t_bk, t_bl; 1125 | 1126 | for (unsigned int i = 0; i < loop_queue_this.size(); i++){ 1127 | std::tie(id_0, id_1, z_aj_bk) = loop_queue_this[i]; 1128 | PointTypePose tmp_pose_0 = _bin_with_id.at(id_0).pose; 1129 | PointTypePose tmp_pose_1 = _bin_with_id.at(id_1).pose; 1130 | t_aj = gtsam::Pose3(gtsam::Rot3::RzRyRx(tmp_pose_0.roll, tmp_pose_0.pitch, tmp_pose_0.yaw), 1131 | gtsam::Point3(tmp_pose_0.x, tmp_pose_0.y, tmp_pose_0.z)); 1132 | t_bk = gtsam::Pose3(gtsam::Rot3::RzRyRx(tmp_pose_1.roll, tmp_pose_1.pitch, tmp_pose_1.yaw), 1133 | gtsam::Point3(tmp_pose_1.x, tmp_pose_1.y, tmp_pose_1.z)); 1134 | 1135 | for (unsigned int j = i + 1; j < loop_queue_this.size(); j++){ 1136 | std::tie(id_0, id_1, z_ai_bl) = loop_queue_this[j]; 1137 | PointTypePose tmp_pose_0 = _bin_with_id.at(id_0).pose; 1138 | PointTypePose tmp_pose_1 = _bin_with_id.at(id_1).pose; 1139 | t_ai = gtsam::Pose3(gtsam::Rot3::RzRyRx(tmp_pose_0.roll, tmp_pose_0.pitch, tmp_pose_0.yaw), 1140 | gtsam::Point3(tmp_pose_0.x, tmp_pose_0.y, tmp_pose_0.z)); 1141 | t_bl = gtsam::Pose3(gtsam::Rot3::RzRyRx(tmp_pose_1.roll, tmp_pose_1.pitch, tmp_pose_1.yaw), 1142 | gtsam::Point3(tmp_pose_1.x, tmp_pose_1.y, tmp_pose_1.z)); 1143 | z_ai_aj = t_ai.between(t_aj); 1144 | z_bk_bl = t_bk.between(t_bl); 1145 | float resi = residualPCM(z_aj_bk, z_ai_bl, z_ai_aj, z_bk_bl, 1); 1146 | if (resi < _pcm_thres) 1147 | PCMMat(i,j) = 1; 1148 | else 1149 | PCMMat(i,j) = 0; 1150 | } 1151 | } 1152 | return PCMMat; 1153 | } 1154 | 1155 | float residualPCM(gtsam::Pose3 inter_jk, gtsam::Pose3 inter_il, gtsam::Pose3 inner_ij, gtsam::Pose3 inner_kl, float intensity){ 1156 | gtsam::Pose3 inter_il_inv = inter_il.inverse(); 1157 | gtsam::Pose3 res_pose = inner_ij * inter_jk * inner_kl * inter_il_inv; 1158 | gtsam::Vector6 res_vec = gtsam::Pose3::Logmap(res_pose); 1159 | 1160 | Eigen::Matrix< double, 6, 1> v ; 1161 | v << intensity, intensity, intensity, 1162 | intensity, intensity, intensity; 1163 | Eigen::Matrix< double, 6, 6> m_cov = v.array().matrix().asDiagonal(); 1164 | 1165 | return sqrt(res_vec.transpose()* m_cov * res_vec); 1166 | } 1167 | 1168 | void printPCMGraph(Eigen::MatrixXi pcm_matrix, std::string file_name) { 1169 | // Intialization 1170 | int nb_consistent_measurements = 0; 1171 | 1172 | // Format edges. 1173 | std::stringstream ss; 1174 | for (int i = 0; i < pcm_matrix.rows(); i++) { 1175 | for (int j = i; j < pcm_matrix.cols(); j++) { 1176 | if (pcm_matrix(i,j) == 1) { 1177 | ss << i+1 << " " << j+1 << std::endl; 1178 | nb_consistent_measurements++; 1179 | } 1180 | } 1181 | } 1182 | 1183 | // Write to file 1184 | std::ofstream output_file; 1185 | output_file.open(file_name); 1186 | output_file << "%%MatrixMarket matrix coordinate pattern symmetric" << std::endl; 1187 | output_file << pcm_matrix.rows() << " " << pcm_matrix.cols() << " " << nb_consistent_measurements << std::endl; 1188 | output_file << ss.str(); 1189 | output_file.close(); 1190 | } 1191 | 1192 | void gtsamExpressionGraph(){ 1193 | if (_loop_accept_queue[_robot_this_th].size()<2) 1194 | return; 1195 | 1196 | gtsam::Vector Vector6(6); 1197 | 1198 | gtsam::Pose3 initial_pose_0, initial_pose_1; 1199 | initial_pose_0 = std::get<0>(_pose_queue[_robot_this_th][ _loop_accept_queue[_robot_this_th][_loop_accept_queue[_robot_this_th].size() -1] ]); 1200 | initial_pose_1 = std::get<1>(_pose_queue[_robot_this_th][ _loop_accept_queue[_robot_this_th][_loop_accept_queue[_robot_this_th].size() -1] ]); 1201 | 1202 | gtsam::Values initial; 1203 | gtsam::ExpressionFactorGraph graph; 1204 | 1205 | gtsam::Pose3_ trans(0); 1206 | 1207 | initial.insert(0, initial_pose_1 * initial_pose_0.inverse()); 1208 | //initial.print(); 1209 | 1210 | gtsam::Pose3 p, measurement; 1211 | float noiseScore; 1212 | 1213 | for (auto i:_loop_accept_queue[_robot_this_th]){ 1214 | std::tie(measurement, p, noiseScore) = _pose_queue[_robot_this_th][i]; 1215 | 1216 | if(noiseScore == 0)// 0 indicates a inter robot outlier 1217 | continue; 1218 | 1219 | gtsam::Pose3_ predicted = transformTo(trans,p); 1220 | 1221 | Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, 1222 | noiseScore; 1223 | auto measurementNoise = gtsam::noiseModel::Diagonal::Variances(Vector6); 1224 | 1225 | // Add the Pose3 expression variable, an initial estimate, and the measurement noise. 1226 | graph.addExpressionFactor(predicted, measurement, measurementNoise); 1227 | } 1228 | gtsam::Values result = gtsam::LevenbergMarquardtOptimizer(graph, initial).optimize(); 1229 | 1230 | gtsam::Pose3 est = result.at(0); 1231 | 1232 | PointTypePose map_trans_this; 1233 | 1234 | //float x, y, z, roll, pitch, yaw; 1235 | map_trans_this.x = est.translation().x(); 1236 | map_trans_this.y = est.translation().y(); 1237 | map_trans_this.z = est.translation().z(); 1238 | map_trans_this.roll = est.rotation().roll(); 1239 | map_trans_this.pitch = est.rotation().pitch(); 1240 | map_trans_this.yaw = est.rotation().yaw(); 1241 | 1242 | auto ite = _global_map_trans.find(_robot_this_th); 1243 | if(ite == _global_map_trans.end()){ 1244 | std::vector tmp_pose_list; 1245 | tmp_pose_list.push_back(map_trans_this); 1246 | _global_map_trans.emplace(std::make_pair( _robot_this_th, tmp_pose_list ) ); 1247 | _global_map_trans_optimized.emplace(std::make_pair( _robot_this_th, map_trans_this)); 1248 | } 1249 | else{ 1250 | _global_map_trans[_robot_this_th].push_back(map_trans_this); 1251 | _global_map_trans_optimized[_robot_this_th] = map_trans_this; 1252 | } 1253 | 1254 | 1255 | if (_global_odom_trans.size() != 0) 1256 | gtsamFactorGraph(); 1257 | 1258 | if (_trans_to_publish.intensity == 0){ 1259 | ite = _global_map_trans.find(0); 1260 | if(ite == _global_map_trans.end()) 1261 | return; 1262 | _global_map_trans_optimized[0].intensity = 1; 1263 | _trans_to_publish = _global_map_trans_optimized[0]; 1264 | } 1265 | 1266 | if (_global_map_trans.size() == 1 && _global_odom_trans.size() == 0) 1267 | _trans_to_publish = _global_map_trans_optimized[0]; 1268 | 1269 | graph.resize(0); 1270 | 1271 | } 1272 | 1273 | void sendMapOutputMessage(){ 1274 | if (_trans_to_publish.intensity == 0) 1275 | return; 1276 | 1277 | //publish transformation to the SLAM node 1278 | nav_msgs::Odometry odom2map; 1279 | odom2map.header.stamp = _cloud_header.stamp; 1280 | odom2map.header.frame_id = _robot_id + "/" + _sc_frame; 1281 | odom2map.child_frame_id = _robot_id + "/" + _sc_frame + "/odom2map"; 1282 | odom2map.pose.pose.position.x = _trans_to_publish.x; 1283 | odom2map.pose.pose.position.y = _trans_to_publish.y; 1284 | odom2map.pose.pose.position.z = _trans_to_publish.z; 1285 | odom2map.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw 1286 | (_trans_to_publish.roll, _trans_to_publish.pitch, _trans_to_publish.yaw); 1287 | _pub_trans_odom2map.publish(odom2map); 1288 | } 1289 | 1290 | void sendGlobalLoopMessageKDTree(){ 1291 | 1292 | auto loop_list = _loop_queue[_robot_this_th]; 1293 | int len_loop_list = loop_list.size() - 1; 1294 | auto loop_this = loop_list[len_loop_list]; 1295 | int id_bin_this = std::get<1>(loop_this); 1296 | 1297 | if (_initial_loop.first == -1){ 1298 | //if not enough time distance 1299 | _initial_loop.first = _robot_this_th; 1300 | _initial_loop.second = len_loop_list; 1301 | //initialize last point 1302 | _id_bin_last = id_bin_this; 1303 | return; 1304 | } 1305 | 1306 | if (!compare_timestamp(id_bin_this, _id_bin_last)){ 1307 | //if not enough history point 1308 | return; 1309 | } 1310 | 1311 | int tmp_robot_id_th = _initial_loop.first; 1312 | int tmp_len_loop_list = _initial_loop.second; 1313 | auto loop_that = _loop_queue[tmp_robot_id_th][tmp_len_loop_list]; 1314 | int id_bin_that = std::get<1>(loop_that); 1315 | sendLoopThis(_robot_this_th, tmp_robot_id_th, len_loop_list, tmp_len_loop_list); 1316 | sendLoopThat(_robot_this_th, tmp_robot_id_th, len_loop_list, tmp_len_loop_list); 1317 | 1318 | _id_bin_last = id_bin_this; 1319 | 1320 | } 1321 | 1322 | 1323 | bool compare_timestamp(int id_0, int id_1){ 1324 | if(abs(_bin_with_id[id_0].pose.intensity - _bin_with_id[id_1].pose.intensity) > _loop_frame_thres) 1325 | return true; 1326 | else 1327 | return false; 1328 | } 1329 | 1330 | void sendLoopThis(int robot_id_this, int robot_id_that, int id_loop_this, int id_loop_that){ 1331 | auto loop_list_this = _loop_queue[robot_id_this]; 1332 | auto loop_list_that = _loop_queue[robot_id_that]; 1333 | auto loop_this = loop_list_this[id_loop_this]; 1334 | auto loop_that = loop_list_that[id_loop_that]; 1335 | 1336 | int id_bin_this = std::get<1>(loop_this); 1337 | int id_bin_last = std::get<1>(loop_that); 1338 | 1339 | auto pose_this = _pose_queue[robot_id_this][id_loop_this]; 1340 | auto pose_that = _pose_queue[robot_id_that][id_loop_that]; 1341 | 1342 | gtsam::Pose3 pose_to_this, pose_to_that; 1343 | if(robot_id_this == robot_id_that){ 1344 | pose_to_this = std::get<1>(pose_this); 1345 | pose_to_that = std::get<1>(pose_that); 1346 | } 1347 | else{ 1348 | if (_global_map_trans_optimized.find(robot_id_this) == _global_map_trans_optimized.end() || 1349 | _global_map_trans_optimized.find(robot_id_that) == _global_map_trans_optimized.end() ) 1350 | return; 1351 | PointTypePose trans_this = _global_map_trans_optimized[robot_id_this]; 1352 | gtsam::Pose3 trans_this3 = gtsam::Pose3( gtsam::Rot3::RzRyRx(trans_this.roll, trans_this.pitch, trans_this.yaw), 1353 | gtsam::Point3(trans_this.x, trans_this.y, trans_this.z) ); 1354 | 1355 | PointTypePose trans_that = _global_map_trans_optimized[robot_id_that]; 1356 | gtsam::Pose3 trans_that3 = gtsam::Pose3( gtsam::Rot3::RzRyRx(trans_that.roll, trans_that.pitch, trans_that.yaw), 1357 | gtsam::Point3(trans_that.x, trans_that.y, trans_that.z) ); 1358 | 1359 | pose_to_this = trans_this3.inverse() * std::get<1>(pose_this); 1360 | pose_to_that = trans_that3.inverse() * std::get<1>(pose_that); 1361 | } 1362 | 1363 | float tmp_intensity = (std::get<2>(pose_this) + std::get<2>(pose_that) ) / 2.0; 1364 | 1365 | auto dpose = pose_to_that.between(pose_to_this); 1366 | auto thisp = _bin_with_id[id_bin_this].pose; 1367 | auto thatp = _bin_with_id[id_bin_last].pose; 1368 | auto pose_to_this1 = gtsam::Pose3( gtsam::Rot3::RzRyRx(thisp.roll, thisp.pitch, thisp.yaw), 1369 | gtsam::Point3(thisp.x, thisp.y, thisp.z) ); 1370 | auto pose_to_that1 = gtsam::Pose3( gtsam::Rot3::RzRyRx(thatp.roll, thatp.pitch, thatp.yaw), 1371 | gtsam::Point3(thatp.x, thatp.y, thatp.z) ); 1372 | auto dpose1 = pose_to_that1.between(pose_to_this1); 1373 | 1374 | update_loop_info(id_bin_last, id_bin_this, pose_to_that, pose_to_this, tmp_intensity); 1375 | 1376 | _pub_loop_info.publish(_loop_info); 1377 | } 1378 | 1379 | void sendLoopThat(int robot_id_this,int robot_id_that, int id_loop_this, int id_loop_that){ 1380 | if(robot_id_this != robot_id_that) 1381 | return; 1382 | auto loop_list = _loop_queue[robot_id_this]; 1383 | auto loop_this = loop_list[id_loop_this]; 1384 | auto loop_that = loop_list[id_loop_that]; 1385 | 1386 | int id_bin_this = std::get<0>(loop_this); 1387 | int id_bin_last = std::get<0>(loop_that); 1388 | 1389 | auto pose_this = _pose_queue[robot_id_this][id_loop_this]; 1390 | auto pose_that = _pose_queue[robot_id_this][id_loop_that]; 1391 | auto pose_to_this = std::get<0>(pose_this) * std::get<2>(loop_this) ; 1392 | auto pose_to_that = std::get<0>(pose_that) * std::get<2>(loop_that) ; 1393 | float tmp_intensity = (std::get<2>(pose_this) + std::get<2>(pose_that) ) / 2.0; 1394 | update_loop_info(id_bin_last, id_bin_this, pose_to_that, pose_to_this, tmp_intensity); 1395 | 1396 | _pub_loop_info_global.publish(_loop_info); 1397 | } 1398 | 1399 | void update_loop_info(int id_bin_last, int id_bin_this, gtsam::Pose3 pose_to_last, gtsam::Pose3 pose_to_this, float intensity){ 1400 | //relative translation btwn 2 poses 1401 | auto dpose = pose_to_last.between(pose_to_this); 1402 | _loop_info.robotID = _bin_with_id[id_bin_last].robotname; 1403 | _loop_info.numRing = _bin_with_id[id_bin_last].pose.intensity;//from 1404 | _loop_info.numSector = _bin_with_id[id_bin_this].pose.intensity;//to 1405 | 1406 | _loop_info.poseX = dpose.translation().x(); 1407 | _loop_info.poseY = dpose.translation().y(); 1408 | _loop_info.poseZ = dpose.translation().z(); 1409 | _loop_info.poseRoll = dpose.rotation().roll(); 1410 | _loop_info.posePitch = dpose.rotation().pitch(); 1411 | _loop_info.poseYaw = dpose.rotation().yaw(); 1412 | _loop_info.poseIntensity = intensity; 1413 | 1414 | } 1415 | 1416 | void sendOdomOutputMessage(){ 1417 | 1418 | sendMapOutputMessage(); 1419 | 1420 | //publish relative transformation to other robots 1421 | nav_msgs::Odometry odom2odom; 1422 | odom2odom.header.stamp = _cloud_header.stamp; 1423 | odom2odom.header.frame_id = _robot_id; 1424 | odom2odom.child_frame_id = _robot_this; 1425 | odom2odom.pose.pose.position.x = _global_map_trans_optimized[_robot_this_th].x; 1426 | odom2odom.pose.pose.position.y = _global_map_trans_optimized[_robot_this_th].y; 1427 | odom2odom.pose.pose.position.z = _global_map_trans_optimized[_robot_this_th].z; 1428 | odom2odom.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw 1429 | (_global_map_trans_optimized[_robot_this_th].roll, 1430 | _global_map_trans_optimized[_robot_this_th].pitch, 1431 | _global_map_trans_optimized[_robot_this_th].yaw); 1432 | _pub_trans_odom2odom.publish(odom2odom); 1433 | 1434 | sendGlobalLoopMessageKDTree(); 1435 | 1436 | } 1437 | }; 1438 | 1439 | int main(int argc, char** argv) 1440 | { 1441 | ros::init(argc, argv, "fushion"); 1442 | MapFusion MapF; 1443 | 1444 | ROS_INFO("\033[1;32m----> Map Fushion Started.\033[0m"); 1445 | 1446 | std::thread publishThread(&MapFusion::publishContextInfoThread, &MapF); 1447 | 1448 | ros::spin(); 1449 | 1450 | publishThread.join(); 1451 | 1452 | return 0; 1453 | } 1454 | -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/COPYRIGHT: -------------------------------------------------------------------------------- 1 | Copyright (C) 2014, Northwestern University 2 | 3 | Portions of this software were developed by Northwestern University. 4 | 5 | Access and use of this software shall impose the following obligations 6 | and understandings on the user. The user is granted the right, without 7 | any fee or cost, to use, copy, modify, alter, enhance and distribute 8 | this software, and any derivative works thereof, and its supporting 9 | documentation for any purpose whatsoever, provided that this entire 10 | notice appears in all copies of the software, derivative works and 11 | supporting documentation. Further, Northwestern University requests 12 | that the user credit Northwestern University in any publications that 13 | result from the use of this software or in any product that includes 14 | this software. The names Northwestern University, however, may not be 15 | used in any advertising or publicity to endorse or promote any products 16 | or commercial entity unless specific written permission is obtained from 17 | Northwestern University. The user also understands that Northwestern 18 | University is not obligated to provide the user with any support, 19 | consulting, training or assistance of any kind with regard to the use, 20 | operation and performance of this software nor to provide the user with 21 | any updates, revisions, new versions or "bug fixes." 22 | 23 | THIS SOFTWARE IS PROVIDED BY NORTHWESTERN UNIVERSITY "AS IS" AND ANY 24 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 26 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL NORTHWESTERN UNIVERSITY BE 27 | LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 28 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 29 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 30 | OR IN CONNECTION WITH THE ACCESS, USE OR PERFORMANCE OF THIS SOFTWARE. 31 | -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/README.md: -------------------------------------------------------------------------------- 1 | You should have received this Fast Max-Clique Finder version along with robust\_multirobot\_map\_merging (https://github.com/lajoiepy/robust\_multirobot\_map\_merging). 2 | See the original Fast Max-Clique Finder library at: http://cucis.ece.northwestern.edu/projects/MAXCLIQUE/ 3 | -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/src/findClique.cpp: -------------------------------------------------------------------------------- 1 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 2 | /* Description: a library for finding the maximum clique of a graph */ 3 | /* */ 4 | /* */ 5 | /* Authors: Bharath Pattabiraman and Md. Mostofa Ali Patwary */ 6 | /* EECS Department, Northwestern University */ 7 | /* email: {bpa342,mpatwary}@eecs.northwestern.edu */ 8 | /* */ 9 | /* Copyright, 2014, Northwestern University */ 10 | /* See COPYRIGHT notice in top-level directory. */ 11 | /* */ 12 | /* Please site the following publication if you use this package: */ 13 | /* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ 14 | /* Wei-keng Liao, and Alok Choudhary. */ 15 | /* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ 16 | /* Applications to Overlapping Community Detection" */ 17 | /* http://arxiv.org/abs/1411.7460 */ 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 19 | 20 | #include "findClique.h" 21 | 22 | namespace FMC { 23 | int pruned1; 24 | int pruned2; 25 | int pruned3; 26 | int pruned5; 27 | 28 | /* Algorithm 2: CLIQUE: Recursive Subroutine of algorithm 1. */ 29 | void maxCliqueHelper( CGraphIO& gio, vector* U, int sizeOfClique, int& maxClq, vector& max_clique_data_inter ) 30 | { 31 | int iPos, index = 0, maxClq_prev; 32 | vector * ptrVertex = gio.GetVerticesPtr(); 33 | vector * ptrEdge = gio.GetEdgesPtr(); 34 | vector U_new; 35 | U_new.reserve(gio.GetVertexCount()); 36 | 37 | if( U->size() == 0 ) 38 | { 39 | if( sizeOfClique > maxClq ) 40 | { 41 | maxClq = sizeOfClique; 42 | max_clique_data_inter.clear(); 43 | } 44 | return; 45 | } 46 | 47 | while( U->size() > 0 ) 48 | { 49 | //Old Pruning 50 | if( sizeOfClique + U->size() <= maxClq ) 51 | return; 52 | 53 | index = U->back(); 54 | U->pop_back(); 55 | 56 | // Loop over neighbrs of v_index. 57 | for(int j = (*ptrVertex)[index]; j < (*ptrVertex)[index + 1]; j++ ) 58 | //Pruning 5 59 | if( getDegree(ptrVertex, (*ptrEdge)[j]) >= maxClq ) 60 | { 61 | // Loop over U. 62 | for(int i = 0; i < U->size(); i++) 63 | { 64 | if( (*ptrEdge)[j] == (*U)[i] ) 65 | U_new.push_back( (*ptrEdge)[j]); 66 | } 67 | } 68 | else 69 | pruned3++; 70 | 71 | maxClq_prev = maxClq; 72 | 73 | maxCliqueHelper( gio, &U_new, sizeOfClique + 1, maxClq, max_clique_data_inter ); 74 | 75 | if(maxClq > maxClq_prev) 76 | max_clique_data_inter.push_back(index); 77 | 78 | U_new.clear(); 79 | } 80 | } 81 | 82 | /* Algorithm 1: MAXCLIQUE: Finds maximum clique of the given graph */ 83 | int maxClique( CGraphIO& gio, int l_bound, vector& max_clique_data ) 84 | { 85 | vector * ptrVertex = gio.GetVerticesPtr(); 86 | vector * ptrEdge = gio.GetEdgesPtr(); 87 | vector U; 88 | U.reserve(gio.GetVertexCount()); 89 | vector max_clique_data_inter; 90 | max_clique_data_inter.reserve(gio.GetVertexCount()); 91 | max_clique_data.reserve(gio.GetVertexCount()); 92 | int maxClq = l_bound; 93 | int prev_maxClq; 94 | 95 | //cout << "Computing Max Clique... with lower bound " << maxClq << endl; 96 | pruned1 = 0; 97 | pruned2 = 0; 98 | pruned3 = 0; 99 | pruned5 = 0; 100 | 101 | //Bit Vector to track if vertex has been considered previously. 102 | int *bitVec = new int[gio.GetVertexCount()]; 103 | memset(bitVec, 0, gio.GetVertexCount() * sizeof(int)); 104 | 105 | for(int i = gio.GetVertexCount()-1; i >= 0; i--) 106 | { 107 | bitVec[i] = 1; 108 | prev_maxClq = maxClq; 109 | 110 | U.clear(); 111 | //Pruning 1 112 | if( getDegree(ptrVertex, i) < maxClq) 113 | { 114 | pruned1++; 115 | continue; 116 | } 117 | 118 | for( int j = (*ptrVertex)[i]; j < (*ptrVertex)[i + 1]; j++ ) 119 | { 120 | //Pruning 2 121 | if(bitVec[(*ptrEdge)[j]] != 1) 122 | { 123 | //Pruning 3 124 | if( getDegree(ptrVertex, (*ptrEdge)[j]) >= maxClq ) 125 | U.push_back((*ptrEdge)[j]); 126 | else 127 | pruned3++; 128 | } 129 | else 130 | pruned2++; 131 | } 132 | 133 | maxCliqueHelper( gio, &U, 1, maxClq, max_clique_data_inter ); 134 | 135 | if(maxClq > prev_maxClq) 136 | { 137 | max_clique_data_inter.push_back(i); 138 | max_clique_data = max_clique_data_inter; 139 | } 140 | max_clique_data_inter.clear(); 141 | } 142 | 143 | delete [] bitVec; 144 | max_clique_data_inter.clear(); 145 | 146 | #ifdef _DEBUG 147 | cout << "Pruning 1 = " << pruned1 << endl; 148 | cout << "Pruning 2 = " << pruned2 << endl; 149 | cout << "Pruning 3 = " << pruned3 << endl; 150 | cout << "Pruning 5 = " << pruned5 << endl; 151 | #endif 152 | 153 | return maxClq; 154 | } 155 | 156 | void print_max_clique(vector& max_clique_data) 157 | { 158 | //cout << "Maximum clique: "; 159 | for(int i = 0; i < max_clique_data.size(); i++) 160 | cout << max_clique_data[i] + 1 << " "; 161 | cout << endl; 162 | } 163 | } -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/src/findClique.h: -------------------------------------------------------------------------------- 1 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 2 | /* Description: a library for finding the maximum clique of a graph */ 3 | /* */ 4 | /* */ 5 | /* Authors: Bharath Pattabiraman and Md. Mostofa Ali Patwary */ 6 | /* EECS Department, Northwestern University */ 7 | /* email: {bpa342,mpatwary}@eecs.northwestern.edu */ 8 | /* */ 9 | /* Copyright, 2014, Northwestern University */ 10 | /* See COPYRIGHT notice in top-level directory. */ 11 | /* */ 12 | /* Please site the following publication if you use this package: */ 13 | /* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ 14 | /* Wei-keng Liao, and Alok Choudhary. */ 15 | /* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ 16 | /* Applications to Overlapping Community Detection" */ 17 | /* http://arxiv.org/abs/1411.7460 */ 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 19 | 20 | #ifndef FINDCLIQUE_H_INCLUDED 21 | #define FINDCLIQUE_H_INCLUDED 22 | 23 | #include "graphIO.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | using namespace std; 32 | 33 | #ifdef _DEBUG 34 | int DEBUG=1; 35 | #endif 36 | 37 | namespace FMC { 38 | 39 | //Function Definitions 40 | bool fexists(const char *filename); 41 | double wtime(); 42 | void usage(char *argv0); 43 | int getDegree(vector* ptrVtx, int idx); 44 | void print_max_clique(vector& max_clique_data); 45 | 46 | int maxClique( CGraphIO& gio, int l_bound, vector& max_clique_data ); 47 | void maxCliqueHelper( CGraphIO& gio, vector* U, int sizeOfClique, int& maxClq, vector& max_clique_data_inter ); 48 | 49 | int maxCliqueHeu( CGraphIO& gio ); 50 | int maxCliqueHeu(CGraphIO& gio, vector& max_clique_data); 51 | void maxCliqueHelperHeu( CGraphIO& gio, vector* U, int sizeOfClique, int& maxClq, vector& max_clique_data_inter ); 52 | 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/src/findCliqueHeu.cpp: -------------------------------------------------------------------------------- 1 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 2 | /* Description: a library for finding the maximum clique from a graph */ 3 | /* */ 4 | /* */ 5 | /* Authors: Md. Mostofa Ali Patwary and Bharath Pattabiraman */ 6 | /* EECS Department, Northwestern University */ 7 | /* email: {mpatwary,bpa342}@eecs.northwestern.edu */ 8 | /* */ 9 | /* Copyright, 2014, Northwestern University */ 10 | /* See COPYRIGHT notice in top-level directory. */ 11 | /* */ 12 | /* Please site the following publication if you use this package: */ 13 | /* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ 14 | /* Wei-keng Liao, and Alok Choudhary. */ 15 | /* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ 16 | /* Applications to Overlapping Community Detection" */ 17 | /* http://arxiv.org/abs/1411.7460 */ 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 19 | 20 | #include "findClique.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace FMC { 27 | int maxDegree, maxClq; 28 | 29 | /* Algorithm 2: MaxCliqueHeu: A heuristic to find maximum clique */ 30 | int maxCliqueHeu(CGraphIO& gio) 31 | { 32 | vector * p_v_i_Vertices = gio.GetVerticesPtr(); 33 | vector * p_v_i_Edges = gio.GetEdgesPtr(); 34 | 35 | //srand(time(NULL)); 36 | 37 | maxDegree = gio.GetMaximumVertexDegree(); 38 | int maxClq = - 1, u, icc; 39 | vector < int > v_i_S; 40 | vector < int > v_i_S1; 41 | v_i_S.resize(maxDegree + 1, 0); 42 | v_i_S1.resize(maxDegree + 1, 0); 43 | 44 | int iPos, iPos1, iCount, iCount1; 45 | 46 | int notComputed = 0; 47 | 48 | // compute the max clique for each vertex 49 | for(int iCandidateVertex = 0; iCandidateVertex < p_v_i_Vertices->size() - 1; iCandidateVertex++) 50 | { 51 | // Pruning 1 52 | if(maxClq > ((*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex])) 53 | { 54 | notComputed++; 55 | continue; 56 | } 57 | 58 | iPos = 0; 59 | v_i_S[iPos++] = iCandidateVertex; 60 | int z, imdv = iCandidateVertex, imd = (*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex]; 61 | 62 | int iLoopCount = (*p_v_i_Vertices)[iCandidateVertex + 1]; 63 | for(int j = (*p_v_i_Vertices)[iCandidateVertex]; j < iLoopCount; j++) 64 | { 65 | // Pruning 3 66 | if(maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[j] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[j]])) 67 | v_i_S[iPos++] = (*p_v_i_Edges)[j]; 68 | } 69 | 70 | icc = 0; 71 | 72 | while(iPos > 0) 73 | { 74 | int imdv1 = -1, imd1 = -1; 75 | 76 | icc++; 77 | 78 | // generate a random number x from 0 to iPos -1 79 | // aasign that imdv = x and imd = d(x) 80 | //imdv = v_i_S[rand() % iPos]; 81 | imdv = v_i_S[iPos-1]; 82 | imd = (*p_v_i_Vertices)[imdv + 1] - (*p_v_i_Vertices)[imdv]; 83 | 84 | iPos1 = 0; 85 | 86 | for(int j = 0; j < iPos; j++) 87 | { 88 | iLoopCount = (*p_v_i_Vertices)[imdv + 1]; 89 | for(int k = (*p_v_i_Vertices)[imdv]; k < iLoopCount; k++) 90 | { 91 | // Pruning 5 92 | if(v_i_S[j] == (*p_v_i_Edges)[k] && maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[k] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[k]])) 93 | { 94 | v_i_S1[iPos1++] = v_i_S[j]; // calculate the max degree vertex here 95 | 96 | break; 97 | } 98 | } 99 | } 100 | 101 | for(int j = 0; j < iPos1; j++) 102 | v_i_S[j] = v_i_S1[j]; 103 | 104 | iPos = iPos1; 105 | 106 | imdv = imdv1; 107 | imd = imd1; 108 | } 109 | 110 | if(maxClq < icc) 111 | maxClq = icc; 112 | 113 | } 114 | 115 | return maxClq; 116 | } 117 | 118 | /* Algorithm 2: MaxCliqueHeu: A heuristic to find maximum clique */ 119 | /* Modified by Josh Mangelson to return possible Max clique */ 120 | int maxCliqueHeu(CGraphIO& gio, vector& max_clique_data) 121 | { 122 | vector * p_v_i_Vertices = gio.GetVerticesPtr(); 123 | vector * p_v_i_Edges = gio.GetEdgesPtr(); 124 | 125 | vector max_clique_data_inter; 126 | max_clique_data_inter.reserve(gio.GetVertexCount()); 127 | max_clique_data.reserve(gio.GetVertexCount()); 128 | 129 | //srand(time(NULL)); 130 | 131 | maxDegree = gio.GetMaximumVertexDegree(); 132 | int maxClq = - 1, u, icc; 133 | vector < int > v_i_S; 134 | vector < int > v_i_S1; 135 | v_i_S.resize(maxDegree + 1, 0); 136 | v_i_S1.resize(maxDegree + 1, 0); 137 | 138 | int iPos, iPos1, iCount, iCount1; 139 | 140 | int notComputed = 0; 141 | 142 | // compute the max clique for each vertex 143 | for(int iCandidateVertex = 0; iCandidateVertex < p_v_i_Vertices->size() - 1; iCandidateVertex++) 144 | { 145 | max_clique_data_inter.clear (); 146 | 147 | // std::cout << iCandidateVertex << " ---------------\n"; 148 | // Pruning 1 149 | if(maxClq > ((*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex])) 150 | { 151 | notComputed++; 152 | continue; 153 | } 154 | 155 | iPos = 0; 156 | v_i_S[iPos++] = iCandidateVertex; 157 | int z, imdv = iCandidateVertex, imd = (*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex]; 158 | 159 | int iLoopCount = (*p_v_i_Vertices)[iCandidateVertex + 1]; 160 | for(int j = (*p_v_i_Vertices)[iCandidateVertex]; j < iLoopCount; j++) 161 | { 162 | // Pruning 3 163 | if(maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[j] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[j]])) 164 | v_i_S[iPos++] = (*p_v_i_Edges)[j]; 165 | } 166 | 167 | // std::cout << iPos << std::endl; 168 | // for(int x = 0; x < v_i_S.size(); x++) 169 | // std::cout << v_i_S[x] << ", "; 170 | // std::cout << std::endl << "------------" << std::endl; 171 | 172 | 173 | icc = 0; 174 | max_clique_data_inter.push_back(iCandidateVertex); 175 | 176 | while(iPos > 0) 177 | { 178 | int imdv1 = -1, imd1 = -1; 179 | 180 | icc++; 181 | 182 | // generate a random number x from 0 to iPos -1 183 | // aasign that imdv = x and imd = d(x) 184 | //imdv = v_i_S[rand() % iPos]; 185 | imdv = v_i_S[iPos-1]; 186 | imd = (*p_v_i_Vertices)[imdv + 1] - (*p_v_i_Vertices)[imdv]; 187 | 188 | iPos1 = 0; 189 | 190 | for(int j = 0; j < iPos; j++) 191 | { 192 | iLoopCount = (*p_v_i_Vertices)[imdv + 1]; 193 | for(int k = (*p_v_i_Vertices)[imdv]; k < iLoopCount; k++) 194 | { 195 | // Pruning 5 196 | if(v_i_S[j] == (*p_v_i_Edges)[k] && maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[k] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[k]])) 197 | { 198 | v_i_S1[iPos1++] = v_i_S[j]; // calculate the max degree vertex here 199 | 200 | break; 201 | } 202 | } 203 | } 204 | 205 | 206 | // for(int x = 0; x < v_i_S.size(); x++) 207 | // std::cout << v_i_S[x] << ", "; 208 | // std::cout << std::endl; 209 | // std::cout << "v_i_S1: "; 210 | // for(int x = 0; x < iPos1; x++) 211 | // std::cout << v_i_S1[x] << ", "; 212 | // std::cout << std::endl; 213 | // std::cout << imdv << ", " << imd << std::endl; 214 | // std::cout << iPos << ", " << iPos1 << std::endl; 215 | 216 | for(int j = 0; j < iPos1; j++) 217 | v_i_S[j] = v_i_S1[j]; 218 | 219 | if (iPos1 != 0) { 220 | // max_clique_data_inter.push_back(iPos); 221 | max_clique_data_inter.push_back(imdv);; 222 | } 223 | 224 | // for(int x=0; x < max_clique_data_inter.size();x++){ 225 | // std::cout << max_clique_data_inter[x] << ", "; 226 | // } 227 | // std::cout << std::endl; 228 | 229 | 230 | iPos = iPos1; 231 | 232 | imdv = imdv1; 233 | imd = imd1; 234 | } 235 | 236 | if(maxClq < icc){ 237 | maxClq = icc; 238 | max_clique_data = max_clique_data_inter; 239 | } 240 | } 241 | 242 | return maxClq; 243 | } 244 | 245 | template 246 | vector sort_indexes(const vector &v) 247 | { 248 | vector idx(v.size()); 249 | iota(idx.begin(), idx.end(), 0); 250 | 251 | sort(idx.begin(), idx.end(), 252 | [&v](size_t i1, size_t i2) {return v[i1] > v[i2];}); 253 | 254 | return idx; 255 | } 256 | 257 | } -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/src/graphIO.cpp: -------------------------------------------------------------------------------- 1 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 2 | /* Description: an I/O library for reading a graph */ 3 | /* */ 4 | /* */ 5 | /* Authors: Md. Mostofa Ali Patwary and Bharath Pattabiraman */ 6 | /* EECS Department, Northwestern University */ 7 | /* email: {mpatwary,bpa342}@eecs.northwestern.edu */ 8 | /* */ 9 | /* Copyright, 2014, Northwestern University */ 10 | /* See COPYRIGHT notice in top-level directory. */ 11 | /* */ 12 | /* Please site the following publication if you use this package: */ 13 | /* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ 14 | /* Wei-keng Liao, and Alok Choudhary. */ 15 | /* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ 16 | /* Applications to Overlapping Community Detection" */ 17 | /* http://arxiv.org/abs/1411.7460 */ 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 19 | 20 | #include "graphIO.h" 21 | 22 | namespace FMC { 23 | CGraphIO::~CGraphIO() 24 | { 25 | m_vi_Vertices.clear(); 26 | m_vi_OrderedVertices.clear(); 27 | m_vi_Edges.clear(); 28 | m_vd_Values.clear(); 29 | } 30 | 31 | bool CGraphIO::readGraph(string s_InputFile, float connStrength) 32 | { 33 | string fileExtension = getFileExtension(s_InputFile); 34 | if(fileExtension == "mtx") 35 | { 36 | // matrix market format 37 | return ReadMatrixMarketAdjacencyGraph(s_InputFile, connStrength); 38 | } 39 | else if(fileExtension == "gr") 40 | { 41 | // gr format 42 | return ReadMeTiSAdjacencyGraph(s_InputFile); 43 | } 44 | else 45 | return false; 46 | } 47 | 48 | bool CGraphIO::ReadMatrixMarketAdjacencyGraph(string s_InputFile, float connStrength) 49 | { 50 | istringstream in2; 51 | string line=""; 52 | map > nodeList; 53 | map > valueList; 54 | int col=0, row=0, rowIndex=0, colIndex=0; 55 | int entry_counter = 0, num_of_entries = 0; 56 | double value; 57 | 58 | ifstream in (s_InputFile.c_str()); 59 | if(!in) 60 | { 61 | cout<0&&line[0]=='%') //ignore comment line 93 | getline(in,line); 94 | 95 | in2.str(line); 96 | in2 >> row >> col >> num_of_entries; 97 | 98 | if(row!=col) 99 | { 100 | cout<<"* WARNING: GraphInputOutput::ReadMatrixMarketAdjacencyGraph()"<> rowIndex >> colIndex >> value; 116 | rowIndex--; 117 | colIndex--; 118 | 119 | if(rowIndex < 0 || rowIndex >= row) 120 | cout << "Something wrong rowIndex " << rowIndex << " row " << row << endl; 121 | 122 | if(colIndex < 0 || colIndex >= col) 123 | cout << "Something wrong colIndex " << colIndex << " col " << col << endl; 124 | 125 | if(rowIndex == colIndex) 126 | { 127 | continue; 128 | } 129 | 130 | // This is to handle directed graphs. If the edge is already present, skip. If not add. 131 | int exists=0; 132 | for(int k=0; k connStrength) 145 | { 146 | nodeList[rowIndex].push_back(colIndex); 147 | nodeList[colIndex].push_back(rowIndex); 148 | } 149 | } 150 | else 151 | { 152 | nodeList[rowIndex].push_back(colIndex); 153 | nodeList[colIndex].push_back(rowIndex); 154 | } 155 | 156 | if(b_getValue && value > connStrength) 157 | { 158 | valueList[rowIndex].push_back(value); 159 | valueList[colIndex].push_back(value); 160 | } 161 | } 162 | } 163 | } 164 | 165 | //cout << "No. of upper triangular pruned: " << num_upper_triangular << endl; 166 | m_vi_Vertices.push_back(m_vi_Edges.size()); 167 | 168 | for(int i=0;i < row; i++) 169 | { 170 | m_vi_Edges.insert(m_vi_Edges.end(),nodeList[i].begin(),nodeList[i].end()); 171 | m_vi_Vertices.push_back(m_vi_Edges.size()); 172 | } 173 | 174 | if(b_getValue) 175 | { 176 | for(int i=0;i i_VertexDegree) 215 | { 216 | m_i_MinimumVertexDegree = i_VertexDegree; 217 | } 218 | } 219 | 220 | m_d_AverageVertexDegree = (double)m_vi_Edges.size()/i_VertexCount; 221 | 222 | return; 223 | } 224 | 225 | string CGraphIO::getFileExtension(string fileName) 226 | { 227 | string::size_type result; 228 | string fileExtension = ""; 229 | 230 | //1. see if the fileName is given in full path 231 | /*result = fileName.rfind("/", fileName.size() - 1); 232 | if(result != string::npos) 233 | { 234 | //found the path (file prefix) 235 | //get the path, including the last DIR_SEPARATOR 236 | path = fileName.substr(0,result+1); 237 | //remove the path from the fileName 238 | fileName = fileName.substr(result+1); 239 | } 240 | */ 241 | 242 | //2. see if the fileName has file extension. For example ".mtx" 243 | result = fileName.rfind('.', fileName.size() - 1); 244 | if(result != string::npos) 245 | { 246 | //found the fileExtension 247 | //get the fileExtension excluding the '.' 248 | fileExtension = fileName.substr(result+1); 249 | //remove the fileExtension from the fileName 250 | //fileName = fileName.substr(0,result); 251 | } 252 | 253 | //3. get the name of the input file 254 | //name = fileName; 255 | 256 | return fileExtension; 257 | } 258 | } -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/src/graphIO.h: -------------------------------------------------------------------------------- 1 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 2 | /* Description: an I/O library for reading a graph */ 3 | /* */ 4 | /* */ 5 | /* Authors: Md. Mostofa Ali Patwary and Bharath Pattabiraman */ 6 | /* EECS Department, Northwestern University */ 7 | /* email: {mpatwary,bpa342}@eecs.northwestern.edu */ 8 | /* */ 9 | /* Copyright, 2014, Northwestern University */ 10 | /* See COPYRIGHT notice in top-level directory. */ 11 | /* */ 12 | /* Please site the following publication if you use this package: */ 13 | /* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ 14 | /* Wei-keng Liao, and Alok Choudhary. */ 15 | /* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ 16 | /* Applications to Overlapping Community Detection" */ 17 | /* http://arxiv.org/abs/1411.7460 */ 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 19 | 20 | #ifndef _graphIO_ 21 | #define _graphIO_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #define LINE_LENGTH 256 32 | 33 | using namespace std; 34 | 35 | namespace FMC { 36 | typedef std::vector IntVector; 37 | 38 | class CGraphIO 39 | { 40 | public: 41 | 42 | CGraphIO(){ } 43 | virtual ~CGraphIO(); 44 | 45 | bool readGraph(string s_InputFile, float connStrength = -DBL_MAX); 46 | string getFileExtension(string fileName); 47 | bool ReadMatrixMarketAdjacencyGraph(string s_InputFile, float connStrength = -DBL_MAX); 48 | bool ReadMeTiSAdjacencyGraph(string s_InputFile); 49 | void CalculateVertexDegrees(); 50 | 51 | int GetVertexCount(){ return m_vi_Vertices.size() - 1; } 52 | int GetEdgeCount(){ return m_vi_Edges.size()/2; } 53 | int GetMaximumVertexDegree(){ return m_i_MaximumVertexDegree; } 54 | int GetMinimumVertexDegree(){ return m_i_MinimumVertexDegree; } 55 | double GetAverageVertexDegree(){ return m_d_AverageVertexDegree; } 56 | string GetInputFile(){ return m_s_InputFile; } 57 | 58 | vector * GetVerticesPtr(){ return &m_vi_Vertices; } 59 | vector * GetEdgesPtr(){ return &m_vi_Edges; } 60 | 61 | public: 62 | int m_i_MaximumVertexDegree; 63 | int m_i_MinimumVertexDegree; 64 | double m_d_AverageVertexDegree; 65 | 66 | string m_s_InputFile; 67 | 68 | vector m_vi_Vertices; 69 | vector m_vi_OrderedVertices; 70 | vector m_vi_Edges; 71 | vector m_vd_Values; 72 | }; 73 | } 74 | #endif 75 | -------------------------------------------------------------------------------- /third_parties/fast_max-clique_finder/src/utils.cpp: -------------------------------------------------------------------------------- 1 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 2 | /* Description: a library for finding the maximum clique of a graph */ 3 | /* */ 4 | /* */ 5 | /* Authors: Bharath Pattabiraman and Md. Mostofa Ali Patwary */ 6 | /* EECS Department, Northwestern University */ 7 | /* email: {bpa342,mpatwary}@eecs.northwestern.edu */ 8 | /* */ 9 | /* Copyright, 2014, Northwestern University */ 10 | /* See COPYRIGHT notice in top-level directory. */ 11 | /* */ 12 | /* Please site the following publication if you use this package: */ 13 | /* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ 14 | /* Wei-keng Liao, and Alok Choudhary. */ 15 | /* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ 16 | /* Applications to Overlapping Community Detection" */ 17 | /* http://arxiv.org/abs/1411.7460 */ 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 19 | 20 | #include "findClique.h" 21 | 22 | namespace FMC { 23 | bool fexists(const char *filename) 24 | { 25 | ifstream ifile(filename); 26 | return ifile.is_open(); 27 | } 28 | 29 | double wtime() // returns wall time in seconds 30 | { 31 | timeval tv; 32 | gettimeofday(&tv, NULL); 33 | return (double)tv.tv_sec + (double)tv.tv_usec/1000000; 34 | } 35 | 36 | void usage(char *argv0) 37 | { 38 | const char *params = 39 | "Usage: %s [options...] inputfile\n" 40 | "OPTIONS:\n" 41 | "\t-t algorithm type\t: 0 for exact, 1 for heuristic(default)\n" 42 | "\t-l input lower bound\t: intial max clique size e.g. default 0\n" 43 | "\t-p print clique\t\t: This parameter if want to print the max clique (only for exact algorithm)\n"; 44 | fprintf(stderr, params, argv0); 45 | exit(-1); 46 | } 47 | 48 | int getDegree(vector* ptrVtx, int idx) 49 | { 50 | return ( (*ptrVtx)[idx+1] - (*ptrVtx)[idx] ); 51 | } 52 | } -------------------------------------------------------------------------------- /third_parties/nanoflann.hpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 5 | * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 6 | * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). 7 | * All rights reserved. 8 | * 9 | * THE BSD LICENSE 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions 13 | * are met: 14 | * 15 | * 1. Redistributions of source code must retain the above copyright 16 | * notice, this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright 18 | * notice, this list of conditions and the following disclaimer in the 19 | * documentation and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 22 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 23 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 24 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 26 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 27 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 28 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *************************************************************************/ 32 | 33 | /** \mainpage nanoflann C++ API documentation 34 | * nanoflann is a C++ header-only library for building KD-Trees, mostly 35 | * optimized for 2D or 3D point clouds. 36 | * 37 | * nanoflann does not require compiling or installing, just an 38 | * #include in your code. 39 | * 40 | * See: 41 | * - C++ API organized by modules 42 | * - Online README 43 | * - Doxygen 44 | * documentation 45 | */ 46 | 47 | #ifndef NANOFLANN_HPP_ 48 | #define NANOFLANN_HPP_ 49 | 50 | #include 51 | #include 52 | #include 53 | #include // for abs() 54 | #include // for fwrite() 55 | #include // for abs() 56 | #include 57 | #include // std::reference_wrapper 58 | #include 59 | #include 60 | 61 | /** Library version: 0xMmP (M=Major,m=minor,P=patch) */ 62 | #define NANOFLANN_VERSION 0x130 63 | 64 | // Avoid conflicting declaration of min/max macros in windows headers 65 | #if !defined(NOMINMAX) && \ 66 | (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) 67 | #define NOMINMAX 68 | #ifdef max 69 | #undef max 70 | #undef min 71 | #endif 72 | #endif 73 | 74 | namespace nanoflann { 75 | /** @addtogroup nanoflann_grp nanoflann C++ library for ANN 76 | * @{ */ 77 | 78 | /** the PI constant (required to avoid MSVC missing symbols) */ 79 | template T pi_const() { 80 | return static_cast(3.14159265358979323846); 81 | } 82 | 83 | /** 84 | * Traits if object is resizable and assignable (typically has a resize | assign 85 | * method) 86 | */ 87 | template struct has_resize : std::false_type {}; 88 | 89 | template 90 | struct has_resize().resize(1), 0)> 91 | : std::true_type {}; 92 | 93 | template struct has_assign : std::false_type {}; 94 | 95 | template 96 | struct has_assign().assign(1, 0), 0)> 97 | : std::true_type {}; 98 | 99 | /** 100 | * Free function to resize a resizable object 101 | */ 102 | template 103 | inline typename std::enable_if::value, void>::type 104 | resize(Container &c, const size_t nElements) { 105 | c.resize(nElements); 106 | } 107 | 108 | /** 109 | * Free function that has no effects on non resizable containers (e.g. 110 | * std::array) It raises an exception if the expected size does not match 111 | */ 112 | template 113 | inline typename std::enable_if::value, void>::type 114 | resize(Container &c, const size_t nElements) { 115 | if (nElements != c.size()) 116 | throw std::logic_error("Try to change the size of a std::array."); 117 | } 118 | 119 | /** 120 | * Free function to assign to a container 121 | */ 122 | template 123 | inline typename std::enable_if::value, void>::type 124 | assign(Container &c, const size_t nElements, const T &value) { 125 | c.assign(nElements, value); 126 | } 127 | 128 | /** 129 | * Free function to assign to a std::array 130 | */ 131 | template 132 | inline typename std::enable_if::value, void>::type 133 | assign(Container &c, const size_t nElements, const T &value) { 134 | for (size_t i = 0; i < nElements; i++) 135 | c[i] = value; 136 | } 137 | 138 | /** @addtogroup result_sets_grp Result set classes 139 | * @{ */ 140 | template 142 | class KNNResultSet { 143 | public: 144 | typedef _DistanceType DistanceType; 145 | typedef _IndexType IndexType; 146 | typedef _CountType CountType; 147 | 148 | private: 149 | IndexType *indices; 150 | DistanceType *dists; 151 | CountType capacity; 152 | CountType count; 153 | 154 | public: 155 | inline KNNResultSet(CountType capacity_) 156 | : indices(0), dists(0), capacity(capacity_), count(0) {} 157 | 158 | inline void init(IndexType *indices_, DistanceType *dists_) { 159 | indices = indices_; 160 | dists = dists_; 161 | count = 0; 162 | if (capacity) 163 | dists[capacity - 1] = (std::numeric_limits::max)(); 164 | } 165 | 166 | inline CountType size() const { return count; } 167 | 168 | inline bool full() const { return count == capacity; } 169 | 170 | /** 171 | * Called during search to add an element matching the criteria. 172 | * @return true if the search should be continued, false if the results are 173 | * sufficient 174 | */ 175 | inline bool addPoint(DistanceType dist, IndexType index) { 176 | CountType i; 177 | for (i = count; i > 0; --i) { 178 | #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same \ 179 | // distance, the one with the lowest-index will be \ 180 | // returned first. 181 | if ((dists[i - 1] > dist) || 182 | ((dist == dists[i - 1]) && (indices[i - 1] > index))) { 183 | #else 184 | if (dists[i - 1] > dist) { 185 | #endif 186 | if (i < capacity) { 187 | dists[i] = dists[i - 1]; 188 | indices[i] = indices[i - 1]; 189 | } 190 | } else 191 | break; 192 | } 193 | if (i < capacity) { 194 | dists[i] = dist; 195 | indices[i] = index; 196 | } 197 | if (count < capacity) 198 | count++; 199 | 200 | // tell caller that the search shall continue 201 | return true; 202 | } 203 | 204 | inline DistanceType worstDist() const { return dists[capacity - 1]; } 205 | }; 206 | 207 | /** operator "<" for std::sort() */ 208 | struct IndexDist_Sorter { 209 | /** PairType will be typically: std::pair */ 210 | template 211 | inline bool operator()(const PairType &p1, const PairType &p2) const { 212 | return p1.second < p2.second; 213 | } 214 | }; 215 | 216 | /** 217 | * A result-set class used when performing a radius based search. 218 | */ 219 | template 220 | class RadiusResultSet { 221 | public: 222 | typedef _DistanceType DistanceType; 223 | typedef _IndexType IndexType; 224 | 225 | public: 226 | const DistanceType radius; 227 | 228 | std::vector> &m_indices_dists; 229 | 230 | inline RadiusResultSet( 231 | DistanceType radius_, 232 | std::vector> &indices_dists) 233 | : radius(radius_), m_indices_dists(indices_dists) { 234 | init(); 235 | } 236 | 237 | inline void init() { clear(); } 238 | inline void clear() { m_indices_dists.clear(); } 239 | 240 | inline size_t size() const { return m_indices_dists.size(); } 241 | 242 | inline bool full() const { return true; } 243 | 244 | /** 245 | * Called during search to add an element matching the criteria. 246 | * @return true if the search should be continued, false if the results are 247 | * sufficient 248 | */ 249 | inline bool addPoint(DistanceType dist, IndexType index) { 250 | if (dist < radius) 251 | m_indices_dists.push_back(std::make_pair(index, dist)); 252 | return true; 253 | } 254 | 255 | inline DistanceType worstDist() const { return radius; } 256 | 257 | /** 258 | * Find the worst result (furtherest neighbor) without copying or sorting 259 | * Pre-conditions: size() > 0 260 | */ 261 | std::pair worst_item() const { 262 | if (m_indices_dists.empty()) 263 | throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " 264 | "an empty list of results."); 265 | typedef 266 | typename std::vector>::const_iterator 267 | DistIt; 268 | DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), 269 | IndexDist_Sorter()); 270 | return *it; 271 | } 272 | }; 273 | 274 | /** @} */ 275 | 276 | /** @addtogroup loadsave_grp Load/save auxiliary functions 277 | * @{ */ 278 | template 279 | void save_value(FILE *stream, const T &value, size_t count = 1) { 280 | fwrite(&value, sizeof(value), count, stream); 281 | } 282 | 283 | template 284 | void save_value(FILE *stream, const std::vector &value) { 285 | size_t size = value.size(); 286 | fwrite(&size, sizeof(size_t), 1, stream); 287 | fwrite(&value[0], sizeof(T), size, stream); 288 | } 289 | 290 | template 291 | void load_value(FILE *stream, T &value, size_t count = 1) { 292 | size_t read_cnt = fread(&value, sizeof(value), count, stream); 293 | if (read_cnt != count) { 294 | throw std::runtime_error("Cannot read from file"); 295 | } 296 | } 297 | 298 | template void load_value(FILE *stream, std::vector &value) { 299 | size_t size; 300 | size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); 301 | if (read_cnt != 1) { 302 | throw std::runtime_error("Cannot read from file"); 303 | } 304 | value.resize(size); 305 | read_cnt = fread(&value[0], sizeof(T), size, stream); 306 | if (read_cnt != size) { 307 | throw std::runtime_error("Cannot read from file"); 308 | } 309 | } 310 | /** @} */ 311 | 312 | /** @addtogroup metric_grp Metric (distance) classes 313 | * @{ */ 314 | 315 | struct Metric {}; 316 | 317 | /** Manhattan distance functor (generic version, optimized for 318 | * high-dimensionality data sets). Corresponding distance traits: 319 | * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, 320 | * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) 321 | * (e.g. float, double, int64_t) 322 | */ 323 | template 324 | struct L1_Adaptor { 325 | typedef T ElementType; 326 | typedef _DistanceType DistanceType; 327 | 328 | const DataSource &data_source; 329 | 330 | L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} 331 | 332 | inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, 333 | DistanceType worst_dist = -1) const { 334 | DistanceType result = DistanceType(); 335 | const T *last = a + size; 336 | const T *lastgroup = last - 3; 337 | size_t d = 0; 338 | 339 | /* Process 4 items with each loop for efficiency. */ 340 | while (a < lastgroup) { 341 | const DistanceType diff0 = 342 | std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); 343 | const DistanceType diff1 = 344 | std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); 345 | const DistanceType diff2 = 346 | std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); 347 | const DistanceType diff3 = 348 | std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); 349 | result += diff0 + diff1 + diff2 + diff3; 350 | a += 4; 351 | if ((worst_dist > 0) && (result > worst_dist)) { 352 | return result; 353 | } 354 | } 355 | /* Process last 0-3 components. Not needed for standard vector lengths. */ 356 | while (a < last) { 357 | result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); 358 | } 359 | return result; 360 | } 361 | 362 | template 363 | inline DistanceType accum_dist(const U a, const V b, const size_t) const { 364 | return std::abs(a - b); 365 | } 366 | }; 367 | 368 | /** Squared Euclidean distance functor (generic version, optimized for 369 | * high-dimensionality data sets). Corresponding distance traits: 370 | * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, 371 | * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) 372 | * (e.g. float, double, int64_t) 373 | */ 374 | template 375 | struct L2_Adaptor { 376 | typedef T ElementType; 377 | typedef _DistanceType DistanceType; 378 | 379 | const DataSource &data_source; 380 | 381 | L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} 382 | 383 | inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, 384 | DistanceType worst_dist = -1) const { 385 | DistanceType result = DistanceType(); 386 | const T *last = a + size; 387 | const T *lastgroup = last - 3; 388 | size_t d = 0; 389 | 390 | /* Process 4 items with each loop for efficiency. */ 391 | while (a < lastgroup) { 392 | const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); 393 | const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); 394 | const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); 395 | const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); 396 | result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; 397 | a += 4; 398 | if ((worst_dist > 0) && (result > worst_dist)) { 399 | return result; 400 | } 401 | } 402 | /* Process last 0-3 components. Not needed for standard vector lengths. */ 403 | while (a < last) { 404 | const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); 405 | result += diff0 * diff0; 406 | } 407 | return result; 408 | } 409 | 410 | template 411 | inline DistanceType accum_dist(const U a, const V b, const size_t) const { 412 | return (a - b) * (a - b); 413 | } 414 | }; 415 | 416 | /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality 417 | * datasets, like 2D or 3D point clouds) Corresponding distance traits: 418 | * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, 419 | * float, uint8_t) \tparam _DistanceType Type of distance variables (must be 420 | * signed) (e.g. float, double, int64_t) 421 | */ 422 | template 423 | struct L2_Simple_Adaptor { 424 | typedef T ElementType; 425 | typedef _DistanceType DistanceType; 426 | 427 | const DataSource &data_source; 428 | 429 | L2_Simple_Adaptor(const DataSource &_data_source) 430 | : data_source(_data_source) {} 431 | 432 | inline DistanceType evalMetric(const T *a, const size_t b_idx, 433 | size_t size) const { 434 | DistanceType result = DistanceType(); 435 | for (size_t i = 0; i < size; ++i) { 436 | const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); 437 | result += diff * diff; 438 | } 439 | return result; 440 | } 441 | 442 | template 443 | inline DistanceType accum_dist(const U a, const V b, const size_t) const { 444 | return (a - b) * (a - b); 445 | } 446 | }; 447 | 448 | /** SO2 distance functor 449 | * Corresponding distance traits: nanoflann::metric_SO2 450 | * \tparam T Type of the elements (e.g. double, float) 451 | * \tparam _DistanceType Type of distance variables (must be signed) (e.g. 452 | * float, double) orientation is constrained to be in [-pi, pi] 453 | */ 454 | template 455 | struct SO2_Adaptor { 456 | typedef T ElementType; 457 | typedef _DistanceType DistanceType; 458 | 459 | const DataSource &data_source; 460 | 461 | SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} 462 | 463 | inline DistanceType evalMetric(const T *a, const size_t b_idx, 464 | size_t size) const { 465 | return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), 466 | size - 1); 467 | } 468 | 469 | /** Note: this assumes that input angles are already in the range [-pi,pi] */ 470 | template 471 | inline DistanceType accum_dist(const U a, const V b, const size_t) const { 472 | DistanceType result = DistanceType(), PI = pi_const(); 473 | result = b - a; 474 | if (result > PI) 475 | result -= 2 * PI; 476 | else if (result < -PI) 477 | result += 2 * PI; 478 | return result; 479 | } 480 | }; 481 | 482 | /** SO3 distance functor (Uses L2_Simple) 483 | * Corresponding distance traits: nanoflann::metric_SO3 484 | * \tparam T Type of the elements (e.g. double, float) 485 | * \tparam _DistanceType Type of distance variables (must be signed) (e.g. 486 | * float, double) 487 | */ 488 | template 489 | struct SO3_Adaptor { 490 | typedef T ElementType; 491 | typedef _DistanceType DistanceType; 492 | 493 | L2_Simple_Adaptor distance_L2_Simple; 494 | 495 | SO3_Adaptor(const DataSource &_data_source) 496 | : distance_L2_Simple(_data_source) {} 497 | 498 | inline DistanceType evalMetric(const T *a, const size_t b_idx, 499 | size_t size) const { 500 | return distance_L2_Simple.evalMetric(a, b_idx, size); 501 | } 502 | 503 | template 504 | inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { 505 | return distance_L2_Simple.accum_dist(a, b, idx); 506 | } 507 | }; 508 | 509 | /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ 510 | struct metric_L1 : public Metric { 511 | template struct traits { 512 | typedef L1_Adaptor distance_t; 513 | }; 514 | }; 515 | /** Metaprogramming helper traits class for the L2 (Euclidean) metric */ 516 | struct metric_L2 : public Metric { 517 | template struct traits { 518 | typedef L2_Adaptor distance_t; 519 | }; 520 | }; 521 | /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ 522 | struct metric_L2_Simple : public Metric { 523 | template struct traits { 524 | typedef L2_Simple_Adaptor distance_t; 525 | }; 526 | }; 527 | /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ 528 | struct metric_SO2 : public Metric { 529 | template struct traits { 530 | typedef SO2_Adaptor distance_t; 531 | }; 532 | }; 533 | /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ 534 | struct metric_SO3 : public Metric { 535 | template struct traits { 536 | typedef SO3_Adaptor distance_t; 537 | }; 538 | }; 539 | 540 | /** @} */ 541 | 542 | /** @addtogroup param_grp Parameter structs 543 | * @{ */ 544 | 545 | /** Parameters (see README.md) */ 546 | struct KDTreeSingleIndexAdaptorParams { 547 | KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) 548 | : leaf_max_size(_leaf_max_size) {} 549 | 550 | size_t leaf_max_size; 551 | }; 552 | 553 | /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ 554 | struct SearchParams { 555 | /** Note: The first argument (checks_IGNORED_) is ignored, but kept for 556 | * compatibility with the FLANN interface */ 557 | SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) 558 | : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} 559 | 560 | int checks; //!< Ignored parameter (Kept for compatibility with the FLANN 561 | //!< interface). 562 | float eps; //!< search for eps-approximate neighbours (default: 0) 563 | bool sorted; //!< only for radius search, require neighbours sorted by 564 | //!< distance (default: true) 565 | }; 566 | /** @} */ 567 | 568 | /** @addtogroup memalloc_grp Memory allocation 569 | * @{ */ 570 | 571 | /** 572 | * Allocates (using C's malloc) a generic type T. 573 | * 574 | * Params: 575 | * count = number of instances to allocate. 576 | * Returns: pointer (of type T*) to memory buffer 577 | */ 578 | template inline T *allocate(size_t count = 1) { 579 | T *mem = static_cast(::malloc(sizeof(T) * count)); 580 | return mem; 581 | } 582 | 583 | /** 584 | * Pooled storage allocator 585 | * 586 | * The following routines allow for the efficient allocation of storage in 587 | * small chunks from a specified pool. Rather than allowing each structure 588 | * to be freed individually, an entire pool of storage is freed at once. 589 | * This method has two advantages over just using malloc() and free(). First, 590 | * it is far more efficient for allocating small objects, as there is 591 | * no overhead for remembering all the information needed to free each 592 | * object or consolidating fragmented memory. Second, the decision about 593 | * how long to keep an object is made at the time of allocation, and there 594 | * is no need to track down all the objects to free them. 595 | * 596 | */ 597 | 598 | const size_t WORDSIZE = 16; 599 | const size_t BLOCKSIZE = 8192; 600 | 601 | class PooledAllocator { 602 | /* We maintain memory alignment to word boundaries by requiring that all 603 | allocations be in multiples of the machine wordsize. */ 604 | /* Size of machine word in bytes. Must be power of 2. */ 605 | /* Minimum number of bytes requested at a time from the system. Must be 606 | * multiple of WORDSIZE. */ 607 | 608 | size_t remaining; /* Number of bytes left in current block of storage. */ 609 | void *base; /* Pointer to base of current block of storage. */ 610 | void *loc; /* Current location in block to next allocate memory. */ 611 | 612 | void internal_init() { 613 | remaining = 0; 614 | base = NULL; 615 | usedMemory = 0; 616 | wastedMemory = 0; 617 | } 618 | 619 | public: 620 | size_t usedMemory; 621 | size_t wastedMemory; 622 | 623 | /** 624 | Default constructor. Initializes a new pool. 625 | */ 626 | PooledAllocator() { internal_init(); } 627 | 628 | /** 629 | * Destructor. Frees all the memory allocated in this pool. 630 | */ 631 | ~PooledAllocator() { free_all(); } 632 | 633 | /** Frees all allocated memory chunks */ 634 | void free_all() { 635 | while (base != NULL) { 636 | void *prev = 637 | *(static_cast(base)); /* Get pointer to prev block. */ 638 | ::free(base); 639 | base = prev; 640 | } 641 | internal_init(); 642 | } 643 | 644 | /** 645 | * Returns a pointer to a piece of new memory of the given size in bytes 646 | * allocated from the pool. 647 | */ 648 | void *malloc(const size_t req_size) { 649 | /* Round size up to a multiple of wordsize. The following expression 650 | only works for WORDSIZE that is a power of 2, by masking last bits of 651 | incremented size to zero. 652 | */ 653 | const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); 654 | 655 | /* Check whether a new block must be allocated. Note that the first word 656 | of a block is reserved for a pointer to the previous block. 657 | */ 658 | if (size > remaining) { 659 | 660 | wastedMemory += remaining; 661 | 662 | /* Allocate new storage. */ 663 | const size_t blocksize = 664 | (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE) 665 | ? size + sizeof(void *) + (WORDSIZE - 1) 666 | : BLOCKSIZE; 667 | 668 | // use the standard C malloc to allocate memory 669 | void *m = ::malloc(blocksize); 670 | if (!m) { 671 | fprintf(stderr, "Failed to allocate memory.\n"); 672 | return NULL; 673 | } 674 | 675 | /* Fill first word of new block with pointer to previous block. */ 676 | static_cast(m)[0] = base; 677 | base = m; 678 | 679 | size_t shift = 0; 680 | // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & 681 | // (WORDSIZE-1))) & (WORDSIZE-1); 682 | 683 | remaining = blocksize - sizeof(void *) - shift; 684 | loc = (static_cast(m) + sizeof(void *) + shift); 685 | } 686 | void *rloc = loc; 687 | loc = static_cast(loc) + size; 688 | remaining -= size; 689 | 690 | usedMemory += size; 691 | 692 | return rloc; 693 | } 694 | 695 | /** 696 | * Allocates (using this pool) a generic type T. 697 | * 698 | * Params: 699 | * count = number of instances to allocate. 700 | * Returns: pointer (of type T*) to memory buffer 701 | */ 702 | template T *allocate(const size_t count = 1) { 703 | T *mem = static_cast(this->malloc(sizeof(T) * count)); 704 | return mem; 705 | } 706 | }; 707 | /** @} */ 708 | 709 | /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff 710 | * @{ */ 711 | 712 | /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors 713 | * when DIM=-1. Fixed size version for a generic DIM: 714 | */ 715 | template struct array_or_vector_selector { 716 | typedef std::array container_t; 717 | }; 718 | /** Dynamic size version */ 719 | template struct array_or_vector_selector<-1, T> { 720 | typedef std::vector container_t; 721 | }; 722 | 723 | /** @} */ 724 | 725 | /** kd-tree base-class 726 | * 727 | * Contains the member functions common to the classes KDTreeSingleIndexAdaptor 728 | * and KDTreeSingleIndexDynamicAdaptor_. 729 | * 730 | * \tparam Derived The name of the class which inherits this class. 731 | * \tparam DatasetAdaptor The user-provided adaptor (see comments above). 732 | * \tparam Distance The distance metric to use, these are all classes derived 733 | * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for 734 | * 3D points) \tparam IndexType Will be typically size_t or int 735 | */ 736 | 737 | template 739 | class KDTreeBaseClass { 740 | 741 | public: 742 | /** Frees the previously-built index. Automatically called within 743 | * buildIndex(). */ 744 | void freeIndex(Derived &obj) { 745 | obj.pool.free_all(); 746 | obj.root_node = NULL; 747 | obj.m_size_at_index_build = 0; 748 | } 749 | 750 | typedef typename Distance::ElementType ElementType; 751 | typedef typename Distance::DistanceType DistanceType; 752 | 753 | /*--------------------- Internal Data Structures --------------------------*/ 754 | struct Node { 755 | /** Union used because a node can be either a LEAF node or a non-leaf node, 756 | * so both data fields are never used simultaneously */ 757 | union { 758 | struct leaf { 759 | IndexType left, right; //!< Indices of points in leaf node 760 | } lr; 761 | struct nonleaf { 762 | int divfeat; //!< Dimension used for subdivision. 763 | DistanceType divlow, divhigh; //!< The values used for subdivision. 764 | } sub; 765 | } node_type; 766 | Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) 767 | }; 768 | 769 | typedef Node *NodePtr; 770 | 771 | struct Interval { 772 | ElementType low, high; 773 | }; 774 | 775 | /** 776 | * Array of indices to vectors in the dataset. 777 | */ 778 | std::vector vind; 779 | 780 | NodePtr root_node; 781 | 782 | size_t m_leaf_max_size; 783 | 784 | size_t m_size; //!< Number of current points in the dataset 785 | size_t m_size_at_index_build; //!< Number of points in the dataset when the 786 | //!< index was built 787 | int dim; //!< Dimensionality of each data point 788 | 789 | /** Define "BoundingBox" as a fixed-size or variable-size container depending 790 | * on "DIM" */ 791 | typedef 792 | typename array_or_vector_selector::container_t BoundingBox; 793 | 794 | /** Define "distance_vector_t" as a fixed-size or variable-size container 795 | * depending on "DIM" */ 796 | typedef typename array_or_vector_selector::container_t 797 | distance_vector_t; 798 | 799 | /** The KD-tree used to find neighbours */ 800 | 801 | BoundingBox root_bbox; 802 | 803 | /** 804 | * Pooled memory allocator. 805 | * 806 | * Using a pooled memory allocator is more efficient 807 | * than allocating memory directly when there is a large 808 | * number small of memory allocations. 809 | */ 810 | PooledAllocator pool; 811 | 812 | /** Returns number of points in dataset */ 813 | size_t size(const Derived &obj) const { return obj.m_size; } 814 | 815 | /** Returns the length of each point in the dataset */ 816 | size_t veclen(const Derived &obj) { 817 | return static_cast(DIM > 0 ? DIM : obj.dim); 818 | } 819 | 820 | /// Helper accessor to the dataset points: 821 | inline ElementType dataset_get(const Derived &obj, size_t idx, 822 | int component) const { 823 | return obj.dataset.kdtree_get_pt(idx, component); 824 | } 825 | 826 | /** 827 | * Computes the inde memory usage 828 | * Returns: memory used by the index 829 | */ 830 | size_t usedMemory(Derived &obj) { 831 | return obj.pool.usedMemory + obj.pool.wastedMemory + 832 | obj.dataset.kdtree_get_point_count() * 833 | sizeof(IndexType); // pool memory and vind array memory 834 | } 835 | 836 | void computeMinMax(const Derived &obj, IndexType *ind, IndexType count, 837 | int element, ElementType &min_elem, 838 | ElementType &max_elem) { 839 | min_elem = dataset_get(obj, ind[0], element); 840 | max_elem = dataset_get(obj, ind[0], element); 841 | for (IndexType i = 1; i < count; ++i) { 842 | ElementType val = dataset_get(obj, ind[i], element); 843 | if (val < min_elem) 844 | min_elem = val; 845 | if (val > max_elem) 846 | max_elem = val; 847 | } 848 | } 849 | 850 | /** 851 | * Create a tree node that subdivides the list of vecs from vind[first] 852 | * to vind[last]. The routine is called recursively on each sublist. 853 | * 854 | * @param left index of the first vector 855 | * @param right index of the last vector 856 | */ 857 | NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right, 858 | BoundingBox &bbox) { 859 | NodePtr node = obj.pool.template allocate(); // allocate memory 860 | 861 | /* If too few exemplars remain, then make this a leaf node. */ 862 | if ((right - left) <= static_cast(obj.m_leaf_max_size)) { 863 | node->child1 = node->child2 = NULL; /* Mark as leaf node. */ 864 | node->node_type.lr.left = left; 865 | node->node_type.lr.right = right; 866 | 867 | // compute bounding-box of leaf points 868 | for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { 869 | bbox[i].low = dataset_get(obj, obj.vind[left], i); 870 | bbox[i].high = dataset_get(obj, obj.vind[left], i); 871 | } 872 | for (IndexType k = left + 1; k < right; ++k) { 873 | for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { 874 | if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) 875 | bbox[i].low = dataset_get(obj, obj.vind[k], i); 876 | if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) 877 | bbox[i].high = dataset_get(obj, obj.vind[k], i); 878 | } 879 | } 880 | } else { 881 | IndexType idx; 882 | int cutfeat; 883 | DistanceType cutval; 884 | middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, 885 | bbox); 886 | 887 | node->node_type.sub.divfeat = cutfeat; 888 | 889 | BoundingBox left_bbox(bbox); 890 | left_bbox[cutfeat].high = cutval; 891 | node->child1 = divideTree(obj, left, left + idx, left_bbox); 892 | 893 | BoundingBox right_bbox(bbox); 894 | right_bbox[cutfeat].low = cutval; 895 | node->child2 = divideTree(obj, left + idx, right, right_bbox); 896 | 897 | node->node_type.sub.divlow = left_bbox[cutfeat].high; 898 | node->node_type.sub.divhigh = right_bbox[cutfeat].low; 899 | 900 | for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { 901 | bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); 902 | bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); 903 | } 904 | } 905 | 906 | return node; 907 | } 908 | 909 | void middleSplit_(Derived &obj, IndexType *ind, IndexType count, 910 | IndexType &index, int &cutfeat, DistanceType &cutval, 911 | const BoundingBox &bbox) { 912 | const DistanceType EPS = static_cast(0.00001); 913 | ElementType max_span = bbox[0].high - bbox[0].low; 914 | for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { 915 | ElementType span = bbox[i].high - bbox[i].low; 916 | if (span > max_span) { 917 | max_span = span; 918 | } 919 | } 920 | ElementType max_spread = -1; 921 | cutfeat = 0; 922 | for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { 923 | ElementType span = bbox[i].high - bbox[i].low; 924 | if (span > (1 - EPS) * max_span) { 925 | ElementType min_elem, max_elem; 926 | computeMinMax(obj, ind, count, i, min_elem, max_elem); 927 | ElementType spread = max_elem - min_elem; 928 | ; 929 | if (spread > max_spread) { 930 | cutfeat = i; 931 | max_spread = spread; 932 | } 933 | } 934 | } 935 | // split in the middle 936 | DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; 937 | ElementType min_elem, max_elem; 938 | computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); 939 | 940 | if (split_val < min_elem) 941 | cutval = min_elem; 942 | else if (split_val > max_elem) 943 | cutval = max_elem; 944 | else 945 | cutval = split_val; 946 | 947 | IndexType lim1, lim2; 948 | planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); 949 | 950 | if (lim1 > count / 2) 951 | index = lim1; 952 | else if (lim2 < count / 2) 953 | index = lim2; 954 | else 955 | index = count / 2; 956 | } 957 | 958 | /** 959 | * Subdivide the list of points by a plane perpendicular on axe corresponding 960 | * to the 'cutfeat' dimension at 'cutval' position. 961 | * 962 | * On return: 963 | * dataset[ind[0..lim1-1]][cutfeat]cutval 966 | */ 967 | void planeSplit(Derived &obj, IndexType *ind, const IndexType count, 968 | int cutfeat, DistanceType &cutval, IndexType &lim1, 969 | IndexType &lim2) { 970 | /* Move vector indices for left subtree to front of list. */ 971 | IndexType left = 0; 972 | IndexType right = count - 1; 973 | for (;;) { 974 | while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) 975 | ++left; 976 | while (right && left <= right && 977 | dataset_get(obj, ind[right], cutfeat) >= cutval) 978 | --right; 979 | if (left > right || !right) 980 | break; // "!right" was added to support unsigned Index types 981 | std::swap(ind[left], ind[right]); 982 | ++left; 983 | --right; 984 | } 985 | /* If either list is empty, it means that all remaining features 986 | * are identical. Split in the middle to maintain a balanced tree. 987 | */ 988 | lim1 = left; 989 | right = count - 1; 990 | for (;;) { 991 | while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) 992 | ++left; 993 | while (right && left <= right && 994 | dataset_get(obj, ind[right], cutfeat) > cutval) 995 | --right; 996 | if (left > right || !right) 997 | break; // "!right" was added to support unsigned Index types 998 | std::swap(ind[left], ind[right]); 999 | ++left; 1000 | --right; 1001 | } 1002 | lim2 = left; 1003 | } 1004 | 1005 | DistanceType computeInitialDistances(const Derived &obj, 1006 | const ElementType *vec, 1007 | distance_vector_t &dists) const { 1008 | assert(vec); 1009 | DistanceType distsq = DistanceType(); 1010 | 1011 | for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { 1012 | if (vec[i] < obj.root_bbox[i].low) { 1013 | dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); 1014 | distsq += dists[i]; 1015 | } 1016 | if (vec[i] > obj.root_bbox[i].high) { 1017 | dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); 1018 | distsq += dists[i]; 1019 | } 1020 | } 1021 | return distsq; 1022 | } 1023 | 1024 | void save_tree(Derived &obj, FILE *stream, NodePtr tree) { 1025 | save_value(stream, *tree); 1026 | if (tree->child1 != NULL) { 1027 | save_tree(obj, stream, tree->child1); 1028 | } 1029 | if (tree->child2 != NULL) { 1030 | save_tree(obj, stream, tree->child2); 1031 | } 1032 | } 1033 | 1034 | void load_tree(Derived &obj, FILE *stream, NodePtr &tree) { 1035 | tree = obj.pool.template allocate(); 1036 | load_value(stream, *tree); 1037 | if (tree->child1 != NULL) { 1038 | load_tree(obj, stream, tree->child1); 1039 | } 1040 | if (tree->child2 != NULL) { 1041 | load_tree(obj, stream, tree->child2); 1042 | } 1043 | } 1044 | 1045 | /** Stores the index in a binary file. 1046 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when 1047 | * loading the index object it must be constructed associated to the same 1048 | * source of data points used while building it. See the example: 1049 | * examples/saveload_example.cpp \sa loadIndex */ 1050 | void saveIndex_(Derived &obj, FILE *stream) { 1051 | save_value(stream, obj.m_size); 1052 | save_value(stream, obj.dim); 1053 | save_value(stream, obj.root_bbox); 1054 | save_value(stream, obj.m_leaf_max_size); 1055 | save_value(stream, obj.vind); 1056 | save_tree(obj, stream, obj.root_node); 1057 | } 1058 | 1059 | /** Loads a previous index from a binary file. 1060 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the 1061 | * index object must be constructed associated to the same source of data 1062 | * points used while building the index. See the example: 1063 | * examples/saveload_example.cpp \sa loadIndex */ 1064 | void loadIndex_(Derived &obj, FILE *stream) { 1065 | load_value(stream, obj.m_size); 1066 | load_value(stream, obj.dim); 1067 | load_value(stream, obj.root_bbox); 1068 | load_value(stream, obj.m_leaf_max_size); 1069 | load_value(stream, obj.vind); 1070 | load_tree(obj, stream, obj.root_node); 1071 | } 1072 | }; 1073 | 1074 | /** @addtogroup kdtrees_grp KD-tree classes and adaptors 1075 | * @{ */ 1076 | 1077 | /** kd-tree static index 1078 | * 1079 | * Contains the k-d trees and other information for indexing a set of points 1080 | * for nearest-neighbor matching. 1081 | * 1082 | * The class "DatasetAdaptor" must provide the following interface (can be 1083 | * non-virtual, inlined methods): 1084 | * 1085 | * \code 1086 | * // Must return the number of data poins 1087 | * inline size_t kdtree_get_point_count() const { ... } 1088 | * 1089 | * 1090 | * // Must return the dim'th component of the idx'th point in the class: 1091 | * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } 1092 | * 1093 | * // Optional bounding-box computation: return false to default to a standard 1094 | * bbox computation loop. 1095 | * // Return true if the BBOX was already computed by the class and returned 1096 | * in "bb" so it can be avoided to redo it again. 1097 | * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 1098 | * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const 1099 | * { 1100 | * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits 1101 | * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits 1102 | * ... 1103 | * return true; 1104 | * } 1105 | * 1106 | * \endcode 1107 | * 1108 | * \tparam DatasetAdaptor The user-provided adaptor (see comments above). 1109 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, 1110 | * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM 1111 | * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will 1112 | * be typically size_t or int 1113 | */ 1114 | template 1116 | class KDTreeSingleIndexAdaptor 1117 | : public KDTreeBaseClass< 1118 | KDTreeSingleIndexAdaptor, 1119 | Distance, DatasetAdaptor, DIM, IndexType> { 1120 | public: 1121 | /** Deleted copy constructor*/ 1122 | KDTreeSingleIndexAdaptor( 1123 | const KDTreeSingleIndexAdaptor 1124 | &) = delete; 1125 | 1126 | /** 1127 | * The dataset used by this index 1128 | */ 1129 | const DatasetAdaptor &dataset; //!< The source of our data 1130 | 1131 | const KDTreeSingleIndexAdaptorParams index_params; 1132 | 1133 | Distance distance; 1134 | 1135 | typedef typename nanoflann::KDTreeBaseClass< 1136 | nanoflann::KDTreeSingleIndexAdaptor, 1138 | Distance, DatasetAdaptor, DIM, IndexType> 1139 | BaseClassRef; 1140 | 1141 | typedef typename BaseClassRef::ElementType ElementType; 1142 | typedef typename BaseClassRef::DistanceType DistanceType; 1143 | 1144 | typedef typename BaseClassRef::Node Node; 1145 | typedef Node *NodePtr; 1146 | 1147 | typedef typename BaseClassRef::Interval Interval; 1148 | /** Define "BoundingBox" as a fixed-size or variable-size container depending 1149 | * on "DIM" */ 1150 | typedef typename BaseClassRef::BoundingBox BoundingBox; 1151 | 1152 | /** Define "distance_vector_t" as a fixed-size or variable-size container 1153 | * depending on "DIM" */ 1154 | typedef typename BaseClassRef::distance_vector_t distance_vector_t; 1155 | 1156 | /** 1157 | * KDTree constructor 1158 | * 1159 | * Refer to docs in README.md or online in 1160 | * https://github.com/jlblancoc/nanoflann 1161 | * 1162 | * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 1163 | * for 3D points) is determined by means of: 1164 | * - The \a DIM template parameter if >0 (highest priority) 1165 | * - Otherwise, the \a dimensionality parameter of this constructor. 1166 | * 1167 | * @param inputData Dataset with the input features 1168 | * @param params Basically, the maximum leaf node size 1169 | */ 1170 | KDTreeSingleIndexAdaptor(const int dimensionality, 1171 | const DatasetAdaptor &inputData, 1172 | const KDTreeSingleIndexAdaptorParams ¶ms = 1173 | KDTreeSingleIndexAdaptorParams()) 1174 | : dataset(inputData), index_params(params), distance(inputData) { 1175 | BaseClassRef::root_node = NULL; 1176 | BaseClassRef::m_size = dataset.kdtree_get_point_count(); 1177 | BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; 1178 | BaseClassRef::dim = dimensionality; 1179 | if (DIM > 0) 1180 | BaseClassRef::dim = DIM; 1181 | BaseClassRef::m_leaf_max_size = params.leaf_max_size; 1182 | 1183 | // Create a permutable array of indices to the input vectors. 1184 | init_vind(); 1185 | } 1186 | 1187 | /** 1188 | * Builds the index 1189 | */ 1190 | void buildIndex() { 1191 | BaseClassRef::m_size = dataset.kdtree_get_point_count(); 1192 | BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; 1193 | init_vind(); 1194 | this->freeIndex(*this); 1195 | BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; 1196 | if (BaseClassRef::m_size == 0) 1197 | return; 1198 | computeBoundingBox(BaseClassRef::root_bbox); 1199 | BaseClassRef::root_node = 1200 | this->divideTree(*this, 0, BaseClassRef::m_size, 1201 | BaseClassRef::root_bbox); // construct the tree 1202 | } 1203 | 1204 | /** \name Query methods 1205 | * @{ */ 1206 | 1207 | /** 1208 | * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored 1209 | * inside the result object. 1210 | * 1211 | * Params: 1212 | * result = the result object in which the indices of the 1213 | * nearest-neighbors are stored vec = the vector for which to search the 1214 | * nearest neighbors 1215 | * 1216 | * \tparam RESULTSET Should be any ResultSet 1217 | * \return True if the requested neighbors could be found. 1218 | * \sa knnSearch, radiusSearch 1219 | */ 1220 | template 1221 | bool findNeighbors(RESULTSET &result, const ElementType *vec, 1222 | const SearchParams &searchParams) const { 1223 | assert(vec); 1224 | if (this->size(*this) == 0) 1225 | return false; 1226 | if (!BaseClassRef::root_node) 1227 | throw std::runtime_error( 1228 | "[nanoflann] findNeighbors() called before building the index."); 1229 | float epsError = 1 + searchParams.eps; 1230 | 1231 | distance_vector_t 1232 | dists; // fixed or variable-sized container (depending on DIM) 1233 | auto zero = static_cast(0); 1234 | assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), 1235 | zero); // Fill it with zeros. 1236 | DistanceType distsq = this->computeInitialDistances(*this, vec, dists); 1237 | searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, 1238 | epsError); // "count_leaf" parameter removed since was neither 1239 | // used nor returned to the user. 1240 | return result.full(); 1241 | } 1242 | 1243 | /** 1244 | * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. 1245 | * Their indices are stored inside the result object. \sa radiusSearch, 1246 | * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility 1247 | * with the original FLANN interface. \return Number `N` of valid points in 1248 | * the result set. Only the first `N` entries in `out_indices` and 1249 | * `out_distances_sq` will be valid. Return may be less than `num_closest` 1250 | * only if the number of elements in the tree is less than `num_closest`. 1251 | */ 1252 | size_t knnSearch(const ElementType *query_point, const size_t num_closest, 1253 | IndexType *out_indices, DistanceType *out_distances_sq, 1254 | const int /* nChecks_IGNORED */ = 10) const { 1255 | nanoflann::KNNResultSet resultSet(num_closest); 1256 | resultSet.init(out_indices, out_distances_sq); 1257 | this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 1258 | return resultSet.size(); 1259 | } 1260 | 1261 | /** 1262 | * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. 1263 | * The output is given as a vector of pairs, of which the first element is a 1264 | * point index and the second the corresponding distance. Previous contents of 1265 | * \a IndicesDists are cleared. 1266 | * 1267 | * If searchParams.sorted==true, the output list is sorted by ascending 1268 | * distances. 1269 | * 1270 | * For a better performance, it is advisable to do a .reserve() on the vector 1271 | * if you have any wild guess about the number of expected matches. 1272 | * 1273 | * \sa knnSearch, findNeighbors, radiusSearchCustomCallback 1274 | * \return The number of points within the given radius (i.e. indices.size() 1275 | * or dists.size() ) 1276 | */ 1277 | size_t 1278 | radiusSearch(const ElementType *query_point, const DistanceType &radius, 1279 | std::vector> &IndicesDists, 1280 | const SearchParams &searchParams) const { 1281 | RadiusResultSet resultSet(radius, IndicesDists); 1282 | const size_t nFound = 1283 | radiusSearchCustomCallback(query_point, resultSet, searchParams); 1284 | if (searchParams.sorted) 1285 | std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); 1286 | return nFound; 1287 | } 1288 | 1289 | /** 1290 | * Just like radiusSearch() but with a custom callback class for each point 1291 | * found in the radius of the query. See the source of RadiusResultSet<> as a 1292 | * start point for your own classes. \sa radiusSearch 1293 | */ 1294 | template 1295 | size_t radiusSearchCustomCallback( 1296 | const ElementType *query_point, SEARCH_CALLBACK &resultSet, 1297 | const SearchParams &searchParams = SearchParams()) const { 1298 | this->findNeighbors(resultSet, query_point, searchParams); 1299 | return resultSet.size(); 1300 | } 1301 | 1302 | /** @} */ 1303 | 1304 | public: 1305 | /** Make sure the auxiliary list \a vind has the same size than the current 1306 | * dataset, and re-generate if size has changed. */ 1307 | void init_vind() { 1308 | // Create a permutable array of indices to the input vectors. 1309 | BaseClassRef::m_size = dataset.kdtree_get_point_count(); 1310 | if (BaseClassRef::vind.size() != BaseClassRef::m_size) 1311 | BaseClassRef::vind.resize(BaseClassRef::m_size); 1312 | for (size_t i = 0; i < BaseClassRef::m_size; i++) 1313 | BaseClassRef::vind[i] = i; 1314 | } 1315 | 1316 | void computeBoundingBox(BoundingBox &bbox) { 1317 | resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); 1318 | if (dataset.kdtree_get_bbox(bbox)) { 1319 | // Done! It was implemented in derived class 1320 | } else { 1321 | const size_t N = dataset.kdtree_get_point_count(); 1322 | if (!N) 1323 | throw std::runtime_error("[nanoflann] computeBoundingBox() called but " 1324 | "no data points found."); 1325 | for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { 1326 | bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); 1327 | } 1328 | for (size_t k = 1; k < N; ++k) { 1329 | for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { 1330 | if (this->dataset_get(*this, k, i) < bbox[i].low) 1331 | bbox[i].low = this->dataset_get(*this, k, i); 1332 | if (this->dataset_get(*this, k, i) > bbox[i].high) 1333 | bbox[i].high = this->dataset_get(*this, k, i); 1334 | } 1335 | } 1336 | } 1337 | } 1338 | 1339 | /** 1340 | * Performs an exact search in the tree starting from a node. 1341 | * \tparam RESULTSET Should be any ResultSet 1342 | * \return true if the search should be continued, false if the results are 1343 | * sufficient 1344 | */ 1345 | template 1346 | bool searchLevel(RESULTSET &result_set, const ElementType *vec, 1347 | const NodePtr node, DistanceType mindistsq, 1348 | distance_vector_t &dists, const float epsError) const { 1349 | /* If this is a leaf node, then do check and return. */ 1350 | if ((node->child1 == NULL) && (node->child2 == NULL)) { 1351 | // count_leaf += (node->lr.right-node->lr.left); // Removed since was 1352 | // neither used nor returned to the user. 1353 | DistanceType worst_dist = result_set.worstDist(); 1354 | for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; 1355 | ++i) { 1356 | const IndexType index = BaseClassRef::vind[i]; // reorder... : i; 1357 | DistanceType dist = distance.evalMetric( 1358 | vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); 1359 | if (dist < worst_dist) { 1360 | if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { 1361 | // the resultset doesn't want to receive any more points, we're done 1362 | // searching! 1363 | return false; 1364 | } 1365 | } 1366 | } 1367 | return true; 1368 | } 1369 | 1370 | /* Which child branch should be taken first? */ 1371 | int idx = node->node_type.sub.divfeat; 1372 | ElementType val = vec[idx]; 1373 | DistanceType diff1 = val - node->node_type.sub.divlow; 1374 | DistanceType diff2 = val - node->node_type.sub.divhigh; 1375 | 1376 | NodePtr bestChild; 1377 | NodePtr otherChild; 1378 | DistanceType cut_dist; 1379 | if ((diff1 + diff2) < 0) { 1380 | bestChild = node->child1; 1381 | otherChild = node->child2; 1382 | cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); 1383 | } else { 1384 | bestChild = node->child2; 1385 | otherChild = node->child1; 1386 | cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); 1387 | } 1388 | 1389 | /* Call recursively to search next level down. */ 1390 | if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { 1391 | // the resultset doesn't want to receive any more points, we're done 1392 | // searching! 1393 | return false; 1394 | } 1395 | 1396 | DistanceType dst = dists[idx]; 1397 | mindistsq = mindistsq + cut_dist - dst; 1398 | dists[idx] = cut_dist; 1399 | if (mindistsq * epsError <= result_set.worstDist()) { 1400 | if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, 1401 | epsError)) { 1402 | // the resultset doesn't want to receive any more points, we're done 1403 | // searching! 1404 | return false; 1405 | } 1406 | } 1407 | dists[idx] = dst; 1408 | return true; 1409 | } 1410 | 1411 | public: 1412 | /** Stores the index in a binary file. 1413 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when 1414 | * loading the index object it must be constructed associated to the same 1415 | * source of data points used while building it. See the example: 1416 | * examples/saveload_example.cpp \sa loadIndex */ 1417 | void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } 1418 | 1419 | /** Loads a previous index from a binary file. 1420 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the 1421 | * index object must be constructed associated to the same source of data 1422 | * points used while building the index. See the example: 1423 | * examples/saveload_example.cpp \sa loadIndex */ 1424 | void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } 1425 | 1426 | }; // class KDTree 1427 | 1428 | /** kd-tree dynamic index 1429 | * 1430 | * Contains the k-d trees and other information for indexing a set of points 1431 | * for nearest-neighbor matching. 1432 | * 1433 | * The class "DatasetAdaptor" must provide the following interface (can be 1434 | * non-virtual, inlined methods): 1435 | * 1436 | * \code 1437 | * // Must return the number of data poins 1438 | * inline size_t kdtree_get_point_count() const { ... } 1439 | * 1440 | * // Must return the dim'th component of the idx'th point in the class: 1441 | * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } 1442 | * 1443 | * // Optional bounding-box computation: return false to default to a standard 1444 | * bbox computation loop. 1445 | * // Return true if the BBOX was already computed by the class and returned 1446 | * in "bb" so it can be avoided to redo it again. 1447 | * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 1448 | * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const 1449 | * { 1450 | * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits 1451 | * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits 1452 | * ... 1453 | * return true; 1454 | * } 1455 | * 1456 | * \endcode 1457 | * 1458 | * \tparam DatasetAdaptor The user-provided adaptor (see comments above). 1459 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, 1460 | * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM 1461 | * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will 1462 | * be typically size_t or int 1463 | */ 1464 | template 1466 | class KDTreeSingleIndexDynamicAdaptor_ 1467 | : public KDTreeBaseClass, 1469 | Distance, DatasetAdaptor, DIM, IndexType> { 1470 | public: 1471 | /** 1472 | * The dataset used by this index 1473 | */ 1474 | const DatasetAdaptor &dataset; //!< The source of our data 1475 | 1476 | KDTreeSingleIndexAdaptorParams index_params; 1477 | 1478 | std::vector &treeIndex; 1479 | 1480 | Distance distance; 1481 | 1482 | typedef typename nanoflann::KDTreeBaseClass< 1483 | nanoflann::KDTreeSingleIndexDynamicAdaptor_, 1485 | Distance, DatasetAdaptor, DIM, IndexType> 1486 | BaseClassRef; 1487 | 1488 | typedef typename BaseClassRef::ElementType ElementType; 1489 | typedef typename BaseClassRef::DistanceType DistanceType; 1490 | 1491 | typedef typename BaseClassRef::Node Node; 1492 | typedef Node *NodePtr; 1493 | 1494 | typedef typename BaseClassRef::Interval Interval; 1495 | /** Define "BoundingBox" as a fixed-size or variable-size container depending 1496 | * on "DIM" */ 1497 | typedef typename BaseClassRef::BoundingBox BoundingBox; 1498 | 1499 | /** Define "distance_vector_t" as a fixed-size or variable-size container 1500 | * depending on "DIM" */ 1501 | typedef typename BaseClassRef::distance_vector_t distance_vector_t; 1502 | 1503 | /** 1504 | * KDTree constructor 1505 | * 1506 | * Refer to docs in README.md or online in 1507 | * https://github.com/jlblancoc/nanoflann 1508 | * 1509 | * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 1510 | * for 3D points) is determined by means of: 1511 | * - The \a DIM template parameter if >0 (highest priority) 1512 | * - Otherwise, the \a dimensionality parameter of this constructor. 1513 | * 1514 | * @param inputData Dataset with the input features 1515 | * @param params Basically, the maximum leaf node size 1516 | */ 1517 | KDTreeSingleIndexDynamicAdaptor_( 1518 | const int dimensionality, const DatasetAdaptor &inputData, 1519 | std::vector &treeIndex_, 1520 | const KDTreeSingleIndexAdaptorParams ¶ms = 1521 | KDTreeSingleIndexAdaptorParams()) 1522 | : dataset(inputData), index_params(params), treeIndex(treeIndex_), 1523 | distance(inputData) { 1524 | BaseClassRef::root_node = NULL; 1525 | BaseClassRef::m_size = 0; 1526 | BaseClassRef::m_size_at_index_build = 0; 1527 | BaseClassRef::dim = dimensionality; 1528 | if (DIM > 0) 1529 | BaseClassRef::dim = DIM; 1530 | BaseClassRef::m_leaf_max_size = params.leaf_max_size; 1531 | } 1532 | 1533 | /** Assignment operator definiton */ 1534 | KDTreeSingleIndexDynamicAdaptor_ 1535 | operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) { 1536 | KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); 1537 | std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); 1538 | std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); 1539 | std::swap(index_params, tmp.index_params); 1540 | std::swap(treeIndex, tmp.treeIndex); 1541 | std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); 1542 | std::swap(BaseClassRef::m_size_at_index_build, 1543 | tmp.BaseClassRef::m_size_at_index_build); 1544 | std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); 1545 | std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); 1546 | std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); 1547 | return *this; 1548 | } 1549 | 1550 | /** 1551 | * Builds the index 1552 | */ 1553 | void buildIndex() { 1554 | BaseClassRef::m_size = BaseClassRef::vind.size(); 1555 | this->freeIndex(*this); 1556 | BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; 1557 | if (BaseClassRef::m_size == 0) 1558 | return; 1559 | computeBoundingBox(BaseClassRef::root_bbox); 1560 | BaseClassRef::root_node = 1561 | this->divideTree(*this, 0, BaseClassRef::m_size, 1562 | BaseClassRef::root_bbox); // construct the tree 1563 | } 1564 | 1565 | /** \name Query methods 1566 | * @{ */ 1567 | 1568 | /** 1569 | * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored 1570 | * inside the result object. 1571 | * 1572 | * Params: 1573 | * result = the result object in which the indices of the 1574 | * nearest-neighbors are stored vec = the vector for which to search the 1575 | * nearest neighbors 1576 | * 1577 | * \tparam RESULTSET Should be any ResultSet 1578 | * \return True if the requested neighbors could be found. 1579 | * \sa knnSearch, radiusSearch 1580 | */ 1581 | template 1582 | bool findNeighbors(RESULTSET &result, const ElementType *vec, 1583 | const SearchParams &searchParams) const { 1584 | assert(vec); 1585 | if (this->size(*this) == 0) 1586 | return false; 1587 | if (!BaseClassRef::root_node) 1588 | return false; 1589 | float epsError = 1 + searchParams.eps; 1590 | 1591 | // fixed or variable-sized container (depending on DIM) 1592 | distance_vector_t dists; 1593 | // Fill it with zeros. 1594 | assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), 1595 | static_cast(0)); 1596 | DistanceType distsq = this->computeInitialDistances(*this, vec, dists); 1597 | searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, 1598 | epsError); // "count_leaf" parameter removed since was neither 1599 | // used nor returned to the user. 1600 | return result.full(); 1601 | } 1602 | 1603 | /** 1604 | * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. 1605 | * Their indices are stored inside the result object. \sa radiusSearch, 1606 | * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility 1607 | * with the original FLANN interface. \return Number `N` of valid points in 1608 | * the result set. Only the first `N` entries in `out_indices` and 1609 | * `out_distances_sq` will be valid. Return may be less than `num_closest` 1610 | * only if the number of elements in the tree is less than `num_closest`. 1611 | */ 1612 | size_t knnSearch(const ElementType *query_point, const size_t num_closest, 1613 | IndexType *out_indices, DistanceType *out_distances_sq, 1614 | const int /* nChecks_IGNORED */ = 10) const { 1615 | nanoflann::KNNResultSet resultSet(num_closest); 1616 | resultSet.init(out_indices, out_distances_sq); 1617 | this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 1618 | return resultSet.size(); 1619 | } 1620 | 1621 | /** 1622 | * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. 1623 | * The output is given as a vector of pairs, of which the first element is a 1624 | * point index and the second the corresponding distance. Previous contents of 1625 | * \a IndicesDists are cleared. 1626 | * 1627 | * If searchParams.sorted==true, the output list is sorted by ascending 1628 | * distances. 1629 | * 1630 | * For a better performance, it is advisable to do a .reserve() on the vector 1631 | * if you have any wild guess about the number of expected matches. 1632 | * 1633 | * \sa knnSearch, findNeighbors, radiusSearchCustomCallback 1634 | * \return The number of points within the given radius (i.e. indices.size() 1635 | * or dists.size() ) 1636 | */ 1637 | size_t 1638 | radiusSearch(const ElementType *query_point, const DistanceType &radius, 1639 | std::vector> &IndicesDists, 1640 | const SearchParams &searchParams) const { 1641 | RadiusResultSet resultSet(radius, IndicesDists); 1642 | const size_t nFound = 1643 | radiusSearchCustomCallback(query_point, resultSet, searchParams); 1644 | if (searchParams.sorted) 1645 | std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); 1646 | return nFound; 1647 | } 1648 | 1649 | /** 1650 | * Just like radiusSearch() but with a custom callback class for each point 1651 | * found in the radius of the query. See the source of RadiusResultSet<> as a 1652 | * start point for your own classes. \sa radiusSearch 1653 | */ 1654 | template 1655 | size_t radiusSearchCustomCallback( 1656 | const ElementType *query_point, SEARCH_CALLBACK &resultSet, 1657 | const SearchParams &searchParams = SearchParams()) const { 1658 | this->findNeighbors(resultSet, query_point, searchParams); 1659 | return resultSet.size(); 1660 | } 1661 | 1662 | /** @} */ 1663 | 1664 | public: 1665 | void computeBoundingBox(BoundingBox &bbox) { 1666 | resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); 1667 | 1668 | if (dataset.kdtree_get_bbox(bbox)) { 1669 | // Done! It was implemented in derived class 1670 | } else { 1671 | const size_t N = BaseClassRef::m_size; 1672 | if (!N) 1673 | throw std::runtime_error("[nanoflann] computeBoundingBox() called but " 1674 | "no data points found."); 1675 | for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { 1676 | bbox[i].low = bbox[i].high = 1677 | this->dataset_get(*this, BaseClassRef::vind[0], i); 1678 | } 1679 | for (size_t k = 1; k < N; ++k) { 1680 | for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { 1681 | if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) 1682 | bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); 1683 | if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) 1684 | bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); 1685 | } 1686 | } 1687 | } 1688 | } 1689 | 1690 | /** 1691 | * Performs an exact search in the tree starting from a node. 1692 | * \tparam RESULTSET Should be any ResultSet 1693 | */ 1694 | template 1695 | void searchLevel(RESULTSET &result_set, const ElementType *vec, 1696 | const NodePtr node, DistanceType mindistsq, 1697 | distance_vector_t &dists, const float epsError) const { 1698 | /* If this is a leaf node, then do check and return. */ 1699 | if ((node->child1 == NULL) && (node->child2 == NULL)) { 1700 | // count_leaf += (node->lr.right-node->lr.left); // Removed since was 1701 | // neither used nor returned to the user. 1702 | DistanceType worst_dist = result_set.worstDist(); 1703 | for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; 1704 | ++i) { 1705 | const IndexType index = BaseClassRef::vind[i]; // reorder... : i; 1706 | if (treeIndex[index] == -1) 1707 | continue; 1708 | DistanceType dist = distance.evalMetric( 1709 | vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); 1710 | if (dist < worst_dist) { 1711 | if (!result_set.addPoint( 1712 | static_cast(dist), 1713 | static_cast( 1714 | BaseClassRef::vind[i]))) { 1715 | // the resultset doesn't want to receive any more points, we're done 1716 | // searching! 1717 | return; // false; 1718 | } 1719 | } 1720 | } 1721 | return; 1722 | } 1723 | 1724 | /* Which child branch should be taken first? */ 1725 | int idx = node->node_type.sub.divfeat; 1726 | ElementType val = vec[idx]; 1727 | DistanceType diff1 = val - node->node_type.sub.divlow; 1728 | DistanceType diff2 = val - node->node_type.sub.divhigh; 1729 | 1730 | NodePtr bestChild; 1731 | NodePtr otherChild; 1732 | DistanceType cut_dist; 1733 | if ((diff1 + diff2) < 0) { 1734 | bestChild = node->child1; 1735 | otherChild = node->child2; 1736 | cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); 1737 | } else { 1738 | bestChild = node->child2; 1739 | otherChild = node->child1; 1740 | cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); 1741 | } 1742 | 1743 | /* Call recursively to search next level down. */ 1744 | searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); 1745 | 1746 | DistanceType dst = dists[idx]; 1747 | mindistsq = mindistsq + cut_dist - dst; 1748 | dists[idx] = cut_dist; 1749 | if (mindistsq * epsError <= result_set.worstDist()) { 1750 | searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); 1751 | } 1752 | dists[idx] = dst; 1753 | } 1754 | 1755 | public: 1756 | /** Stores the index in a binary file. 1757 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when 1758 | * loading the index object it must be constructed associated to the same 1759 | * source of data points used while building it. See the example: 1760 | * examples/saveload_example.cpp \sa loadIndex */ 1761 | void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } 1762 | 1763 | /** Loads a previous index from a binary file. 1764 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the 1765 | * index object must be constructed associated to the same source of data 1766 | * points used while building the index. See the example: 1767 | * examples/saveload_example.cpp \sa loadIndex */ 1768 | void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } 1769 | }; 1770 | 1771 | /** kd-tree dynaimic index 1772 | * 1773 | * class to create multiple static index and merge their results to behave as 1774 | * single dynamic index as proposed in Logarithmic Approach. 1775 | * 1776 | * Example of usage: 1777 | * examples/dynamic_pointcloud_example.cpp 1778 | * 1779 | * \tparam DatasetAdaptor The user-provided adaptor (see comments above). 1780 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, 1781 | * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM 1782 | * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will 1783 | * be typically size_t or int 1784 | */ 1785 | template 1787 | class KDTreeSingleIndexDynamicAdaptor { 1788 | public: 1789 | typedef typename Distance::ElementType ElementType; 1790 | typedef typename Distance::DistanceType DistanceType; 1791 | 1792 | protected: 1793 | size_t m_leaf_max_size; 1794 | size_t treeCount; 1795 | size_t pointCount; 1796 | 1797 | /** 1798 | * The dataset used by this index 1799 | */ 1800 | const DatasetAdaptor &dataset; //!< The source of our data 1801 | 1802 | std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which 1803 | //!< point at idx is stored. treeIndex[idx]=-1 1804 | //!< means that point has been removed. 1805 | 1806 | KDTreeSingleIndexAdaptorParams index_params; 1807 | 1808 | int dim; //!< Dimensionality of each data point 1809 | 1810 | typedef KDTreeSingleIndexDynamicAdaptor_ 1811 | index_container_t; 1812 | std::vector index; 1813 | 1814 | public: 1815 | /** Get a const ref to the internal list of indices; the number of indices is 1816 | * adapted dynamically as the dataset grows in size. */ 1817 | const std::vector &getAllIndices() const { return index; } 1818 | 1819 | private: 1820 | /** finds position of least significant unset bit */ 1821 | int First0Bit(IndexType num) { 1822 | int pos = 0; 1823 | while (num & 1) { 1824 | num = num >> 1; 1825 | pos++; 1826 | } 1827 | return pos; 1828 | } 1829 | 1830 | /** Creates multiple empty trees to handle dynamic support */ 1831 | void init() { 1832 | typedef KDTreeSingleIndexDynamicAdaptor_ 1833 | my_kd_tree_t; 1834 | std::vector index_( 1835 | treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); 1836 | index = index_; 1837 | } 1838 | 1839 | public: 1840 | Distance distance; 1841 | 1842 | /** 1843 | * KDTree constructor 1844 | * 1845 | * Refer to docs in README.md or online in 1846 | * https://github.com/jlblancoc/nanoflann 1847 | * 1848 | * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 1849 | * for 3D points) is determined by means of: 1850 | * - The \a DIM template parameter if >0 (highest priority) 1851 | * - Otherwise, the \a dimensionality parameter of this constructor. 1852 | * 1853 | * @param inputData Dataset with the input features 1854 | * @param params Basically, the maximum leaf node size 1855 | */ 1856 | KDTreeSingleIndexDynamicAdaptor(const int dimensionality, 1857 | const DatasetAdaptor &inputData, 1858 | const KDTreeSingleIndexAdaptorParams ¶ms = 1859 | KDTreeSingleIndexAdaptorParams(), 1860 | const size_t maximumPointCount = 1000000000U) 1861 | : dataset(inputData), index_params(params), distance(inputData) { 1862 | treeCount = static_cast(std::log2(maximumPointCount)); 1863 | pointCount = 0U; 1864 | dim = dimensionality; 1865 | treeIndex.clear(); 1866 | if (DIM > 0) 1867 | dim = DIM; 1868 | m_leaf_max_size = params.leaf_max_size; 1869 | init(); 1870 | const size_t num_initial_points = dataset.kdtree_get_point_count(); 1871 | if (num_initial_points > 0) { 1872 | addPoints(0, num_initial_points - 1); 1873 | } 1874 | } 1875 | 1876 | /** Deleted copy constructor*/ 1877 | KDTreeSingleIndexDynamicAdaptor( 1878 | const KDTreeSingleIndexDynamicAdaptor &) = delete; 1880 | 1881 | /** Add points to the set, Inserts all points from [start, end] */ 1882 | void addPoints(IndexType start, IndexType end) { 1883 | size_t count = end - start + 1; 1884 | treeIndex.resize(treeIndex.size() + count); 1885 | for (IndexType idx = start; idx <= end; idx++) { 1886 | int pos = First0Bit(pointCount); 1887 | index[pos].vind.clear(); 1888 | treeIndex[pointCount] = pos; 1889 | for (int i = 0; i < pos; i++) { 1890 | for (int j = 0; j < static_cast(index[i].vind.size()); j++) { 1891 | index[pos].vind.push_back(index[i].vind[j]); 1892 | if (treeIndex[index[i].vind[j]] != -1) 1893 | treeIndex[index[i].vind[j]] = pos; 1894 | } 1895 | index[i].vind.clear(); 1896 | index[i].freeIndex(index[i]); 1897 | } 1898 | index[pos].vind.push_back(idx); 1899 | index[pos].buildIndex(); 1900 | pointCount++; 1901 | } 1902 | } 1903 | 1904 | /** Remove a point from the set (Lazy Deletion) */ 1905 | void removePoint(size_t idx) { 1906 | if (idx >= pointCount) 1907 | return; 1908 | treeIndex[idx] = -1; 1909 | } 1910 | 1911 | /** 1912 | * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored 1913 | * inside the result object. 1914 | * 1915 | * Params: 1916 | * result = the result object in which the indices of the 1917 | * nearest-neighbors are stored vec = the vector for which to search the 1918 | * nearest neighbors 1919 | * 1920 | * \tparam RESULTSET Should be any ResultSet 1921 | * \return True if the requested neighbors could be found. 1922 | * \sa knnSearch, radiusSearch 1923 | */ 1924 | template 1925 | bool findNeighbors(RESULTSET &result, const ElementType *vec, 1926 | const SearchParams &searchParams) const { 1927 | for (size_t i = 0; i < treeCount; i++) { 1928 | index[i].findNeighbors(result, &vec[0], searchParams); 1929 | } 1930 | return result.full(); 1931 | } 1932 | }; 1933 | 1934 | /** An L2-metric KD-tree adaptor for working with data directly stored in an 1935 | * Eigen Matrix, without duplicating the data storage. Each row in the matrix 1936 | * represents a point in the state space. 1937 | * 1938 | * Example of usage: 1939 | * \code 1940 | * Eigen::Matrix mat; 1941 | * // Fill out "mat"... 1942 | * 1943 | * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > 1944 | * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf 1945 | * ); mat_index.index->buildIndex(); mat_index.index->... \endcode 1946 | * 1947 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality 1948 | * for the points in the data set, allowing more compiler optimizations. \tparam 1949 | * Distance The distance metric to use: nanoflann::metric_L1, 1950 | * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 1951 | */ 1952 | template 1953 | struct KDTreeEigenMatrixAdaptor { 1954 | typedef KDTreeEigenMatrixAdaptor self_t; 1955 | typedef typename MatrixType::Scalar num_t; 1956 | typedef typename MatrixType::Index IndexType; 1957 | typedef 1958 | typename Distance::template traits::distance_t metric_t; 1959 | typedef KDTreeSingleIndexAdaptor 1961 | index_t; 1962 | 1963 | index_t *index; //! The kd-tree index for the user to call its methods as 1964 | //! usual with any other FLANN index. 1965 | 1966 | /// Constructor: takes a const ref to the matrix object with the data points 1967 | KDTreeEigenMatrixAdaptor(const size_t dimensionality, 1968 | const std::reference_wrapper &mat, 1969 | const int leaf_max_size = 10) 1970 | : m_data_matrix(mat) { 1971 | const auto dims = mat.get().cols(); 1972 | if (size_t(dims) != dimensionality) 1973 | throw std::runtime_error( 1974 | "Error: 'dimensionality' must match column count in data matrix"); 1975 | if (DIM > 0 && int(dims) != DIM) 1976 | throw std::runtime_error( 1977 | "Data set dimensionality does not match the 'DIM' template argument"); 1978 | index = 1979 | new index_t(static_cast(dims), *this /* adaptor */, 1980 | nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); 1981 | index->buildIndex(); 1982 | } 1983 | 1984 | public: 1985 | /** Deleted copy constructor */ 1986 | KDTreeEigenMatrixAdaptor(const self_t &) = delete; 1987 | 1988 | ~KDTreeEigenMatrixAdaptor() { delete index; } 1989 | 1990 | const std::reference_wrapper m_data_matrix; 1991 | 1992 | /** Query for the \a num_closest closest points to a given point (entered as 1993 | * query_point[0:dim-1]). Note that this is a short-cut method for 1994 | * index->findNeighbors(). The user can also call index->... methods as 1995 | * desired. \note nChecks_IGNORED is ignored but kept for compatibility with 1996 | * the original FLANN interface. 1997 | */ 1998 | inline void query(const num_t *query_point, const size_t num_closest, 1999 | IndexType *out_indices, num_t *out_distances_sq, 2000 | const int /* nChecks_IGNORED */ = 10) const { 2001 | nanoflann::KNNResultSet resultSet(num_closest); 2002 | resultSet.init(out_indices, out_distances_sq); 2003 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 2004 | } 2005 | 2006 | /** @name Interface expected by KDTreeSingleIndexAdaptor 2007 | * @{ */ 2008 | 2009 | const self_t &derived() const { return *this; } 2010 | self_t &derived() { return *this; } 2011 | 2012 | // Must return the number of data points 2013 | inline size_t kdtree_get_point_count() const { 2014 | return m_data_matrix.get().rows(); 2015 | } 2016 | 2017 | // Returns the dim'th component of the idx'th point in the class: 2018 | inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { 2019 | return m_data_matrix.get().coeff(idx, IndexType(dim)); 2020 | } 2021 | 2022 | // Optional bounding-box computation: return false to default to a standard 2023 | // bbox computation loop. 2024 | // Return true if the BBOX was already computed by the class and returned in 2025 | // "bb" so it can be avoided to redo it again. Look at bb.size() to find out 2026 | // the expected dimensionality (e.g. 2 or 3 for point clouds) 2027 | template bool kdtree_get_bbox(BBOX & /*bb*/) const { 2028 | return false; 2029 | } 2030 | 2031 | /** @} */ 2032 | 2033 | }; // end of KDTreeEigenMatrixAdaptor 2034 | /** @} */ 2035 | 2036 | /** @} */ // end of grouping 2037 | } // namespace nanoflann 2038 | 2039 | #endif /* NANOFLANN_HPP_ */ 2040 | -------------------------------------------------------------------------------- /third_parties/nanoflann_pcl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2012, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id: kdtree_flann.h 36261 2011-02-26 01:34:42Z mariusm $ 38 | * 39 | */ 40 | 41 | #ifndef NANO_KDTREE_KDTREE_FLANN_H_ 42 | #define NANO_KDTREE_KDTREE_FLANN_H_ 43 | 44 | #include 45 | #include 46 | #include 47 | #include "nanoflann.hpp" 48 | 49 | namespace nanoflann 50 | { 51 | 52 | // Adapter class to give to nanoflann the same "look and fell" of pcl::KdTreeFLANN. 53 | // limited to squared distance between 3D points 54 | template 55 | class KdTreeFLANN 56 | { 57 | public: 58 | 59 | typedef boost::shared_ptr > Ptr; 60 | typedef boost::shared_ptr > ConstPtr; 61 | 62 | typedef typename pcl::PointCloud PointCloud; 63 | typedef typename pcl::PointCloud::Ptr PointCloudPtr; 64 | typedef typename pcl::PointCloud::ConstPtr PointCloudConstPtr; 65 | 66 | typedef boost::shared_ptr > IndicesPtr; 67 | typedef boost::shared_ptr > IndicesConstPtr; 68 | 69 | KdTreeFLANN (bool sorted = true); 70 | 71 | KdTreeFLANN (const KdTreeFLANN &k); 72 | 73 | void setEpsilon (float eps); 74 | 75 | void setSortedResults (bool sorted); 76 | 77 | inline Ptr makeShared () { return Ptr (new KdTreeFLANN (*this)); } 78 | 79 | void setInputCloud (const PointCloudPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); 80 | 81 | int nearestKSearch (const PointT &point, int k, std::vector &k_indices, 82 | std::vector &k_sqr_distances) const; 83 | 84 | int radiusSearch (const PointT &point, double radius, std::vector &k_indices, 85 | std::vector &k_sqr_distances) const; 86 | 87 | private: 88 | 89 | nanoflann::SearchParams _params; 90 | 91 | struct PointCloud_Adaptor 92 | { 93 | inline size_t kdtree_get_point_count() const; 94 | inline float kdtree_get_pt(const size_t idx, int dim) const; 95 | template bool kdtree_get_bbox(BBOX&) const { return false; } 96 | PointCloudConstPtr pcl; 97 | IndicesConstPtr indices; 98 | }; 99 | 100 | typedef nanoflann::KDTreeSingleIndexAdaptor< 101 | nanoflann::SO3_Adaptor , 102 | PointCloud_Adaptor, 3, int> KDTreeFlann_PCL_SO3; 103 | 104 | PointCloud_Adaptor _adaptor; 105 | 106 | KDTreeFlann_PCL_SO3 _kdtree; 107 | 108 | }; 109 | 110 | //---------- Definitions --------------------- 111 | 112 | template inline 113 | KdTreeFLANN::KdTreeFLANN(bool sorted): 114 | _kdtree(3,_adaptor) 115 | { 116 | _params.sorted = sorted; 117 | } 118 | 119 | template inline 120 | void KdTreeFLANN::setEpsilon(float eps) 121 | { 122 | _params.eps = eps; 123 | } 124 | 125 | template inline 126 | void KdTreeFLANN::setSortedResults(bool sorted) 127 | { 128 | _params.sorted = sorted; 129 | } 130 | 131 | template inline 132 | void KdTreeFLANN::setInputCloud(const KdTreeFLANN::PointCloudPtr &cloud, 133 | const IndicesConstPtr &indices) 134 | { 135 | _adaptor.pcl = cloud; 136 | _adaptor.indices = indices; 137 | _kdtree.buildIndex(); 138 | } 139 | 140 | template inline 141 | int KdTreeFLANN::nearestKSearch(const PointT &point, int num_closest, 142 | std::vector &k_indices, 143 | std::vector &k_sqr_distances) const 144 | { 145 | k_indices.resize(num_closest); 146 | k_sqr_distances.resize(num_closest); 147 | 148 | nanoflann::KNNResultSet resultSet(num_closest); 149 | resultSet.init( k_indices.data(), k_sqr_distances.data()); 150 | _kdtree.findNeighbors(resultSet, point.data, nanoflann::SearchParams() ); 151 | return resultSet.size(); 152 | } 153 | 154 | template 155 | inline int KdTreeFLANN::radiusSearch( 156 | const PointT &point, double radius, std::vector &k_indices, 157 | std::vector &k_sqr_distances) const 158 | { 159 | std::vector> indices_dist; 160 | indices_dist.reserve(128); 161 | RadiusResultSet resultSet(static_cast(radius * radius), 162 | indices_dist); 163 | _kdtree.findNeighbors(resultSet, point.data, _params); 164 | const size_t nFound = resultSet.size(); 165 | if (_params.sorted) { 166 | std::sort(indices_dist.begin(), indices_dist.end(), IndexDist_Sorter()); 167 | } 168 | k_indices.resize(nFound); 169 | k_sqr_distances.resize(nFound); 170 | for (int i = 0; i < nFound; i++) { 171 | k_indices[i] = indices_dist[i].first; 172 | k_sqr_distances[i] = indices_dist[i].second; 173 | } 174 | return nFound; 175 | } 176 | 177 | template inline 178 | size_t KdTreeFLANN::PointCloud_Adaptor::kdtree_get_point_count() const { 179 | if( indices ) return indices->size(); 180 | if( pcl) return pcl->points.size(); 181 | return 0; 182 | } 183 | 184 | template inline 185 | float KdTreeFLANN::PointCloud_Adaptor::kdtree_get_pt(const size_t idx, int dim) const{ 186 | const PointT& p = ( indices ) ? pcl->points[(*indices)[idx]] : pcl->points[idx]; 187 | if (dim==0) return p.x; 188 | else if (dim==1) return p.y; 189 | else if (dim==2) return p.z; 190 | else return 0.0; 191 | } 192 | 193 | } 194 | 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /third_parties/scanContext/.Rhistory: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/DiSCo-SLAM/772755702840a029f4fd0eaedd11bab82ac1e3fd/third_parties/scanContext/.Rhistory -------------------------------------------------------------------------------- /third_parties/scanContext/scanContext.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yewei on 2/27/20. 3 | // 4 | 5 | // This is an unofficial c++ implementation of Scan Context: 6 | // @ARTICLE{ gkim-2019-ral, 7 | // author = {G. {Kim} and B. {Park} and A. {Kim}}, 8 | // journal = {IEEE Robotics and Automation Letters}, 9 | // title = {1-Day Learning, 1-Year Localization: Long-Term LiDAR Localization Using Scan Context Image}, 10 | // year = {2019}, 11 | // volume = {4}, 12 | // number = {2}, 13 | // pages = {1948-1955}, 14 | // month = {April} 15 | // } 16 | // For more information please visit: https://github.com/irapkaist/scancontext 17 | 18 | #include "scanContext.h" 19 | ScanContext::ScanContext(int max_range, int num_rings, int num_sectors) 20 | : _max_range(max_range), _num_rings(num_rings), _num_sectors(num_sectors){ 21 | _gap = float(_max_range) / float(_num_rings); 22 | _angle_one_sector = 360.0 / float(_num_sectors); 23 | } 24 | 25 | ScanContextBin ScanContext::ptcloud2bin(pcl::PointCloud::Ptr pt_cloud){ 26 | ScanContextBin sc_bin; 27 | sc_bin.cloud.reset(new pcl::PointCloud()); 28 | std::swap( sc_bin.cloud, pt_cloud); 29 | //sc_bin.cloud = pt_cloud; 30 | sc_bin.bin = ptCloud2ScanContext(sc_bin.cloud); 31 | sc_bin.ringkey = scanContext2RingKey(sc_bin.bin); 32 | return sc_bin; 33 | } 34 | 35 | Eigen::MatrixXf ScanContext::ptCloud2ScanContext(pcl::PointCloud::Ptr pt_cloud){ 36 | Eigen::MatrixXf max_bin = Eigen::MatrixXf::Zero(_num_rings, _num_sectors); 37 | Eigen::MatrixXf bin_counter = Eigen::MatrixXf::Zero(_num_rings, _num_sectors); 38 | 39 | int num_points = pt_cloud->points.size(); 40 | 41 | for (int i = 0; i < num_points; ++i){ 42 | //point info 43 | PointType point_this = pt_cloud->points[i]; 44 | float range = sqrt(point_this.x*point_this.x + point_this.y*point_this.y); 45 | float theta = xy2Theta(point_this.x, point_this.y); 46 | 47 | //find corresponding ring index 48 | //ring index: 0 ~ num rings-1 49 | int ring_index = floor(range/_gap); 50 | if (ring_index >= _num_rings) 51 | ring_index = _num_rings - 1; 52 | 53 | //find corresponding sector index 54 | //sector index: 1 ~ num sectors-1 55 | int sector_index = ceil(theta/_angle_one_sector); 56 | if (sector_index == 0) 57 | continue; 58 | else if(sector_index > _num_sectors || sector_index < 1) 59 | sector_index = _num_sectors - 1; 60 | else 61 | sector_index = sector_index - 1; 62 | 63 | if (point_this.z > max_bin(ring_index, sector_index)) 64 | max_bin(ring_index, sector_index) = point_this.z; 65 | 66 | bin_counter(ring_index, sector_index)++; 67 | } 68 | 69 | for (int i = 0; i < _num_rings; i++){ 70 | for (int j = 0; j < _num_sectors; j++){ 71 | if(bin_counter(i,j)<5) 72 | max_bin(i,j) = 0; 73 | } 74 | } 75 | return max_bin; 76 | } 77 | 78 | Eigen::VectorXf ScanContext::scanContext2RingKey(Eigen::MatrixXf sc_bin){ 79 | Eigen::VectorXf ringkey = Eigen::VectorXf::Zero(_num_rings); 80 | int nonzeros; 81 | for (int i = 0; i< _num_rings; i++){ 82 | nonzeros = 0; 83 | for (int j = 0; j < _num_sectors; j++) 84 | if (sc_bin(i,j) != 0) 85 | nonzeros++; 86 | ringkey(i) = float(nonzeros)/float(_num_sectors); 87 | } 88 | return ringkey; 89 | } 90 | 91 | float ScanContext::xy2Theta(float x, float y){ 92 | if ( x>=0 && y>=0) 93 | return 180/M_PI * atan(y/x); 94 | 95 | if ( x<0 && y>=0) 96 | return 180 - ((180/M_PI) * atan(y/(-x))); 97 | 98 | if (x < 0 && y < 0) 99 | return 180 + ((180/M_PI) * atan(y/x)); 100 | 101 | if ( x >= 0 && y < 0) 102 | return 360 - ((180/M_PI) * atan((-y)/x)); 103 | } -------------------------------------------------------------------------------- /third_parties/scanContext/scanContext.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yewei on 2/27/20. 3 | 4 | // This is an unofficial c++ implementation of Scan Context: 5 | // @ARTICLE{ gkim-2019-ral, 6 | // author = {G. {Kim} and B. {Park} and A. {Kim}}, 7 | // journal = {IEEE Robotics and Automation Letters}, 8 | // title = {1-Day Learning, 1-Year Localization: Long-Term LiDAR Localization Using Scan Context Image}, 9 | // year = {2019}, 10 | // volume = {4}, 11 | // number = {2}, 12 | // pages = {1948-1955}, 13 | // month = {April} 14 | // } 15 | // For more information please visit: https://github.com/irapkaist/scancontext 16 | 17 | #ifndef SRC_SCANCONTEXT_H 18 | #define SRC_SCANCONTEXT_H 19 | //#include "utility.h" 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #pragma once 36 | #include 37 | 38 | 39 | typedef pcl::PointXYZI PointType; 40 | 41 | struct PointXYZIRPYT 42 | { 43 | PCL_ADD_POINT4D 44 | PCL_ADD_INTENSITY; 45 | float roll; 46 | float pitch; 47 | float yaw; 48 | double time; 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | } EIGEN_ALIGN16; 51 | 52 | typedef PointXYZIRPYT PointTypePose; 53 | 54 | 55 | struct ScanContextBin 56 | { 57 | std::string robotname; 58 | 59 | double time; 60 | 61 | PointTypePose pose; 62 | 63 | pcl::PointCloud::Ptr cloud; 64 | 65 | Eigen::MatrixXf bin; 66 | 67 | Eigen::VectorXf ringkey; 68 | }; 69 | 70 | class ScanContext { 71 | public: 72 | ScanContext(int max_range, int num_rings, int num_sectors); 73 | ScanContextBin ptcloud2bin(pcl::PointCloud::Ptr pt_cloud); 74 | 75 | private: 76 | int _max_range; 77 | int _num_rings; 78 | int _num_sectors; 79 | 80 | float _gap; 81 | float _angle_one_sector; 82 | 83 | //pcl::VoxelGrid downSizeFilterInput; 84 | 85 | float xy2Theta(float x, float y); 86 | Eigen::VectorXf scanContext2RingKey(Eigen::MatrixXf _max_bin); 87 | Eigen::MatrixXf ptCloud2ScanContext(pcl::PointCloud::Ptr pt_cloud); 88 | }; 89 | 90 | //circShift from https://github.com/irapkaist/SC-LeGO-LOAM 91 | Eigen::MatrixXf circShift( Eigen::MatrixXf &_mat, int _num_shift ) 92 | { 93 | // shift columns to right direction 94 | assert(_num_shift >= 0); 95 | 96 | if( _num_shift == 0 ) 97 | { 98 | Eigen::MatrixXf shifted_mat( _mat ); 99 | return shifted_mat; // Early return 100 | } 101 | 102 | Eigen::MatrixXf shifted_mat = Eigen::MatrixXf::Zero( _mat.rows(), _mat.cols() ); 103 | for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) 104 | { 105 | int new_location = (col_idx + _num_shift) % _mat.cols(); 106 | shifted_mat.col(new_location) = _mat.col(col_idx); 107 | } 108 | 109 | return shifted_mat; 110 | 111 | } 112 | 113 | #endif //SRC_SCANCONTEXT_H 114 | --------------------------------------------------------------------------------