From a854dfe31a41ce3447279cb1c1b1228f1ebecfbc Mon Sep 17 00:00:00 2001 From: ZhuangYanDLUT Date: Fri, 20 Dec 2013 21:03:19 +0800 Subject: [PATCH] Adding bearing angle image to pcl --- common/CMakeLists.txt | 2 + .../pcl/range_image/bearing_angle_image.h | 87 ++++++++++++ common/src/bearing_angle_image.cpp | 130 ++++++++++++++++++ test/CMakeLists.txt | 6 +- test/test_bearing_angle_image.cpp | 94 +++++++++++++ 5 files changed, 318 insertions(+), 1 deletion(-) create mode 100644 common/include/pcl/range_image/bearing_angle_image.h create mode 100644 common/src/bearing_angle_image.cpp create mode 100644 test/test_bearing_angle_image.cpp diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 4d66caa3328..6db7dc400b1 100644 --- a/common/CMakeLists.txt +++ b/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 ) diff --git a/common/include/pcl/range_image/bearing_angle_image.h b/common/include/pcl/range_image/bearing_angle_image.h new file mode 100644 index 00000000000..818a0680ae8 --- /dev/null +++ b/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 +#include +#include + +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 + { + public: + // ===== TYPEDEFS ===== + typedef pcl::PointCloud 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& 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_ diff --git a/common/src/bearing_angle_image.cpp b/common/src/bearing_angle_image.cpp new file mode 100644 index 00000000000..9cb82e32d86 --- /dev/null +++ b/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 +#include + +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& 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 diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6aa645902bc..51f45ee15c0 100644 --- a/test/CMakeLists.txt +++ b/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) diff --git a/test/test_bearing_angle_image.cpp b/test/test_bearing_angle_image.cpp new file mode 100644 index 00000000000..b2a445e387b --- /dev/null +++ b/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 +#include +#include + +pcl::BearingAngleImage bearing_angle_image; +pcl::PointCloud 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 ()); +}