Changed hog class with a new optimized version which exploits SSE2 op…

…erations (3x speedup in descriptor computation).

common/CMakeLists.txt

@@ -135,6 +135,7 @@ if(build)
         include/pcl/common/impl/random.hpp
         include/pcl/common/impl/generate.hpp
         include/pcl/common/impl/projection_matrix.hpp
+        include/pcl/common/impl/sse.hpp
         )
 
     set(impl_incs 

common/include/pcl/common/impl/sse.hpp

@@ -0,0 +1,97 @@
+/*
+ * Software License Agreement (Simplified BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2013-, Open Perception, Inc.
+ * Copyright (c) 2012, Piotr Dollar & Ron Appel.[pdollar-at-caltech.edu]
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met: 
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ *list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *this list of conditions and the following disclaimer in the documentation
+ *and/or other materials provided with the distribution. 
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the FreeBSD Project.
+ *
+ * Taken from Piotr Dollar's MATLAB Image&Video ToolboxVersion 3.00.
+ *
+ */
+
+#ifndef PCL_COMMON_SSE_HPP_
+#define PCL_COMMON_SSE_HPP_
+#if defined(__SSE2__)
+#include <emmintrin.h> // SSE2:<e*.h>, SSE3:<p*.h>, SSE4:<s*.h>
+
+#define RETf inline __m128
+#define RETi inline __m128i
+
+// set, load and store values
+RETf SET( const float &x ) { return _mm_set1_ps(x); }
+RETf SET( float x, float y, float z, float w ) { return _mm_set_ps(x,y,z,w); }
+RETi SET( const int &x ) { return _mm_set1_epi32(x); }
+RETf LD( const float &x ) { return _mm_load_ps(&x); }
+RETf LDu( const float &x ) { return _mm_loadu_ps(&x); }
+RETf STR( float &x, const __m128 y ) { _mm_store_ps(&x,y); return y; }
+RETf STR1( float &x, const __m128 y ) { _mm_store_ss(&x,y); return y; }
+RETf STRu( float &x, const __m128 y ) { _mm_storeu_ps(&x,y); return y; }
+RETf STR( float &x, const float y ) { return STR(x,SET(y)); }
+
+// arithmetic operators
+RETi ADD( const __m128i x, const __m128i y ) { return _mm_add_epi32(x,y); }
+RETf ADD( const __m128 x, const __m128 y ) { return _mm_add_ps(x,y); }
+RETf ADD( const __m128 x, const __m128 y, const __m128 z ) {
+  return ADD(ADD(x,y),z); }
+RETf ADD( const __m128 a, const __m128 b, const __m128 c, const __m128 &d ) {
+  return ADD(ADD(ADD(a,b),c),d); }
+RETf SUB( const __m128 x, const __m128 y ) { return _mm_sub_ps(x,y); }
+RETf MUL( const __m128 x, const __m128 y ) { return _mm_mul_ps(x,y); }
+RETf MUL( const __m128 x, const float y ) { return MUL(x,SET(y)); }
+RETf MUL( const float x, const __m128 y ) { return MUL(SET(x),y); }
+RETf INC( __m128 &x, const __m128 y ) { return x = ADD(x,y); }
+RETf INC( float &x, const __m128 y ) { __m128 t=ADD(LD(x),y); return STR(x,t); }
+RETf DEC( __m128 &x, const __m128 y ) { return x = SUB(x,y); }
+RETf DEC( float &x, const __m128 y ) { __m128 t=SUB(LD(x),y); return STR(x,t); }
+RETf MIN( const __m128 x, const __m128 y ) { return _mm_min_ps(x,y); }
+RETf RCP( const __m128 x ) { return _mm_rcp_ps(x); }
+RETf RCPSQRT( const __m128 x ) { return _mm_rsqrt_ps(x); }
+
+// logical operators
+RETf AND( const __m128 x, const __m128 y ) { return _mm_and_ps(x,y); }
+RETi AND( const __m128i x, const __m128i y ) { return _mm_and_si128(x,y); }
+RETf ANDNOT( const __m128 x, const __m128 y ) { return _mm_andnot_ps(x,y); }
+RETf OR( const __m128 x, const __m128 y ) { return _mm_or_ps(x,y); }
+RETf XOR( const __m128 x, const __m128 y ) { return _mm_xor_ps(x,y); }
+
+// comparison operators
+RETf CMPGT( const __m128 x, const __m128 y ) { return _mm_cmpgt_ps(x,y); }
+RETi CMPGT( const __m128i x, const __m128i y ) { return _mm_cmpgt_epi32(x,y); }
+
+// conversion operators
+RETf CVT( const __m128i x ) { return _mm_cvtepi32_ps(x); }
+RETi CVT( const __m128 x ) { return _mm_cvttps_epi32(x); }
+
+#undef RETf
+#undef RETi
+#else
+PCL_ERROR("SSE2 instructions not supported");
+#endif /* defined(__SSE2__) && !defined(__i386__) */
+#endif /* PCL_COMMON_SSE_HPP_ */

people/apps/main_ground_based_people_detection.cpp

@@ -72,6 +72,7 @@ int print_help()
   cout << "   --conf    <minimum_HOG_confidence (default = -1.5)>" << std::endl;
   cout << "   --min_h   <minimum_person_height (default = 1.3)>" << std::endl;
   cout << "   --max_h   <maximum_person_height (default = 2.3)>" << std::endl;
+  cout << "   --sample  <sampling_factor (default = 1)>" << std::endl;
   cout << "*******************************************************" << std::endl;
   return 0;
 }
