Adding bearing angle image to pcl

common/CMakeLists.txt

@@ -10,6 +10,7 @@ PCL_ADD_DOC(${SUBSYS_NAME})
 
 if(build)
     set(range_image_incs
+        include/pcl/range_image/bearing_angle_image.h
         include/pcl/range_image/range_image.h
         include/pcl/range_image/range_image_planar.h
         include/pcl/range_image/range_image_spherical.h
@@ -22,6 +23,7 @@ if(build)
         )
 
     set(range_image_srcs
+        src/bearing_angle_image.cpp
         src/range_image.cpp
         src/range_image_planar.cpp
         )

common/include/pcl/range_image/bearing_angle_image.h

@@ -0,0 +1,87 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
+ *  Author: Qinghua Li, Yan Zhuang, Fei Yan
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the name of Intelligent Robotics Lab, DLUT. nor the names
+ *     of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ *  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.
+ */
+
+/**
+  * \file bearing_angle_image.h
+  * \Created on: July 07, 2012
+  */
+
+#ifndef PCL_BEARING_ANGLE_IMAGE_H_
+#define PCL_BEARING_ANGLE_IMAGE_H_
+
+#include <cmath>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+namespace pcl
+{
+  /** \brief class BearingAngleImage is used as an interface to generate Bearing Angle(BA) image.
+    * \author: Qinghua Li (qinghua__li@163.com)
+    */
+  class BearingAngleImage : public pcl::PointCloud<PointXYZRGBA>
+  {
+    public:
+      // ===== TYPEDEFS =====
+      typedef pcl::PointCloud<PointXYZRGBA> BaseClass;
+
+      // =====CONSTRUCTOR & DESTRUCTOR=====
+      /** Constructor */
+      BearingAngleImage ();
+      /** Destructor */
+      virtual ~BearingAngleImage ();
+
+    public:
+      /** \brief Reset all values to an empty Bearing Angle image */
+      void
+      reset ();
+
+      /** \brief Calculate the angle between the laser beam and the segment joining two consecutive
+        * \measurement points.
+        */
+      double
+      getAngle (PointXYZ point1, PointXYZ point2);
+
+      /** \brief Transform 3D point cloud into a 2D Bearing Angle(BA) image */
+      void
+      generateBAImage (PointCloud<PointXYZ>& point_cloud);
+
+    protected:
+      /**< This point is used to be able to return a reference to a unknown gray point */
+      PointXYZRGBA unobserved_point_;
+  };
+}
+
+#endif // PCL_BEARING_ANGLE_IMAGE_H_

common/src/bearing_angle_image.cpp

@@ -0,0 +1,130 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
+ *  Author: Qinghua Li, Yan Zhuang, Fei Yan
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the name of Intelligent Robotics Lab, DLUT. nor the names
+ *     of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ *  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.
+ */
+
+/**
+  * \file bearing_angle_image.cpp
+  * \created on: July 07, 2012
+  * \author: Qinghua Li (qinghua__li@163.com)
+  */
+
+#include <pcl/common/eigen.h>
+#include <pcl/range_image/bearing_angle_image.h>
+
+namespace pcl
+{
+/////////////////////////////////////////////////////////
+BearingAngleImage::BearingAngleImage () :
+  BearingAngleImage::BaseClass (),
+  unobserved_point_ ()
+{
+  reset ();
+  unobserved_point_.x = unobserved_point_.y = unobserved_point_.z = 0.0;
+  unobserved_point_.rgba = 255;
+}
+
+/////////////////////////////////////////////////////////
+BearingAngleImage::~BearingAngleImage ()
+{
+}
+
+/////////////////////////////////////////////////////////
+void
+BearingAngleImage::reset ()
+{
+  width = height = 0;
+  points.clear ();
+}
+
+/////////////////////////////////////////////////////////
+double
+BearingAngleImage::getAngle (PointXYZ point1, PointXYZ point2)
+{
+  double a, b, c;
+  double theta;
+  const Eigen::Vector3f& p1 = point1.getVector3fMap ();
+  const Eigen::Vector3f& p2 = point2.getVector3fMap ();
+  a = p1.squaredNorm ();
+  b = (p1 - p2).squaredNorm ();
+  c = p2.squaredNorm ();
+
+  if (a != 0 && b != 0)
+  {
+    theta = acos ((a + b - c) / (2 * sqrt (a) * sqrt (b))) * 180 / M_PI;
+  }
+  else
+  {
+    theta = 0.0;
+  }
+
+  return theta;
+}
+
+/////////////////////////////////////////////////////////
+void
+BearingAngleImage::generateBAImage (PointCloud<PointXYZ>& point_cloud)
+{
+  width = point_cloud.width;
+  height = point_cloud.height;
+  unsigned int size = width * height;
+  points.clear ();
+  points.resize (size, unobserved_point_);
+
+  double theta;
+  uint8_t r, g, b, gray;
+
+  // primary transformation process
+  for (int i = 0; i < height - 1; ++i)
+  {
+    for (int j = 0; j < width - 1; ++j)
+    {
+      theta = getAngle (point_cloud.at (j, i + 1), point_cloud.at (j + 1, i));
+
+      // based on the theta, calculate the gray value of every pixel point
+      gray = theta * 255 / 180;
+      r = gray;
+      g = gray;
+      b = gray;
+
+      points[(i + 1) * width + j].x = point_cloud.at (j, i + 1).x;
+      points[(i + 1) * width + j].y = point_cloud.at (j, i + 1).y;
+      points[(i + 1) * width + j].z = point_cloud.at (j, i + 1).z;
+      // set the gray value for every pixel point
+      points[(i + 1) * width + j].rgba = ((int)r) << 24 | ((int)g) << 16 | ((int)b) << 8 | 0xff;
+    }
+  }
+}
+
+}  // namespace end

