Various Updates (#14)

* Fixed default material manager

* Replaced PCL PCA

* Removed dependency on PCL

* Added override keyword to destructors

* Added Eigen target for Xenial

Author: marip8

CMakeLists.txt

@@ -5,14 +5,23 @@ set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
+find_package(Eigen3 REQUIRED)
+if(NOT TARGET Eigen3::Eigen)
+  if(NOT EIGEN3_INCLUDE_DIRS)
+    set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+  endif()
+  find_package(Threads REQUIRED)
+  add_library(Eigen3::Eigen IMPORTED INTERFACE)
+  set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_COMPILE_DEFINITIONS ${EIGEN3_DEFINITIONS})
+  set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS})
+endif()
+
+find_package(Qt5 REQUIRED COMPONENTS Widgets)
 find_package(catkin REQUIRED COMPONENTS
   rviz
   pluginlib
-  pcl_ros
 )
 
-find_package(Eigen3 REQUIRED)
-
 catkin_package(
   INCLUDE_DIRS
     include
@@ -21,7 +30,6 @@ catkin_package(
   CATKIN_DEPENDS
     rviz
     pluginlib
-    pcl_ros
 )
 
 ###########
@@ -33,9 +41,6 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-# QT
-find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
-
 qt5_wrap_cpp(MOC_FILES
   include/${PROJECT_NAME}/rviz_tool_cursor.h
   src/circle_tool_cursor.h
@@ -51,6 +56,7 @@ add_library(tool_cursor
 target_link_libraries(tool_cursor
   ${catkin_LIBRARIES}
   Qt5::Widgets
+  Eigen3::Eigen
 )
 
 #############

package.xml

@@ -10,9 +10,11 @@
   <license>Apache 2.0</license>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <depend>rviz</depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>qtbase5-dev</build_depend>
+  <depend>libqt5-widgets</depend>
   <depend>pluginlib</depend>
-  <depend>pcl_ros</depend>
+  <depend>rviz</depend>
 
   <export>
     <rviz plugin="${prefix}/plugin_description.xml"/>

src/circle_tool_cursor.h

@@ -23,7 +23,7 @@ Q_OBJECT
 
   CircleToolCursor();
 
-  virtual ~CircleToolCursor();
+  virtual ~CircleToolCursor() override;
 
 public Q_SLOTS:
 

src/mesh_tool_cursor.cpp

@@ -24,7 +24,7 @@ MeshToolCursor::MeshToolCursor()
                                          "The mesh resource to display as a cursor",
                                          getPropertyContainer(), SLOT(updateToolVisualization()), this);
 
-  material_ = Ogre::MaterialManager::getSingletonPtr()->create(COLOR_NAME, "rviz");
+  material_ = Ogre::MaterialManager::getSingletonPtr()->create(COLOR_NAME, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
 
   const Ogre::ColourValue& color = color_property_->getOgreColor();
   material_->getTechnique(0)->getPass(0)->setDiffuse(color.r, color.g, color.b, 1.0);

src/mesh_tool_cursor.h

@@ -19,7 +19,7 @@ Q_OBJECT
 
   MeshToolCursor();
 
-  virtual ~MeshToolCursor();
+  virtual ~MeshToolCursor() override;
 
 public Q_SLOTS:
 

src/rviz_tool_cursor/rviz_tool_cursor.cpp

@@ -17,8 +17,6 @@
 
 #include <Eigen/Dense>
 
-#include <pcl/common/pca.h>
-
 namespace
 {
 
@@ -52,29 +50,25 @@ Eigen::Matrix3f createMatrix(const Eigen::Vector3f& norm)
 Ogre::Quaternion estimateNormal(const std::vector<Ogre::Vector3>& points,
                                 const Ogre::Vector3& camera_norm)
 {
-  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
+  Eigen::MatrixXf data;
+  data.resize(points.size(), 3);
 
   for(std::size_t i = 0; i < points.size(); ++i)
   {
-    pcl::PointXYZ pt;
-    pt.x = points[i].x;
-    pt.y = points[i].y;
-    pt.z = points[i].z;
-
-    cloud->push_back(pt);
+    data.row(i) = Eigen::Map<const Eigen::Vector3f>(points[i].ptr());
   }
 
+  Eigen::MatrixXf centered = data.rowwise() - data.colwise().mean();
+
   // Use principal component analysis to the get eigenvectors
-  pcl::PCA<pcl::PointXYZ> pca;
-  pca.setInputCloud(cloud);
-  Eigen::Matrix3f& evecs = pca.getEigenVectors();
+  Eigen::MatrixXf cov = centered.transpose() * centered;
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig(cov);
 
   // Get the eigenvector associated with the smallest eigenvalue
   // (should be Z-axis, assuming points are relatively planar)
-  Eigen::Vector3f norm = evecs.col(2);
+  Eigen::Vector3f norm = eig.eigenvectors().col(0);
   norm.normalize();
 
-  // The
   Eigen::Vector3f camera_normal;
   camera_normal << camera_norm.x, camera_norm.y, camera_norm.z;
   camera_normal.normalize();
@@ -84,7 +78,7 @@ Ogre::Quaternion estimateNormal(const std::vector<Ogre::Vector3>& points,
     norm *= -1;
   }
 
-  // Create a random orientation matrix with the normal being in the direction of the smalles eigenvector
+  // Create an arbitrary orientation matrix with the normal being in the direction of the smallest eigenvector
   Eigen::Matrix3f mat = createMatrix(norm);
 
   Eigen::Quaternionf q(mat); //Eigen::AngleAxisf(0.0, evecs.col(2)));