intersections.hpp
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* 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 the copyright holder(s) 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.
*
* $Id$
*
*/
#ifndef PCL_COMMON_INTERSECTIONS_IMPL_HPP_
#define PCL_COMMON_INTERSECTIONS_IMPL_HPP_
#include <pcl/pcl_macros.h>
#include <pcl/console/print.h>
//////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a,
const Eigen::VectorXf &line_b,
Eigen::Vector4f &point, double sqr_eps)
{
Eigen::Vector4f p1, p2;
lineToLineSegment (line_a, line_b, p1, p2);
// If the segment size is smaller than a pre-given epsilon...
double sqr_dist = (p1 - p2).squaredNorm ();
if (sqr_dist < sqr_eps)
{
point = p1;
return (true);
}
point.setZero ();
return (false);
}
bool
pcl::lineWithLineIntersection (const pcl::ModelCoefficients &line_a,
const pcl::ModelCoefficients &line_b,
Eigen::Vector4f &point, double sqr_eps)
{
Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a.values[0], line_a.values.size ());
Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b.values[0], line_b.values.size ());
return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps));
}
template <typename Scalar> bool
pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
const Eigen::Matrix<Scalar, 4, 1> &plane_b,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
double angular_tolerance)
{
using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
using Vector5 = Eigen::Matrix<Scalar, 5, 1>;
using Matrix5 = Eigen::Matrix<Scalar, 5, 5>;
// Normalize plane normals
Vector3 plane_a_norm (plane_a.template head<3> ());
Vector3 plane_b_norm (plane_b.template head<3> ());
plane_a_norm.normalize ();
plane_b_norm.normalize ();
// Test if planes are parallel
double test_cos = plane_a_norm.dot (plane_b_norm);
double tolerance_cos = 1 - sin (std::abs (angular_tolerance));
if (std::abs (test_cos) > tolerance_cos)
{
PCL_DEBUG ("Plane A and Plane B are parallel.\n");
return (false);
}
Vector4 line_direction = plane_a.cross3 (plane_b);
line_direction.normalized();
// Construct system of equations using lagrange multipliers with one objective function and two constraints
Matrix5 langrange_coefs;
langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
0,2,0, plane_a[1], plane_b[1],
0,0,2, plane_a[2], plane_b[2],
plane_a[0], plane_a[1], plane_a[2], 0, 0,
plane_b[0], plane_b[1], plane_b[2], 0, 0;
Vector5 b;
b << 0, 0, 0, -plane_a[3], -plane_b[3];
line.resize(6);
// Solve for the lagrange multipliers
line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
line.template tail<3>() = line_direction.template head<3>();
return (true);
}
template <typename Scalar> bool
pcl::threePlanesIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
const Eigen::Matrix<Scalar, 4, 1> &plane_b,
const Eigen::Matrix<Scalar, 4, 1> &plane_c,
Eigen::Matrix<Scalar, 3, 1> &intersection_point,
double determinant_tolerance)
{
using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
// TODO: Using Eigen::HyperPlanes is better to solve this problem
// Check if some planes are parallel
Matrix3 normals_in_lines;
for (int i = 0; i < 3; i++)
{
normals_in_lines (i, 0) = plane_a[i];
normals_in_lines (i, 1) = plane_b[i];
normals_in_lines (i, 2) = plane_c[i];
}
Scalar determinant = normals_in_lines.determinant ();
if (std::abs (determinant) < determinant_tolerance)
{
// det ~= 0
PCL_DEBUG ("At least two planes are parallel.\n");
return (false);
}
// Left part of the 3 equations
Matrix3 left_member;
for (int i = 0; i < 3; i++)
{
left_member (0, i) = plane_a[i];
left_member (1, i) = plane_b[i];
left_member (2, i) = plane_c[i];
}
// Right side of the 3 equations
Vector3 right_member;
right_member << -plane_a[3], -plane_b[3], -plane_c[3];
// Solve the system
intersection_point = left_member.fullPivLu ().solve (right_member);
return (true);
}
#endif //PCL_COMMON_INTERSECTIONS_IMPL_HPP
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::lineWithLineIntersection
PCL_EXPORTS bool lineWithLineIntersection(const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &point, double sqr_eps=1e-4)
Get the intersection of a two 3D lines in space as a 3D point.
Definition: intersections.hpp:47
pcl::threePlanesIntersection
PCL_EXPORTS bool threePlanesIntersection(const Eigen::Matrix< Scalar, 4, 1 > &plane_a, const Eigen::Matrix< Scalar, 4, 1 > &plane_b, const Eigen::Matrix< Scalar, 4, 1 > &plane_c, Eigen::Matrix< Scalar, 3, 1 > &intersection_point, double determinant_tolerance=1e-6)
Determine the point of intersection of three non-parallel planes by solving the equations.
pcl::planeWithPlaneIntersection
PCL_EXPORTS bool planeWithPlaneIntersection(const Eigen::Matrix< Scalar, 4, 1 > &plane_a, const Eigen::Matrix< Scalar, 4, 1 > &plane_b, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line, double angular_tolerance=0.1)
Determine the line of intersection of two non-parallel planes using lagrange multipliers.
pcl::ModelCoefficients::values
std::vector< float > values
Definition: ModelCoefficients.h:20
pcl::lineToLineSegment
PCL_EXPORTS void lineToLineSegment(const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg)
Get the shortest 3D segment between two 3D lines.
pcl::ModelCoefficients
Definition: ModelCoefficients.h:12