Skip to content

Commit b0cb299

Browse files
vpisarevvpisarev
committed
Merge pull request #275 from FelixMartel:master
2 parents 526f04e + a544213 commit b0cb299

File tree

4 files changed

+183
-107
lines changed

4 files changed

+183
-107
lines changed

‎modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,14 @@ CV_EXPORTS Mat loadPLYSimple(const char* fileName, int withNormals);
7070
*/
7171
CV_EXPORTS void writePLY(Mat PC, const char* fileName);
7272

73+
/**
74+
* @brief Used for debbuging pruposes, writes a point cloud to a PLY file with the tip
75+
* of the normal vectors as visible red points
76+
* @param [in] PC Input point cloud
77+
* @param [in] fileName The PLY model file to write
78+
*/
79+
CV_EXPORTS void writePLYVisibleNormals(Mat PC, const char* fileName);
80+
7381
Mat samplePCUniform(Mat PC, int sampleStep);
7482
Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int>& indices);
7583

@@ -93,6 +101,7 @@ void computeBboxStd(Mat pc, float xRange[2], float yRange[2], float zRange[2]);
93101
void* indexPCFlann(Mat pc);
94102
void destroyFlann(void* flannIndex);
95103
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances);
104+
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors);
96105

97106
/**
98107
* Mostly for visualization purposes. Normalizes the point cloud in a Hartley-Zissermann

‎modules/surface_matching/samples/ppf_normal_computation.cpp

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
//
2+
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
3+
//
4+
// By downloading, copying, installing or using the software you agree to this license.
5+
// If you do not agree to this license, do not download, install,
6+
// copy or use the software.
7+
//
8+
//
9+
// License Agreement
10+
// For Open Source Computer Vision Library
11+
//
12+
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
13+
// Third party copyrights are property of their respective owners.
14+
//
15+
// Redistribution and use in source and binary forms, with or without modification,
16+
// are permitted provided that the following conditions are met:
17+
//
18+
// * Redistribution's of source code must retain the above copyright notice,
19+
// this list of conditions and the following disclaimer.
20+
//
21+
// * Redistribution's in binary form must reproduce the above copyright notice,
22+
// this list of conditions and the following disclaimer in the documentation
23+
// and/or other materials provided with the distribution.
24+
//
25+
// * The name of the copyright holders may not be used to endorse or promote products
26+
// derived from this software without specific prior written permission.
27+
//
28+
// This software is provided by the copyright holders and contributors "as is" and
29+
// any express or implied warranties, including, but not limited to, the implied
30+
// warranties of merchantability and fitness for a particular purpose are disclaimed.
31+
// In no event shall the Intel Corporation or contributors be liable for any direct,
32+
// indirect, incidental, special, exemplary, or consequential damages
33+
// (including, but not limited to, procurement of substitute goods or services;
34+
// loss of use, data, or profits; or business interruption) however caused
35+
// and on any theory of liability, whether in contract, strict liability,
36+
// or tort (including negligence or otherwise) arising in any way out of
37+
// the use of this software, even if advised of the possibility of such damage.
38+
39+
#include <iostream>
40+
#include "opencv2/surface_matching.hpp"
41+
#include "opencv2/surface_matching/ppf_helpers.hpp"
42+
43+
using namespace std;
44+
45+
static void help(const string& errorMessage)
46+
{
47+
cout << "Program init error : " << errorMessage << endl;
48+
cout << "\nUsage : ppf_normal_computation [input model file] [output model file]" << endl;
49+
cout << "\nPlease start again with new parameters" << endl;
50+
}
51+
52+
int main(int argc, char** argv)
53+
{
54+
if (argc < 3)
55+
{
56+
help("Not enough input arguments");
57+
exit(1);
58+
}
59+
60+
string modelFileName = (string)argv[1];
61+
string outputFileName = (string)argv[2];
62+
cv::Mat points, pointsAndNormals;
63+
64+
cout << "Loading points\n";
65+
cv::ppf_match_3d::loadPLYSimple(modelFileName.c_str(), 1).copyTo(points);
66+
67+
cout << "Computing normals\n";
68+
double viewpoint[3] = { 0.0, 0.0, 0.0 };
69+
cv::ppf_match_3d::computeNormalsPC3d(points, pointsAndNormals, 6, false, viewpoint);
70+
71+
std::cout << "Writing points\n";
72+
cv::ppf_match_3d::writePLY(pointsAndNormals, outputFileName.c_str());
73+
//the following function can also be used for debugging purposes
74+
//cv::ppf_match_3d::writePLYVisibleNormals(pointsAndNormals, outputFileName.c_str());
75+
76+
std::cout << "Done\n";
77+
return 0;
78+
}

‎modules/surface_matching/src/c_utils.hpp

Lines changed: 0 additions & 94 deletions
Original file line numberDiff line numberDiff line change
@@ -659,100 +659,6 @@ static inline void quatToDCM(double *q, double *R)
659659
R[7] = 2.0 * (tmp1 - tmp2);
660660
}
661661

662-
/**
663-
* @brief Analytical solution to find the eigenvector corresponding to the smallest
664-
* eigenvalue of a 3x3 matrix. As this implements the analytical solution, it's not
665-
* really the most robust way. Whenever possible, this implementation can be replaced
666-
* via a robust numerical scheme.
667-
* @param [in] C The matrix
668-
* @param [in] A The eigenvector corresponding to the lowest eigenvalue
669-
* @author Tolga Birdal
670-
*/
671-
static inline void eigenLowest33(const double C[3][3], double A[3])
672-
{
673-
const double a = C[0][0];
674-
const double b = C[0][1];
675-
const double c = C[0][2];
676-
const double d = C[1][1];
677-
const double e = C[1][2];
678-
const double f = C[2][2];
679-
const double t2 = c*c;
680-
const double t3 = e*e;
681-
const double t4 = b*t2;
682-
const double t5 = c*d*e;
683-
const double t34 = b*t3;
684-
const double t35 = a*c*e;
685-
const double t6 = t4+t5-t34-t35;
686-
const double t7 = 1.0/t6;
687-
const double t8 = a+d+f;
688-
const double t9 = b*b;
689-
const double t23 = a*d;
690-
const double t24 = a*f;
691-
const double t25 = d*f;
692-
const double t10 = t2+t3+t9-t23-t24-t25;
693-
const double t11 = t8*t10*(1.0/6.0);
694-
const double t12 = t8*t8;
695-
const double t20 = t8*t12*(1.0/2.7E1);
696-
const double t21 = b*c*e;
697-
const double t22 = a*d*f*(1.0/2.0);
698-
const double t26 = a*t3*(1.0/2.0);
699-
const double t27 = d*t2*(1.0/2.0);
700-
const double t28 = f*t9*(1.0/2.0);
701-
const double t14 = t9*(1.0/3.0);
702-
const double t15 = t2*(1.0/3.0);
703-
const double t16 = t3*(1.0/3.0);
704-
const double t17 = t12*(1.0/9.0);
705-
const double t30 = a*d*(1.0/3.0);
706-
const double t31 = a*f*(1.0/3.0);
707-
const double t32 = d*f*(1.0/3.0);
708-
const double t18 = t14+t15+t16+t17-t30-t31-t32;
709-
const double t19 = t18*t18;
710-
const double t36 = a*(1.0/3.0);
711-
const double t37 = d*(1.0/3.0);
712-
const double t38 = f*(1.0/3.0);
713-
const double t39 = t8*(t2+t3+t9-t23-t24-t25)*(1.0/6.0);
714-
const double t41 = t18*t19;
715-
const double t43 = e*t2;
716-
const double t60 = b*c*f;
717-
const double t61 = d*e*f;
718-
const double t44 = t43-t60-t61+e*t3;
719-
const double t45 = t7*t44;
720-
const double t57 = sqrt(3.0);
721-
const double t51 = b*c;
722-
const double t52 = d*e;
723-
const double t53 = e*f;
724-
const double t54 = t51+t52+t53;
725-
const double t62 = t11+t20+t21+t22-t26-t27-t28;
726-
const double t63 = t11+t20+t21+t22-t26-t27-t28;
727-
const double t64 = t11+t20+t21+t22-t26-t27-t28;
728-
const double t65 = t11+t20+t21+t22-t26-t27-t28;
729-
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);
730-
const double t67 = t11+t20+t21+t22-t26-t27-t28;
731-
const double t68 = t11+t20+t21+t22-t26-t27-t28;
732-
const double t69 = t11+t20+t21+t22-t26-t27-t28;
733-
const double t70 = t11+t20+t21+t22-t26-t27-t28;
734-
const double t76 = c*t3;
735-
const double t91 = a*c*f;
736-
const double t92 = b*e*f;
737-
const double t77 = t76-t91-t92+c*t2;
738-
const double t83 = a*c;
739-
const double t84 = b*e;
740-
const double t85 = c*f;
741-
const double t86 = t83+t84+t85;
742-
const double t93 = t11+t20+t21+t22-t26-t27-t28;
743-
const double t94 = t11+t20+t21+t22-t26-t27-t28;
744-
const double t95 = t11+t20+t21+t22-t26-t27-t28;
745-
const double t96 = t11+t20+t21+t22-t26-t27-t28;
746-
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);
747-
const double t98 = t11+t20+t21+t22-t26-t27-t28;
748-
const double t99 = t11+t20+t21+t22-t26-t27-t28;
749-
const double t100 = t11+t20+t21+t22-t26-t27-t28;
750-
const double t101 = t11+t20+t21+t22-t26-t27-t28;
751-
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));
752-
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));
753-
A[2] = 1.0;
754-
}
755-
756662
} // namespace ppf_match_3d
757663

