Skip to content

O
opencv_contrib
Commit b0cb2991 authored by Vadim Pisarevsky's avatar Vadim Pisarevsky
Browse files
Options

Merge pull request #275 from FelixMartel:master

parents 526f04e6 a544213d
Hide whitespace changes
Inline Side-by-side
Showing with 183 additions and 107 deletions
modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp
View file @ b0cb2991
......@@ -70,6 +70,14 @@ CV_EXPORTS Mat loadPLYSimple(const char* fileName, int withNormals);
*/
CV_EXPORTS void writePLY(Mat PC, const char* fileName);
/**
* @brief Used for debbuging pruposes, writes a point cloud to a PLY file with the tip
* of the normal vectors as visible red points
* @param [in] PC Input point cloud
* @param [in] fileName The PLY model file to write
*/
CV_EXPORTS void writePLYVisibleNormals(Mat PC, const char* fileName);
Mat samplePCUniform(Mat PC, int sampleStep);
Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int>& indices);
......@@ -93,6 +101,7 @@ void computeBboxStd(Mat pc, float xRange[2], float yRange[2], float zRange[2]);
void* indexPCFlann(Mat pc);
void destroyFlann(void* flannIndex);
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances);
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors);
/**
* Mostly for visualization purposes. Normalizes the point cloud in a Hartley-Zissermann
......
modules/surface_matching/samples/ppf_normal_computation.cpp 0 → 100644
View file @ b0cb2991
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
#include <iostream>
#include "opencv2/surface_matching.hpp"
#include "opencv2/surface_matching/ppf_helpers.hpp"
using namespace std;
static void help(const string& errorMessage)
{
cout << "Program init error : " << errorMessage << endl;
cout << "\nUsage : ppf_normal_computation [input model file] [output model file]" << endl;
cout << "\nPlease start again with new parameters" << endl;
}
int main(int argc, char** argv)
{
if (argc < 3)
{
help("Not enough input arguments");
exit(1);
}
string modelFileName = (string)argv[1];
string outputFileName = (string)argv[2];
cv::Mat points, pointsAndNormals;
cout << "Loading points\n";
cv::ppf_match_3d::loadPLYSimple(modelFileName.c_str(), 1).copyTo(points);
cout << "Computing normals\n";
double viewpoint[3] = { 0.0, 0.0, 0.0 };
cv::ppf_match_3d::computeNormalsPC3d(points, pointsAndNormals, 6, false, viewpoint);
std::cout << "Writing points\n";
cv::ppf_match_3d::writePLY(pointsAndNormals, outputFileName.c_str());
//the following function can also be used for debugging purposes
//cv::ppf_match_3d::writePLYVisibleNormals(pointsAndNormals, outputFileName.c_str());
std::cout << "Done\n";
return 0;
}
modules/surface_matching/src/c_utils.hpp
View file @ b0cb2991
......@@ -659,100 +659,6 @@ static inline void quatToDCM(double *q, double *R)
R[7] = 2.0 * (tmp1 - tmp2);
}
/**
* @brief Analytical solution to find the eigenvector corresponding to the smallest
* eigenvalue of a 3x3 matrix. As this implements the analytical solution, it's not
* really the most robust way. Whenever possible, this implementation can be replaced
* via a robust numerical scheme.
* @param [in] C The matrix
* @param [in] A The eigenvector corresponding to the lowest eigenvalue
* @author Tolga Birdal
*/
static inline void eigenLowest33(const double C[3][3], double A[3])
{
const double a = C[0][0];
const double b = C[0][1];
const double c = C[0][2];
const double d = C[1][1];
const double e = C[1][2];
const double f = C[2][2];
const double t2 = c*c;
const double t3 = e*e;
const double t4 = b*t2;
const double t5 = c*d*e;
const double t34 = b*t3;
const double t35 = a*c*e;
const double t6 = t4+t5-t34-t35;
const double t7 = 1.0/t6;
const double t8 = a+d+f;
const double t9 = b*b;
const double t23 = a*d;
const double t24 = a*f;
const double t25 = d*f;
const double t10 = t2+t3+t9-t23-t24-t25;
const double t11 = t8*t10*(1.0/6.0);
const double t12 = t8*t8;
const double t20 = t8*t12*(1.0/2.7E1);
const double t21 = b*c*e;
const double t22 = a*d*f*(1.0/2.0);
const double t26 = a*t3*(1.0/2.0);
const double t27 = d*t2*(1.0/2.0);
const double t28 = f*t9*(1.0/2.0);
const double t14 = t9*(1.0/3.0);
const double t15 = t2*(1.0/3.0);
const double t16 = t3*(1.0/3.0);
const double t17 = t12*(1.0/9.0);
const double t30 = a*d*(1.0/3.0);
const double t31 = a*f*(1.0/3.0);
const double t32 = d*f*(1.0/3.0);
const double t18 = t14+t15+t16+t17-t30-t31-t32;
const double t19 = t18*t18;
const double t36 = a*(1.0/3.0);
const double t37 = d*(1.0/3.0);
const double t38 = f*(1.0/3.0);
const double t39 = t8*(t2+t3+t9-t23-t24-t25)*(1.0/6.0);
const double t41 = t18*t19;
const double t43 = e*t2;
const double t60 = b*c*f;
const double t61 = d*e*f;
const double t44 = t43-t60-t61+e*t3;
const double t45 = t7*t44;
const double t57 = sqrt(3.0);
const double t51 = b*c;
const double t52 = d*e;
const double t53 = e*f;
const double t54 = t51+t52+t53;
const double t62 = t11+t20+t21+t22-t26-t27-t28;
const double t63 = t11+t20+t21+t22-t26-t27-t28;
const double t64 = t11+t20+t21+t22-t26-t27-t28;
const double t65 = t11+t20+t21+t22-t26-t27-t28;
const double t66 = t36+t37+t38-t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t62*t62),1.0/3.0)*(1.0/2.0)+t57*(t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t64*t64),1.0/3.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t65*t65),1.0/3.0))*5.0E-1*sqrt(-1.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t63*t63),1.0/3.0)*(1.0/2.0);
const double t67 = t11+t20+t21+t22-t26-t27-t28;
const double t68 = t11+t20+t21+t22-t26-t27-t28;
const double t69 = t11+t20+t21+t22-t26-t27-t28;
const double t70 = t11+t20+t21+t22-t26-t27-t28;
const double t76 = c*t3;
const double t91 = a*c*f;
const double t92 = b*e*f;
const double t77 = t76-t91-t92+c*t2;
const double t83 = a*c;
const double t84 = b*e;
const double t85 = c*f;
const double t86 = t83+t84+t85;
const double t93 = t11+t20+t21+t22-t26-t27-t28;
const double t94 = t11+t20+t21+t22-t26-t27-t28;
const double t95 = t11+t20+t21+t22-t26-t27-t28;
const double t96 = t11+t20+t21+t22-t26-t27-t28;
const double t97 = t36+t37+t38-t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t93*t93),1.0/3.0)*(1.0/2.0)+t57*(t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t95*t95),1.0/3.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t96*t96),1.0/3.0))*5.0E-1*sqrt(-1.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t94*t94),1.0/3.0)*(1.0/2.0);
const double t98 = t11+t20+t21+t22-t26-t27-t28;
const double t99 = t11+t20+t21+t22-t26-t27-t28;
const double t100 = t11+t20+t21+t22-t26-t27-t28;
const double t101 = t11+t20+t21+t22-t26-t27-t28;
A[0] = t45-e*t7*(t66*t66)+t7*t54*(t36+t37+t38-t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t67*t67),1.0/3.0)*(1.0/2.0)+t57*(t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t69*t69),1.0/3.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t70*t70),1.0/3.0))*5.0E-1*sqrt(-1.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t68*t68),1.0/3.0)*(1.0/2.0));
A[1] = -t7*t77+c*t7*(t97*t97)-t7*t86*(t36+t37+t38-t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t98*t98),1.0/3.0)*(1.0/2.0)+t57*(t18*1.0/pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t100*t100),1.0/3.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t101*t101),1.0/3.0))*5.0E-1*sqrt(-1.0)-pow(t20+t21+t22-t26-t27-t28+t39+sqrt(-t41+t99*t99),1.0/3.0)*(1.0/2.0));
A[2] = 1.0;
}
} // namespace ppf_match_3d
} // namespace cv
......
modules/surface_matching/src/ppf_helpers.cpp
View file @ b0cb2991
......@@ -163,6 +163,62 @@ void writePLY(Mat PC, const char* FileName)
return;
}
void writePLYVisibleNormals(Mat PC, const char* FileName)
{
std::ofstream outFile(FileName);
if (!outFile)
{
//cerr << "Error opening output file: " << FileName << "!" << endl;
printf("Error opening output file: %s!\n", FileName);
exit(1);
}
////
// Header
////
const int pointNum = (int)PC.rows;
const int vertNum = (int)PC.cols;
const bool hasNormals = vertNum == 6;
outFile << "ply" << std::endl;
outFile << "format ascii 1.0" << std::endl;
outFile << "element vertex " << (hasNormals? 2*pointNum:pointNum) << std::endl;
outFile << "property float x" << std::endl;
outFile << "property float y" << std::endl;
outFile << "property float z" << std::endl;
if (hasNormals)
{
outFile << "property uchar red" << std::endl;
outFile << "property uchar green" << std::endl;
outFile << "property uchar blue" << std::endl;
}
outFile << "end_header" << std::endl;
////
// Points
////
for (int pi = 0; pi < pointNum; ++pi)
{
const float* point = (float*)(&PC.data[pi*PC.step]);
outFile << point[0] << " " << point[1] << " " << point[2];
if (hasNormals)
{
outFile << " 127 127 127" << std::endl;
outFile << point[0]+point[3] << " " << point[1]+point[4] << " " << point[2]+point[5];
outFile << " 255 0 0";
}
outFile << std::endl;
}
return;
}
Mat samplePCUniform(Mat PC, int sampleStep)
{
int numRows = PC.rows/sampleStep;
......@@ -207,10 +263,15 @@ void destroyFlann(void* flannIndex)
// For speed purposes this function assumes that PC, Indices and Distances are created with continuous structures
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances)
{
queryPCFlann(flannIndex, pc, indices, distances, 1);
}
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors)
{
Mat obj_32f;
pc.colRange(0,3).copyTo(obj_32f);
((FlannIndex*)flannIndex)->knnSearch(obj_32f, indices, distances, 1, cvflann::SearchParams(32) );
pc.colRange(0, 3).copyTo(obj_32f);
((FlannIndex*)flannIndex)->knnSearch(obj_32f, indices, distances, numNeighbors, cvflann::SearchParams(32));
}
// uses a volume instead of an octree
......@@ -499,17 +560,21 @@ Mat transformPCPose(Mat pc, double Pose[16])
pcDataT[2] = (float)(p2[2]/p2[3]);
}
// Rotate the normals, too
double n[3] = {(double)n1[0], (double)n1[1], (double)n1[2]}, n2[3];
// If the point cloud has normals,
// then rotate them as well
if (pc.cols == 6)
{
double n[3] = { (double)n1[0], (double)n1[1], (double)n1[2] }, n2[3];
matrixProduct331(R, n, n2);
double nNorm = sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]);
matrixProduct331(R, n, n2);
double nNorm = sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]);
if (nNorm>EPS)
{
nT[0]=(float)(n2[0]/nNorm);
nT[1]=(float)(n2[1]/nNorm);
nT[2]=(float)(n2[2]/nNorm);
if (nNorm>EPS)
{
nT[0]=(float)(n2[0]/nNorm);
nT[1]=(float)(n2[1]/nNorm);
nT[2]=(float)(n2[2]/nNorm);
}
}
}
......@@ -688,7 +753,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
Mat Indices(2, sizesResult, CV_32S, indices, 0);
Mat Distances(2, sizesResult, CV_32F, distances, 0);
queryPCFlann(flannIndex, PCInput, Indices, Distances);
queryPCFlann(flannIndex, PCInput, Indices, Distances, NumNeighbors);
destroyFlann(flannIndex);
flannIndex = 0;
......@@ -707,7 +772,25 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
meanCovLocalPCInd(dataset, indLocal, 3, NumNeighbors, C, mu);
// eigenvectors of covariance matrix
eigenLowest33(C, nr);
Mat cov(3, 3, CV_64F), eigVect, eigVal;
double* covData = (double*)cov.data;
covData[0] = C[0][0];
covData[1] = C[0][1];
covData[2] = C[0][2];
covData[3] = C[1][0];
covData[4] = C[1][1];
covData[5] = C[1][2];
covData[6] = C[2][0];
covData[7] = C[2][1];
covData[8] = C[2][2];
eigen(cov, eigVal, eigVect);
Mat lowestEigVec;
//the eigenvector for the lowest eigenvalue is in the last row
eigVect.row(eigVect.rows - 1).copyTo(lowestEigVec);
double* eigData = (double*)lowestEigVec.data;
nr[0] = eigData[0];
nr[1] = eigData[1];
nr[2] = eigData[2];
pcr[0] = pci[0];
pcr[1] = pci[1];
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment