Fix bugs in cone visualization

PointCloudLibrary · Sep 2, 2014 · 9ac5bf6 · stefanbuettner · Dec 2, 2015 · 9ac5bf6
1 parent 5c8fa8a
commit 9ac5bf6
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 5 deletions.
diff --git a/visualization/include/pcl/visualization/common/shapes.h b/visualization/include/pcl/visualization/common/shapes.h
@@ -222,7 +222,27 @@ namespace pcl
     create2DCircle (const pcl::ModelCoefficients &coefficients, double z = 0.0);
 
     /** \brief Create a cone shape from a set of model coefficients.
-      * \param[in] coefficients the cone coefficients (point_on_axis, axis_direction, radius)
+      * \param[in] coefficients the cone coefficients (cone_apex, axis_direction, angle)
+      *
+      * \code
+      * // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelCone)
+      * // Eigen::Vector3f cone_apex, axis_direction;
+      * // float angle;
+      * // Note: The height of the cone is set using the magnitude of the axis_direction vector.
+      *
+      * pcl::ModelCoefficients cone_coeff;
+      * plane_coeff.values.resize (7);    // We need 7 values
+      * plane_coeff.values[0] = cone_apex.x ();
+      * plane_coeff.values[1] = cone_apex.y ();
+      * plane_coeff.values[2] = cone_apex.z ();
+      * plane_coeff.values[3] = axis_direction.x ();
+      * plane_coeff.values[4] = axis_direction.y ();
+      * plane_coeff.values[5] = axis_direction.z ();
+      * plane_coeff.values[6] = angle (); // degrees
+      *
+      * vtkSmartPointer<vtkDataSet> data = pcl::visualization::createCone (cone_coeff);
+      * \endcode
+      *
       * \ingroup visualization
       */
     PCL_EXPORTS vtkSmartPointer<vtkDataSet> 

diff --git a/visualization/src/common/shapes.cpp b/visualization/src/common/shapes.cpp
@@ -249,10 +249,12 @@ vtkSmartPointer<vtkDataSet>
 pcl::visualization::createCone (const pcl::ModelCoefficients &coefficients)
 {
   vtkSmartPointer<vtkConeSource> cone = vtkSmartPointer<vtkConeSource>::New ();
-  cone->SetHeight (1.0);
-  cone->SetCenter (coefficients.values[0] + coefficients.values[3] * 0.5, 
-                   coefficients.values[1] + coefficients.values[1] * 0.5,
-                   coefficients.values[2] + coefficients.values[2] * 0.5);
+  cone->SetHeight (std::sqrt (coefficients.values[3] * coefficients.values[3] +
+                              coefficients.values[4] * coefficients.values[4] +
+                              coefficients.values[5] * coefficients.values[5]));
+  cone->SetCenter (coefficients.values[0] + coefficients.values[3] * 0.5,
+                   coefficients.values[1] + coefficients.values[4] * 0.5,
+                   coefficients.values[2] + coefficients.values[5] * 0.5);
   cone->SetDirection (-coefficients.values[3], -coefficients.values[4], -coefficients.values[5]);
   cone->SetResolution (100);
   cone->SetAngle (coefficients.values[6]);