758664
} // namespace cv

‎modules/surface_matching/src/ppf_helpers.cpp

Lines changed: 96 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,62 @@ void writePLY(Mat PC, const char* FileName)
163163
return;
164164
}
165165

166+
void writePLYVisibleNormals(Mat PC, const char* FileName)
167+
{
168+
std::ofstream outFile(FileName);
169+
170+
if (!outFile)
171+
{
172+
//cerr << "Error opening output file: " << FileName << "!" << endl;
173+
printf("Error opening output file: %s!\n", FileName);
174+
exit(1);
175+
}
176+
177+
////
178+
// Header
179+
////
180+
181+
const int pointNum = (int)PC.rows;
182+
const int vertNum = (int)PC.cols;
183+
const bool hasNormals = vertNum == 6;
184+
185+
outFile << "ply" << std::endl;
186+
outFile << "format ascii 1.0" << std::endl;
187+
outFile << "element vertex " << (hasNormals? 2*pointNum:pointNum) << std::endl;
188+
outFile << "property float x" << std::endl;
189+
outFile << "property float y" << std::endl;
190+
outFile << "property float z" << std::endl;
191+
if (hasNormals)
192+
{
193+
outFile << "property uchar red" << std::endl;
194+
outFile << "property uchar green" << std::endl;
195+
outFile << "property uchar blue" << std::endl;
196+
}
197+
outFile << "end_header" << std::endl;
198+
199+
////
200+
// Points
201+
////
202+
203+
for (int pi = 0; pi < pointNum; ++pi)
204+
{
205+
const float* point = (float*)(&PC.data[pi*PC.step]);
206+
207+
outFile << point[0] << " " << point[1] << " " << point[2];
208+
209+
if (hasNormals)
210+
{
211+
outFile << " 127 127 127" << std::endl;
212+
outFile << point[0]+point[3] << " " << point[1]+point[4] << " " << point[2]+point[5];
213+
outFile << " 255 0 0";
214+
}
215+
216+
outFile << std::endl;
217+
}
218+
219+
return;
220+
}
221+
166222
Mat samplePCUniform(Mat PC, int sampleStep)
167223
{
168224
int numRows = PC.rows/sampleStep;
@@ -207,10 +263,15 @@ void destroyFlann(void* flannIndex)
207263

208264
// For speed purposes this function assumes that PC, Indices and Distances are created with continuous structures
209265
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances)
266+
{
267+
queryPCFlann(flannIndex, pc, indices, distances, 1);
268+
}
269+
270+
void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors)
210271
{
211272
Mat obj_32f;
212-
pc.colRange(0,3).copyTo(obj_32f);
213-
((FlannIndex*)flannIndex)->knnSearch(obj_32f, indices, distances, 1, cvflann::SearchParams(32) );
273+
pc.colRange(0, 3).copyTo(obj_32f);
274+
((FlannIndex*)flannIndex)->knnSearch(obj_32f, indices, distances, numNeighbors, cvflann::SearchParams(32));
214275
}
215276

