From 0b7bff6865bc2246646cc366d2146371bd376ef9 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Mon, 1 Nov 2021 15:46:16 +0000 Subject: [PATCH 1/7] [plane_seg] Remove unused OpenCV dependency --- plane_seg/CMakeLists.txt | 2 -- plane_seg/src/plane-seg-20150414.cpp | 2 -- plane_seg/src/plane-seg-20150422.cpp | 2 -- plane_seg/src/plane_seg_test2.cpp | 2 -- 4 files changed, 8 deletions(-) diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index b0488c2..133eb05 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -9,8 +9,6 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) -find_package(OpenCV REQUIRED) - catkin_package( INCLUDE_DIRS include diff --git a/plane_seg/src/plane-seg-20150414.cpp b/plane_seg/src/plane-seg-20150414.cpp index 8ed3764..d1cc064 100644 --- a/plane_seg/src/plane-seg-20150414.cpp +++ b/plane_seg/src/plane-seg-20150414.cpp @@ -15,8 +15,6 @@ #include #include -#include - typedef pcl::PointCloud NormalCloud; typedef pcl::PointCloud LabeledCloud; diff --git a/plane_seg/src/plane-seg-20150422.cpp b/plane_seg/src/plane-seg-20150422.cpp index 5abe355..73c3fef 100644 --- a/plane_seg/src/plane-seg-20150422.cpp +++ b/plane_seg/src/plane-seg-20150422.cpp @@ -15,8 +15,6 @@ #include #include -#include - typedef pcl::PointCloud NormalCloud; typedef pcl::PointCloud LabeledCloud; typedef Eigen::Matrix MatrixX3f; diff --git a/plane_seg/src/plane_seg_test2.cpp b/plane_seg/src/plane_seg_test2.cpp index 4be22c3..49f41f2 100644 --- a/plane_seg/src/plane_seg_test2.cpp +++ b/plane_seg/src/plane_seg_test2.cpp @@ -6,8 +6,6 @@ #include "plane_seg/RobustNormalEstimator.hpp" -#include - int main() { // read pcd file std::string home_dir = getenv("HOME"); From 060b314be7f1faa970cf05d8c7296c646ed73e5b Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Mon, 1 Nov 2021 15:46:38 +0000 Subject: [PATCH 2/7] [plane_seg] Minor --- plane_seg/CMakeLists.txt | 4 ++-- plane_seg/src/block-fitter.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index 133eb05..826c0cd 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -2,8 +2,8 @@ cmake_minimum_required(VERSION 3.0.2) project(plane_seg) -add_compile_options(-std=c++1y -Wall) -#add_definitions(-std=c++1y -Wall) +set(CMAKE_CXX_STANDARD 14) +add_compile_options(-Wall -Wextra -Wpedantic) find_package(catkin REQUIRED COMPONENTS pcl_ros diff --git a/plane_seg/src/block-fitter.cpp b/plane_seg/src/block-fitter.cpp index ef232be..700157a 100644 --- a/plane_seg/src/block-fitter.cpp +++ b/plane_seg/src/block-fitter.cpp @@ -349,7 +349,7 @@ struct State { */ }; -int main(const int iArgc, const char** iArgv) { +int main(const int /*iArgc*/, const char** /*iArgv*/) { std::string sizeString(""); std::string triggerChannel; From 2cf842ed1ac180cd02914c816a0ccd00940016fa Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Mon, 1 Nov 2021 16:39:38 +0000 Subject: [PATCH 3/7] [plane_seg] CMake --- plane_seg/CMakeLists.txt | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index 826c0cd..f674caa 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -10,22 +10,18 @@ find_package(catkin REQUIRED COMPONENTS ) catkin_package( - INCLUDE_DIRS - include + INCLUDE_DIRS include LIBRARIES plane_seg - CATKIN_DEPENDS pcl_ros + CATKIN_DEPENDS pcl_ros ) include_directories( include ${catkin_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} ) - # create library -set(LIB_NAME plane_seg) -add_library(${LIB_NAME} SHARED +add_library(${PROJECT_NAME} SHARED src/PlaneFitter.cpp src/RobustNormalEstimator.cpp src/IncrementalPlaneEstimator.cpp @@ -34,7 +30,7 @@ add_library(${LIB_NAME} SHARED src/BlockFitter.cpp ) add_dependencies(plane_seg ${catkin_EXPORTED_TARGETS}) -target_link_libraries(plane_seg ${catkin_LIBRARIES}) +target_link_libraries(plane_seg PUBLIC ${catkin_LIBRARIES}) # standalone lcm-based block fitter @@ -52,7 +48,7 @@ target_link_libraries(${APP_NAME} boost_system ${catkin_LIBRARIES} plane_seg) # fit single block set(APP_NAME plane_seg_test2) add_executable(${APP_NAME} src/plane_seg_test2.cpp) -target_link_libraries(${APP_NAME} boost_system ${catkin_LIBRARIES} plane_seg ${OpenCV_LIBS}) +target_link_libraries(${APP_NAME} boost_system ${catkin_LIBRARIES} plane_seg) # install From 73df2e0de29dd44920faf2b99e93c5da4e2c31de Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Mon, 1 Nov 2021 16:47:31 +0000 Subject: [PATCH 4/7] [plane_seg_ros] Add optional timing --- plane_seg_ros/src/plane_seg_ros.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 0c79282..66796ec 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -26,6 +26,12 @@ #include "plane_seg/BlockFitter.hpp" +// #define WITH_TIMING + +#ifdef WITH_TIMING +#include +#endif + // convenience methods auto vecToStr = [](const Eigen::Vector3f& iVec) { @@ -268,6 +274,9 @@ void Pass::processFromFile(int test_example){ void Pass::processCloud(const std::string& cloudFrame, planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir){ +#ifdef WITH_TIMING + auto tic = std::chrono::high_resolution_clock::now(); +#endif planeseg::BlockFitter fitter; fitter.setSensorPose(origin, lookDir); @@ -280,6 +289,9 @@ void Pass::processCloud(const std::string& cloudFrame, planeseg::LabeledCloud::P fitter.setMaxAngleOfPlaneSegmenter(10); result_ = fitter.go(); +#ifdef WITH_TIMING + auto toc_1 = std::chrono::high_resolution_clock::now(); +#endif if (look_pose_pub_.getNumSubscribers() > 0) { Eigen::Vector3f rz = lookDir; @@ -310,6 +322,13 @@ void Pass::processCloud(const std::string& cloudFrame, planeseg::LabeledCloud::P } publishResult(cloudFrame); + +#ifdef WITH_TIMING + auto toc_2 = std::chrono::high_resolution_clock::now(); + + std::cout << "[BlockFitter] took " << 1e-3 * std::chrono::duration_cast(toc_1 - tic).count() << "ms" << std::endl; + // std::cout << "[Publishing] took " << 1e-3 * std::chrono::duration_cast(toc_2 - toc_1).count() << "ms" << std::endl; +#endif } From 8947d18bc602288ac0e47593b522109e6ab2cd02 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Mon, 1 Nov 2021 16:51:14 +0000 Subject: [PATCH 5/7] [plane_seg] Avoid dynamic vector resize --- plane_seg/src/BlockFitter.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/plane_seg/src/BlockFitter.cpp b/plane_seg/src/BlockFitter.cpp index 5583561..159bcf0 100644 --- a/plane_seg/src/BlockFitter.cpp +++ b/plane_seg/src/BlockFitter.cpp @@ -157,6 +157,7 @@ go() { maxZ = minZ + 0.5; } LabeledCloud::Ptr tempCloud(new LabeledCloud()); + tempCloud->reserve(cloud->size()); for (int i = 0; i < (int)cloud->size(); ++i) { const Eigen::Vector3f& p = cloud->points[i].getVector3fMap(); if ((p[2] < minZ) || (p[2] > maxZ)) continue; @@ -198,6 +199,7 @@ go() { result.mGroundPlane = groundPlane; { tempCloud.reset(new LabeledCloud()); + tempCloud->reserve(cloud->size()); for (int i = 0; i < (int)cloud->size(); ++i) { Eigen::Vector3f p = cloud->points[i].getVector3fMap(); float dist = groundPlane.head<3>().dot(p) + groundPlane[3]; @@ -219,6 +221,7 @@ go() { // remove points below or near ground tempCloud.reset(new LabeledCloud()); + tempCloud->reserve(cloud->size()); for (int i = 0; i < (int)cloud->size(); ++i) { Eigen::Vector3f p = cloud->points[i].getVector3fMap(); float dist = p.dot(groundPlane.head<3>()) + groundPlane[3]; @@ -257,10 +260,12 @@ go() { const float maxNormalAngle = mMaxAngleFromHorizontal*M_PI/180; LabeledCloud::Ptr tempCloud(new LabeledCloud()); NormalCloud::Ptr tempNormals(new NormalCloud()); + tempCloud->reserve(normals->size()); + tempNormals->reserve(normals->size()); for (int i = 0; i < (int)normals->size(); ++i) { - const auto& norm = normals->points[i]; - Eigen::Vector3f normal(norm.normal_x, norm.normal_y, norm.normal_z); - float angle = std::acos(normal[2]); + // const auto& norm = normals->points[i]; + // Eigen::Vector3f normal(norm.normal_x, norm.normal_y, norm.normal_z); + float angle = std::acos(normals->points[i].normal_z); //std::acos(normal[2]); if (angle > maxNormalAngle) continue; tempCloud->push_back(cloud->points[i]); tempNormals->push_back(normals->points[i]); @@ -329,6 +334,7 @@ go() { } std::vector results; + results.reserve(planes.size()); for (auto& plane : planes) { RectangleFitter fitter; fitter.setDimensions(mBlockDimensions.head<2>()); From ae262ea3cbd1af94778686b57ebab3aabff53d67 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Mon, 1 Nov 2021 16:55:51 +0000 Subject: [PATCH 6/7] [plane_seg] Replace std::accumulate with Eigen's sum redux for ~16% speed-up --- plane_seg/src/IncrementalPlaneEstimator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plane_seg/src/IncrementalPlaneEstimator.cpp b/plane_seg/src/IncrementalPlaneEstimator.cpp index 76fc9f7..2d667e3 100644 --- a/plane_seg/src/IncrementalPlaneEstimator.cpp +++ b/plane_seg/src/IncrementalPlaneEstimator.cpp @@ -87,8 +87,8 @@ tryPoint(const Eigen::Vector3f& iPoint, const Eigen::Vector3f& iNormal, std::cos(iMaxAngle)) return false; std::vector prevErrors2 = computeErrors(getCurrentPlane(), mPoints); - float prevTotalError2 = - std::accumulate(prevErrors2.begin(), prevErrors2.end(), 0.0f); + // NB: Using Eigen's sum redux is significantly faster than std::accumulate + float prevTotalError2 = Eigen::Map(prevErrors2.data(), prevErrors2.size()).sum(); std::vector errors2 = computeErrors(plane, mPoints); errors2.push_back(computeError(plane, iPoint)); From de5a8f76e4f6a4a523ff18c49c5b0e14bb42c83c Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Mon, 1 Nov 2021 17:09:45 +0000 Subject: [PATCH 7/7] Ignore warnings arising from GridMap and PCL --- plane_seg/src/BlockFitter.cpp | 21 +++++++++++++++++++++ plane_seg/src/RectangleFitter.cpp | 20 ++++++++++++++++++++ plane_seg_ros/CMakeLists.txt | 1 + plane_seg_ros/src/plane_seg_ros.cpp | 16 ++++++++++++++++ 4 files changed, 58 insertions(+) diff --git a/plane_seg/src/BlockFitter.cpp b/plane_seg/src/BlockFitter.cpp index 159bcf0..5a888a3 100644 --- a/plane_seg/src/BlockFitter.cpp +++ b/plane_seg/src/BlockFitter.cpp @@ -3,11 +3,32 @@ #include #include +// PCL's octree_key.h (included from convex_hull.h) uses anonymous structs and nested anonymous unions. +// These are GNU extensions - we want to ignore warnings about them, though. + +#if defined(__clang__) +# pragma clang diagnostic push +#endif + +#if defined(__clang__) && defined(__has_warning) +# if __has_warning( "-Wgnu-anonymous-struct" ) +# pragma clang diagnostic ignored "-Wgnu-anonymous-struct" +# endif +# if __has_warning( "-Wnested-anon-types" ) +# pragma clang diagnostic ignored "-Wnested-anon-types" +# endif +#endif + #include #include #include #include +#if defined(__clang__) +# pragma clang diagnostic pop +#endif + + #include "plane_seg/PlaneFitter.hpp" #include "plane_seg/RobustNormalEstimator.hpp" #include "plane_seg/PlaneSegmenter.hpp" diff --git a/plane_seg/src/RectangleFitter.cpp b/plane_seg/src/RectangleFitter.cpp index 2562797..0f1967e 100644 --- a/plane_seg/src/RectangleFitter.cpp +++ b/plane_seg/src/RectangleFitter.cpp @@ -1,9 +1,29 @@ #include "plane_seg/RectangleFitter.hpp" +// PCL's octree_key.h (included from convex_hull.h) uses anonymous structs and nested anonymous unions. +// These are GNU extensions - we want to ignore warnings about them, though. + +#if defined(__clang__) +# pragma clang diagnostic push +#endif + +#if defined(__clang__) && defined(__has_warning) +# if __has_warning( "-Wgnu-anonymous-struct" ) +# pragma clang diagnostic ignored "-Wgnu-anonymous-struct" +# endif +# if __has_warning( "-Wnested-anon-types" ) +# pragma clang diagnostic ignored "-Wnested-anon-types" +# endif +#endif + #include #include #include +#if defined(__clang__) +# pragma clang diagnostic pop +#endif + using namespace planeseg; RectangleFitter:: diff --git a/plane_seg_ros/CMakeLists.txt b/plane_seg_ros/CMakeLists.txt index 7976540..9ba9bb6 100644 --- a/plane_seg_ros/CMakeLists.txt +++ b/plane_seg_ros/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(plane_seg_ros) +set(CMAKE_C_STANDARD 11) # OpenCV4 is using _Atomic, a C11 extension set(CMAKE_CXX_STANDARD 14) add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 66796ec..ba3dbc5 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -15,10 +15,26 @@ #include #include +// GridMapRosConverter includes cv_bridge which includes OpenCV4 which uses _Atomic +// We want to ignore this warning entirely. +#if defined(__clang__) +# pragma clang diagnostic push +#endif + +#if defined(__clang__) && defined(__has_warning) +# if __has_warning( "-Wc11-extensions" ) +# pragma clang diagnostic ignored "-Wc11-extensions" +# endif +#endif + #include #include #include +#if defined(__clang__) +# pragma clang diagnostic pop +#endif + // tf #include #include