angles.h

Go to the documentation of this file.

/*
 * Copyright (c) 2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * 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.
 *
 * 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.
 *
 * $Id$
 *
 */

#ifndef _CLOUD_GEOMETRY_ANGLES_H_
#define _CLOUD_GEOMETRY_ANGLES_H_

// ROS includes
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/PointStamped.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Polygon.h>

#include <door_handle_detector/geometry/point.h>
#include <door_handle_detector/geometry/nearest.h>

#include <Eigen/Core>

namespace cloud_geometry
{

  namespace angles
  {

    inline double
      getAngleBetweenPlanes (const std::vector<double> &plane_a, const std::vector<double> &plane_b)
    {
      return (acos (plane_a[0] * plane_b[0] + plane_a[1] * plane_b[1] + plane_a[2] * plane_b[2]));
    }


    inline double
      getAngleBetweenPlanes (const geometry_msgs::Point32 &plane_a, const geometry_msgs::Point32 &plane_b)
    {
      return (acos (plane_a.x * plane_b.x + plane_a.y * plane_b.y + plane_a.z * plane_b.z));
    }


    inline double
      getAngleBetweenPlanes (const geometry_msgs::Point32 &plane_a, const std::vector<double> &plane_b)
    {
      return (acos (plane_a.x * plane_b[0] + plane_a.y * plane_b[1] + plane_a.z * plane_b[2]));
    }


    inline double
      getAngleBetweenPlanes (const std::vector<double> &plane_a, const geometry_msgs::Point32 &plane_b)
    {
      return (acos (plane_a[0] * plane_b.x + plane_a[1] * plane_b.y + plane_a[2] * plane_b.z));
    }


    inline double
      getAngle2D (const double point[2])
    {
      double rad;
      if (point[0] == 0)
        rad = (point[1] < 0) ? -M_PI / 2.0 : M_PI / 2.0;
      else
      {
        rad = atan (point[1] / point[0]);
        if (point[0] < 0)
          rad += M_PI;
      }
      if (rad < 0)
        rad += 2 * M_PI;

      return (rad);
    }


    inline double
      getAngle2D (double x, double y)
    {
      double rad;
      if (x == 0)
        rad = (y < 0) ? -M_PI / 2.0 : M_PI / 2.0;
      else
      {
        rad = atan (y / x);
        if (x < 0)
          rad += M_PI;
      }
      if (rad < 0)
        rad += 2 * M_PI;

      return (rad);
    }


    inline double
      getAngle3D (const geometry_msgs::Point32 &v1, const geometry_msgs::Point32 &v2)
    {
      // Compute the vectors norms
      double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
      double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);

      // Compute the actual angle
      double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );

      // Check against NaN
      if (std::isnan (rad))
        ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
      return (rad);
    }


    inline double
      getAngle3D (const geometry_msgs::Point &v1, const geometry_msgs::Point &v2)
    {
      // Compute the vectors norms
      double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
      double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);

      // Compute the actual angle
      double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );

      // Check against NaN
      if (std::isnan (rad))
        ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
      return (rad);
    }


    inline double
      getAngle3D (const geometry_msgs::Point32 &v1, const geometry_msgs::Point &v2)
    {
      // Compute the vectors norms
      double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
      double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);

      // Compute the actual angle
      double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );

      // Check against NaN
      if (std::isnan (rad))
        ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
      return (rad);
    }


    inline double
      getAngle3D (const geometry_msgs::Point &v1, const geometry_msgs::Point32 &v2)
    {
      // Compute the vectors norms
      double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
      double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);

      // Compute the actual angle
      double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );

      // Check against NaN
      if (std::isnan (rad))
        ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
      return (rad);
    }



    inline void
      flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::PointStamped &viewpoint)
    {
      // See if we need to flip any plane normals
      float vp_m[3];
      vp_m[0] = viewpoint.point.x - point.x;
      vp_m[1] = viewpoint.point.y - point.y;
      vp_m[2] = viewpoint.point.z - point.z;

      // Dot product between the (viewpoint - point) and the plane normal
      double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));

      // Flip the plane normal
      if (cos_theta < 0)
      {
        for (int d = 0; d < 3; d++)
          normal (d) *= -1;
        // Hessian form (D = nc . p_plane (centroid here) + p)
        normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
      }
    }


    inline void
      flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::Point &viewpoint)
    {
      // See if we need to flip any plane normals
      float vp_m[3];
      vp_m[0] = viewpoint.x - point.x;
      vp_m[1] = viewpoint.y - point.y;
      vp_m[2] = viewpoint.z - point.z;

      // Dot product between the (viewpoint - point) and the plane normal
      double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));

      // Flip the plane normal
      if (cos_theta < 0)
      {
        for (int d = 0; d < 3; d++)
          normal (d) *= -1;
        // Hessian form (D = nc . p_plane (centroid here) + p)
        normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
      }
    }


    inline void
      flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::Point32 &viewpoint)
    {
      // See if we need to flip any plane normals
      float vp_m[3];
      vp_m[0] = viewpoint.x - point.x;
      vp_m[1] = viewpoint.y - point.y;
      vp_m[2] = viewpoint.z - point.z;

      // Dot product between the (viewpoint - point) and the plane normal
      double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));

      // Flip the plane normal
      if (cos_theta < 0)
      {
        for (int d = 0; d < 3; d++)
          normal (d) *= -1;
        // Hessian form (D = nc . p_plane (centroid here) + p)
        normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
      }
    }


    inline void
      flipNormalTowardsViewpoint (std::vector<double> &normal, const geometry_msgs::Point32 &point, const geometry_msgs::PointStamped &viewpoint)
    {
      // See if we need to flip any plane normals
      float vp_m[3];
      vp_m[0] = viewpoint.point.x - point.x;
      vp_m[1] = viewpoint.point.y - point.y;
      vp_m[2] = viewpoint.point.z - point.z;

      // Dot product between the (viewpoint - point) and the plane normal
      double cos_theta = (vp_m[0] * normal[0] + vp_m[1] * normal[1] + vp_m[2] * normal[2]);

      // Flip the plane normal
      if (cos_theta < 0)
      {
        for (int d = 0; d < 3; d++)
          normal[d] *= -1;
        // Hessian form (D = nc . p_plane (centroid here) + p)
        normal[3] = -1 * (normal[0] * point.x + normal[1] * point.y + normal[2] * point.z);
      }
    }


    inline void
      flipNormalTowardsViewpoint (std::vector<double> &normal, const geometry_msgs::Point32 &point, const geometry_msgs::Point32 &viewpoint)
    {
      // See if we need to flip any plane normals
      float vp_m[3];
      vp_m[0] = viewpoint.x - point.x;
      vp_m[1] = viewpoint.y - point.y;
      vp_m[2] = viewpoint.z - point.z;

      // Dot product between the (viewpoint - point) and the plane normal
      double cos_theta = (vp_m[0] * normal[0] + vp_m[1] * normal[1] + vp_m[2] * normal[2]);

      // Flip the plane normal
      if (cos_theta < 0)
      {
        for (int d = 0; d < 3; d++)
          normal[d] *= -1;
        // Hessian form (D = nc . p_plane (centroid here) + p)
        normal[3] = -1 * (normal[0] * point.x + normal[1] * point.y + normal[2] * point.z);
      }
    }

  }
}

#endif