bearing_angle_image.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
00005  *  Author: Qinghua Li, Yan Zhuang, Fei Yan
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Intelligent Robotics Lab, DLUT. nor the names
00020  *     of its contributors may be used to endorse or promote products
00021  *     derived from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  */
00036 
00037 #include "dlut_place_recognition/bearing_angle_image.h"
00038 
00039 BearingAngleImage::BearingAngleImage ()
00040 {
00041 }
00042 
00043 BearingAngleImage::~BearingAngleImage ()
00044 {
00045   cvReleaseImage (&BA_image);
00046   cvReleaseImage (&channels_image);
00047 }
00048 
00049 
00050 // Calculate the angle between the laser beam and the segment joining two consecutive measurement points
00051 double BearingAngleImage::getAngle (const pcl::PointXYZ &point1, const pcl::PointXYZ &point2)
00052 {
00053   double a, b, c;
00054   double theta;
00055   a = sqrt (point1.x * point1.x + point1.y * point1.y + point1.z * point1.z);
00056   b = sqrt ((point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y) +
00057             (point1.z - point2.z) * (point1.z - point2.z));
00058   c = sqrt (point2.x * point2.x + point2.y * point2.y + point2.z * point2.z);
00059 
00060   if (a != 0 && b != 0)
00061   {
00062     theta = acos ((a * a + b * b - c * c) / (2 * a * b)) * 180 / M_PI;
00063   }
00064   else
00065   {
00066     theta = 0;
00067   }
00068 
00069   return theta;
00070 }
00071 
00072 // Based on the theta, calculate the gray value of every pixel point
00073 double BearingAngleImage::getGray (double theta)
00074 {
00075   double gray;
00076   gray = theta / 180 * 255;
00077   return gray;
00078 }
00079 
00080 // Transform 3D point cloud into a 2D improved bearing-angle(BA) image
00081 IplImage* BearingAngleImage::generateBAImage (std::vector< std::vector<pcl::PointXYZ> > &points, int width, int height)
00082 {
00083   BA_image = cvCreateImage (cvSize (width, height), IPL_DEPTH_8U, 1);
00084   cvSetZero (BA_image);
00085 
00086   int gray;
00087   double theta;
00088   pcl::PointXYZ current_point, next_point;
00089 
00090   // Primary transformation process
00091   for (int i = 1; i < (int) points.size (); ++i)
00092   {
00093     for (int j = 0; j < (int) points[i].size () - 1; ++j)
00094     {
00095       current_point.x = points[i][j].x;
00096       current_point.y = points[i][j].y;
00097       current_point.z = points[i][j].z;
00098       next_point.x = points[i - 1][j + 1].x;
00099       next_point.y = points[i - 1][j + 1].y;
00100       next_point.z = points[i - 1][j + 1].z;
00101 
00102       theta = getAngle (current_point, next_point);
00103       gray = getGray (theta);
00104       /* Set the gray value for every pixel point */
00105       cvSetReal2D (BA_image, height - i - 1, width - j - 1, gray);
00106     }
00107   }
00108 
00109   return BA_image;
00110 }
00111 
00112 IplImage* BearingAngleImage::getChannelsImage (IplImage* ipl_image)
00113 {
00114   /* Create a three channels image */
00115   channels_image = cvCreateImage (cvSize (ipl_image->width, ipl_image->height), IPL_DEPTH_8U, 3);
00116   cvCvtColor (ipl_image, channels_image, CV_GRAY2BGR);
00117   return channels_image;
00118 }
00119 
00120 QImage BearingAngleImage::cvIplImage2QImage (IplImage* ipl_image)
00121 {
00122   int width = ipl_image->width;
00123   int height = ipl_image->height;
00124 
00125   if (ipl_image->depth == IPL_DEPTH_8U && ipl_image->nChannels == 3)
00126   {
00127     const uchar *qImageBuffer = (const uchar*) ipl_image->imageData;
00128     QImage img (qImageBuffer, width, height, QImage::Format_RGB888);
00129     return img.rgbSwapped ();
00130   }
00131   else if (ipl_image->depth == IPL_DEPTH_8U && ipl_image->nChannels == 1)
00132   {
00133     const uchar *qImageBuffer = (const uchar*) ipl_image->imageData;
00134     QImage img (qImageBuffer, width, height, QImage::Format_Indexed8);
00135 
00136     QVector < QRgb > colorTable;
00137     for (int i = 0; i < 256; i++)
00138     {
00139       colorTable.push_back (qRgb (i, i, i));
00140     }
00141     img.setColorTable (colorTable);
00142     return img;
00143   }
00144   else
00145   {
00146     PCL_ERROR ("Error\n Image cannot be converted.\n");
00147     return QImage ();
00148   }
00149 }


dlut_place_recognition
Author(s): Qinghua Li, Yan Zhuang, Fei Yan
autogenerated on Sun Oct 5 2014 23:29:57