opencv/opencv_contrib
1
0
Fork 0
mirror of https://github.com/opencv/opencv_contrib.git synced 2025-10-19 02:16:34 +08:00
Code Issues Releases Wiki Activity

Do not apply pose transformation on normals if the cloud does not have normals

Browse Source
This commit is contained in:
felix
2015-06-20 12:08:33 -04:00
parent f8757c3fd4
commit b378236317
1 changed files with 14 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
modules/surface_matching/src/ppf_helpers.cpp
View File

@@ -504,17 +504,21 @@ Mat transformPCPose(Mat pc, double Pose[16])
pcDataT[2] = (float)(p2[2]/p2[3]); pcDataT[2] = (float)(p2[2]/p2[3]);
} }
// Rotate the normals, too // If the point cloud has normals,
double n[3] = {(double)n1[0], (double)n1[1], (double)n1[2]}, n2[3]; // then rotate them as well
if (pc.cols == 6)
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); double n[3] = { (double)n1[0], (double)n1[1], (double)n1[2] }, n2[3];
nT[1]=(float)(n2[1]/nNorm);
nT[2]=(float)(n2[2]/nNorm); 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);
}
} }
} }