test/CMakeLists.txt

@@ -42,7 +42,11 @@ if(build)
     add_subdirectory(surface)
     add_subdirectory(segmentation)
 
-	PCL_ADD_TEST(a_recognition_ism_test test_recognition_ism
+    PCL_ADD_TEST(a_bearing_angle_image_test test_bearing_angle_image
+                 FILES test_bearing_angle_image.cpp
+                 LINK_WITH pcl_gtest pcl_common pcl_io)
+
+	  PCL_ADD_TEST(a_recognition_ism_test test_recognition_ism
                  FILES test_recognition_ism.cpp
                  LINK_WITH pcl_gtest pcl_io pcl_features
                  ARGUMENTS ${PCL_SOURCE_DIR}/test/ism_train.pcd ${PCL_SOURCE_DIR}/test/ism_test.pcd)

test/test_bearing_angle_image.cpp

@@ -0,0 +1,94 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
+ *  Author: Qinghua Li, Yan Zhuang, Fei Yan
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the name of Intelligent Robotics Lab, DLUT. nor the names
+ *     of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ *  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.
+ */
+
+/**
+  * \file test_bearing_angle_image.cpp
+  * \created on: July 07, 2013
+  * \author: Qinghua Li (qinghua__li@163.com)
+  */
+
+#include <gtest/gtest.h>
+#include <iostream>
+#include <pcl/range_image/bearing_angle_image.h>
+
+pcl::BearingAngleImage bearing_angle_image;
+pcl::PointCloud<pcl::PointXYZ> point_cloud (3, 2);
+
+/////////////////////////////////////////////////////////////////////
+TEST (BearingAngleImageTest, GetAngle)
+{
+  pcl::PointXYZ point1 (1.0, 2.0, 3.0);
+  pcl::PointXYZ point2 (2.0, 1.0, 1.0);
+
+  double angle = bearing_angle_image.getAngle (point1, point2);
+  EXPECT_NEAR (40.203, angle, 1e-3);
+}
+
+/////////////////////////////////////////////////////////////////////
+TEST (BearingAngleImageTest, GenerateBAImage)
+{
+  point_cloud.points[0] = pcl::PointXYZ (3.0, 1.5, -2.0);
+  point_cloud.points[1] = pcl::PointXYZ (1.0, 3.0, 2.0);
+  point_cloud.points[2] = pcl::PointXYZ (2.0, 3.0, 2.0);
+
+  point_cloud.points[3] = pcl::PointXYZ (2.0, 3.0, 1.0);
+  point_cloud.points[4] = pcl::PointXYZ (4.0, 2.0, 2.0);
+  point_cloud.points[5] = pcl::PointXYZ (-1.5, 3.0, 1.0);
+
+  bearing_angle_image.generateBAImage (point_cloud);
+
+  uint8_t grays[6];
+  for (int i = 0; i < 3 * 2; ++i)
+  {
+    grays[i] = (bearing_angle_image.points[i].rgba >> 8) & 0xff;
+  }
+
+  EXPECT_EQ (0, grays[0]);
+  EXPECT_EQ (0, grays[1]);
+  EXPECT_EQ (0, grays[2]);
+  EXPECT_EQ (112, grays[3]);
+  EXPECT_EQ (80, grays[4]);
+  EXPECT_EQ (0, grays[5]);
+}
+
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+  testing::InitGoogleTest (&argc, argv);
+  return (RUN_ALL_TESTS ());
+}