Skip to content

Commit 53e3426

Browse files
alalekalalek
committed
Merge pull request #1022 from Sahloul:features/python_wrapper/surface_matching_step
2 parents 4924bb8 + 9cd5768 commit 53e3426

File tree

4 files changed

+36
-40
lines changed

4 files changed

+36
-40
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -148,7 +148,7 @@ class CV_EXPORTS PPF3DDetector
148148
double angle_step, angle_step_radians, distance_step;
149149
double sampling_step_relative, angle_step_relative, distance_step_relative;
150150
Mat sampled_pc, ppf;
151-
int num_ref_points, ppf_step;
151+
int num_ref_points;
152152
hashtable_int* hash_table;
153153
THash* hash_nodes;
154154

‎modules/surface_matching/src/icp.cpp

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ static void subtractColumns(Mat srcPC, double mean[3])
5050

5151
for (int i=0; i<height; i++)
5252
{
53-
float *row = (float*)(&srcPC.data[i*srcPC.step]);
53+
float *row = srcPC.ptr<float>(i);
5454
{
5555
row[0]-=(float)mean[0];
5656
row[1]-=(float)mean[1];
@@ -68,7 +68,7 @@ static void computeMeanCols(Mat srcPC, double mean[3])
6868

6969
for (int i=0; i<height; i++)
7070
{
71-
const float *row = (float*)(&srcPC.data[i*srcPC.step]);
71+
const float *row = srcPC.ptr<float>(i);
7272
{
7373
mean1 += (double)row[0];
7474
mean2 += (double)row[1];
@@ -100,7 +100,7 @@ static double computeDistToOrigin(Mat srcPC)
100100

101101
for (int i=0; i<height; i++)
102102
{
103-
const float *row = (float*)(&srcPC.data[i*srcPC.step]);
103+
const float *row = srcPC.ptr<float>(i);
104104
dist += sqrt(row[0]*row[0]+row[1]*row[1]+row[2]*row[2]);
105105
}
106106

@@ -203,11 +203,11 @@ static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Mat& X)
203203
#endif
204204
for (int i=0; i<Src.rows; i++)
205205
{
206-
const double *srcPt = (double*)&Src.data[i*Src.step];
207-
const double *dstPt = (double*)&Dst.data[i*Dst.step];
206+
const double *srcPt = Src.ptr<double>(i);
207+
const double *dstPt = Dst.ptr<double>(i);
208208
const double *normals = &dstPt[3];
209-
double *bVal = (double*)&b.data[i*b.step];
210-
double *aRow = (double*)&A.data[i*A.step];
209+
double *bVal = b.ptr<double>(i);
210+
double *aRow = A.ptr<double>(i);
211211

212212
const double sub[3]={dstPt[0]-srcPt[0], dstPt[1]-srcPt[1], dstPt[2]-srcPt[2]};
213213

@@ -382,13 +382,13 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
382382

383383
while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && i<MaxIterationsPyr)
384384
{
385-
size_t di=0, selInd = 0;
385+
uint di=0, selInd = 0;
386386

387387
queryPCFlann(flann, Src_Moved, Indices, Distances);
388388

389389
for (di=0; di<numElSrc; di++)
390390
{
391-
newI[di] = (int)di;
391+
newI[di] = di;
392392
newJ[di] = indices[di];
393393
}
394394

@@ -455,17 +455,17 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
455455
if (selInd)
456456
{
457457

458-
Mat Src_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
459-
Mat Dst_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
458+
Mat Src_Match = Mat(selInd, srcPCT.cols, CV_64F);
459+
Mat Dst_Match = Mat(selInd, srcPCT.cols, CV_64F);
460460

461461
for (di=0; di<selInd; di++)
462462
{
463463
const int indModel = indicesModel[di];
464464
const int indScene = indicesScene[di];
465-
const float *srcPt = (float*)&srcPCT.data[indModel*srcPCT.step];
466-
const float *dstPt = (float*)&dstPC0.data[indScene*dstPC0.step];
467-
double *srcMatchPt = (double*)&Src_Match.data[di*Src_Match.step];
468-
double *dstMatchPt = (double*)&Dst_Match.data[di*Dst_Match.step];
465+
const float *srcPt = srcPCT.ptr<float>(indModel);
466+
const float *dstPt = dstPC0.ptr<float>(indScene);
467+
double *srcMatchPt = Src_Match.ptr<double>(di);
468+
double *dstMatchPt = Dst_Match.ptr<double>(di);
469469
int ci=0;
470470

471471
for (ci=0; ci<srcPCT.cols; ci++)

‎modules/surface_matching/src/ppf_helpers.cpp

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,7 @@ Mat loadPLYSimple(const char* fileName, int withNormals)
8686

8787
for (int i = 0; i < numVertices; i++)
8888
{
89-
float* data = (float*)(&cloud.data[i*cloud.step[0]]);
89+
float* data = cloud.ptr<float>(i);
9090
if (withNormals)
9191
{
9292
ifs >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5];
@@ -148,7 +148,7 @@ void writePLY(Mat PC, const char* FileName)
148148

149149
for ( int pi = 0; pi < pointNum; ++pi )
150150
{
151-
const float* point = (float*)(&PC.data[ pi*PC.step ]);
151+
const float* point = PC.ptr<float>(pi);
152152

153153
outFile << point[0] << " "<<point[1]<<" "<<point[2];
154154

@@ -202,7 +202,7 @@ void writePLYVisibleNormals(Mat PC, const char* FileName)
202202

203203
for (int pi = 0; pi < pointNum; ++pi)
204204
{
205-
const float* point = (float*)(&PC.data[pi*PC.step]);
205+
const float* point = PC.ptr<float>(pi);
206206

207207
outFile << point[0] << " " << point[1] << " " << point[2];
208208

@@ -295,7 +295,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
295295
//#pragma omp parallel for
296296
for (int i=0; i<pc.rows; i++)
297297
{
298-
const float* point = (float*)(&pc.data[i * pc.step]);
298+
const float* point = pc.ptr<float>(i);
299299

300300
// quantize a point
301301
const int xCell =(int) ((float)numSamplesDim*(point[0]-xrange[0])/xr);
@@ -342,7 +342,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
342342
for (int j=0; j<cn; j++)
343343
{
344344
const int ptInd = curCell[j];
345-
float* point = (float*)(&pc.data[ptInd * pc.step]);
345+
float* point = pc.ptr<float>(ptInd);
346346
const double dx = point[0]-xc;
347347
const double dy = point[1]-yc;
348348
const double dz = point[2]-zc;
@@ -380,7 +380,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
380380
for (int j=0; j<cn; j++)
381381
{
382382
const int ptInd = curCell[j];
383-
float* point = (float*)(&pc.data[ptInd * pc.step]);
383+
float* point = pc.ptr<float>(ptInd);
384384

385385
px += (double)point[0];
386386
py += (double)point[1];
@@ -399,7 +399,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
399399

400400
}
401401

402-
float *pcData = (float*)(&pcSampled.data[c*pcSampled.step[0]]);
402+
float *pcData = pcSampled.ptr<float>(c);
403403
pcData[0]=(float)px;
404404
pcData[1]=(float)py;
405405
pcData[2]=(float)pz;
@@ -542,8 +542,8 @@ Mat transformPCPose(Mat pc, double Pose[16])
542542
#endif
543543
for (int i=0; i<pc.rows; i++)
544544
{
545-
const float *pcData = (float*)(&pc.data[i*pc.step]);
546-
float *pcDataT = (float*)(&pct.data[i*pct.step]);
545+
const float *pcData = pc.ptr<float>(i);
546+
float *pcDataT = pct.ptr<float>(i);
547547
const float *n1 = &pcData[3];
548548
float *nT = &pcDataT[3];
549549

@@ -738,7 +738,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
738738

739739
for (i=0; i<PC.rows; i++)
740740
{
741-
const float* src = (float*)(&PC.data[i*PC.step]);
741+
const float* src = PC.ptr<float>(i);
742742
float* dst = (float*)(&dataset[i*3]);
743743

744744
dst[0] = src[0];
@@ -763,7 +763,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
763763
{
764764
double C[3][3], mu[4];
765765
const float* pci = &dataset[i*3];
766-
float* pcr = (float*)(&PCNormals.data[i*PCNormals.step]);
766+
float* pcr = PCNormals.ptr<float>(i);
767767
double nr[3];
768768

769769
int* indLocal = &indices[i*NumNeighbors];

‎modules/surface_matching/src/ppf_match_3d.cpp

Lines changed: 9 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@
4141
#include "precomp.hpp"
4242
#include "hash_murmur.hpp"
4343

44-
namespace cv
44+
namespace cv
4545
{
4646
namespace ppf_match_3d
4747
{
@@ -252,8 +252,6 @@ void PPF3DDetector::trainModel(const Mat &PC)
252252

253253
int numPPF = sampled.rows*sampled.rows;
254254
ppf = Mat(numPPF, PPF_LENGTH, CV_32FC1);
255-
int ppfStep = (int)ppf.step;
256-
int sampledStep = (int)sampled.step;
257255

258256
// TODO: Maybe I could sample 1/5th of them here. Check the performance later.
259257
int numRefPoints = sampled.rows;
@@ -268,7 +266,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
268266
// since this is just a training part.
269267
for (int i=0; i<numRefPoints; i++)
270268
{
271-
float* f1 = (float*)(&sampled.data[i * sampledStep]);
269+
float* f1 = sampled.ptr<float>(i);
272270
const double p1[4] = {f1[0], f1[1], f1[2], 0};
273271
const double n1[4] = {f1[3], f1[4], f1[5], 0};
274272

@@ -278,16 +276,15 @@ void PPF3DDetector::trainModel(const Mat &PC)
278276
// cannnot compute the ppf with myself
279277
if (i!=j)
280278
{
281-
float* f2 = (float*)(&sampled.data[j * sampledStep]);
279+
float* f2 = sampled.ptr<float>(j);
282280
const double p2[4] = {f2[0], f2[1], f2[2], 0};
283281
const double n2[4] = {f2[3], f2[4], f2[5], 0};
284282

285283
double f[4]={0};
286284
computePPFFeatures(p1, n1, p2, n2, f);
287285
KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
288286
double alpha = computeAlpha(p1, n1, p2);
289-
unsigned int corrInd = i*numRefPoints+j;
290-
unsigned int ppfInd = corrInd*ppfStep;
287+
unsigned int ppfInd = i*numRefPoints+j;
291288

292289
THash* hashNode = &hash_nodes[i*numRefPoints+j];
293290
hashNode->id = hashValue;
@@ -296,7 +293,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
296293

297294
hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);
298295

299-
float* ppfRow = (float*)(&(ppf.data[ ppfInd ]));
296+
float* ppfRow = ppf.ptr<float>(ppfInd);
300297
ppfRow[0] = (float)f[0];
301298
ppfRow[1] = (float)f[1];
302299
ppfRow[2] = (float)f[2];
@@ -309,7 +306,6 @@ void PPF3DDetector::trainModel(const Mat &PC)
309306
angle_step = angle_step_radians;
310307
distance_step = distanceStep;
311308
hash_table = hashTable;
312-
ppf_step = ppfStep;
313309
num_ref_points = numRefPoints;
314310
sampled_pc = sampled;
315311
trained = true;
@@ -513,7 +509,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
513509
unsigned int refIndMax = 0, alphaIndMax = 0;
514510
unsigned int maxVotes = 0;
515511

516-
float* f1 = (float*)(&sampled.data[i * sampled.step]);
512+
float* f1 = sampled.ptr<float>(i);
517513
const double p1[4] = {f1[0], f1[1], f1[2], 0};
518514
const double n1[4] = {f1[3], f1[4], f1[5], 0};
519515
double *row2, *row3, tsg[3]={0}, Rsg[9]={0}, RInv[9]={0};
@@ -532,7 +528,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
532528
{
533529
if (i!=j)
534530
{
535-
float* f2 = (float*)(&sampled.data[j * sampled.step]);
531+
float* f2 = sampled.ptr<float>(j);
536532
const double p2[4] = {f2[0], f2[1], f2[2], 0};
537533
const double n2[4] = {f2[3], f2[4], f2[5], 0};
538534
double p2t[4], alpha_scene;
@@ -565,7 +561,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
565561
THash* tData = (THash*) node->data;
566562
int corrI = (int)tData->i;
567563
int ppfInd = (int)tData->ppfInd;
568-
float* ppfCorrScene = (float*)(&ppf.data[ppfInd]);
564+
float* ppfCorrScene = ppf.ptr<float>(ppfInd);
569565
double alpha_model = (double)ppfCorrScene[PPF_LENGTH-1];
570566
double alpha = alpha_model - alpha_scene;
571567

@@ -620,7 +616,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
620616
};
621617

622618
// TODO : Compute pose
623-
const float* fMax = (float*)(&sampled_pc.data[refIndMax * sampled_pc.step]);
619+
const float* fMax = sampled_pc.ptr<float>(refIndMax);
624620
const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1};
625621
const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1};
626622

0 commit comments

Comments
 (0)