normals.h
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: normals.h 693 2012-04-20 09:22:39Z ihulik $
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Rostislav Hulik (ihulik@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 11.01.2012 (version 1.0)
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00033 #pragma once
00034 #ifndef BUT_SEG_UTILS_NORMALS_H
00035 #define BUT_SEG_UTILS_NORMALS_H
00036 
00037 // Opencv 2
00038 #include <opencv2/highgui/highgui.hpp>
00039 #include <opencv2/imgproc/imgproc_c.h>
00040 
00041 // ROS
00042 #include <sensor_msgs/CameraInfo.h>
00043 
00044 // Eigen
00045 #include <Eigen/Core>
00046 
00047 // PCL
00048 #include <pcl_ros/point_cloud.h>
00049 #include <pcl/point_cloud.h>
00050 #include <pcl/point_types.h>
00051 
00052 namespace but_plane_detector
00053 {
00057         class NormalType
00058         {
00059                 public:
00063                         static const int DIRECT = 1;
00067                         static const int LSQ = 2;
00071                         static const int LSQAROUND = 4;
00075                         static const int LTS = 8;
00079                         static const int LTSAROUND = 16;
00080 
00084                         static const int PCL = 32;
00085         };
00086 
00090         template <typename Scalar>
00091         class Plane
00092         {
00093                 public:
00101                         Plane(Scalar A, Scalar B, Scalar C, Scalar D)
00102                         {
00103                                 a = A;
00104                                 b = B;
00105                                 c = C;
00106                                 d = D;
00107                                 norm = sqrt(a*a + b*b + c*c);
00108 
00109                                 if (norm != 0)
00110                                 {
00111                                         a /= norm;
00112                                         b /= norm;
00113                                         c /= norm;
00114                                         d /= norm;
00115                                 }
00116                         }
00117 
00122                         Scalar distance(cv::Vec<Scalar, 3> pt)
00123                         {
00124                                 Scalar val = a*pt[0] + b*pt[1] + c*pt[2] + d;
00125                                 return (val > 0? val : -val);
00126                         }
00127 
00134                         bool isSimilar(Plane &plane, Scalar angleErr, Scalar shiftErr)
00135                         {
00136                                 Scalar angle = acos(((a * plane.a) + (b * plane.b) + (c * plane.c)));
00137                                 Scalar xd1 = plane.d - d;
00138                                 Scalar xd2 = d - plane.d;
00139                                 if ((angle < angleErr  && (xd1 > 0 ? xd1 : - xd1) < shiftErr) || (angle > M_PI - angleErr && (xd2 > 0 ? xd2 : - xd2) < shiftErr))
00140                                         return true;
00141                                 else
00142                                         return false;
00143                         }
00144 
00152                         void getQuaternionRotation(Scalar &x, Scalar &y, Scalar &z, Scalar &w)
00153                         {
00154                                 Eigen::Vector3f current(a, b, c);
00155                                 Eigen::Vector3f target(1.0, 0.0, 0.0);
00156 
00157                                 Eigen::Quaternion<float> q;
00158                                 q.setFromTwoVectors(current, target);
00159 //
00160 //                              Scalar nx = b*1 - c*0;
00161 //                              Scalar ny = c*0 - a*1;
00162 //                              Scalar nz = a*0 - b*0;
00163 //
00164 //                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
00165 //                              nx /= length;
00166 //                              ny /= length;
00167 //                              nz /= length;
00168 //
00169 //
00170 //                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
00171 //                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
00172 //
00173 //                              Eigen::Quaternion<Scalar> q;
00174 //                              q = Eigen::AngleAxis<Scalar>(-angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));
00175 
00176                                 x = q.x();
00177                                 y = q.y();
00178                                 z = q.z();
00179                                 w = q.w();
00180                         }
00181 
00182                         void getInverseQuaternionRotation(Scalar &x, Scalar &y, Scalar &z, Scalar &w)
00183                         {
00184                                 Eigen::Vector3f current(a, b, c);
00185                                 Eigen::Vector3f target(1.0, 0.0, 0.0);
00186 
00187                                 Eigen::Quaternion<float> q;
00188                                 q.setFromTwoVectors(target, current);
00189 
00190 
00191 //                              Scalar nx = b*1 - c*0;
00192 //                              Scalar ny = c*0 - a*1;
00193 //                              Scalar nz = a*0 - b*0;
00194 //
00195 //                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
00196 //                              nx /= length;
00197 //                              ny /= length;
00198 //                              nz /= length;
00199 //
00200 //                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
00201 //                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
00202 //
00203 //                              Eigen::Quaternion<Scalar> q;
00204 //                              q = Eigen::AngleAxis<Scalar>(angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));
00205 
00206                                 x = q.x();
00207                                 y = q.y();
00208                                 z = q.z();
00209                                 w = q.w();
00210                         }
00211 
00212                         void getQuaternionRotation(Scalar &x, Scalar &y, Scalar &z, Scalar &w, cv::Vec<Scalar, 3> &normal)
00213                         {
00214                                 Eigen::Vector3f current(a, b, c);
00215                                 Eigen::Vector3f target(normal[0], normal[1], normal[2]);
00216 
00217                                 Eigen::Quaternion<float> q;
00218                                 q.setFromTwoVectors(current, target);
00219 
00220 //                              Scalar nx = b*normal[2] - c*normal[1];
00221 //                              Scalar ny = c*normal[0] - a*normal[2];
00222 //                              Scalar nz = a*normal[1] - b*normal[0];
00223 //
00224 //                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
00225 //                              nx /= length;
00226 //                              ny /= length;
00227 //                              nz /= length;
00228 //
00229 //
00230 //                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
00231 //                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
00232 //
00233 //                              Eigen::Quaternion<Scalar> q;
00234 //                              q = Eigen::AngleAxis<Scalar>(-angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));
00235 
00236                                 x = q.x();
00237                                 y = q.y();
00238                                 z = q.z();
00239                                 w = q.w();
00240                         }
00241 
00242                         void getQuaternionRotationInverse(Scalar &x, Scalar &y, Scalar &z, Scalar &w, cv::Vec<Scalar, 3> &normal)
00243                         {
00244                                 Eigen::Vector3f current(a, b, c);
00245                                 Eigen::Vector3f target(normal[0], normal[1], normal[2]);
00246 
00247                                 Eigen::Quaternion<float> q;
00248                                 q.setFromTwoVectors(target, current);
00249 
00250 //                              Scalar nx = b*normal[2] - c*normal[1];
00251 //                              Scalar ny = c*normal[0] - a*normal[2];
00252 //                              Scalar nz = a*normal[1] - b*normal[0];
00253 //
00254 //                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
00255 //                              nx /= length;
00256 //                              ny /= length;
00257 //                              nz /= length;
00258 //
00259 //                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
00260 //                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
00261 //
00262 //                              Eigen::Quaternion<Scalar> q;
00263 //                              q = Eigen::AngleAxis<Scalar>(angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));
00264 
00265                                 x = q.x();
00266                                 y = q.y();
00267                                 z = q.z();
00268                                 w = q.w();
00269                         }
00270 
00274                         Scalar a;
00275 
00279                         Scalar b;
00280 
00284                         Scalar c;
00285 
00289                         Scalar norm;
00290 
00294                         Scalar d;
00295         };
00296 
00300         class Normals
00301         {
00302                 public:
00314                         Normals(cv::Mat &points, const sensor_msgs::CameraInfoConstPtr& cam_info, int normalType = NormalType::PCL, int neighborhood = 4,
00315                                                                                                                                                  float threshold = 0.2, float outlierThreshold = 0.02, int iter = 3);
00316 
00324                         Normals(pcl::PointCloud<pcl::PointXYZ> &pointcloud, int normalType = NormalType::PCL, int neighborhood = 4,
00325                                          float threshold = 0.2, float outlierThreshold = 0.02, int iter = 3);
00326 
00334                         cv::Vec4f getNormal(int i, int j, int step, float depthThreshold);
00335 
00345                         cv::Vec4f getNormalLTS(int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter);
00346 
00356                         cv::Vec4f getNormalLTSAround(int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter);
00357 
00365                         cv::Vec4f getNormalLSQ(int i, int j, int step, float depthThreshold);
00366 
00374                         cv::Vec4f getNormalLSQAround(int i, int j, int step, float depthThreshold);
00375 
00382                         static Plane<float> LeastSquaresPlane(std::vector<cv::Vec3f> &points);
00383 
00387                         cv::Mat m_points;
00388 
00392                         cv::Mat m_planes;
00393 
00397                         sensor_msgs::CameraInfoConstPtr m_cam_info;
00398 
00402                         std::vector<cv::Vec3f> m_quantVectors;
00403 
00407                         int m_quantbins;
00408 
00409                 private:
00410 
00419                         void nextStep(int step, int &x, int &y, int &plusX, int &plusY);
00420 
00426                         void initQuantVectors(int n_bins, std::vector<cv::Vec3f> &vec);
00427 
00433                         unsigned int getQuantVector(std::vector<cv::Vec3f> &vec, cv::Vec3f &vector);
00434         }; // class
00435 
00436 } // but_scenemodel
00437 
00438 #endif //BUT_SEG_UTILS_NORMALS_H


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:51:56