216277
// uses a volume instead of an octree
@@ -499,17 +560,21 @@ Mat transformPCPose(Mat pc, double Pose[16])
499560
pcDataT[2] = (float)(p2[2]/p2[3]);
500561
}
501562

502-
// Rotate the normals, too
503-
double n[3] = {(double)n1[0], (double)n1[1], (double)n1[2]}, n2[3];
563+
// If the point cloud has normals,
564+
// then rotate them as well
565+
if (pc.cols == 6)
566+
{
567+
double n[3] = { (double)n1[0], (double)n1[1], (double)n1[2] }, n2[3];
504568

505-
matrixProduct331(R, n, n2);
506-
double nNorm = sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]);
569+
matrixProduct331(R, n, n2);
570+
double nNorm = sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]);
507571

508-
if (nNorm>EPS)
509-
{
510-
nT[0]=(float)(n2[0]/nNorm);
511-
nT[1]=(float)(n2[1]/nNorm);
512-
nT[2]=(float)(n2[2]/nNorm);
572+
if (nNorm>EPS)
573+
{
574+
nT[0]=(float)(n2[0]/nNorm);
575+
nT[1]=(float)(n2[1]/nNorm);
576+
nT[2]=(float)(n2[2]/nNorm);
577+
}
513578
}
514579
}
515580

