38 #ifndef PCL_GEOMETRY_H_
39 #define PCL_GEOMETRY_H_
42 # pragma GCC system_header
46 #include <pcl/console/print.h>
60 template <
typename Po
intT>
inline float
63 Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
64 return (diff.norm ());
68 template<
typename Po
intT>
inline float
71 Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
72 return (diff.squaredNorm ());
81 template<
typename Po
intT,
typename NormalT>
inline void
85 Eigen::Vector3f po = point - plane_origin;
86 const Eigen::Vector3f normal = plane_normal.getVector3fMapConst ();
87 float lambda = normal.dot(po);
88 projected.getVector3fMap () = point.getVector3fMapConst () - (lambda * normal);
98 project (
const Eigen::Vector3f& point,
const Eigen::Vector3f &plane_origin,
99 const Eigen::Vector3f& plane_normal, Eigen::Vector3f& projected)
101 Eigen::Vector3f po = point - plane_origin;
102 float lambda = plane_normal.dot(po);
103 projected = point - (lambda * plane_normal);
115 inline Eigen::Vector3f
117 Eigen::Vector3f
const &plane_origin,
118 Eigen::Vector3f
const &plane_normal)
120 Eigen::Vector3f projection;
121 project (point, plane_origin, plane_normal, projection);
122 Eigen::Vector3f projected_as_unit_vector = projection - plane_origin;
123 projected_as_unit_vector.normalize ();
124 return projected_as_unit_vector;
134 inline Eigen::Vector3f
137 Eigen::Vector3f rand_ortho_axis;
138 rand_ortho_axis.setRandom();
139 if (std::abs (axis.z ()) > 1E-8f)
141 rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
143 else if (std::abs (axis.y ()) > 1E-8f)
145 rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
147 else if (std::abs (axis.x ()) > 1E-8f)
149 rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
153 PCL_WARN (
"[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f");
156 rand_ortho_axis.normalize ();
157 return rand_ortho_axis;
165 #endif //#ifndef PCL_GEOMETRY_H_