plane_seg: ~15-20% performance improvement

plane_seg/CMakeLists.txt

@@ -2,32 +2,26 @@ 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
 )
 
-find_package(OpenCV REQUIRED)
-
 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
@@ -36,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
@@ -54,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

plane_seg/src/BlockFitter.cpp

@@ -3,11 +3,32 @@
 #include <chrono>
 #include <fstream>
 
+// 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 <pcl/filters/voxel_grid.h>
 #include <pcl/common/common.h>
 #include <pcl/surface/convex_hull.h>
 #include <pcl/io/pcd_io.h>
 
+#if defined(__clang__)
+# pragma clang diagnostic pop
+#endif
+
+
 #include "plane_seg/PlaneFitter.hpp"
 #include "plane_seg/RobustNormalEstimator.hpp"
 #include "plane_seg/PlaneSegmenter.hpp"
@@ -157,6 +178,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 +220,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 +242,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 +281,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 +355,7 @@ go() {
   }
 
   std::vector<RectangleFitter::Result> results;
+  results.reserve(planes.size());
   for (auto& plane : planes) {
     RectangleFitter fitter;
     fitter.setDimensions(mBlockDimensions.head<2>());

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<float> 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<Eigen::VectorXf>(prevErrors2.data(), prevErrors2.size()).sum();
 
   std::vector<float> errors2 = computeErrors(plane, mPoints);
   errors2.push_back(computeError(plane, iPoint));

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 <pcl/surface/convex_hull.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
 
+#if defined(__clang__)
+# pragma clang diagnostic pop
+#endif
+
 using namespace planeseg;
 
 RectangleFitter::

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;

plane_seg/src/plane-seg-20150414.cpp

@@ -15,8 +15,6 @@
 #include <pcl/features/integral_image_normal.h>
 #include <pcl/features/normal_3d.h>
 
-#include <opencv2/opencv.hpp>
-
 typedef pcl::PointCloud<pcl::Normal> NormalCloud;
 typedef pcl::PointCloud<pcl::PointXYZL> LabeledCloud;
 

plane_seg/src/plane-seg-20150422.cpp

@@ -15,8 +15,6 @@
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
 
-#include <opencv2/opencv.hpp>
-
 typedef pcl::PointCloud<pcl::Normal> NormalCloud;
 typedef pcl::PointCloud<pcl::PointXYZL> LabeledCloud;
 typedef Eigen::Matrix<float,Eigen::Dynamic,3> MatrixX3f;

plane_seg/src/plane_seg_test2.cpp

@@ -6,8 +6,6 @@
 
 #include "plane_seg/RobustNormalEstimator.hpp"
 
-#include <opencv2/opencv.hpp>
-
 int main() {
   // read pcd file
   std::string home_dir = getenv("HOME");

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)
 

plane_seg_ros/src/plane_seg_ros.cpp

@@ -15,17 +15,39 @@
 #include <visualization_msgs/MarkerArray.h>
 #include <sensor_msgs/PointCloud2.h>
 
+// 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 <grid_map_msgs/GridMap.h>
 #include <grid_map_ros/grid_map_ros.hpp>
 #include <grid_map_ros/GridMapRosConverter.hpp>
 
+#if defined(__clang__)
+# pragma clang diagnostic pop
+#endif
+
 // tf
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_eigen/tf2_eigen.h>
 
 #include "plane_seg/BlockFitter.hpp"
 
+// #define WITH_TIMING
+
+#ifdef WITH_TIMING
+#include <chrono>
+#endif
+
 
 // convenience methods
 auto vecToStr = [](const Eigen::Vector3f& iVec) {
@@ -268,6 +290,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 +305,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 +338,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<std::chrono::microseconds>(toc_1 - tic).count() << "ms" << std::endl;
+  // std::cout << "[Publishing] took " << 1e-3 * std::chrono::duration_cast<std::chrono::microseconds>(toc_2 - toc_1).count() << "ms" << std::endl;
+#endif
 }