diff --git a/map_merge/CMakeLists.txt b/map_merge/CMakeLists.txt index 0d46ea6..96d06f0 100644 --- a/map_merge/CMakeLists.txt +++ b/map_merge/CMakeLists.txt @@ -46,6 +46,54 @@ else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() +# Build Types +set(CMAKE_BUILD_TYPE Debug) +# CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel tsan asan lsan msan ubsan" +# FORCE) +# set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} +# CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel tsan asan lsan msan ubsan" +# FORCE) + +# # ThreadSanitizer +# set(CMAKE_C_FLAGS_TSAN +# "-fsanitize=thread -g -O1" +# CACHE STRING "Flags used by the C compiler during ThreadSanitizer builds." +# FORCE) +# set(CMAKE_CXX_FLAGS_TSAN +# "-fsanitize=thread -g -O1" +# CACHE STRING "Flags used by the C++ compiler during ThreadSanitizer builds." +# FORCE) + +# # AddressSanitize +# set(CMAKE_C_FLAGS_ASAN +# "-fsanitize=address -fno-optimize-sibling-calls -fno-omit-frame-pointer -g -O1" +# CACHE STRING "Flags used by the C compiler during AddressSanitizer builds." +# FORCE) +# set(CMAKE_CXX_FLAGS_ASAN +# "-fsanitize=address -fno-optimize-sibling-calls -fno-omit-frame-pointer -g -O1" +# CACHE STRING "Flags used by the C++ compiler during AddressSanitizer builds." +# FORCE) + +# # LeakSanitizer +# set(CMAKE_C_FLAGS_LSAN +# "-fsanitize=leak -fno-omit-frame-pointer -g -O1" +# CACHE STRING "Flags used by the C compiler during LeakSanitizer builds." +# FORCE) +# set(CMAKE_CXX_FLAGS_LSAN +# "-fsanitize=leak -fno-omit-frame-pointer -g -O1" +# CACHE STRING "Flags used by the C++ compiler during LeakSanitizer builds." +# FORCE) + +# # MemorySanitizer +# set(CMAKE_C_FLAGS_MSAN +# "-fsanitize=memory -fno-optimize-sibling-calls -fsanitize-memory-track-origins=2 -fno-omit-frame-pointer -g -O2" +# CACHE STRING "Flags used by the C compiler during MemorySanitizer builds." +# FORCE) +# set(CMAKE_CXX_FLAGS_MSAN +# "-fsanitize=memory -fno-optimize-sibling-calls -fsanitize-memory-track-origins=2 -fno-omit-frame-pointer -g -O2" +# CACHE STRING "Flags used by the C++ compiler during MemorySanitizer builds." +# FORCE) + ## Specify additional locations of header files include_directories( ${catkin_INCLUDE_DIRS} diff --git a/map_merge/include/combine_grids/merging_pipeline.h b/map_merge/include/combine_grids/merging_pipeline.h index ef02f5c..6b75409 100644 --- a/map_merge/include/combine_grids/merging_pipeline.h +++ b/map_merge/include/combine_grids/merging_pipeline.h @@ -43,6 +43,7 @@ #include #include +#include namespace combine_grids { @@ -69,6 +70,10 @@ class MergingPipeline std::vector grids_; std::vector images_; std::vector transforms_; + float result_map_width = 0.0; + float result_map_height = 0.0; + std::vector roi_info_; + cv::Rect complete_roi_; }; template @@ -88,8 +93,11 @@ void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end) grids_.push_back(*it); /* convert to opencv images. it creates only a view for opencv and does * not copy or own actual data. */ - images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1, - const_cast((*it)->data.data())); + cv::Mat image_denoised; + cv::Mat image((*it)->info.height, (*it)->info.width, CV_8UC1, + const_cast((*it)->data.data())); + cv::fastNlMeansDenoising(image, image_denoised, 10, 9, 15); + images_.emplace_back(image_denoised); } else { grids_.emplace_back(); images_.emplace_back(); diff --git a/map_merge/include/map_merge/map_merge.h b/map_merge/include/map_merge/map_merge.h index cb40c3c..0f62d01 100644 --- a/map_merge/include/map_merge/map_merge.h +++ b/map_merge/include/map_merge/map_merge.h @@ -45,8 +45,10 @@ #include #include +#include #include #include +#include #include #include @@ -56,6 +58,7 @@ struct MapSubscription { // protects consistency of writable_map and readonly_map // also protects reads and writes of shared_ptrs std::mutex mutex; + std::string map_frame; geometry_msgs::Transform initial_pose; nav_msgs::OccupancyGrid::Ptr writable_map; @@ -74,12 +77,14 @@ class MapMerge double merging_rate_; double discovery_rate_; double estimation_rate_; + double publish_rate_; double confidence_threshold_; std::string robot_map_topic_; std::string robot_map_updates_topic_; std::string robot_namespace_; std::string world_frame_; bool have_initial_poses_; + bool publish_tf = true; // publishing ros::Publisher merged_map_publisher_; @@ -92,6 +97,18 @@ class MapMerge combine_grids::MergingPipeline pipeline_; std::mutex pipeline_mutex_; + // publishing tfs + std::vector map_frames_; + std::vector tf_transforms_; + tf2_ros::TransformBroadcaster tf_publisher_; + std::thread tf_thread_; // tf needs it own thread + std::atomic_flag tf_current_flag_; // whether tf_transforms_ are up to date + // with transforms_ + + + void publishTF(); + std::vector stampTransforms(const std::vector); + std::string robotNameFromTopic(const std::string& topic); bool isRobotMapTopic(const ros::master::TopicInfo& topic); bool getInitPose(const std::string& name, geometry_msgs::Transform& pose); @@ -108,6 +125,7 @@ class MapMerge void executetopicSubscribing(); void executemapMerging(); void executeposeEstimation(); + void executeposePublishTf(); void topicSubscribing(); void mapMerging(); diff --git a/map_merge/launch/map_merge.launch b/map_merge/launch/map_merge.launch index 4dcf5d2..32347b5 100644 --- a/map_merge/launch/map_merge.launch +++ b/map_merge/launch/map_merge.launch @@ -1,15 +1,14 @@ - - + - + + - diff --git a/map_merge/src/combine_grids/grid_compositor.cpp b/map_merge/src/combine_grids/grid_compositor.cpp index 96f49dd..472c1d3 100644 --- a/map_merge/src/combine_grids/grid_compositor.cpp +++ b/map_merge/src/combine_grids/grid_compositor.cpp @@ -63,6 +63,7 @@ nav_msgs::OccupancyGrid::Ptr GridCompositor::compose( result_grid->info.width = static_cast(dst_roi.width); result_grid->info.height = static_cast(dst_roi.height); + result_grid->data.resize(static_cast(dst_roi.area()), -1); // create view for opencv pointing to newly allocated grid cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data()); diff --git a/map_merge/src/combine_grids/merging_pipeline.cpp b/map_merge/src/combine_grids/merging_pipeline.cpp index 868388c..544fdb0 100644 --- a/map_merge/src/combine_grids/merging_pipeline.cpp +++ b/map_merge/src/combine_grids/merging_pipeline.cpp @@ -42,6 +42,8 @@ #include #include #include "estimation_internal.h" +#define _USE_MATH_DEFINES +#include namespace combine_grids { @@ -59,6 +61,8 @@ bool MergingPipeline::estimateTransforms(FeatureType feature_type, cv::makePtr(); cv::Ptr estimator = cv::makePtr(); +// cv::Ptr estimator = +// cv::makePtr(); cv::Ptr adjuster = cv::makePtr(); @@ -71,6 +75,8 @@ bool MergingPipeline::estimateTransforms(FeatureType feature_type, image_features.reserve(images_.size()); for (const cv::Mat& image : images_) { image_features.emplace_back(); +// cv::Mat image_denoised; +// cv::fastNlMeansDenoising(image, image_denoised, 3, 7, 21); if (!image.empty()) { (*finder)(image, image_features.back()); } @@ -121,19 +127,21 @@ bool MergingPipeline::estimateTransforms(FeatureType feature_type, } ROS_DEBUG("optimizing global transforms"); adjuster->setConfThresh(confidence); - if (!(*adjuster)(image_features, pairwise_matches, transforms)) { - ROS_WARN("Bundle adjusting failed. Could not estimate transforms."); - return false; - } +// if (!(*adjuster)(image_features, pairwise_matches, transforms)) { +// ROS_WARN("Bundle adjusting failed. Could not estimate transforms."); +// return false; +// } transforms_.clear(); transforms_.resize(images_.size()); size_t i = 0; + ROS_DEBUG("Adding transforms into transforms_"); for (auto& j : good_indices) { // we want to work with transforms as doubles transforms[i].R.convertTo(transforms_[static_cast(j)], CV_64F); ++i; } + ROS_DEBUG("Finished adding transforms into transforms_"); return true; } @@ -179,7 +187,18 @@ nav_msgs::OccupancyGrid::Ptr MergingPipeline::composeGrids() ROS_DEBUG("compositing result grid"); nav_msgs::OccupancyGrid::Ptr result; internal::GridCompositor compositor; + std::vector corners; + corners.reserve(images_.size()); + std::vector sizes; + sizes.reserve(images_.size()); result = compositor.compose(imgs_warped, rois); + roi_info_.clear(); + for (auto& roi : rois) { + roi_info_.push_back(roi); + corners.push_back(roi.tl()); + sizes.push_back(roi.size()); + } + complete_roi_ = cv::detail::resultRoi(corners, sizes); // set correct resolution to output grid. use resolution of identity (works // for estimated trasforms), or any resolution (works for know_init_positions) @@ -199,12 +218,12 @@ nav_msgs::OccupancyGrid::Ptr MergingPipeline::composeGrids() result->info.resolution = any_resolution; } - // set grid origin to its centre - result->info.origin.position.x = - -(result->info.width / 2.0) * double(result->info.resolution); - result->info.origin.position.y = - -(result->info.height / 2.0) * double(result->info.resolution); + // set grid origin to its centre - Facilitates transforms + result->info.origin.position.x = 0.0; + result->info.origin.position.y = 0.0; result->info.origin.orientation.w = 1.0; + result_map_width = (float)result->info.width * result->info.resolution; // in meters + result_map_height = (float)result->info.height * result->info.resolution; // in meters return result; } @@ -214,27 +233,183 @@ std::vector MergingPipeline::getTransforms() const std::vector result; result.reserve(transforms_.size()); + geometry_msgs::Transform identity_quaternion; + identity_quaternion.rotation.w = 1; + int loop_counter = 0; + double alpha; + float MinX = 9999.0; + float MinY = 9999.0; + bool wrong_transform = false; + std::vector corners(4); + std::vector bounding_box(4); + std::vector biases(4); + std::vector displacements(4); + + corners.clear(); + bounding_box.clear(); + biases.clear(); + displacements.clear(); + + for (auto& transform : transforms_) { + + if (transform.empty()) { + continue; + } + + if (result_map_width > 0.0 && result_map_height > 0.0){ + float resolution = grids_[loop_counter]->info.resolution; + float map_height = grids_[loop_counter]->info.height * resolution; + float map_width = grids_[loop_counter]->info.width * resolution; + float origin_x = grids_[loop_counter]->info.origin.position.x; + float origin_y = grids_[loop_counter]->info.origin.position.y; + + // our rotation is in fact only 2D, thus quaternion can be simplified + double a = transform.at(0, 0); + double b = transform.at(1, 0); + ROS_DEBUG("original cosinus: %f", a); + if (std::abs(a) > 1){ + a = std::copysign(1, a); + wrong_transform = true; + } + alpha = std::acos(a); + ROS_DEBUG("cosinus modified: %f", a); + ROS_DEBUG("angle: %f", alpha); + + // Create map corners wrt its origin + corners.clear(); + for (int i = 0; i < 4; i++){ + if (i == 0){ + corners[i].x = 0.0f + origin_x; + corners[i].y = 0.0f + origin_y; + } + if (i == 1){ + corners[i].x = 0.0f + origin_x; + corners[i].y = map_height + origin_y; + } + if (i == 2){ + corners[i].x = map_width + origin_x; + corners[i].y = 0.0f + origin_y; + } + if (i == 3){ + corners[i].x = map_width + origin_x; + corners[i].y = map_height + origin_y; + } + } + + // Rotate corners as image and move them to the origin + for (int i = 0; i < 4; i++){ + float aux_x = corners[i].x * std::cos(alpha) - corners[i].y * std::sin(alpha); + float aux_y = corners[i].x * std::sin(alpha) + corners[i].y * std::cos(alpha); + corners[i].x = aux_x; + corners[i].y = aux_y; + } + + //Find the bounding box for the image rotated. + float max_x = -9999.9; + float max_y = -9999.9; + float min_x = 9999.9; + float min_y = 9999.9; + for (int i = 0; i < 4; i++){ + if (corners[i].x > max_x) { + max_x = corners[i].x; + } + if (corners[i].y > max_y) { + max_y = corners[i].y; + } + if (corners[i].x < min_x) { + min_x = corners[i].x; + } + if (corners[i].y < min_y) { + min_y = corners[i].y; + } + } + // Bounding box order: TopLeft, TopRight, BottomRight and BottomLeft + for (int i = 0; i < 4; i++) { + if (i==0){ + bounding_box[i].x = min_x; + bounding_box[i].y = max_y; + } + if (i==1){ + bounding_box[i].x = max_x; + bounding_box[i].y = max_y; + } + if (i==2){ + bounding_box[i].x = max_x; + bounding_box[i].y = min_y; + } + if (i==3){ + bounding_box[i].x = min_x; + bounding_box[i].y = min_y; + } + } + + // Move the BottomLeft corner of the bounding box towards the BottomLeft corner of the main map + float bottom_left_dist_x; + float bottom_left_dist_y; + bottom_left_dist_x = 0.0 - bounding_box[3].x; + bottom_left_dist_y = 0.0 - bounding_box[3].y; + displacements[loop_counter].x = bottom_left_dist_x; + displacements[loop_counter].y = bottom_left_dist_y; + + + for (int i = 0; i < 4; i++) { + bounding_box[i].x = bounding_box[i].x + bottom_left_dist_x; + bounding_box[i].y = bounding_box[i].y + bottom_left_dist_y; + } + + float center_bias_x = (roi_info_[loop_counter].x + complete_roi_.x) * resolution; + float center_bias_y = (roi_info_[loop_counter].y + complete_roi_.y) * resolution; + biases[loop_counter].x = center_bias_x; + biases[loop_counter].y = center_bias_y; + + for (int i = 0; i < 4; i++) { + bounding_box[i].x = bounding_box[i].x + center_bias_x; + bounding_box[i].y = bounding_box[i].y + center_bias_y; + if (MinX > bounding_box[i].x) { + MinX = bounding_box[i].x; + } + if (MinY > bounding_box[i].y) { + MinY = bounding_box[i].y; + } + } + loop_counter = loop_counter + 1; + } + } + + + loop_counter = 0; for (auto& transform : transforms_) { - if (transform.empty()) { - result.emplace_back(); + if (transform.empty() || wrong_transform) { + result.emplace_back(identity_quaternion); continue; } - ROS_ASSERT(transform.type() == CV_64F); - geometry_msgs::Transform ros_transform; - ros_transform.translation.x = transform.at(0, 2); - ros_transform.translation.y = transform.at(1, 2); - ros_transform.translation.z = 0.; - - // our rotation is in fact only 2D, thus quaternion can be simplified - double a = transform.at(0, 0); - double b = transform.at(1, 0); - ros_transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5; - ros_transform.rotation.x = 0.; - ros_transform.rotation.y = 0.; - ros_transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b); - - result.push_back(ros_transform); + if (result_map_width > 0.0 && result_map_height > 0.0) + { + + // our rotation is in fact only 2D, thus quaternion can be simplified + double a = transform.at(0, 0); + double b = transform.at(1, 0); + if (std::abs(a) > 1){ + a = std::copysign(1, a); + } + alpha = std::acos(a); + + ROS_ASSERT(transform.type() == CV_64F); + geometry_msgs::Transform ros_transform; + + ros_transform.translation.x = displacements[loop_counter].x + biases[loop_counter].x - MinX; + ros_transform.translation.y = displacements[loop_counter].y + biases[loop_counter].y - MinY; + + // our rotation is in fact only 2D, thus quaternion can be simplified + ros_transform.rotation.w = std::cos(alpha * 0.5); //std::sqrt(2. + 2. * a) * 0.5; + ros_transform.rotation.x = 0.; + ros_transform.rotation.y = 0.; + ros_transform.rotation.z = std::sin(alpha * 0.5); //std::copysign(std::sqrt(2. - 2. * a) * 0.5, b); + + result.push_back(ros_transform); + loop_counter = loop_counter + 1; + } } return result; diff --git a/map_merge/src/map_merge.cpp b/map_merge/src/map_merge.cpp index 4fef706..a20de1e 100644 --- a/map_merge/src/map_merge.cpp +++ b/map_merge/src/map_merge.cpp @@ -50,21 +50,25 @@ MapMerge::MapMerge() : subscriptions_size_(0) std::string frame_id; std::string merged_map_topic; - private_nh.param("merging_rate", merging_rate_, 4.0); + private_nh.param("merging_rate", merging_rate_, 0.5); private_nh.param("discovery_rate", discovery_rate_, 0.05); private_nh.param("estimation_rate", estimation_rate_, 0.5); - private_nh.param("known_init_poses", have_initial_poses_, true); + private_nh.param("publish_rate", publish_rate_, 1.0); + private_nh.param("known_init_poses", have_initial_poses_, false); private_nh.param("estimation_confidence", confidence_threshold_, 1.0); private_nh.param("robot_map_topic", robot_map_topic_, "map"); private_nh.param("robot_map_updates_topic", robot_map_updates_topic_, "map_updates"); private_nh.param("robot_namespace", robot_namespace_, ""); private_nh.param("merged_map_topic", merged_map_topic, "map"); - private_nh.param("world_frame", world_frame_, "world"); + private_nh.param("world_frame", world_frame_, "map"); + private_nh.param("publish_tf", publish_tf, true); /* publishing */ merged_map_publisher_ = node_.advertise(merged_map_topic, 50, true); + tf_current_flag_.clear(); + } /* @@ -186,35 +190,43 @@ void MapMerge::mapMerging() void MapMerge::poseEstimation() { + bool new_transform = false; ROS_DEBUG("Grid pose estimation started."); std::vector grids; + grids.reserve(subscriptions_size_); { + map_frames_.clear(); boost::shared_lock lock(subscriptions_mutex_); for (auto& subscription : subscriptions_) { std::lock_guard s_lock(subscription.mutex); grids.push_back(subscription.readonly_map); + map_frames_.push_back(subscription.map_frame); } } std::lock_guard lock(pipeline_mutex_); pipeline_.feed(grids.begin(), grids.end()); // TODO allow user to change feature type - pipeline_.estimateTransforms(combine_grids::FeatureType::AKAZE, + new_transform = pipeline_.estimateTransforms(combine_grids::FeatureType::AKAZE, confidence_threshold_); + if (new_transform){ + tf_current_flag_.clear(); + } } void MapMerge::fullMapUpdate(const nav_msgs::OccupancyGrid::ConstPtr& msg, MapSubscription& subscription) { - ROS_DEBUG("received full map update"); std::lock_guard lock(subscription.mutex); if (subscription.readonly_map && subscription.readonly_map->header.stamp > msg->header.stamp) { // we have been overrunned by faster update. our work was useless. + ROS_DEBUG("received full map update but overrunned by timestamp"); return; } + subscription.map_frame = msg->header.frame_id; subscription.readonly_map = msg; subscription.writable_map = nullptr; } @@ -378,6 +390,54 @@ void MapMerge::executeposeEstimation() } } +void MapMerge::executeposePublishTf() +{ + if (publish_tf) + { + ros::Rate r(publish_rate_); + while (node_.ok()) + { + publishTF(); + r.sleep(); + } + } +} + +void MapMerge::publishTF() +{ + if (!tf_current_flag_.test_and_set()) { + std::lock_guard lock(pipeline_mutex_); + // need to recalculate stored transforms + auto transforms = pipeline_.getTransforms(); + tf_transforms_ = stampTransforms(transforms); + } + if (tf_transforms_.empty()) { + return; + } + + for (auto& transform : tf_transforms_) { + tf_publisher_.sendTransform(transform); + } +} + +std::vector MapMerge::stampTransforms(const std::vector transforms) +{ + std::vector output; + output.reserve(transforms.size()); + int ii = 0; + for (auto transform : transforms){ + geometry_msgs::TransformStamped tf_stamped; + tf_stamped.transform = transform; + tf_stamped.child_frame_id = map_frames_[ii]; + ROS_DEBUG("map frame_name: %s", tf_stamped.child_frame_id.c_str()); + tf_stamped.header.frame_id = world_frame_; + tf_stamped.header.stamp = ros::Time::now(); + output.push_back(tf_stamped); + ii++; + } + return output; +} + /* * spin() */ @@ -387,10 +447,13 @@ void MapMerge::spin() std::thread merging_thr([this]() { executemapMerging(); }); std::thread subscribing_thr([this]() { executetopicSubscribing(); }); std::thread estimation_thr([this]() { executeposeEstimation(); }); + std::thread publish_tf_thr([this]() { executeposePublishTf(); }); + ros::spin(); estimation_thr.join(); merging_thr.join(); subscribing_thr.join(); + publish_tf_thr.join(); } } // namespace map_merge @@ -398,7 +461,7 @@ void MapMerge::spin() int main(int argc, char** argv) { ros::init(argc, argv, "map_merge"); - // this package is still in development -- start wil debugging enabled + // this package is still in development -- start will debugging enabled if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged();