233 lines
7.3 KiB
C++
233 lines
7.3 KiB
C++
////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Paella
|
|
// Copyright (C) 2015 - Thomas FORGIONE, Emilie JALRAS, Marion LENFANT, Thierry MALON, Amandine PAILLOUX
|
|
// Authors :
|
|
// Thomas FORGIONE
|
|
// Emilie JALRAS
|
|
// Marion LENFANT
|
|
// Thierry MALON
|
|
// Amandine PAILLOUX
|
|
//
|
|
// This file is part of the project Paella
|
|
// This software is provided 'as-is', without any express or implied warranty.
|
|
// In no event will the authors be held liable for any damages arising from the use of this software.
|
|
//
|
|
// Permission is granted to anyone to use this software for any purpose,
|
|
// including commercial applications, and to alter it and redistribute it freely,
|
|
// subject to the following restrictions:
|
|
//
|
|
// 1. The origin of this software must not be misrepresented;
|
|
// you must not claim that you wrote the original software.
|
|
// If you use this software in a product, an acknowledgment
|
|
// in the product documentation would be appreciated but is not required.
|
|
//
|
|
// 2. Altered source versions must be plainly marked as such,
|
|
// and must not be misrepresented as being the original software.
|
|
//
|
|
// 3. This notice may not be removed or altered from any source distribution.
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
#include <limits>
|
|
|
|
#include <opencv2/features2d/features2d.hpp>
|
|
#include <eigen3/Eigen/Dense>
|
|
|
|
#include "Utility/VectorFunctions.hpp"
|
|
#include "Geometry/Vector.hpp"
|
|
#include "Geometry/MathFunctions.hpp"
|
|
|
|
namespace geo
|
|
{
|
|
|
|
namespace detail
|
|
{
|
|
template<typename T>
|
|
auto& get_x(T& t) { return t.x; }
|
|
|
|
template<typename T>
|
|
auto const& get_x(T const& t) { return t.x; }
|
|
|
|
template <typename T>
|
|
auto& get_y(T& t) { return t.y; }
|
|
|
|
template <typename T>
|
|
auto const& get_y(T const& t) { return t.y; }
|
|
|
|
template <typename T>
|
|
auto& get_z(T& t) { return t.z; }
|
|
|
|
template <typename T>
|
|
auto const& get_z(T const& t) { return t.z; }
|
|
}
|
|
|
|
template<typename T>
|
|
struct vector_size<cv::Point_<T>>
|
|
{
|
|
static constexpr std::size_t value = 2;
|
|
};
|
|
|
|
template<typename InputIt, typename T, typename Distance>
|
|
InputIt project(T const& elementToProject, InputIt first, InputIt last, Distance const& distance)
|
|
{
|
|
float best_dist = std::numeric_limits<float>::max();
|
|
auto best_element = first;
|
|
|
|
for (; first != last; first++)
|
|
{
|
|
float current_dist = distance(elementToProject, *first);
|
|
|
|
if (current_dist < best_dist)
|
|
{
|
|
best_dist = current_dist;
|
|
best_element = first;
|
|
}
|
|
}
|
|
|
|
return best_element;
|
|
}
|
|
|
|
template<typename T1, typename T2, typename T3, std::size_t N>
|
|
struct distance_to_segment_helper;
|
|
|
|
template<typename T1, typename T2, typename T3>
|
|
struct distance_to_segment_helper<T1,T2,T3,2>
|
|
{
|
|
|
|
float operator()(T1 const& v, T2 const& p1, T3 const& p2)
|
|
{
|
|
using detail::get_x;
|
|
using detail::get_y;
|
|
|
|
auto a = -get_y(p1) +get_y(p2);
|
|
auto b = get_x(p1) - get_x(p2);;
|
|
auto xH = (b*b*get_x(v) - a*b*get_y(v) + a*a*get_x(p1) + a*b*get_y(p1))/(a*a+b*b);
|
|
auto yH = xH;
|
|
|
|
if (std::abs(b) < std::abs(a))
|
|
yH = (b*xH-b*get_x(v)+a*get_y(v))/a;
|
|
else
|
|
yH = (-a*xH+a*get_x(p1)+b*get_y(p1))/b;
|
|
|
|
float prod = (xH - get_x(p1)) * (xH - get_x(p2)) + (yH - get_y(p1)) * (yH - get_y(p2));
|
|
float dist;
|
|
|
|
if (prod <= 0)
|
|
dist = (get_x(v)-xH)*(get_x(v)-xH) + (get_y(v)-yH)*(get_y(v)-yH) ;
|
|
else
|
|
dist = std::min( (get_x(v)-get_x(p1))*(get_x(v)-get_x(p1)) + (get_y(v)-get_y(p1))*(get_y(v)-get_y(p1)),
|
|
(get_x(v)-get_x(p2))*(get_x(v)-get_x(p2)) + (get_y(v)-get_y(p2))*(get_y(v)-get_y(p2)));
|
|
|
|
return dist;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
template<typename T1, typename T2, typename T3>
|
|
struct distance_to_segment_helper<T1,T2,T3,3>
|
|
{
|
|
float operator()(T1 const& v, T2 const& p1, T3 const& p2)
|
|
{
|
|
using detail::get_x;
|
|
using detail::get_y;
|
|
using detail::get_z;
|
|
|
|
auto x12 = get_x(p2)-get_x(p1);
|
|
auto y12 = get_y(p2)-get_y(p1);
|
|
auto z12 = get_z(p2)-get_z(p1);
|
|
|
|
auto x1v = get_x(v)-get_x(p1);
|
|
auto y1v = get_y(v)-get_y(p1);
|
|
auto z1v = get_z(v)-get_z(p1);
|
|
|
|
auto x2v = get_x(v)-get_x(p2);
|
|
auto y2v = get_y(v)-get_y(p2);
|
|
auto z2v = get_z(v)-get_z(p2);
|
|
|
|
auto scal1 = x12 * x1v + y12 * y1v + z12 * z1v;
|
|
auto scal2 = x12 * x2v + y12 * y2v + z12 * z2v;
|
|
auto norm12 = x12 * x12 + y12 * y12 + z12 * z12;
|
|
|
|
auto xH = get_x(p1) + (scal1 / norm12) * x12;
|
|
auto yH = get_y(p1) + (scal1 / norm12) * y12;
|
|
auto zH = get_z(p1) + (scal1 / norm12) * z12;
|
|
|
|
float dist;
|
|
|
|
if ((scal1*scal2) <0)
|
|
{
|
|
dist = (get_x(v)-xH) * (get_x(v)-xH) + (get_y(v)-yH) * (get_y(v)-yH) + (get_z(v)-zH) * (get_z(v)*zH);
|
|
}
|
|
else
|
|
{
|
|
dist = std::min( (get_x(v)-get_x(p1))*(get_x(v)-get_x(p1)) + (get_y(v)-get_y(p1))*(get_y(v)-get_y(p1)) + (get_z(v)-get_z(p1))*(get_z(v)-get_z(p1)),
|
|
(get_x(v)-get_x(p2))*(get_x(v)-get_x(p2)) + (get_y(v)-get_y(p2))*(get_y(v)-get_y(p2)) + (get_z(v)-get_z(p2))*(get_z(v)-get_z(p2)));
|
|
}
|
|
|
|
return dist;
|
|
}
|
|
|
|
};
|
|
|
|
template<typename T1, typename T2, typename T3>
|
|
float distanceToSegment(T1 const& v, T2 const& p1, T3 const& p2)
|
|
{
|
|
static_assert(vector_size<T1>::value == 2 || vector_size<T1>::value == 3,
|
|
"distanceToSegment is only implemented in dimensions 2 and 3");
|
|
|
|
static_assert(vector_size<T1>::value == vector_size<T2>::value,
|
|
"When computing distance between point and segment,"
|
|
"all elements must have the same size");
|
|
|
|
static_assert(vector_size<T1>::value == vector_size<T3>::value,
|
|
"When computing distance between point and segment,"
|
|
"all elements must have the same size");
|
|
|
|
return distance_to_segment_helper<T1,T2,T3,vector_size<T1>::value>{}(v,p1,p2);
|
|
}
|
|
|
|
template<typename T>
|
|
geo::Vector4<T> closestPlane(std::vector<geo::Vector3<T>> const& points)
|
|
{
|
|
// for 3 points we just compute the plane that contains the 3 points
|
|
if (points.size() == 3)
|
|
{
|
|
auto vec = crossProduct(points[1] - points[0], points[2] - points[0]);
|
|
vec /= vec.norm();
|
|
float t = - (points[0].x()*vec.x() + points[0].y()*vec.y() + points[0].z()*vec.z());
|
|
return geo::Vector4f{vec.x(), vec.y(), vec.z(), t};
|
|
}
|
|
// for more points we use a PCA to find the best plane
|
|
else
|
|
{
|
|
Eigen::MatrixXf m{points.size(),3};
|
|
|
|
for ( unsigned int i = 0; i < points.size(); i++)
|
|
{
|
|
m(i,0) = points[i].x();
|
|
m(i,1) = points[i].y();
|
|
m(i,2) = points[i].z();
|
|
}
|
|
|
|
Eigen::MatrixXf centered = m.rowwise() - m.colwise().mean();
|
|
Eigen::MatrixXf cov = centered.adjoint() * centered;
|
|
|
|
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig{cov};
|
|
|
|
Eigen::Vector3f v1 = eig.eigenvectors().col(1);
|
|
Eigen::Vector3f v2 = eig.eigenvectors().col(2);
|
|
|
|
Eigen::Vector3f n = v1.cross(v2);
|
|
|
|
return( geo::Vector4f{n(0),n(1),n(2), -n.dot(m.colwise().mean())} );
|
|
}
|
|
}
|
|
|
|
template<typename T>
|
|
bool whichSide(geo::Vector4<T> const& plane, geo::Vector3<T> const& point)
|
|
{
|
|
return plane.x()*point.x() + plane.y()*point.y() + plane.z()*point.z() < - plane.t();
|
|
}
|
|
|
|
} // namespace geo
|