@@ -688,7 +753,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
688753
Mat Indices(2, sizesResult, CV_32S, indices, 0);
689754
Mat Distances(2, sizesResult, CV_32F, distances, 0);
690755

691-
queryPCFlann(flannIndex, PCInput, Indices, Distances);
756+
queryPCFlann(flannIndex, PCInput, Indices, Distances, NumNeighbors);
692757
destroyFlann(flannIndex);
693758
flannIndex = 0;
694759

@@ -707,7 +772,25 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
707772
meanCovLocalPCInd(dataset, indLocal, 3, NumNeighbors, C, mu);
708773

709774
// eigenvectors of covariance matrix
710-
eigenLowest33(C, nr);
775+
Mat cov(3, 3, CV_64F), eigVect, eigVal;
776+
double* covData = (double*)cov.data;
777+
covData[0] = C[0][0];
778+
covData[1] = C[0][1];
779+
covData[2] = C[0][2];
780+
covData[3] = C[1][0];
781+
covData[4] = C[1][1];
782+
covData[5] = C[1][2];
783+
covData[6] = C[2][0];
784+
covData[7] = C[2][1];
785+
covData[8] = C[2][2];
786+
eigen(cov, eigVal, eigVect);
787+
Mat lowestEigVec;
788+
//the eigenvector for the lowest eigenvalue is in the last row
789+
eigVect.row(eigVect.rows - 1).copyTo(lowestEigVec);
790+
double* eigData = (double*)lowestEigVec.data;
791+
nr[0] = eigData[0];
792+
nr[1] = eigData[1];
793+
nr[2] = eigData[2];
711794

712795
pcr[0] = pci[0];
713796
pcr[1] = pci[1];

0 commit comments

Comments
 (0)