@@ -119,6 +120,7 @@ int main (int argc, char** argv)
   float min_height = 1.3;
   float max_height = 2.3;
   float voxel_size = 0.06;
+  float sampling_factor = 1;
   Eigen::Matrix3f rgb_intrinsics_matrix;
   rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics
 
@@ -127,6 +129,7 @@ int main (int argc, char** argv)
   pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
   pcl::console::parse_argument (argc, argv, "--min_h", min_height);
   pcl::console::parse_argument (argc, argv, "--max_h", max_height);
+  pcl::console::parse_argument (argc, argv, "--sample", sampling_factor);
 
   // Read Kinect live stream:
   PointCloudT::Ptr cloud (new PointCloudT);
@@ -187,7 +190,7 @@ int main (int argc, char** argv)
   people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
   people_detector.setClassifier(person_classifier);                // set person classifier
   people_detector.setHeightLimits(min_height, max_height);         // set person classifier
-//  people_detector.setScaleFactor(4);                               // set a downsampling factor to the point cloud (for increasing speed)
+  people_detector.setSamplingFactor(sampling_factor);              // set a downsampling factor to the point cloud (for increasing speed)
 //  people_detector.setSensorPortraitOrientation(true);              // set sensor orientation to vertical
 
   // For timing:

people/include/pcl/people/ground_based_people_detection_app.h

@@ -99,12 +99,12 @@ namespace pcl
       setGround (Eigen::VectorXf& ground_coeffs);
 
       /**
-       * \brief Set scale factor. 
+       * \brief Set sampling factor. 
        *
-       * \param[in] scale_factor Value of the downsampling factor (in each dimension) which is applied to the raw point cloud (default = 1.).
+       * \param[in] sampling_factor Value of the downsampling factor (in each dimension) which is applied to the raw point cloud (default = 1.).
        */
       void
-      setScaleFactor (int scale_factor);
+      setSamplingFactor (int sampling_factor);
 
       /**
        * \brief Set voxel size. 
@@ -236,8 +236,8 @@ namespace pcl
       compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters);
 
     protected:
-      /** \brief scale factor used to downsample the point cloud */
-      int scale_factor_; 
+      /** \brief sampling factor used to downsample the point cloud */
+      int sampling_factor_; 
 
       /** \brief voxel size */
       float voxel_size_;