Chiebot-Mirror
/
opencv_contrib
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request #1022 from Sahloul:features/python_wrapper/surface_matching_step

Browse Source
pull/1026/head
Alexander Alekhin 8 years ago
parent 4924bb8d83 9cd576813f
commit 53e34260b1
4 changed files with 36 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp
  2. 30
      modules/surface_matching/src/icp.cpp
  3. 22
      modules/surface_matching/src/ppf_helpers.cpp
  4. 22
      modules/surface_matching/src/ppf_match_3d.cpp

2
modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp
Unescape View File

@ -148,7 +148,7 @@ protected:
double angle_step, angle_step_radians, distance_step;
double sampling_step_relative, angle_step_relative, distance_step_relative;
Mat sampled_pc, ppf;
int num_ref_points, ppf_step;
int num_ref_points;
hashtable_int* hash_table;
THash* hash_nodes;

30
modules/surface_matching/src/icp.cpp
Unescape View File

@ -50,7 +50,7 @@ static void subtractColumns(Mat srcPC, double mean[3])
for (int i=0; i<height; i++)
{
float *row = (float*)(&srcPC.data[i*srcPC.step]);
float *row = srcPC.ptr<float>(i);
{
row[0]-=(float)mean[0];
row[1]-=(float)mean[1];
@ -68,7 +68,7 @@ static void computeMeanCols(Mat srcPC, double mean[3])
for (int i=0; i<height; i++)
{
const float *row = (float*)(&srcPC.data[i*srcPC.step]);
const float *row = srcPC.ptr<float>(i);
{
mean1 += (double)row[0];
mean2 += (double)row[1];
@ -100,7 +100,7 @@ static double computeDistToOrigin(Mat srcPC)
for (int i=0; i<height; i++)
{
const float *row = (float*)(&srcPC.data[i*srcPC.step]);
const float *row = srcPC.ptr<float>(i);
dist += sqrt(row[0]*row[0]+row[1]*row[1]+row[2]*row[2]);
}
@ -203,11 +203,11 @@ static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Mat& X)
#endif
for (int i=0; i<Src.rows; i++)
{
const double *srcPt = (double*)&Src.data[i*Src.step];
const double *dstPt = (double*)&Dst.data[i*Dst.step];
const double *srcPt = Src.ptr<double>(i);
const double *dstPt = Dst.ptr<double>(i);
const double *normals = &dstPt[3];
double *bVal = (double*)&b.data[i*b.step];
double *aRow = (double*)&A.data[i*A.step];
double *bVal = b.ptr<double>(i);
double *aRow = A.ptr<double>(i);
const double sub[3]={dstPt[0]-srcPt[0], dstPt[1]-srcPt[1], dstPt[2]-srcPt[2]};
@ -382,13 +382,13 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && i<MaxIterationsPyr)
{
size_t di=0, selInd = 0;
uint di=0, selInd = 0;
queryPCFlann(flann, Src_Moved, Indices, Distances);
for (di=0; di<numElSrc; di++)
{
newI[di] = (int)di;
newI[di] = di;
newJ[di] = indices[di];
}
@ -455,17 +455,17 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
if (selInd)
{
Mat Src_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
Mat Dst_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
Mat Src_Match = Mat(selInd, srcPCT.cols, CV_64F);
Mat Dst_Match = Mat(selInd, srcPCT.cols, CV_64F);
for (di=0; di<selInd; di++)
{
const int indModel = indicesModel[di];
const int indScene = indicesScene[di];
const float *srcPt = (float*)&srcPCT.data[indModel*srcPCT.step];
const float *dstPt = (float*)&dstPC0.data[indScene*dstPC0.step];
double *srcMatchPt = (double*)&Src_Match.data[di*Src_Match.step];
double *dstMatchPt = (double*)&Dst_Match.data[di*Dst_Match.step];
const float *srcPt = srcPCT.ptr<float>(indModel);
const float *dstPt = dstPC0.ptr<float>(indScene);
double *srcMatchPt = Src_Match.ptr<double>(di);
double *dstMatchPt = Dst_Match.ptr<double>(di);
int ci=0;
for (ci=0; ci<srcPCT.cols; ci++)

22
modules/surface_matching/src/ppf_helpers.cpp
Unescape View File

@ -86,7 +86,7 @@ Mat loadPLYSimple(const char* fileName, int withNormals)
for (int i = 0; i < numVertices; i++)
{
float* data = (float*)(&cloud.data[i*cloud.step[0]]);
float* data = cloud.ptr<float>(i);
if (withNormals)
{
ifs >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5];
@ -148,7 +148,7 @@ void writePLY(Mat PC, const char* FileName)
for ( int pi = 0; pi < pointNum; ++pi )
{
const float* point = (float*)(&PC.data[ pi*PC.step ]);
const float* point = PC.ptr<float>(pi);
outFile << point[0] << " "<<point[1]<<" "<<point[2];
@ -202,7 +202,7 @@ void writePLYVisibleNormals(Mat PC, const char* FileName)
for (int pi = 0; pi < pointNum; ++pi)
{
const float* point = (float*)(&PC.data[pi*PC.step]);
const float* point = PC.ptr<float>(pi);
outFile << point[0] << " " << point[1] << " " << point[2];
@ -295,7 +295,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
//#pragma omp parallel for
for (int i=0; i<pc.rows; i++)
{
const float* point = (float*)(&pc.data[i * pc.step]);
const float* point = pc.ptr<float>(i);
// quantize a point
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
for (int j=0; j<cn; j++)
{
const int ptInd = curCell[j];
float* point = (float*)(&pc.data[ptInd * pc.step]);
float* point = pc.ptr<float>(ptInd);
const double dx = point[0]-xc;
const double dy = point[1]-yc;
const double dz = point[2]-zc;
@ -380,7 +380,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
for (int j=0; j<cn; j++)
{
const int ptInd = curCell[j];
float* point = (float*)(&pc.data[ptInd * pc.step]);
float* point = pc.ptr<float>(ptInd);
px += (double)point[0];
py += (double)point[1];
@ -399,7 +399,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
}
float *pcData = (float*)(&pcSampled.data[c*pcSampled.step[0]]);
float *pcData = pcSampled.ptr<float>(c);
pcData[0]=(float)px;
pcData[1]=(float)py;
pcData[2]=(float)pz;
@ -542,8 +542,8 @@ Mat transformPCPose(Mat pc, double Pose[16])
#endif
for (int i=0; i<pc.rows; i++)
{
const float *pcData = (float*)(&pc.data[i*pc.step]);
float *pcDataT = (float*)(&pct.data[i*pct.step]);
const float *pcData = pc.ptr<float>(i);
float *pcDataT = pct.ptr<float>(i);
const float *n1 = &pcData[3];
float *nT = &pcDataT[3];
@ -738,7 +738,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
for (i=0; i<PC.rows; i++)
{
const float* src = (float*)(&PC.data[i*PC.step]);
const float* src = PC.ptr<float>(i);
float* dst = (float*)(&dataset[i*3]);
dst[0] = src[0];
@ -763,7 +763,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
{
double C[3][3], mu[4];
const float* pci = &dataset[i*3];
float* pcr = (float*)(&PCNormals.data[i*PCNormals.step]);
float* pcr = PCNormals.ptr<float>(i);
double nr[3];
int* indLocal = &indices[i*NumNeighbors];

22
modules/surface_matching/src/ppf_match_3d.cpp
Unescape View File

@ -41,7 +41,7 @@
#include "precomp.hpp"
#include "hash_murmur.hpp"
namespace cv
namespace cv
{
namespace ppf_match_3d
{
@ -252,8 +252,6 @@ void PPF3DDetector::trainModel(const Mat &PC)
int numPPF = sampled.rows*sampled.rows;
ppf = Mat(numPPF, PPF_LENGTH, CV_32FC1);
int ppfStep = (int)ppf.step;
int sampledStep = (int)sampled.step;
// TODO: Maybe I could sample 1/5th of them here. Check the performance later.
int numRefPoints = sampled.rows;
@ -268,7 +266,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
// since this is just a training part.
for (int i=0; i<numRefPoints; i++)
{
float* f1 = (float*)(&sampled.data[i * sampledStep]);
float* f1 = sampled.ptr<float>(i);
const double p1[4] = {f1[0], f1[1], f1[2], 0};
const double n1[4] = {f1[3], f1[4], f1[5], 0};
@ -278,7 +276,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
// cannnot compute the ppf with myself
if (i!=j)
{
float* f2 = (float*)(&sampled.data[j * sampledStep]);
float* f2 = sampled.ptr<float>(j);
const double p2[4] = {f2[0], f2[1], f2[2], 0};
const double n2[4] = {f2[3], f2[4], f2[5], 0};
@ -286,8 +284,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
computePPFFeatures(p1, n1, p2, n2, f);
KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
double alpha = computeAlpha(p1, n1, p2);
unsigned int corrInd = i*numRefPoints+j;
unsigned int ppfInd = corrInd*ppfStep;
unsigned int ppfInd = i*numRefPoints+j;
THash* hashNode = &hash_nodes[i*numRefPoints+j];
hashNode->id = hashValue;
@ -296,7 +293,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);
float* ppfRow = (float*)(&(ppf.data[ ppfInd ]));
float* ppfRow = ppf.ptr<float>(ppfInd);
ppfRow[0] = (float)f[0];
ppfRow[1] = (float)f[1];
ppfRow[2] = (float)f[2];
@ -309,7 +306,6 @@ void PPF3DDetector::trainModel(const Mat &PC)
angle_step = angle_step_radians;
distance_step = distanceStep;
hash_table = hashTable;
ppf_step = ppfStep;
num_ref_points = numRefPoints;
sampled_pc = sampled;
trained = true;
@ -513,7 +509,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
unsigned int refIndMax = 0, alphaIndMax = 0;
unsigned int maxVotes = 0;
float* f1 = (float*)(&sampled.data[i * sampled.step]);
float* f1 = sampled.ptr<float>(i);
const double p1[4] = {f1[0], f1[1], f1[2], 0};
const double n1[4] = {f1[3], f1[4], f1[5], 0};
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
{
if (i!=j)
{
float* f2 = (float*)(&sampled.data[j * sampled.step]);
float* f2 = sampled.ptr<float>(j);
const double p2[4] = {f2[0], f2[1], f2[2], 0};
const double n2[4] = {f2[3], f2[4], f2[5], 0};
double p2t[4], alpha_scene;
@ -565,7 +561,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
THash* tData = (THash*) node->data;
int corrI = (int)tData->i;
int ppfInd = (int)tData->ppfInd;
float* ppfCorrScene = (float*)(&ppf.data[ppfInd]);
float* ppfCorrScene = ppf.ptr<float>(ppfInd);
double alpha_model = (double)ppfCorrScene[PPF_LENGTH-1];
double alpha = alpha_model - alpha_scene;
@ -620,7 +616,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
};
// TODO : Compute pose
const float* fMax = (float*)(&sampled_pc.data[refIndMax * sampled_pc.step]);
const float* fMax = sampled_pc.ptr<float>(refIndMax);
const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1};
const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1};

Write Preview
Loading…
Cancel
Save