double CovarianceModel::covMatrixDistance(const cv::Mat& a, const cv::Mat& b)
{
// if the values are the same in each matrix, save the trouble of eigenvalues
// the behaviour of the math is the same anyway
if(cv::countNonZero(a == b) == a.cols*a.rows)
{
return 0.0;
}
else
{
double result = 0.0;
// use the Eigen library to compute the generalized eigenvalues of two covariance matrices
// Ax = \lambda Bx where x_k != 0
Eigen::MatrixXd C_i, C_j;
cv::cv2eigen(a, C_i);
cv::cv2eigen(b, C_j);
Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es;
es.compute(C_i, C_j, Eigen::ComputeEigenvectors|Eigen::Ax_lBx);
cv::Mat eigenValues;
cv::eigen2cv(Eigen::MatrixXd(es.eigenvalues().real()), eigenValues);
for(int i=0; i<eigenValues.rows; ++i) // eigenValues.rows == 5
result += pow(log(eigenValues.at<double>(i,0)), 2); // ln^2 \lambda_k (C_i, C_j)
//result = sqrt(result);
// http://www.developerstation.org/2012/04/general-eigen-values-and-eigen-vectors.html
// http://eigen.tuxfamily.org/dox/classEigen_1_1GeneralizedSelfAdjointEigenSolver.html#aaa204ef15aaefac270c0376269083ed6
// if(isnan(result))
// {
// /* Problem: the generalized eigenvalues of these matrices cannot be computed because
// * each column should be independent and zero vectors are clearly not like that.
// *Example:
// A:[0, 0, 0, 0, 0;
// 0, 1482.289314516129, 0, 4.623991935483871, 0;
// 0, 0, 0, 0, 0;
// 0, 4.623991935483871, 0, 1138.168346774193, 0;
// 0, 0, 0, 0, 0]
// B:[0, 0, 0, 0, 0;
// 0, 21.51992753623188, 0, -1.005434782608696, 0;
// 0, 0, 0, 0, 0;
// 0, -1.005434782608696, 0, 11.07065217391304, 0;
// 0, 0, 0, 0, 0]
// */
// std::cerr << "A:" << a << std::endl
// << "B:" << b << std::endl
// << "eigenvalues: " << eigenValues << std::endl
// << "distance: " << result << std::endl;
// throw std::runtime_error("distance is nan, possibly eigenvalues are also nan");
// }
return result;
}
}
float BodyRecognizer::calculateDistance(cv::Mat& d1, cv::Mat& d2)
{
Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::Matrix4f> ges;
Eigen::Matrix4f d1Eigen;
Eigen::Matrix4f d2Eigen;
cv::cv2eigen(d1, d1Eigen);
cv::cv2eigen(d2, d2Eigen);
ges.compute(d1Eigen, d2Eigen);
double sum;
for(int i=0; i < ges.eigenvalues().size(); i++)
{
sum += pow(log(ges.eigenvalues()[i]), 2.0);
}
double d = sqrt(sum);
//cout << "The generalzied eigenvalues are : " << ges.eigenvalues().transpose() << endl;
std::cout << "distance is " << d << std::endl;
return d;
}
void pclbo::LBOEstimation<PointT, NormalT>::compute() {
typename pcl::KdTreeFLANN<PointT>::Ptr kdt(new pcl::KdTreeFLANN<PointT>());
kdt->setInputCloud(_cloud);
const double avg_dist = pclbo::avg_distance<PointT>(10, _cloud, kdt);
const double h = 5 * avg_dist;
std::cout << "Average distance between points: " << avg_dist << std::endl;
int points_with_mass = 0;
double avg_mass = 0.0;
B.resize(_cloud->size());
std::cout << "Computing the Mass matrix..." << std::flush;
// Compute the mass matrix diagonal B
for (int i = 0; i < _cloud->size(); i++) {
const auto& point = _cloud->at(i);
const auto& normal = _normals->at(i);
const auto& normal_vector = normal.getNormalVector3fMap().template cast<double>();
if (!pcl::isFinite(point)) continue;
std::vector<int> indices;
std::vector<float> distances;
kdt->radiusSearch(point, h, indices, distances);
if (indices.size() < 4) {
B[i] = 0.0;
continue;
}
// Project the neighbor points in the tangent plane at p_i with normal n_i
std::vector<Eigen::Vector3d> projected_points;
for (const auto& neighbor_index : indices) {
if (neighbor_index != i) {
const auto& neighbor_point = _cloud->at(neighbor_index);
projected_points.push_back(project(point, normal, neighbor_point));
}
}
assert(projected_points.size() >= 3);
// Use the first vector to create a 2D basis
Eigen::Vector3d u = projected_points[0];
u.normalize();
Eigen::Vector3d v = (u.cross(normal_vector));
v.normalize();
// Add the points to a 2D plane
std::vector<Eigen::Vector2d> plane;
// Add the point at the center
plane.push_back(Eigen::Vector2d::Zero());
// Add the rest of the points
for (const auto& projected : projected_points) {
double x = projected.dot(u);
double y = projected.dot(v);
// Add the 2D point to the vector
plane.push_back(Eigen::Vector2d(x, y));
}
assert(plane.size() >= 4);
// Compute the voronoi cell area of the point
double area = VoronoiDiagram::area(plane);
B[i] = area;
avg_mass += area;
points_with_mass++;
}
// Average mass
if (points_with_mass > 0) {
avg_mass /= static_cast<double>(points_with_mass);
}
// Set border points to have average mass
for (auto& b : B) {
if (b == 0.0) {
b = avg_mass;
}
}
std::cout << "done" << std::endl;
std::cout << "Computing the stiffness matrix..." << std::flush;
std::vector<double> diag(_cloud->size(), 0.0);
// Compute the stiffness matrix Q
for (int i = 0; i < _cloud->size(); i++) {
const auto& point = _cloud->at(i);
if (!pcl::isFinite(point)) continue;
std::vector<int> indices;
std::vector<float> distances;
kdt->radiusSearch(point, h, indices, distances);
for (const auto& j : indices) {
if (j != i) {
const auto& neighbor = _cloud->at(j);
double d = (neighbor.getVector3fMap() - point.getVector3fMap()).norm();
double w = B[i] * B[j] * (1.0 / (4.0 * M_PI * h * h)) * exp(-(d * d) / (4.0 * h));
I.push_back(i);
J.push_back(j);
S.push_back(w);
diag[i] += w;
}
}
}
// Fill the diagonal as the negative sum of the rows
for (int i = 0; i < diag.size(); i++) {
I.push_back(i);
J.push_back(i);
S.push_back(-diag[i]);
}
// Compute the B^{-1}Q matrix
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(_cloud->size(), _cloud->size());
for (int i = 0; i < I.size(); i++) {
const int row = I[i];
const int col = J[i];
Q(row, col) = S[i];
}
std::cout << "done" << std::endl;
std::cout << "Computing eigenvectors" << std::endl;
Eigen::Map<Eigen::VectorXd> B_vec(B.data(), B.size());
Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> ges;
ges.compute(Q, B_vec.asDiagonal());
eigenvalues = ges.eigenvalues();
eigenfunctions = ges.eigenvectors();
// Sort the eigenvalues by magnitude
std::vector<std::pair<double, int> > map_vector(eigenvalues.size());
for (auto i = 0; i < eigenvalues.size(); i++) {
map_vector[i].first = std::abs(eigenvalues(i));
map_vector[i].second = i;
}
std::sort(map_vector.begin(), map_vector.end());
// truncate the first 100 eigenfunctions
Eigen::MatrixXd eigenvectors(eigenfunctions.rows(), eigenfunctions.cols());
Eigen::VectorXd eigenvals(eigenfunctions.cols());
eigenvalues.resize(map_vector.size());
for (auto i = 0; i < map_vector.size(); i++) {
const auto& pair = map_vector[i];
eigenvectors.col(i) = eigenfunctions.col(pair.second);
eigenvals(i) = pair.first;
}
eigenfunctions = eigenvectors;
eigenvalues = eigenvals;
}
