Skip to content

Commit

Permalink
Merge pull request #646 from VictorLamoine/template_intersections
Browse files Browse the repository at this point in the history 
Template intersections functions
  • Loading branch information
@taketwo
taketwo committed Apr 18, 2014
2 parents 6f7f0c5 + 8eb53d8 commit 04b2a6a
Show file tree
Hide file tree
Showing 4 changed files with 310 additions and 252 deletions.
2 changes: 1 addition & 1 deletion common/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@ if(build)
src/common.cpp
src/correspondence.cpp
src/distances.cpp
src/intersections.cpp
src/parse.cpp
src/poses_from_matches.cpp
src/print.cpp
Expand Down Expand Up @@ -117,6 +116,7 @@ if(build)
include/pcl/common/impl/centroid.hpp
include/pcl/common/impl/common.hpp
include/pcl/common/impl/eigen.hpp
include/pcl/common/impl/intersections.hpp
include/pcl/common/impl/copy_point.hpp
include/pcl/common/impl/io.hpp
include/pcl/common/impl/file_io.hpp
Expand Down
75 changes: 44 additions & 31 deletions common/src/intersections.cpp → ...include/pcl/common/impl/intersections.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,9 +35,14 @@
*
*/

#include <pcl/common/intersections.h>
#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,
Expand Down Expand Up @@ -67,62 +72,69 @@ pcl::lineWithLineIntersection (const pcl::ModelCoefficients &line_a,
return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps));
}

bool
pcl::planeWithPlaneIntersection (const Eigen::Vector4f &plane_a,
const Eigen::Vector4f &plane_b,
Eigen::VectorXf &line,
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)
{
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 5, 1> Vector5;
typedef Eigen::Matrix<Scalar, 5, 3> Matrix5;

//planes shouldn't be parallel
double test_cosine = plane_a.head<3>().dot(plane_b.head<3>());
double test_cosine = plane_a.template head<3> ().dot (plane_b.template head<3> ());
double upper_limit = 1 + angular_tolerance;
double lower_limit = 1 - angular_tolerance;

if ((test_cosine < upper_limit) && (test_cosine > lower_limit))
{
PCL_ERROR ("Plane A and Plane B are parallel\n");
PCL_DEBUG ("Plane A and Plane B are parallel\n");
return (false);
}

if ((test_cosine > -upper_limit) && (test_cosine < -lower_limit))
{
PCL_ERROR ("Plane A and Plane B are parallel\n");
PCL_DEBUG ("Plane A and Plane B are parallel\n");
return (false);
}

Eigen::Vector4f line_direction = plane_a.cross3(plane_b);
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
Eigen::MatrixXf langegrange_coefs(5,5);
langegrange_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;

Eigen::VectorXf b;
b.resize(5);
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];

//solve for the lagrange Multipliers
Eigen::VectorXf x;
x.resize(5);
x = langegrange_coefs.colPivHouseholderQr().solve(b);

line.resize(6);
line.head<3>() = x.head<3>(); // the x[3] and x[4] are the values of the lagrange multipliers and are neglected
//solve for the lagrange Multipliers
line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
line[3] = line_direction[0];
line[4] = line_direction[1];
line[5] = line_direction[2];
return true;
}
}

bool
pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a,
const Eigen::Vector4f &plane_b,
const Eigen::Vector4f &plane_c,
Eigen::Vector3f &intersection_point,
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)
{
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;

// TODO: Using Eigen::HyperPlanes is better to solve this problem
// Check if some planes are parallel
Eigen::Matrix3f normals_in_lines;
Matrix3 normals_in_lines;

for (int i = 0; i < 3; i++)
{
Expand All @@ -131,16 +143,16 @@ pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a,
normals_in_lines (i, 2) = plane_c[i];
}

double determinant = normals_in_lines.determinant ();
Scalar determinant = normals_in_lines.determinant ();
if (fabs (determinant) < determinant_tolerance)
{
// det ~= 0
PCL_ERROR ("Two or more planes are parralel.\n");
PCL_DEBUG ("At least two planes are parralel.\n");
return (false);
}

// Left part of the 3 equations
Eigen::Matrix3f left_member;
Matrix3 left_member;

for (int i = 0; i < 3; i++)
{
Expand All @@ -150,11 +162,12 @@ pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a,
}

// Right side of the 3 equations
Eigen::Vector3f right_member;
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
68 changes: 57 additions & 11 deletions common/include/pcl/common/intersections.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
* $Id$
*
*/

#ifndef PCL_INTERSECTIONS_H_
#define PCL_INTERSECTIONS_H_

Expand All @@ -57,10 +58,11 @@ namespace pcl
* \param[in] sqr_eps maximum allowable squared distance to the true solution
* \ingroup common
*/
PCL_EXPORTS bool
PCL_EXPORTS inline bool
lineWithLineIntersection (const Eigen::VectorXf &line_a,
const Eigen::VectorXf &line_b,
Eigen::Vector4f &point, double sqr_eps = 1e-4);
Eigen::Vector4f &point,
double sqr_eps = 1e-4);

