406 lines
13 KiB
C++
406 lines
13 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 <iostream>
|
|
#include <fstream>
|
|
#include <algorithm>
|
|
#include <valarray>
|
|
#include <GL/gl.h>
|
|
#include <boost/numeric/ublas/matrix.hpp>
|
|
|
|
#include <opencv2/features2d/features2d.hpp>
|
|
#include <Skeleton/Skeleton.hpp>
|
|
#include <Skeleton/Box.hpp>
|
|
#include <Geometry/MathFunctions.hpp>
|
|
|
|
Skeleton::Skeleton() : m_vertices{}, m_edges{}, m_branches{}
|
|
{
|
|
|
|
}
|
|
|
|
|
|
bool Skeleton::loadFromFile(std::string const& path)
|
|
{
|
|
std::ifstream file{path};
|
|
std::string line;
|
|
std::stringstream stream;
|
|
|
|
if (!file)
|
|
{
|
|
std::cerr << "Warning : could not open file " << path << std::endl;
|
|
return false;
|
|
}
|
|
|
|
while (std::getline(file, line))
|
|
{
|
|
if (line.size() == 0)
|
|
continue;
|
|
|
|
char first = line[0];
|
|
line[0]=' ';
|
|
|
|
stream.clear();
|
|
stream.str(line);
|
|
|
|
switch (first)
|
|
{
|
|
case 'v':
|
|
{
|
|
float x;
|
|
float y;
|
|
float z;
|
|
stream >> x >> y >> z;
|
|
m_vertices.push_back(geo::Vector3<float>{x,y,z});
|
|
break;
|
|
}
|
|
|
|
case 'e':
|
|
{
|
|
unsigned int v1;
|
|
unsigned int v2;
|
|
stream >> v1 >> v2;
|
|
m_edges.push_back(geo::Vector2u{v1-1,v2-1});
|
|
break;
|
|
}
|
|
|
|
case 'b':
|
|
{
|
|
unsigned int vi;
|
|
Branch branch;
|
|
while (stream >> vi)
|
|
branch.push_back(vi);
|
|
m_branches.push_back(branch);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
void Skeleton::loadFromVectors(std::vector<geo::Vector3<float>> const& vertices,
|
|
Branch const& path
|
|
)
|
|
{
|
|
m_vertices = vertices;
|
|
m_branches.push_back(path);
|
|
}
|
|
|
|
|
|
std::vector<unsigned int> Skeleton::countNeighbors()
|
|
{
|
|
std::vector<unsigned int> neighbors_counters;
|
|
neighbors_counters.reserve(m_vertices.size());
|
|
for (auto i = 0u; i < m_vertices.size(); i++)
|
|
{
|
|
neighbors_counters.push_back(0);
|
|
}
|
|
|
|
for (auto const& edge : m_edges)
|
|
{
|
|
neighbors_counters[edge.x()]++;
|
|
neighbors_counters[edge.y()]++;
|
|
}
|
|
return neighbors_counters;
|
|
}
|
|
|
|
|
|
void Skeleton::draw() const
|
|
{
|
|
glEnableClientState(GL_VERTEX_ARRAY);
|
|
glVertexPointer(2,GL_FLOAT,sizeof(geo::Vector3<float>),&m_vertices[0]);
|
|
glDrawElements(GL_LINES,m_edges.size()*2,GL_UNSIGNED_INT,&m_edges[0]);
|
|
glDisableClientState(GL_VERTEX_ARRAY);
|
|
}
|
|
|
|
void Skeleton::draw(unsigned int i) const
|
|
{
|
|
glEnableClientState(GL_VERTEX_ARRAY);
|
|
glVertexPointer(2,GL_FLOAT,sizeof(geo::Vector3<float>),&m_vertices[0]);
|
|
|
|
glDrawElements(GL_LINE_STRIP,m_branches[i].size(), GL_UNSIGNED_INT,&(m_branches[i][0]));
|
|
|
|
glDisableClientState(GL_VERTEX_ARRAY);
|
|
}
|
|
|
|
void Skeleton::split()
|
|
{
|
|
std::vector<std::vector<bool>> connections;
|
|
std::vector<unsigned int> neighbors_counters;
|
|
|
|
neighbors_counters = countNeighbors();
|
|
|
|
for (unsigned int i = 0; i < m_vertices.size(); i++)
|
|
{
|
|
connections.push_back(std::vector<bool>{});
|
|
for (unsigned int j = 0; j <= i; j++)
|
|
{
|
|
connections[i].push_back(false);
|
|
}
|
|
}
|
|
|
|
for(auto const& edge : m_edges)
|
|
{
|
|
auto v1 = edge.x();
|
|
auto v2 = edge.y();
|
|
connections[std::max(v1,v2)][std::min(v1,v2)]= true;
|
|
}
|
|
|
|
for (unsigned int i = 0; i < m_vertices.size(); i++)
|
|
{
|
|
if (neighbors_counters[i]>2 || neighbors_counters[i]==1)
|
|
{
|
|
unsigned int numberOfBranchesFound = 0;
|
|
std::vector <unsigned int> onesPosition = findOnes(connections,neighbors_counters[i], i);
|
|
|
|
for (auto const& pathNumber : onesPosition)
|
|
{
|
|
Branch path;
|
|
path.push_back(i);
|
|
path.push_back(pathNumber);
|
|
auto j = 0u;
|
|
unsigned int k;
|
|
|
|
do
|
|
{
|
|
addNextPoint(connections, path);
|
|
j++;
|
|
k = path[path.size()-1];
|
|
} while (neighbors_counters[k] == 2);
|
|
|
|
bool toBeAdded = true;
|
|
|
|
for (unsigned int j = 0; j < m_branches.size(); j++)
|
|
{
|
|
if ( m_branches[j] == path )
|
|
{
|
|
toBeAdded = false;
|
|
break;
|
|
}
|
|
|
|
}
|
|
if (toBeAdded)
|
|
{
|
|
m_branches.push_back(path);
|
|
numberOfBranchesFound++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
std::cout << std::endl;
|
|
std::cout << "Nombre de branches de squelettes trouvées : " << m_branches.size() << std::endl;
|
|
}
|
|
|
|
|
|
void Skeleton::addNextPoint(std::vector<std::vector<bool>> const& connections, Branch& path) const
|
|
{
|
|
auto lastPoint = path[path.size()-1];
|
|
auto nextPointFound = false;
|
|
auto i = 0u;
|
|
while (!nextPointFound && i < m_vertices.size())
|
|
{
|
|
if (lastPoint != i && connections[std::max(i,lastPoint)][std::min(i,lastPoint)]
|
|
&& std::find(std::begin(path), std::end(path), i) == std::end(path))
|
|
{
|
|
path.push_back(i);
|
|
nextPointFound = true;
|
|
}
|
|
|
|
i++;
|
|
}
|
|
}
|
|
|
|
std::vector<unsigned int> Skeleton::findOnes(std::vector<std::vector<bool>> const& connections, unsigned int numberOfOnesToFind, unsigned int lineNumber) const
|
|
{
|
|
std::vector<unsigned int> onesPosition;
|
|
unsigned int numberOfOnesFound = 0;
|
|
unsigned int counter = 0;
|
|
|
|
while (numberOfOnesFound< numberOfOnesToFind && counter < m_vertices.size())
|
|
{
|
|
if (connections[std::max(counter,lineNumber)][std::min(counter,lineNumber)])
|
|
{
|
|
onesPosition.push_back(counter);
|
|
numberOfOnesFound++;
|
|
}
|
|
counter++;
|
|
}
|
|
|
|
return onesPosition;
|
|
}
|
|
|
|
std::vector<std::pair<unsigned int,unsigned int>> branchesMatching(std::vector<std::pair<cv::Point,cv::Point>> const& keypoints, Skeleton branches1, Skeleton branches2)
|
|
{
|
|
std::vector<std::pair<unsigned int,unsigned int>> matchingBranches;
|
|
|
|
// We verify that skeletons both have as many branches
|
|
if (branches1.m_branches.size() != branches2.m_branches.size())
|
|
{
|
|
std::cout << "Le premier squelette dispose de " << branches1.m_branches.size() << " branches, tandis que le second en dispose de " << branches2.m_branches.size() << "." << std::endl;
|
|
}
|
|
|
|
// We divide the x and y coordinates of each vertex by the z-coordinate for both skeletons
|
|
for (unsigned int i = 0; i < branches1.m_vertices.size() ; i++)
|
|
{
|
|
branches1.m_vertices[i].x() /= branches1.m_vertices[i].z();
|
|
branches1.m_vertices[i].y() /= branches1.m_vertices[i].z();
|
|
}
|
|
|
|
for (unsigned int i = 0; i < branches2.m_vertices.size() ; i++)
|
|
{
|
|
branches2.m_vertices[i].x() /= branches2.m_vertices[i].z();
|
|
branches2.m_vertices[i].y() /= branches2.m_vertices[i].z();
|
|
}
|
|
|
|
std::vector<unsigned int> correspondingBranches1;
|
|
std::vector<unsigned int> correspondingBranches2;
|
|
|
|
std::cout << "Hello" << std::endl;
|
|
|
|
// Matching between key points of the first image and branches of the first skeleton
|
|
for (unsigned int i = 0; i < keypoints.size(); i++)
|
|
{
|
|
auto const& keypoint = keypoints[i].first;
|
|
unsigned int nearest_branche_index;
|
|
nearest_branche_index = branches1.searchNearestBrancheIndex(keypoint, branches1);
|
|
correspondingBranches1.push_back(nearest_branche_index);
|
|
}
|
|
|
|
// Matching between key points of the second image and branches of the second skeleton
|
|
for (unsigned int i = 0; i < keypoints.size(); i++)
|
|
{
|
|
auto const& keypoint = keypoints[i].second;
|
|
unsigned int nearest_branche_index;
|
|
nearest_branche_index = branches2.searchNearestBrancheIndex(keypoint, branches2);
|
|
correspondingBranches2.push_back(nearest_branche_index);
|
|
}
|
|
|
|
// Initialisation of a 2D matching matrix couting the number of times that each branche of the first skeleton is associated to another branche of the second skeleton as their respective associated keypoints are paired
|
|
boost::numeric::ublas::matrix<unsigned int> matchingMatrix
|
|
= boost::numeric::ublas::zero_matrix<unsigned int>{
|
|
branches1.m_branches.size(),
|
|
branches2.m_branches.size()
|
|
};
|
|
|
|
for (unsigned int i = 0; i < correspondingBranches1.size() ; i++)
|
|
{
|
|
std::cout << "Le morceau du premier squelette d'indice " << correspondingBranches1[i] << " semble correspondre au morceau du second squelette d'indice " << correspondingBranches2[i] << "." << std::endl;
|
|
matchingMatrix(correspondingBranches1[i],correspondingBranches2[i])++;
|
|
}
|
|
|
|
std::cout << "--------Matrix is--------------" << std::endl;
|
|
for (auto i = 0u; i < branches1.m_branches.size(); i++)
|
|
{
|
|
for (auto j = 0u; j < branches2.m_branches.size(); j++)
|
|
std::cout << matchingMatrix(i,j) << " ";
|
|
std::cout << std::endl;
|
|
}
|
|
std::cout << std::endl;
|
|
std::cout << "----------------------------------" << std::endl;
|
|
|
|
for (unsigned int i = 0; i < branches1.m_branches.size(); i++)
|
|
{
|
|
unsigned int maximumMatchingScore = 0;
|
|
unsigned int maximumMatchingScoreIndex = 0;
|
|
|
|
for (unsigned int j = 0; j < branches2.m_branches.size(); j++)
|
|
{
|
|
if (matchingMatrix(i,j) > maximumMatchingScore)
|
|
{
|
|
maximumMatchingScore = matchingMatrix(i,j);
|
|
maximumMatchingScoreIndex = j;
|
|
}
|
|
}
|
|
std::pair<unsigned int, unsigned int> pairOfBranches(i,maximumMatchingScoreIndex);
|
|
matchingBranches.push_back(pairOfBranches);
|
|
}
|
|
|
|
return matchingBranches;
|
|
}
|
|
|
|
unsigned int Skeleton::searchNearestBrancheIndex(cv::Point const& keypoint, Skeleton branches)
|
|
{
|
|
unsigned int nearest_branche_index = 0;
|
|
float shortest_distance = std::numeric_limits<float>::max();
|
|
|
|
std::cout << "Sizes : m_vertices " << branches.m_vertices.size() << std::endl;
|
|
std::cout << " m_edges " << branches.m_edges.size() << std::endl;
|
|
std::cout << " m_branches " << branches.m_branches.size() << std::endl;
|
|
|
|
for (unsigned int j = 0 ; j < branches.m_branches.size() ; j++)
|
|
{
|
|
auto const& branche = branches.m_branches[j];
|
|
for (unsigned int k = 0 ; k < branche.size()-1 ; k++)
|
|
{
|
|
// We compute the coordinates of the projection of the keypoint on each edge
|
|
float d_keypoint_edge = geo::distanceToSegment(
|
|
keypoint,
|
|
// m_vertices[m_edges[branche[k]].x()],
|
|
// m_vertices[m_edges[branche[k]].y()]
|
|
geo::Vector2f{m_vertices.at(branche.at(k)).x(),
|
|
m_vertices.at(branche.at(k)).y()},
|
|
geo::Vector2f{m_vertices.at(branche.at(k+1)).x(),
|
|
m_vertices.at(branche.at(k+1)).y()}
|
|
);
|
|
|
|
if (d_keypoint_edge < shortest_distance)
|
|
{
|
|
shortest_distance = d_keypoint_edge;
|
|
nearest_branche_index = j;
|
|
}
|
|
|
|
}
|
|
}
|
|
return nearest_branche_index;
|
|
}
|
|
|
|
std::ostream& operator<<(std::ostream& out, Skeleton const& s)
|
|
{
|
|
for (auto const& vertex : s.m_vertices)
|
|
{
|
|
out << "v " << vertex.x() << " " << vertex.y() << " " << vertex.z() << "\n";
|
|
}
|
|
|
|
out << "\n";
|
|
|
|
for (auto const& edge : s.m_edges)
|
|
{
|
|
out << "e " << edge.x() << " " << edge.y() << "\n";
|
|
}
|
|
|
|
return out;
|
|
}
|