normals.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 *
 * $Id: normals.h 693 2012-04-20 09:22:39Z ihulik $
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Rostislav Hulik (ihulik@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: 11.01.2012 (version 1.0)
 * 
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef BUT_SEG_UTILS_NORMALS_H
#define BUT_SEG_UTILS_NORMALS_H

// Opencv 2
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc_c.h>

// ROS
#include <sensor_msgs/CameraInfo.h>

// Eigen
#include <Eigen/Core>

// PCL
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace but_plane_detector
{
        class NormalType
        {
                public:
                        static const int DIRECT = 1;
                        static const int LSQ = 2;
                        static const int LSQAROUND = 4;
                        static const int LTS = 8;
                        static const int LTSAROUND = 16;

                        static const int PCL = 32;
        };

        template <typename Scalar>
        class Plane
        {
                public:
                        Plane(Scalar A, Scalar B, Scalar C, Scalar D)
                        {
                                a = A;
                                b = B;
                                c = C;
                                d = D;
                                norm = sqrt(a*a + b*b + c*c);

                                if (norm != 0)
                                {
                                        a /= norm;
                                        b /= norm;
                                        c /= norm;
                                        d /= norm;
                                }
                        }

                        Scalar distance(cv::Vec<Scalar, 3> pt)
                        {
                                Scalar val = a*pt[0] + b*pt[1] + c*pt[2] + d;
                                return (val > 0? val : -val);
                        }

                        bool isSimilar(Plane &plane, Scalar angleErr, Scalar shiftErr)
                        {
                                Scalar angle = acos(((a * plane.a) + (b * plane.b) + (c * plane.c)));
                                Scalar xd1 = plane.d - d;
                                Scalar xd2 = d - plane.d;
                                if ((angle < angleErr  && (xd1 > 0 ? xd1 : - xd1) < shiftErr) || (angle > M_PI - angleErr && (xd2 > 0 ? xd2 : - xd2) < shiftErr))
                                        return true;
                                else
                                        return false;
                        }

                        void getQuaternionRotation(Scalar &x, Scalar &y, Scalar &z, Scalar &w)
                        {
                                Eigen::Vector3f current(a, b, c);
                                Eigen::Vector3f target(1.0, 0.0, 0.0);

                                Eigen::Quaternion<float> q;
                                q.setFromTwoVectors(current, target);
//
//                              Scalar nx = b*1 - c*0;
//                              Scalar ny = c*0 - a*1;
//                              Scalar nz = a*0 - b*0;
//
//                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
//                              nx /= length;
//                              ny /= length;
//                              nz /= length;
//
//
//                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
//                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
//
//                              Eigen::Quaternion<Scalar> q;
//                              q = Eigen::AngleAxis<Scalar>(-angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));

                                x = q.x();
                                y = q.y();
                                z = q.z();
                                w = q.w();
                        }

                        void getInverseQuaternionRotation(Scalar &x, Scalar &y, Scalar &z, Scalar &w)
                        {
                                Eigen::Vector3f current(a, b, c);
                                Eigen::Vector3f target(1.0, 0.0, 0.0);

                                Eigen::Quaternion<float> q;
                                q.setFromTwoVectors(target, current);


//                              Scalar nx = b*1 - c*0;
//                              Scalar ny = c*0 - a*1;
//                              Scalar nz = a*0 - b*0;
//
//                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
//                              nx /= length;
//                              ny /= length;
//                              nz /= length;
//
//                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
//                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
//
//                              Eigen::Quaternion<Scalar> q;
//                              q = Eigen::AngleAxis<Scalar>(angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));

                                x = q.x();
                                y = q.y();
                                z = q.z();
                                w = q.w();
                        }

                        void getQuaternionRotation(Scalar &x, Scalar &y, Scalar &z, Scalar &w, cv::Vec<Scalar, 3> &normal)
                        {
                                Eigen::Vector3f current(a, b, c);
                                Eigen::Vector3f target(normal[0], normal[1], normal[2]);

                                Eigen::Quaternion<float> q;
                                q.setFromTwoVectors(current, target);

//                              Scalar nx = b*normal[2] - c*normal[1];
//                              Scalar ny = c*normal[0] - a*normal[2];
//                              Scalar nz = a*normal[1] - b*normal[0];
//
//                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
//                              nx /= length;
//                              ny /= length;
//                              nz /= length;
//
//
//                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
//                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
//
//                              Eigen::Quaternion<Scalar> q;
//                              q = Eigen::AngleAxis<Scalar>(-angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));

                                x = q.x();
                                y = q.y();
                                z = q.z();
                                w = q.w();
                        }

                        void getQuaternionRotationInverse(Scalar &x, Scalar &y, Scalar &z, Scalar &w, cv::Vec<Scalar, 3> &normal)
                        {
                                Eigen::Vector3f current(a, b, c);
                                Eigen::Vector3f target(normal[0], normal[1], normal[2]);

                                Eigen::Quaternion<float> q;
                                q.setFromTwoVectors(target, current);

//                              Scalar nx = b*normal[2] - c*normal[1];
//                              Scalar ny = c*normal[0] - a*normal[2];
//                              Scalar nz = a*normal[1] - b*normal[0];
//
//                              Scalar length = sqrt(nx*nx + ny*ny + nz*nz);
//                              nx /= length;
//                              ny /= length;
//                              nz /= length;
//
//                              Scalar sign = ((nx * 1 + ny * 0 + nz * 0) < 0) ? -1:1;
//                              Scalar angle = acos(a*0 + b*0 + c*1) * sign;
//
//                              Eigen::Quaternion<Scalar> q;
//                              q = Eigen::AngleAxis<Scalar>(angle, Eigen::Matrix<Scalar, 3, 1>(nx, ny, nz));

                                x = q.x();
                                y = q.y();
                                z = q.z();
                                w = q.w();
                        }

                        Scalar a;

                        Scalar b;

                        Scalar c;

                        Scalar norm;

                        Scalar d;
        };

        class Normals
        {
                public:
                        Normals(cv::Mat &points, const sensor_msgs::CameraInfoConstPtr& cam_info, int normalType = NormalType::PCL, int neighborhood = 4,
                                                                                                                                                 float threshold = 0.2, float outlierThreshold = 0.02, int iter = 3);

                        Normals(pcl::PointCloud<pcl::PointXYZ> &pointcloud, int normalType = NormalType::PCL, int neighborhood = 4,
                                         float threshold = 0.2, float outlierThreshold = 0.02, int iter = 3);

                        cv::Vec4f getNormal(int i, int j, int step, float depthThreshold);

                        cv::Vec4f getNormalLTS(int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter);

                        cv::Vec4f getNormalLTSAround(int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter);

                        cv::Vec4f getNormalLSQ(int i, int j, int step, float depthThreshold);

                        cv::Vec4f getNormalLSQAround(int i, int j, int step, float depthThreshold);

                        static Plane<float> LeastSquaresPlane(std::vector<cv::Vec3f> &points);

                        cv::Mat m_points;

                        cv::Mat m_planes;

                        sensor_msgs::CameraInfoConstPtr m_cam_info;

                        std::vector<cv::Vec3f> m_quantVectors;

                        int m_quantbins;

                private:

                        void nextStep(int step, int &x, int &y, int &plusX, int &plusY);

                        void initQuantVectors(int n_bins, std::vector<cv::Vec3f> &vec);

                        unsigned int getQuantVector(std::vector<cv::Vec3f> &vec, cv::Vec3f &vector);
        }; // class

} // but_scenemodel

#endif //BUT_SEG_UTILS_NORMALS_H