├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── octomap_merger.h ├── launch ├── align_test.launch ├── octomap_merger.launch ├── octomap_merger_base.launch ├── octomap_merger_robot.launch └── octomap_merger_sim.launch ├── msg ├── OctomapArray.msg └── OctomapNeighbors.msg ├── package.xml └── src ├── align_test_node.cpp ├── icp_align.cpp ├── map_merger.cpp ├── octomap_merger.cpp └── octomap_merger_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(octomap_merger) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | octomap_ros 6 | octomap_msgs 7 | roscpp 8 | std_msgs 9 | sensor_msgs 10 | nav_msgs 11 | message_generation 12 | pcl_conversions 13 | pcl_ros 14 | ) 15 | 16 | add_message_files( 17 | FILES 18 | OctomapArray.msg 19 | OctomapNeighbors.msg 20 | ) 21 | 22 | generate_messages( 23 | DEPENDENCIES 24 | nav_msgs 25 | octomap_msgs 26 | ) 27 | 28 | include_directories( 29 | include 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | catkin_package( 34 | CATKIN_DEPENDS message_runtime 35 | ) 36 | 37 | add_library(icp_align src/icp_align.cpp) 38 | target_link_libraries(icp_align ${catkin_LIBRARIES}) 39 | 40 | add_library(map_merger src/map_merger.cpp) 41 | target_link_libraries(map_merger ${catkin_LIBRARIES}) 42 | 43 | add_executable(octomap_merger src/octomap_merger.cpp) 44 | target_link_libraries(octomap_merger icp_align map_merger ${catkin_LIBRARIES}) 45 | 46 | add_executable(octomap_merger_node src/octomap_merger_node.cpp) 47 | target_link_libraries(octomap_merger_node icp_align map_merger ${catkin_LIBRARIES}) 48 | 49 | add_executable(align_test_node src/align_test_node.cpp) 50 | target_link_libraries(align_test_node icp_align map_merger ${catkin_LIBRARIES}) 51 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Danny Riley 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # octomap_merger 2 | 3 | NOTE: This node has been replaced by https://github.com/dan-riley/marble_mapping which generates differences Octomaps for sharing more efficiently. 4 | 5 | Merges multiple Octomaps and optionally aligns them using ICP prior to merging. 6 | 7 | Derived from James Jessup, Sidney N. Givigi, and Alain Beaulieu. "Merging of octree based 3d occupancy grid maps." In 2014 IEEE International Systems Conference Proceedings, pp. 371-377. IEEE, 2014. DOI: 10.1109/SMC.2014.6974556 8 | 9 | 
10 | Usage: octomap_merger <input_file_1> <input_file_2> <output_file\> [align]
11 |                       [<translation_x\> <translation_y\> <translation_z\>]
12 |                       [<roll\> <pitch\> <yaw\>]
13 |
14 | 15 | Input files can be binary or full, but both must the same. 16 | Output can be either, but does not have to be the same as input. 17 | Use .bt or .ot file extension to specify 18 | 19 | Add 'align' option to run ICP alignment on maps before merging. 20 | 21 | ## Source Description 22 | 23 | octomap_merger.cpp - Command-line merger 24 | 25 | octomap_merger_node.cpp - ROS node for merging multiple maps in an array 26 | 27 | map_merger.cpp - Core functions that manage actual Octomap merging 28 | 29 | icp_align.cpp - Converts Octomaps to point clouds, finds ICP alignment, and transforms the second map to align with the first. 30 | -------------------------------------------------------------------------------- /include/octomap_merger.h: -------------------------------------------------------------------------------- 1 | #ifndef OCTOMAP_MERGER_H_ 2 | #define OCTOMAP_MERGER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include "octomap_merger/OctomapArray.h" 28 | #include "octomap_merger/OctomapNeighbors.h" 29 | 30 | using std::cout; 31 | using std::endl; 32 | using namespace octomap; 33 | using namespace octomath; 34 | 35 | // convenient typedefs 36 | typedef pcl::PointXYZ PointT; 37 | typedef pcl::PointCloud PointCloud; 38 | typedef pcl::PointNormal PointNormalT; 39 | typedef pcl::PointCloud PointCloudWithNormals; 40 | 41 | #define MAXITER 500 42 | 43 | // Convert a OcTree or OcTreeStamped to a PointCloud2 44 | template 45 | void tree2PointCloud(T *tree, pcl::PointCloud& pclCloud) { 46 | for (typename T::leaf_iterator it = tree->begin_leafs(), 47 | end = tree->end_leafs(); it != end; ++it) 48 | { 49 | if (tree->isNodeOccupied(*it)) { 50 | pclCloud.push_back( 51 | pcl::PointXYZ(it.getX(), 52 | it.getY(), 53 | it.getZ() 54 | ) 55 | ); 56 | } 57 | } 58 | } 59 | 60 | bool pointInBBox(pcl::PointXYZ& point, 61 | pcl::PointXYZ& bboxMin, 62 | pcl::PointXYZ& bboxMax); 63 | 64 | Eigen::Matrix4f getICPTransformation( 65 | pcl::PointCloud& cloud1, 66 | pcl::PointCloud& cloud2, 67 | Eigen::Matrix4f& tfEst, 68 | double mapRes); 69 | 70 | void transformTree(OcTree *tree, Eigen::Matrix4f& transform); 71 | 72 | void align_maps(OcTreeStamped *tree1, OcTree *tree2, point3d translation, 73 | double roll, double pitch, double yaw, double res); 74 | 75 | void merge_maps(OcTreeStamped *tree1, OcTree *tree2, bool full_merge, 76 | bool replace, bool overwrite, bool free_prioritize); 77 | 78 | double build_diff_tree(OcTree *tree1, OcTree *tree2, OcTree *tree_diff); 79 | 80 | class OctomapMerger { 81 | public: 82 | // Constructor 83 | OctomapMerger(ros::NodeHandle* nodehandle); 84 | // Destructor 85 | ~OctomapMerger(); 86 | // Callbacks 87 | void callback_myMap(const octomap_msgs::Octomap::ConstPtr& msg); 88 | void callback_neighborMaps(const octomap_merger::OctomapNeighborsConstPtr &msg); 89 | void callback_alignMap(const octomap_msgs::Octomap::ConstPtr& msg); 90 | // Public Methods 91 | void merge(); 92 | // Variables 93 | bool myMapNew; 94 | bool otherMapsNew; 95 | std::string id; 96 | std::string type; 97 | bool full_merge; 98 | bool free_prioritize; 99 | bool publish_merged_pcl; 100 | bool publish_diff_pcl; 101 | int octo_type; 102 | double resolution; 103 | int map_thresh; 104 | std::string map_topic; 105 | std::string neighbors_topic; 106 | std::string merged_topic; 107 | std::string map_diffs_topic; 108 | std::string num_diffs_topic; 109 | std::string pcl_diff_topic; 110 | std::string pcl_merged_topic; 111 | 112 | /* Private Variables and Methods */ 113 | private: 114 | ros::NodeHandle nh_; 115 | 116 | octomap_msgs::Octomap myMap; 117 | octomap_msgs::Octomap alignMap; 118 | octomap_merger::OctomapArray mapdiffs; 119 | octomap_merger::OctomapNeighbors neighbors; 120 | octomap::OcTreeStamped *tree_merged; 121 | octomap::OcTreeStamped *tree_pcl; 122 | octomap::OcTree *tree_sys; 123 | octomap::OcTree *tree_old; 124 | octomap::OcTree *tree_temp; 125 | octomap::OcTree *tree_diff; 126 | int num_diffs; 127 | std::map> seqs; 128 | 129 | ros::Subscriber sub_mymap; 130 | ros::Subscriber sub_neighbors; 131 | 132 | ros::Publisher pub_merged; 133 | ros::Publisher pub_size; 134 | ros::Publisher pub_mapdiffs; 135 | ros::Publisher pub_merged_pcl; 136 | ros::Publisher pub_diff_pcl; 137 | 138 | void initializeSubscribers(); 139 | void initializePublishers(); 140 | sensor_msgs::PointCloud2 buildPCL(OcTreeStamped *tree); 141 | }; 142 | 143 | #endif 144 | -------------------------------------------------------------------------------- /launch/align_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /launch/octomap_merger.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /launch/octomap_merger_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/octomap_merger_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/octomap_merger_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /msg/OctomapArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | octomap_msgs/Octomap[] octomaps 3 | string owner 4 | uint8 num_octomaps 5 | -------------------------------------------------------------------------------- /msg/OctomapNeighbors.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | OctomapArray[] neighbors 3 | uint8 num_neighbors 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | octomap_merger 4 | 0.1.0 5 | The octomap_merger package 6 | 7 | Dan Riley 8 | 9 | TODO 10 | 11 | https://www.github.com/dan-riley/octomap_merger 12 | 13 | Dan Riley 14 | 15 | catkin 16 | 17 | octomap_ros 18 | octomap_msgs 19 | roscpp 20 | rospy 21 | std_msgs 22 | sensor_msgs 23 | nav_msgs 24 | message_generation 25 | pcl_conversions 26 | pcl_ros 27 | 28 | octomap_ros 29 | octomap_msgs 30 | roscpp 31 | rospy 32 | std_msgs 33 | sensor_msgs 34 | nav_msgs 35 | message_generation 36 | pcl_conversions 37 | pcl_ros 38 | 39 | roscpp 40 | rospy 41 | std_msgs 42 | octomap_ros 43 | octomap_msgs 44 | sensor_msgs 45 | nav_msgs 46 | message_runtime 47 | pcl_conversions 48 | pcl_ros 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /src/align_test_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | OctomapMerger::OctomapMerger(ros::NodeHandle* nodehandle):nh_(*nodehandle) { 4 | ROS_INFO("Constructing OctomapMerger Class"); 5 | 6 | std::string nn = ros::this_node::getName(); 7 | // Load parameters from launch file 8 | nh_.param(nn + "/vehicle", id, "H01"); 9 | // Type of agent (robot or base) 10 | nh_.param(nn + "/type", type, "robot"); 11 | // Full merging or prioritize own map 12 | nh_.param(nn + "/fullMerge", full_merge, false); 13 | // Keep free space from any agent instead of adding obstacles 14 | nh_.param(nn + "/freePrioritize", free_prioritize, false); 15 | // Whether to publish PCL of merged map 16 | nh_.param(nn + "/publishMergedPCL", publish_merged_pcl, false); 17 | // Whether to publish PCL of diff map 18 | nh_.param(nn + "/publishDiffPCL", publish_diff_pcl, false); 19 | // Octomap type: 0: Binary, 1: Full 20 | nh_.param(nn + "/octoType", octo_type, 0); 21 | // Map resolution 22 | nh_.param(nn + "/resolution", resolution, (double)0.2); 23 | // Map size threshold to trigger a map merge 24 | nh_.param(nn + "/mapThresh", map_thresh, 50); 25 | 26 | // Topics for Subscribing and Publishing 27 | nh_.param(nn + "/mapTopic", map_topic, "octomap_binary"); 28 | nh_.param(nn + "/neighborsTopic", neighbors_topic, "neighbor_maps"); 29 | nh_.param(nn + "/mergedTopic", merged_topic, "merged_map"); 30 | nh_.param(nn + "/mapDiffsTopic", map_diffs_topic, "map_diffs"); 31 | nh_.param(nn + "/numDiffsTopic", num_diffs_topic, "numDiffs"); 32 | nh_.param(nn + "/pclMergedTopic", pcl_merged_topic, "pc2_merged"); 33 | nh_.param(nn + "/pclDiffTopic", pcl_diff_topic, "pc2_diff"); 34 | 35 | initializeSubscribers(); 36 | initializePublishers(); 37 | myMapNew = false; 38 | otherMapsNew = false; 39 | 40 | // Initialize Octomap holders once, assign/overwrite each loop 41 | tree_merged = new octomap::OcTreeStamped(resolution); 42 | tree_sys = new octomap::OcTree(resolution); 43 | tree_old = new octomap::OcTree(resolution); 44 | tree_temp = new octomap::OcTree(resolution); 45 | tree_diff = new octomap::OcTree(resolution); 46 | num_diffs = 0; 47 | 48 | if (publish_diff_pcl) tree_pcl = new octomap::OcTreeStamped(resolution); 49 | } 50 | 51 | // Destructor 52 | OctomapMerger::~OctomapMerger() { 53 | } 54 | 55 | void OctomapMerger::initializeSubscribers() { 56 | ROS_INFO("Initializing Subscribers"); 57 | sub_mymap = nh_.subscribe(map_topic, 100, 58 | &OctomapMerger::callback_myMap, this); 59 | sub_neighbors = nh_.subscribe(neighbors_topic, 100, 60 | &OctomapMerger::callback_alignMap, this); 61 | } 62 | 63 | void OctomapMerger::initializePublishers() { 64 | ROS_INFO("Initializing Publishers"); 65 | pub_merged = nh_.advertise(merged_topic, 1, true); 66 | pub_size = nh_.advertise(num_diffs_topic, 1, true); 67 | pub_mapdiffs = nh_.advertise(map_diffs_topic, 1, true); 68 | if (publish_merged_pcl) 69 | pub_merged_pcl = nh_.advertise(pcl_merged_topic, 1, true); 70 | if (publish_diff_pcl) 71 | pub_diff_pcl = nh_.advertise(pcl_diff_topic, 1, true); 72 | } 73 | 74 | // Callbacks 75 | void OctomapMerger::callback_myMap(const octomap_msgs::OctomapConstPtr& msg) { 76 | myMap = *msg; 77 | myMapNew = true; 78 | } 79 | 80 | void OctomapMerger::callback_alignMap(const octomap_msgs::OctomapConstPtr& msg) { 81 | alignMap = *msg; 82 | otherMapsNew = true; 83 | } 84 | 85 | sensor_msgs::PointCloud2 OctomapMerger::buildPCL(OcTreeStamped *tree) { 86 | tree->expand(); 87 | sensor_msgs::PointCloud2 pcl; 88 | PointCloud::Ptr occupiedCells(new PointCloud); 89 | tree2PointCloud(tree, *occupiedCells); 90 | pcl::toROSMsg(*occupiedCells, pcl); 91 | pcl.header.stamp = ros::Time::now(); 92 | pcl.header.frame_id = "world"; 93 | 94 | return pcl; 95 | } 96 | 97 | void OctomapMerger::merge() { 98 | double num_nodes = 0; 99 | if (type == "robot") { 100 | if (octo_type == 0) 101 | tree_sys = (octomap::OcTree*)octomap_msgs::binaryMsgToMap(myMap); 102 | else 103 | tree_sys = (octomap::OcTree*)octomap_msgs::fullMsgToMap(myMap); 104 | 105 | if (!tree_sys) return; 106 | 107 | // Get the diff tree from the current robot map and the last one saved 108 | num_nodes = build_diff_tree(tree_old, tree_sys, tree_diff); 109 | } 110 | 111 | octomap_msgs::Octomap msg; 112 | 113 | // If there are enough new nodes, save the robot map for next iter, and merge differences 114 | if (num_nodes > map_thresh) { 115 | tree_old->swapContent(*tree_sys); 116 | merge_maps(tree_merged, tree_diff, full_merge, true, false, free_prioritize); 117 | 118 | // Publish the diffs octomap 119 | tree_diff->prune(); 120 | num_diffs++; 121 | if (octo_type == 0) 122 | octomap_msgs::binaryMapToMsg(*tree_diff, msg); 123 | else 124 | octomap_msgs::fullMapToMsg(*tree_diff, msg); 125 | msg.header.stamp = ros::Time::now(); 126 | msg.header.frame_id = "world"; 127 | msg.header.seq = num_diffs - 1; 128 | 129 | // Add the diff to the map diffs array 130 | mapdiffs.octomaps.push_back(msg); 131 | mapdiffs.num_octomaps = num_diffs; 132 | pub_mapdiffs.publish(mapdiffs); 133 | 134 | // Publish the number of diffs so multi_agent doesn't have to subscribe to the whole map 135 | std_msgs::UInt32 size_msg; 136 | size_msg.data = num_diffs; 137 | pub_size.publish(size_msg); 138 | } 139 | 140 | // Remove all of the nodes whether we used them or not, for the next iter 141 | tree_diff->clear(); 142 | 143 | // Merge each neighbors' diff map to the merged map 144 | if (octo_type == 0) 145 | tree_temp = (octomap::OcTree*)octomap_msgs::binaryMsgToMap(alignMap); 146 | else 147 | tree_temp = (octomap::OcTree*)octomap_msgs::fullMsgToMap(alignMap); 148 | 149 | point3d translation; 150 | align_maps(tree_merged, tree_temp, translation, 0, 0.047, 0, resolution); 151 | 152 | // Merge neighbor map 153 | merge_maps(tree_merged, tree_temp, full_merge, false, true, free_prioritize); 154 | 155 | // Free the memory before the next neighbor 156 | delete tree_temp; 157 | 158 | // Prune and publish the Octomap 159 | tree_merged->prune(); 160 | if (octo_type == 0) 161 | octomap_msgs::binaryMapToMsg(*tree_merged, msg); 162 | else 163 | octomap_msgs::fullMapToMsg(*tree_merged, msg); 164 | msg.header.stamp = ros::Time::now(); 165 | msg.header.frame_id = "world"; 166 | msg.id = "OcTree"; // Required to convert OcTreeStamped into regular OcTree 167 | pub_merged.publish(msg); 168 | 169 | if (type == "robot") delete tree_sys; 170 | } 171 | 172 | int main (int argc, char **argv) { 173 | ros::init(argc, argv, "octomap_merger", ros::init_options::AnonymousName); 174 | ros::NodeHandle nh; 175 | 176 | double rate; 177 | std::string nn = ros::this_node::getName(); 178 | nh.param(nn + "/rate", rate, (double)0.1); 179 | 180 | OctomapMerger *octomap_merger = new OctomapMerger(&nh); 181 | 182 | ros::Rate r(rate); 183 | while(nh.ok()) { 184 | ros::spinOnce(); 185 | if(octomap_merger->myMapNew || octomap_merger->otherMapsNew) { 186 | octomap_merger->myMapNew = false; 187 | octomap_merger->otherMapsNew = false; 188 | octomap_merger->merge(); 189 | } 190 | r.sleep(); 191 | } 192 | return 0; 193 | } 194 | -------------------------------------------------------------------------------- /src/icp_align.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | bool pointInBBox(pcl::PointXYZ& point, 4 | pcl::PointXYZ& bboxMin, 5 | pcl::PointXYZ& bboxMax) { 6 | 7 | return (point.x < bboxMax.x && point.x > bboxMin.x) && 8 | (point.y < bboxMax.y && point.y > bboxMin.y) && 9 | (point.z < bboxMax.z && point.z > bboxMin.z); 10 | } 11 | 12 | Eigen::Matrix4f getICPTransformation( 13 | pcl::PointCloud& cloud1, 14 | pcl::PointCloud& cloud2, 15 | Eigen::Matrix4f& tfEst, 16 | double mapRes) { 17 | 18 | // apply the tfEst to cloud2 19 | pcl::transformPointCloud(cloud2, cloud2, tfEst); 20 | 21 | // get the bounding region of cloud1 to 22 | // extract the points from cloud2 contained in the region 23 | pcl::PointXYZ minCloud1; pcl::PointXYZ maxCloud1; 24 | pcl::getMinMax3D(cloud1, minCloud1, maxCloud1); 25 | 26 | // filter out the points in cloud2 that are not in cloud1’s range 27 | pcl::PointCloud::Ptr cloud2filtered( 28 | new pcl::PointCloud); 29 | 30 | for (pcl::PointCloud::iterator it = cloud2.begin(); 31 | it != cloud2.end(); it++) { 32 | 33 | if (pointInBBox(*it, minCloud1, maxCloud1)) { 34 | cloud2filtered->push_back(*it); 35 | } 36 | } 37 | 38 | // filter out the points in cloud1 that are not in cloud2’s range 39 | pcl::PointCloud::Ptr cloud1filtered( 40 | new pcl::PointCloud); 41 | 42 | // same for other cloud 43 | pcl::PointXYZ minCloud2filtered; pcl::PointXYZ maxCloud2filtered; 44 | pcl::getMinMax3D(*cloud2filtered, minCloud2filtered, maxCloud2filtered); 45 | 46 | minCloud2filtered = pcl::PointXYZ( 47 | minCloud2filtered.x - 1, 48 | minCloud2filtered.y - 1, 49 | minCloud2filtered.z - 1 50 | ); 51 | 52 | maxCloud2filtered = pcl::PointXYZ( 53 | maxCloud2filtered.x + 1, 54 | maxCloud2filtered.y + 1, 55 | maxCloud2filtered.z + 1 56 | ); 57 | 58 | for (pcl::PointCloud::iterator it = cloud1.begin(); 59 | it != cloud1.end(); it++) { 60 | 61 | if (pointInBBox(*it, minCloud2filtered, maxCloud2filtered)) { 62 | cloud1filtered->push_back(*it); 63 | } 64 | } 65 | 66 | // Downsample for consistency and speed 67 | PointCloud::Ptr src(new PointCloud); 68 | PointCloud::Ptr tgt(new PointCloud); 69 | pcl::VoxelGrid grid; 70 | grid.setLeafSize(10 * mapRes, 10 * mapRes, 10 * mapRes); 71 | grid.setInputCloud(cloud1filtered); 72 | grid.filter(*tgt); 73 | 74 | grid.setInputCloud(cloud2filtered); 75 | grid.filter(*src); 76 | 77 | // Align 78 | pcl::IterativeClosestPointNonLinear reg; 79 | reg.setTransformationEpsilon(mapRes / 60); 80 | // Increased this from 10 to 200 because of errors when running 81 | // Not sure what the right numbers should be 82 | reg.setMaxCorrespondenceDistance(200 * mapRes); 83 | 84 | Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev; 85 | PointCloud::Ptr reg_result; 86 | 87 | if (src->size() < tgt->size()) { 88 | reg.setInputSource(src); 89 | reg.setInputTarget(tgt); 90 | 91 | // Run the same optimization in a loop and visualize the results 92 | reg_result = src; 93 | reg.setMaximumIterations(2); 94 | for (int i=0; i < MAXITER; ++i) { 95 | // save cloud for visualization purpose 96 | src = reg_result; 97 | 98 | // Estimate 99 | reg.setInputSource(src); 100 | reg.align(*reg_result); 101 | 102 | // accumulate transformation between each Iteration 103 | Ti = reg.getFinalTransformation() * Ti; 104 | 105 | // if the difference between this transformation and the previous one 106 | // is smaller than the threshold, refine the process by reducing 107 | // the maximal correspondence distance 108 | if (reg.getMaxCorrespondenceDistance() > 0.2) { 109 | if (fabs((reg.getLastIncrementalTransformation().sum() - prev.sum())) 110 | < reg.getTransformationEpsilon()) 111 | reg.setMaxCorrespondenceDistance( 112 | reg.getMaxCorrespondenceDistance() - 0.1 113 | ); 114 | } else if (reg.getMaxCorrespondenceDistance() > 0.002) { 115 | if (fabs((reg.getLastIncrementalTransformation().sum() - prev.sum())) 116 | < reg.getTransformationEpsilon()) 117 | reg.setMaxCorrespondenceDistance( 118 | reg.getMaxCorrespondenceDistance() - 0.001 119 | ); 120 | } 121 | 122 | prev = reg.getLastIncrementalTransformation(); 123 | } 124 | } else { 125 | reg.setInputSource(tgt); 126 | reg.setInputTarget(src); 127 | 128 | // Run the same optimization in a loop and visualize the results 129 | reg_result = tgt; 130 | reg.setMaximumIterations(2); 131 | for (int i=0; i < MAXITER; ++i) { 132 | // save cloud for visualization purpose 133 | tgt = reg_result; 134 | 135 | // Estimate 136 | reg.setInputSource(tgt); 137 | reg.align(*reg_result); 138 | 139 | // accumulate transformation between each Iteration 140 | Ti = reg.getFinalTransformation() * Ti; 141 | 142 | // if the difference between this transformation and the previous one 143 | // is smaller than the threshold, refine the process by reducing 144 | // the maximal correspondence distance 145 | if (reg.getMaxCorrespondenceDistance() > 0.2) { 146 | if (fabs((reg.getLastIncrementalTransformation().sum() - prev.sum())) 147 | < reg.getTransformationEpsilon()) 148 | reg.setMaxCorrespondenceDistance( 149 | reg.getMaxCorrespondenceDistance() - 0.1 150 | ); 151 | } else if (reg.getMaxCorrespondenceDistance() > 0.002) { 152 | if (fabs((reg.getLastIncrementalTransformation().sum() - prev.sum())) 153 | < reg.getTransformationEpsilon()) 154 | reg.setMaxCorrespondenceDistance( 155 | reg.getMaxCorrespondenceDistance() - 0.001 156 | ); 157 | } 158 | 159 | prev = reg.getLastIncrementalTransformation(); 160 | } 161 | Ti = Ti.inverse(); 162 | } 163 | 164 | return Ti * tfEst; 165 | } 166 | 167 | double getSign(double x) { 168 | if (x < 0) return -1; 169 | else return 1; 170 | } 171 | 172 | void transformTree(OcTree *tree, Eigen::Matrix4f& transform) { 173 | double treeRes = tree->getResolution(); 174 | OcTree* transformed = new OcTree(treeRes); 175 | 176 | // build inverse transform 177 | Eigen::Matrix3f rotation; 178 | Eigen::Matrix3f invRotation; 179 | Eigen::Matrix4f invTransform; 180 | rotation << transform(0, 0), transform(0, 1), transform(0, 2), 181 | transform(1, 0), transform(1, 1), transform(1, 2), 182 | transform(2, 0), transform(0, 2), transform(2, 2); 183 | invRotation = rotation.transpose(); 184 | invTransform << 185 | invRotation(0, 0), invRotation(0, 1), invRotation(0, 2), -transform(0, 3), 186 | invRotation(1, 0), invRotation(1, 1), invRotation(1, 2), -transform( 1, 3), 187 | invRotation(2, 0), invRotation(2, 1), invRotation(2, 2), -transform( 2, 3), 188 | 0, 0, 0, 1; 189 | 190 | // size in each coordinate of each axis. 191 | double minX, maxX, minY, maxY, minZ, maxZ; 192 | 193 | // get the minimum and maxin y so we can step along each row 194 | tree->getMetricMin(minX, minY, minZ); 195 | tree->getMetricMax(maxX, maxY, maxZ); 196 | 197 | // allocate a vector of points 198 | std::vector points; 199 | 200 | // make 8 points to make a map bounding box, performing the tf on them 201 | // to get the range of values in the transformed map 202 | points.push_back(point3d(maxX,minY,minZ)); 203 | points.push_back(point3d(minX,minY,minZ)); 204 | points.push_back(point3d(minX,maxY,minZ)); 205 | points.push_back(point3d(maxX,maxY,minZ)); 206 | points.push_back(point3d(maxX,minY,maxZ)); 207 | points.push_back(point3d(minX,minY,maxZ)); 208 | points.push_back(point3d(minX,maxY,maxZ)); 209 | points.push_back(point3d(maxX,maxY,maxZ)); 210 | 211 | // transform the points 212 | for (unsigned i=0; i < points.size(); i++) { 213 | Eigen::Vector4f point(points[i].x(), points[i].y(), points[i].z(), 1); 214 | point = transform * point; 215 | points[i] = point3d(point(0), point(1), point(2)); 216 | } 217 | 218 | // go through tf’d points to get a newb box 219 | minX = points[0].x(); minY = points[0].y(); minZ = points[0].z(); 220 | maxX = points[0].x(); maxY = points[0].y(); maxZ = points[0].z(); 221 | for (unsigned i=0; i < points.size(); i++) { 222 | minX = (points[i].x() < minX) ? points[i].x() : minX; 223 | minY = (points[i].y() < minY) ? points[i].y() : minY; 224 | minZ = (points[i].z() < minZ) ? points[i].z() : minZ; 225 | maxX = (points[i].x() > maxX) ? points[i].x() : maxX; 226 | maxY = (points[i].y() > maxY) ? points[i].y() : maxY; 227 | maxZ = (points[i].z() > maxZ) ? points[i].z() : maxZ; 228 | } 229 | 230 | // go through the possible destination voxels on a row by row basis 231 | // and calculate occupancy from source voxels with inverse tf 232 | for (double z = minZ - treeRes / 2; z < (maxZ + treeRes / 2); z += treeRes) { 233 | for (double y = minY - treeRes / 2; y < (maxY + treeRes / 2); y += treeRes) { 234 | for (double x = minX - treeRes / 2; x < (maxX + treeRes / 2); x += treeRes) { 235 | OcTreeKey destVoxel = transformed->coordToKey( 236 | point3d(x,y,z) 237 | ); 238 | Eigen::Vector4f point(x,y,z,1); 239 | point = invTransform * point; 240 | point3d sourcePoint = point3d(point(0), point(1), point(2)); 241 | OcTreeKey sourceVoxel = tree->coordToKey(sourcePoint); 242 | point3d nn = tree->keyToCoord(sourceVoxel); 243 | 244 | // use nearest neighbour to set new occupancy in the transformed map 245 | OcTreeNode *oldNode = tree->search(sourceVoxel); 246 | 247 | // Occupancies to interpolate between 248 | double c000, c001, c010, c011, c100, c101, c110, 249 | c111, c00, c01, c10, c11, c0, c1; 250 | double xd,yd,zd; 251 | 252 | // differences in each direction between next closest voxel 253 | xd = (sourcePoint.x() - nn.x()) / treeRes; 254 | yd = (sourcePoint.y() - nn.y()) / treeRes; 255 | zd = (sourcePoint.z() - nn.z()) / treeRes; 256 | 257 | if (oldNode != NULL) { 258 | c000 = oldNode->getOccupancy(); 259 | OcTreeNode *node; 260 | 261 | // c001 262 | if ((node = tree->search( 263 | point3d(nn.x(), nn.y(), nn.z() + 264 | getSign(zd) * treeRes))) 265 | != NULL) { 266 | c001 = node->getOccupancy(); 267 | } else 268 | c001 = 0; 269 | 270 | // c010 271 | if ((node=tree->search( 272 | point3d(nn.x(), 273 | nn.y() + getSign(yd) * treeRes, 274 | nn.z()))) 275 | != NULL) { 276 | c010 =node->getOccupancy(); 277 | } else 278 | c010 = 0; 279 | 280 | // c011 281 | if ((node=tree->search( 282 | point3d(nn.x(), 283 | nn.y() + getSign(yd) * treeRes, 284 | nn.z() + getSign(zd) * treeRes))) 285 | != NULL) { 286 | c011 = node->getOccupancy(); 287 | } else 288 | c011 = 0; 289 | 290 | // c100 291 | if ((node=tree->search( 292 | point3d(nn.x() + getSign(xd) * treeRes, 293 | nn.y(), 294 | nn.z()))) 295 | != NULL) { 296 | c100 = node->getOccupancy(); 297 | } else 298 | c100 = 0; 299 | 300 | // c101 301 | if ((node=tree->search( 302 | point3d(nn.x() + getSign(xd) * treeRes, 303 | nn.y(), 304 | nn.z() + getSign(zd) * treeRes))) 305 | != NULL) { 306 | c101 = node->getOccupancy(); 307 | } else 308 | c101 = 0; 309 | 310 | // c110 311 | if ((node=tree->search( 312 | point3d(nn.x() + getSign(xd) * treeRes, 313 | nn.y() + getSign(yd) * treeRes, 314 | nn.z()))) 315 | != NULL) { 316 | c110 = node->getOccupancy(); 317 | } else 318 | c110 = 0; 319 | 320 | // c111 321 | if ((node=tree->search( 322 | point3d(nn.x() + getSign(xd) * treeRes, 323 | nn.y() + getSign(yd) * treeRes, 324 | nn.z() + getSign(zd) * treeRes))) 325 | != NULL) { 326 | c111 = node->getOccupancy(); 327 | } else 328 | c111 = 0; 329 | 330 | // Interpolate in x 331 | c00 = (1-fabs(xd)) * c000 + fabs(xd) * c100; 332 | c10 = (1-fabs(xd)) * c010 + fabs(xd) * c110; 333 | c01 = (1-fabs(xd)) * c001 + fabs(xd) * c101; 334 | c11 = (1-fabs(xd)) * c011 + fabs(xd) * c111; 335 | 336 | // interpolate in y 337 | c0 = (1-fabs(yd)) * c00 + fabs(yd) * c10; 338 | c1 = (1-fabs(yd)) * c01 + fabs(yd) * c11; 339 | 340 | // now let’s assign the new node value 341 | OcTreeNode *newNode = transformed->updateNode(destVoxel, true); 342 | newNode->setLogOdds(logodds((1 - fabs(zd)) * c0 + fabs(zd) * c1)); 343 | } 344 | } 345 | } 346 | } 347 | 348 | tree->swapContent(*transformed); 349 | 350 | delete transformed; 351 | } 352 | 353 | void align_maps(OcTreeStamped *tree1, OcTree *tree2, point3d translation, 354 | double roll, double pitch, double yaw, double res) { 355 | Pose6D pose(translation.x(), 356 | translation.y(), 357 | translation.z(), 358 | roll, pitch, yaw); 359 | 360 | // build a transform matrix 361 | Eigen::Matrix4f transform; 362 | std::vector coeffs; 363 | pose.rot().toRotMatrix(coeffs); 364 | 365 | transform << coeffs[0], coeffs[1], coeffs[2], translation.x(), 366 | coeffs[3], coeffs[4], coeffs[5], translation.y(), 367 | coeffs[6], coeffs[7], coeffs[8], translation.z(), 368 | 0, 0, 0, 1; 369 | 370 | // initial TF Matrix 371 | cout << transform << endl; 372 | 373 | // make point clouds from each map 374 | pcl::PointCloud tree1Points; 375 | tree2PointCloud(tree1, tree1Points); 376 | pcl::PointCloud tree2Points; 377 | tree2PointCloud(tree2, tree2Points); 378 | 379 | // get refined matrix 380 | transform = getICPTransformation(tree1Points, tree2Points, transform, res); 381 | 382 | // Resulting transform after correction 383 | cout << transform << endl; 384 | 385 | if (roll != 0 || 386 | pitch != 0 || 387 | yaw != 0 || 388 | translation.x() != 0 || 389 | translation.y() != 0 || 390 | translation.z() != 0 ) { 391 | transformTree(tree2, transform); 392 | } 393 | } 394 | -------------------------------------------------------------------------------- /src/map_merger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void merge_maps(OcTreeStamped *tree1, OcTree *tree2, bool full_merge, 4 | bool replace, bool overwrite, bool free_prioritize) { 5 | // full_merge = add node values instead of replace/overwrite 6 | // replace = always replace an existing node 7 | // overwrite = replace an existing node if the node is marked with timestamp = 0 8 | // free_prioritize = only available with full_merge; if any map shows free node, make free 9 | // Expand tree so we search all nodes 10 | tree2->expand(); 11 | 12 | // Timestamp to mark a node as an original from the owner or not 13 | int ts = replace ? 1 : 0; 14 | 15 | // traverse nodes in tree2 to add them to tree1 16 | for (OcTree::leaf_iterator it = tree2->begin_leafs(); it != tree2->end_leafs(); ++it) { 17 | OcTreeKey nodeKey = it.getKey(); 18 | OcTreeNodeStamped *nodeIn1 = tree1->search(nodeKey); 19 | if (nodeIn1 != NULL) { 20 | // Merge or replace the node in tree1 based on input parameters 21 | if (full_merge) { 22 | // If set, if any map shows a node free, make it free 23 | if (free_prioritize) { 24 | if ((nodeIn1->getOccupancy() < 0.5) || (it->getOccupancy() < 0.5)) 25 | tree1->setNodeValue(nodeKey, octomap::logodds(0)); 26 | else 27 | tree1->updateNode(nodeKey, it->getLogOdds()); 28 | } else { 29 | tree1->updateNode(nodeKey, it->getLogOdds()); 30 | } 31 | 32 | nodeIn1->setTimestamp(ts); 33 | } else if (replace || (overwrite && (nodeIn1->getTimestamp() == 0))) { 34 | tree1->setNodeValue(nodeKey, it->getLogOdds()); 35 | nodeIn1->setTimestamp(ts); 36 | } 37 | } else { 38 | // Add the node to tree1 39 | OcTreeNodeStamped *newNode = tree1->setNodeValue(nodeKey, it->getLogOdds()); 40 | newNode->setTimestamp(ts); 41 | } 42 | } 43 | } 44 | 45 | double build_diff_tree(OcTree *tree1, OcTree *tree2, OcTree *tree_diff) { 46 | // Find the differences in tree2 from tree1 and write to a new diff tree 47 | 48 | // Expand tree so we search all nodes 49 | tree2->expand(); 50 | double num_new_nodes = 0; 51 | 52 | // traverse nodes in tree2 to add them to tree1 53 | for (OcTree::leaf_iterator it = tree2->begin_leafs(); it != tree2->end_leafs(); ++it) { 54 | OcTreeKey nodeKey = it.getKey(); 55 | OcTreeNode *nodeIn1 = tree1->search(nodeKey); 56 | if (nodeIn1 != NULL) { 57 | // Change the diff tree (may be add, or may be new) if the two maps are different 58 | if (nodeIn1->getOccupancy() != it->getOccupancy()) { 59 | tree_diff->setNodeValue(nodeKey, it->getLogOdds()); 60 | } 61 | } else { 62 | // Add to the diff tree if it's a new node 63 | OcTreeNode *newDiffNode = tree_diff->setNodeValue(nodeKey, it->getLogOdds()); 64 | num_new_nodes++; 65 | } 66 | } 67 | 68 | return num_new_nodes; 69 | } 70 | -------------------------------------------------------------------------------- /src/octomap_merger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) { 4 | if (argc < 4) { 5 | cout << "\nUsage: octomap_merger [align]"; 6 | cout << "\n "; 7 | cout << "[ ]"; 8 | cout << "\n "; 9 | cout << "[ ]"; 10 | cout << "\n\nInput files can be binary or full, but both must the same.\n"; 11 | cout << "Output can be either, but does not have to be the same as input.\n"; 12 | cout << "Use .bt or .ot file extension to specify\n\n"; 13 | cout << "Add 'align' option to run ICP alignment on maps before merging.\n\n"; 14 | exit(0); 15 | } 16 | 17 | std::string filename1 = std::string(argv[1]); 18 | std::string filename2 = std::string(argv[2]); 19 | std::string outputFilename = std::string(argv[3]); 20 | 21 | cout << "\nReading octree files...\n"; 22 | 23 | OcTree *treet, *tree2; 24 | if (filename1.substr(filename1.length() - 2) == "ot") { 25 | treet = dynamic_cast(OcTree::read(filename1)); 26 | tree2 = dynamic_cast(OcTree::read(filename2)); 27 | } else { 28 | treet = new OcTree(filename1); 29 | tree2 = new OcTree(filename2); 30 | } 31 | 32 | // Assume the resolution of each map is the same 33 | double res = treet->getResolution(); 34 | 35 | // Convert treet to OcTreeStamped 36 | OcTreeStamped *tree1 = new OcTreeStamped(res); 37 | for (OcTree::leaf_iterator it = treet->begin_leafs(); it != treet->end_leafs(); ++it) { 38 | OcTreeKey nodeKey = it.getKey(); 39 | tree1->setNodeValue(nodeKey, it->getLogOdds()); 40 | } 41 | 42 | // Align the maps if desired. Will slow down the process! 43 | if ((argc > 4) && (std::string(argv[4]) == "align")) { 44 | cout << "Registering map to Improve TF Estimate" << endl << endl; 45 | 46 | double roll, pitch, yaw; 47 | 48 | point3d translation; 49 | if (argc == 8 || argc == 11) { 50 | translation = point3d(atof(argv[5]), atof(argv[6]), atof(argv[7])); 51 | } 52 | if (argc == 11) { 53 | roll = atof(argv[8]); 54 | pitch = atof(argv[9]); 55 | yaw = atof(argv[10]); 56 | } else { 57 | roll = 0; 58 | pitch = 0; 59 | yaw = 0; 60 | } 61 | 62 | align_maps(tree1, tree2, translation, roll, pitch, yaw, res); 63 | } 64 | 65 | cout << "about to merge maps" << endl; 66 | merge_maps(tree1, tree2, true, false, false, false); 67 | 68 | cout << "Compressing merged result\n"; 69 | tree1->prune(); 70 | // tree1 is now the compressed merged map 71 | 72 | // write merged map to file 73 | if (outputFilename.substr(outputFilename.length() - 2) == "ot") { 74 | tree1->write(outputFilename); 75 | } else { 76 | tree1->writeBinary(outputFilename); 77 | } 78 | 79 | delete tree1; 80 | delete tree2; 81 | } 82 | -------------------------------------------------------------------------------- /src/octomap_merger_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | OctomapMerger::OctomapMerger(ros::NodeHandle* nodehandle):nh_(*nodehandle) { 4 | ROS_INFO("Constructing OctomapMerger Class"); 5 | 6 | std::string nn = ros::this_node::getName(); 7 | // Load parameters from launch file 8 | nh_.param(nn + "/vehicle", id, "H01"); 9 | // Type of agent (robot or base) 10 | nh_.param(nn + "/type", type, "robot"); 11 | // Full merging or prioritize own map 12 | nh_.param(nn + "/fullMerge", full_merge, false); 13 | // Keep free space from any agent instead of adding obstacles 14 | nh_.param(nn + "/freePrioritize", free_prioritize, false); 15 | // Whether to publish PCL of merged map 16 | nh_.param(nn + "/publishMergedPCL", publish_merged_pcl, false); 17 | // Whether to publish PCL of diff map 18 | nh_.param(nn + "/publishDiffPCL", publish_diff_pcl, false); 19 | // Octomap type: 0: Binary, 1: Full 20 | nh_.param(nn + "/octoType", octo_type, 0); 21 | // Map resolution 22 | nh_.param(nn + "/resolution", resolution, (double)0.2); 23 | // Map size threshold to trigger a map merge 24 | nh_.param(nn + "/mapThresh", map_thresh, 50); 25 | 26 | // Topics for Subscribing and Publishing 27 | nh_.param(nn + "/mapTopic", map_topic, "octomap_binary"); 28 | nh_.param(nn + "/neighborsTopic", neighbors_topic, "neighbor_maps"); 29 | nh_.param(nn + "/mergedTopic", merged_topic, "merged_map"); 30 | nh_.param(nn + "/mapDiffsTopic", map_diffs_topic, "map_diffs"); 31 | nh_.param(nn + "/numDiffsTopic", num_diffs_topic, "numDiffs"); 32 | nh_.param(nn + "/pclMergedTopic", pcl_merged_topic, "pc2_merged"); 33 | nh_.param(nn + "/pclDiffTopic", pcl_diff_topic, "pc2_diff"); 34 | 35 | initializeSubscribers(); 36 | initializePublishers(); 37 | myMapNew = false; 38 | otherMapsNew = false; 39 | 40 | // Initialize Octomap holders once, assign/overwrite each loop 41 | tree_merged = new octomap::OcTreeStamped(resolution); 42 | tree_sys = new octomap::OcTree(resolution); 43 | tree_old = new octomap::OcTree(resolution); 44 | tree_temp = new octomap::OcTree(resolution); 45 | tree_diff = new octomap::OcTree(resolution); 46 | num_diffs = 0; 47 | 48 | if (publish_diff_pcl) tree_pcl = new octomap::OcTreeStamped(resolution); 49 | } 50 | 51 | // Destructor 52 | OctomapMerger::~OctomapMerger() { 53 | } 54 | 55 | void OctomapMerger::initializeSubscribers() { 56 | ROS_INFO("Initializing Subscribers"); 57 | sub_mymap = nh_.subscribe(map_topic, 100, 58 | &OctomapMerger::callback_myMap, this); 59 | sub_neighbors = nh_.subscribe(neighbors_topic, 100, 60 | &OctomapMerger::callback_neighborMaps, this); 61 | } 62 | 63 | void OctomapMerger::initializePublishers() { 64 | ROS_INFO("Initializing Publishers"); 65 | pub_merged = nh_.advertise(merged_topic, 1, true); 66 | pub_size = nh_.advertise(num_diffs_topic, 1, true); 67 | pub_mapdiffs = nh_.advertise(map_diffs_topic, 1, true); 68 | if (publish_merged_pcl) 69 | pub_merged_pcl = nh_.advertise(pcl_merged_topic, 1, true); 70 | if (publish_diff_pcl) 71 | pub_diff_pcl = nh_.advertise(pcl_diff_topic, 1, true); 72 | } 73 | 74 | // Callbacks 75 | void OctomapMerger::callback_myMap(const octomap_msgs::OctomapConstPtr& msg) { 76 | myMap = *msg; 77 | myMapNew = true; 78 | } 79 | 80 | void OctomapMerger::callback_neighborMaps( 81 | const octomap_merger::OctomapNeighborsConstPtr& msg) { 82 | neighbors = *msg; 83 | otherMapsNew = true; 84 | } 85 | 86 | sensor_msgs::PointCloud2 OctomapMerger::buildPCL(OcTreeStamped *tree) { 87 | tree->expand(); 88 | sensor_msgs::PointCloud2 pcl; 89 | PointCloud::Ptr occupiedCells(new PointCloud); 90 | tree2PointCloud(tree, *occupiedCells); 91 | pcl::toROSMsg(*occupiedCells, pcl); 92 | pcl.header.stamp = ros::Time::now(); 93 | pcl.header.frame_id = "world"; 94 | 95 | return pcl; 96 | } 97 | 98 | void OctomapMerger::merge() { 99 | double num_nodes = 0; 100 | if (type == "robot") { 101 | if (octo_type == 0) 102 | tree_sys = (octomap::OcTree*)octomap_msgs::binaryMsgToMap(myMap); 103 | else 104 | tree_sys = (octomap::OcTree*)octomap_msgs::fullMsgToMap(myMap); 105 | 106 | if (!tree_sys) return; 107 | 108 | // Get the diff tree from the current robot map and the last one saved 109 | num_nodes = build_diff_tree(tree_old, tree_sys, tree_diff); 110 | } 111 | 112 | octomap_msgs::Octomap msg; 113 | 114 | // If there are enough new nodes, save the robot map for next iter, and merge differences 115 | if (num_nodes > map_thresh) { 116 | tree_old->swapContent(*tree_sys); 117 | merge_maps(tree_merged, tree_diff, full_merge, true, false, free_prioritize); 118 | 119 | // Publish the diffs octomap 120 | tree_diff->prune(); 121 | num_diffs++; 122 | if (octo_type == 0) 123 | octomap_msgs::binaryMapToMsg(*tree_diff, msg); 124 | else 125 | octomap_msgs::fullMapToMsg(*tree_diff, msg); 126 | msg.header.stamp = ros::Time::now(); 127 | msg.header.frame_id = "world"; 128 | msg.header.seq = num_diffs - 1; 129 | 130 | // Add the diff to the map diffs array 131 | mapdiffs.octomaps.push_back(msg); 132 | mapdiffs.num_octomaps = num_diffs; 133 | pub_mapdiffs.publish(mapdiffs); 134 | 135 | // Publish the number of diffs so multi_agent doesn't have to subscribe to the whole map 136 | std_msgs::UInt32 size_msg; 137 | size_msg.data = num_diffs; 138 | pub_size.publish(size_msg); 139 | } 140 | 141 | // Remove all of the nodes whether we used them or not, for the next iter 142 | tree_diff->clear(); 143 | 144 | // Merge each neighbors' diff map to the merged map 145 | bool overwrite_node; 146 | for (int i=0; i < neighbors.num_neighbors; i++) { 147 | std::string nid = neighbors.neighbors[i].owner; 148 | // Check each diff for new ones to merge 149 | for (int j=0; j < neighbors.neighbors[i].num_octomaps; j++) { 150 | uint32_t cur_seq = neighbors.neighbors[i].octomaps[j].header.seq; 151 | bool exists = std::count(seqs[nid.data()].cbegin(), seqs[nid.data()].cend(), cur_seq); 152 | 153 | if (!exists) { 154 | // ROS_INFO("%s Merging neighbor %s seq %d", id.data(), nid.data(), cur_seq); 155 | seqs[nid.data()].push_back(cur_seq); 156 | if (octo_type == 0) 157 | tree_temp = (octomap::OcTree*)octomap_msgs::binaryMsgToMap(neighbors.neighbors[i].octomaps[j]); 158 | else 159 | tree_temp = (octomap::OcTree*)octomap_msgs::fullMsgToMap(neighbors.neighbors[i].octomaps[j]); 160 | 161 | // TODO Still problem where only replacing, not merging. If multiple neighbors see the same node, only the last one received gets used 162 | // If it's latest, merge and append. If not, only append 163 | if (cur_seq >= *std::max_element(seqs[nid.data()].cbegin(), seqs[nid.data()].cend())) 164 | overwrite_node = true; 165 | else 166 | overwrite_node = false; 167 | 168 | // Merge neighbor map 169 | merge_maps(tree_merged, tree_temp, full_merge, false, overwrite_node, free_prioritize); 170 | 171 | if (publish_diff_pcl) 172 | merge_maps(tree_pcl, tree_temp, true, false, false, free_prioritize); 173 | 174 | // Free the memory before the next neighbor 175 | delete tree_temp; 176 | } 177 | } 178 | } 179 | 180 | // Publish the merged PCL 181 | if (publish_merged_pcl && (tree_merged->getRoot() != NULL)) { 182 | pub_merged_pcl.publish(buildPCL(tree_merged)); 183 | } 184 | 185 | // Publish the PCL diff 186 | if (publish_diff_pcl && (tree_pcl->getRoot() != NULL)) { 187 | pub_diff_pcl.publish(buildPCL(tree_pcl)); 188 | tree_pcl->clear(); 189 | } 190 | 191 | // Prune and publish the Octomap 192 | tree_merged->prune(); 193 | if (octo_type == 0) 194 | octomap_msgs::binaryMapToMsg(*tree_merged, msg); 195 | else 196 | octomap_msgs::fullMapToMsg(*tree_merged, msg); 197 | msg.header.stamp = ros::Time::now(); 198 | msg.header.frame_id = "world"; 199 | msg.id = "OcTree"; // Required to convert OcTreeStamped into regular OcTree 200 | pub_merged.publish(msg); 201 | 202 | if (type == "robot") delete tree_sys; 203 | } 204 | 205 | int main (int argc, char **argv) { 206 | ros::init(argc, argv, "octomap_merger", ros::init_options::AnonymousName); 207 | ros::NodeHandle nh; 208 | 209 | double rate; 210 | std::string nn = ros::this_node::getName(); 211 | nh.param(nn + "/rate", rate, (double)0.1); 212 | 213 | OctomapMerger *octomap_merger = new OctomapMerger(&nh); 214 | 215 | ros::Rate r(rate); 216 | while(nh.ok()) { 217 | ros::spinOnce(); 218 | if(octomap_merger->myMapNew || octomap_merger->otherMapsNew) { 219 | octomap_merger->myMapNew = false; 220 | octomap_merger->otherMapsNew = false; 221 | octomap_merger->merge(); 222 | } 223 | r.sleep(); 224 | } 225 | return 0; 226 | } 227 | --------------------------------------------------------------------------------