Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Optimized curvature calculation #117

Closed
wants to merge 1 commit into from
Closed

Optimized curvature calculation #117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions include/Neighbourhood.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -279,6 +279,10 @@ namespace CCCoreLib
//! Returns the set 'radius' (i.e. the distance between the gravity center and the its farthest point)
PointCoordinateType computeLargestRadius();

double* initFromParameters(double alpha_rad,
const Vector3Tpl<double>& axis3D,
const Vector3Tpl<double>& t3D);

protected:

//! 2.5D Quadric equation
Expand Down
126 changes: 105 additions & 21 deletions src/Neighbourhood.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -325,22 +325,23 @@ bool Neighbourhood::computeQuadric()
const PointCoordinateType nxx = lsPlane[0]*lsPlane[0];
const PointCoordinateType nyy = lsPlane[1]*lsPlane[1];
const PointCoordinateType nzz = lsPlane[2]*lsPlane[2];
if (nxx > nyy)
{
if (nxx > nzz)
{
//as x is the "normal" direction, we use (y,z) as the base plane
idx.x = 1/*y*/; idx.y = 2/*z*/; idx.z = 0/*x*/;
}
}
else
{
if (nyy > nzz)
{
//as y is the "normal" direction, we use (z,x) as the base plane
idx.x = 2/*z*/; idx.y = 0/*x*/; idx.z = 1/*y*/;
}
}

//if (nxx > nyy)
//{
// if (nxx > nzz)
// {
// //as x is the "normal" direction, we use (y,z) as the base plane
// idx.x = 1/*y*/; idx.y = 2/*z*/; idx.z = 0/*x*/;
// }
//}
//else
//{
// if (nyy > nzz)
// {
// //as y is the "normal" direction, we use (z,x) as the base plane
// idx.x = 2/*z*/; idx.y = 0/*x*/; idx.z = 1/*y*/;
// }
//}

//compute the A matrix and b vector
std::vector<float> A;
Expand All @@ -364,11 +365,44 @@ bool Neighbourhood::computeQuadric()
float* _b = b.data();
for (unsigned i = 0; i < count; ++i)
{
CCVector3 P = *m_associatedCloud->getPoint(i) - *G;

float lX = static_cast<float>(P.u[idx.x]);
float lY = static_cast<float>(P.u[idx.y]);
float lZ = static_cast<float>(P.u[idx.z]);
//CCVector3 P = *m_associatedCloud->getPoint(i) - *G;
//============Rotation axis====================
CCVector3 axis;
axis.x = -lsPlane[1];
axis.y = lsPlane[0];
axis.z = 0;

float numerator = lsPlane[0]*0+ lsPlane[1] * 0+ lsPlane[2] * 1;
float denominator = sqrt(0 + 0 + 1)*sqrt(nxx+nyy+nzz);
float alpha;
alpha = std::acos(numerator/ denominator);

CCVector3 t;
t.x = t.y = t.z = 0;

double* m_mat = Neighbourhood::initFromParameters(alpha, -axis, t);

const CCVector3* PP = m_associatedCloud->getPoint(i);

CCVector3 dP;
dP.x = PP->x - G->x;
dP.y = PP->y - G->y;
dP.z = PP->z - G->z;

CCVector3 P;
P.x = m_mat[0] * dP.x+ m_mat[4] * dP.y+ m_mat[8] * dP.z;
P.y = m_mat[1] * dP.x + m_mat[5] * dP.y + m_mat[9] * dP.z;
P.z = m_mat[2] * dP.x + m_mat[6] * dP.y + m_mat[10] * dP.z;
float lX = static_cast<float>(P.x);
float lY = static_cast<float>(P.y);
float lZ = static_cast<float>(P.z);

delete m_mat;
//=============end==========

//float lX = static_cast<float>(P.u[idx.x]);
//float lY = static_cast<float>(P.u[idx.y]);
//float lZ = static_cast<float>(P.u[idx.z]);

*_A++ = 1.0f;
*_A++ = lX;
Expand Down Expand Up @@ -1037,3 +1071,53 @@ ScalarType Neighbourhood::computeCurvature(const CCVector3& P, CurvatureType cTy

return NAN_VALUE;
}

//! Inits transformation from a rotation axis, an angle and a translation
/** \param[in] alpha_rad rotation angle (in radians)
\param[in] axis3D rotation axis
\param[in] t3D translation
**/
double* Neighbourhood::initFromParameters(double alpha_rad, const Vector3Tpl<double>& axis3D, const Vector3Tpl<double>& t3D)
{
double cos_t = cos(alpha_rad);
double sin_t = sin(alpha_rad);
double inv_cos_t = static_cast<double>(1) - cos_t;

//normalize rotation axis
Vector3Tpl<double> uAxis3D = axis3D;
uAxis3D.normalize();

const double& l1 = uAxis3D.x;
const double& l2 = uAxis3D.y;
const double& l3 = uAxis3D.z;

double l1_inv_cos_t = l1 * inv_cos_t;
double l3_inv_cos_t = l3 * inv_cos_t;

double* m_mat=new double[16];
//1st column
m_mat[0] = cos_t + l1 * l1_inv_cos_t;
m_mat[1] = l2 * l1_inv_cos_t + l3 * sin_t;
m_mat[2] = l3 * l1_inv_cos_t - l2 * sin_t;
m_mat[3] = 0;

//2nd column
m_mat[4] = l2 * l1_inv_cos_t - l3 * sin_t;
m_mat[5] = cos_t + l2 * l2*inv_cos_t;
m_mat[6] = l2 * l3_inv_cos_t + l1 * sin_t;
m_mat[7] = 0;

//3rd column
m_mat[8] = l3 * l1_inv_cos_t + l2 * sin_t;
m_mat[9] = l2 * l3_inv_cos_t - l1 * sin_t;
m_mat[10] = cos_t + l3 * l3_inv_cos_t;
m_mat[11] = 0;

//4th column
m_mat[12] = t3D.x;
m_mat[13] = t3D.y;
m_mat[14] = t3D.z;
m_mat[15] = static_cast<double>(1);

return m_mat;
}