Skip to content

Template intersections functions #646

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Apr 18, 2014
Merged

Template intersections functions #646

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion common/CMakeLists.txt
View file
Open in desktop
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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should not remove this. Let it be and put just includes in there.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't understand what you want here; please be more specific.
You want me not to delete intersections.cpp ?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As far as I see Victor moved the code from 'intersections.cpp' to 'impl/intersections.hpp', so the former does not even exist anymore.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes I know but we had this kind of stuff in the past and Radu suggested this approach : a cpp file with headers inclusion only that is why I suggested the same.
Basically we have to avoid files going missing.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure if I get this. So what you/Radu suggest is to put template definitions in 'intersections.cpp' (to preserve this file) and from 'intersections.h' include it instead of 'hpp'?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think what nizar suggest is an "empty" .cpp file wich only includes the .h (which includes .hpp)

You know better than me what is the impact of deleting a file in PCL. Tell me what you want me to do.

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
Open in desktop
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
Open in desktop
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