/** \brief Get the intersection of a two 3D lines in space as a 3D point
* \param[in] line_a the coefficients of the first line (point, direction)
Expand All @@ -69,10 +71,12 @@ namespace pcl
* \param[in] sqr_eps maximum allowable squared distance to the true solution
* \ingroup common
*/
PCL_EXPORTS bool

PCL_EXPORTS inline bool
lineWithLineIntersection (const pcl::ModelCoefficients &line_a,
const pcl::ModelCoefficients &line_b,
Eigen::Vector4f &point, double sqr_eps = 1e-4);
Eigen::Vector4f &point,
double sqr_eps = 1e-4);

/** \brief Determine the line of intersection of two non-parallel planes using lagrange multipliers
* \note Described in: "Intersection of Two Planes, John Krumm, Microsoft Research, Redmond, WA, USA"
Expand All @@ -81,11 +85,29 @@ namespace pcl
* line.head<3>() the point on the line clossest to (0, 0, 0)
* \return true if succeeded/planes aren't parallel
*/
PCL_EXPORTS bool
PCL_EXPORTS template <typename Scalar> 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);

PCL_EXPORTS inline bool
planeWithPlaneIntersection (const Eigen::Vector4f &plane_a,
const Eigen::Vector4f &fplane_b,
const Eigen::Vector4f &plane_b,
Eigen::VectorXf &line,
double angular_tolerance = 0.1);
double angular_tolerance = 0.1)
{
return (planeWithPlaneIntersection<float> (plane_a, plane_b, line, angular_tolerance));
}

PCL_EXPORTS inline bool
planeWithPlaneIntersection (const Eigen::Vector4d &plane_a,
const Eigen::Vector4d &plane_b,
Eigen::VectorXd &line,
double angular_tolerance = 0.1)
{
return (planeWithPlaneIntersection<double> (plane_a, plane_b, line, angular_tolerance));
}

/** \brief Determine the point of intersection of three non-parallel planes by solving the equations.
* \note If using nearly parralel planes you can lower the determinant_tolerance value. This can
Expand All @@ -98,15 +120,39 @@ namespace pcl
* \param[out] intersection_point, the three coordinates x, y, z of the intersection point
* \return true if succeeded/planes aren't parallel
*/
PCL_EXPORTS bool
threePlanesIntersection (const Eigen::Vector4f &plane_a,
PCL_EXPORTS template <typename Scalar> 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);


PCL_EXPORTS inline bool
threePlanesIntersection (const Eigen::Vector4f &plane_a,
const Eigen::Vector4f &plane_b,
const Eigen::Vector4f &plane_c,
Eigen::Vector3f &intersection_point,
double determinant_tolerance = 1e-6);
double determinant_tolerance = 1e-6)
{
return (threePlanesIntersection<float> (plane_a, plane_b, plane_c,
intersection_point, determinant_tolerance));
}

PCL_EXPORTS inline bool
threePlanesIntersection (const Eigen::Vector4d &plane_a,
const Eigen::Vector4d &plane_b,
const Eigen::Vector4d &plane_c,
Eigen::Vector3d &intersection_point,
double determinant_tolerance = 1e-6)
{
return (threePlanesIntersection<double> (plane_a, plane_b, plane_c,
intersection_point, determinant_tolerance));
}

}
/*@}*/
#endif //#ifndef PCL_INTERSECTIONS_H_

#include <pcl/common/impl/intersections.hpp>

#endif //#ifndef PCL_INTERSECTIONS_H_
Loading

0 comments on commit 04b2a6a

Please sign in to comment.