void pnlRand(int numElem, int* vec, int left, int right)
{
int myid = PAR_OMP_NUM_CURR_THREAD;
//set distribution type
g_RNG[myid].m_cxRandState.disttype = CX_RAND_UNI;
//set range
cxRandSetRange( &g_RNG[myid].m_cxRandState, left, right, -1 );
//create matrix wrapper for 1 floating point value
float *array = new float[numElem];
CxMat mat = cxMat( 1, numElem, CX_32FC1, array );
//generate value
cxRand( &g_RNG[myid].m_cxRandState, &mat );
double ip;
double x1;
int i = 0;
for( i = 0; i < numElem; ++i )
{
ip = modf(array[i], &x1);
vec[i] = (int)(x1 >= 0 ? ( ip > 0.5 ? ++x1 : x1 ) : ( ip > -0.5 ? --x1 : x1 ));
}
delete []array;
}
PNL_END
#endif
PNL_BEGIN
void pnlRandNormal(floatVector* vls, floatVector &mean, floatVector &sigma )
{
int myid = PAR_OMP_NUM_CURR_THREAD;
int nEl = mean.size();
vls->resize(nEl);
if( nEl > 1 )
{
floatVector normVls(nEl);
//vsRngGaussian( VSL_METHOD_SGAUSSIAN_BOXMULLER2, g_RNG[myid].m_vslStream, nEl, &normVls.front(), 0, 1 );
pnlRandNormal(nEl,&normVls.front(), 0, 1);
CxMat matNormVls = cxMat( nEl, 1, CX_32FC1, &normVls.front() );
CxMat matMean = cxMat( nEl, 1, CX_32FC1, &mean.front() );
CxMat matCov = cxMat( nEl, nEl, CX_32FC1, &sigma.front() );
floatVector evecData( nEl*nEl );
CxMat evecMat = cxMat( nEl, nEl, CX_32FC1, &evecData.front() );
floatVector evalData( nEl*nEl );
CxMat evalMat = cxMat( nEl, nEl, CX_32FC1, &evalData.front() );
cxSVD( &matCov, &evalMat, &evecMat, NULL, CX_SVD_MODIFY_A );
int i;
for( i = 0; i < nEl; i++)
{
evalData[i*(nEl+1)] = float( sqrt( evalData[i*(nEl+1)] ) );
}
CxMat *prodMat1 = cxCreateMat( nEl, nEl, CX_32FC1 );
cxMatMul( &evecMat, &evalMat, prodMat1 );
CxMat matResultVls = cxMat( nEl, 1, CX_32FC1, &(vls->front()) );
cxMatMulAdd( prodMat1, &matNormVls, &matMean, &matResultVls );
cxReleaseMat( &prodMat1 );
}
else
{
//vsRngGaussian( VSL_METHOD_SGAUSSIAN_BOXMULLER2, g_RNG[myid].m_vslStream, 1,
// &(vls->front()), mean.front(), sigma.front() );
vls->front() = pnlRandNormal( mean.front(), sigma.front() );
}
}
void pnlRandNormal( int numElem, double* vec, double mean, double sigma )
{
int myid = PAR_OMP_NUM_CURR_THREAD;
//set distribution type
g_RNG[myid].m_cxRandState.disttype = CX_RAND_NORMAL;
//set range
cxRandSetRange( &g_RNG[myid].m_cxRandState, sqrt(sigma), mean, -1 );
//create matrix wrapper for 1 floating point value
CxMat mat = cxMat( 1, numElem, CX_64FC1, vec );
//generate value
cxRand( &g_RNG[myid].m_cxRandState, &mat );
}
void pnlRand(int numElem, double* vec, double left, double right)
{
int myid = PAR_OMP_NUM_CURR_THREAD;
//set distribution type
g_RNG[myid].m_cxRandState.disttype = CX_RAND_UNI;
//set range
cxRandSetRange( &g_RNG[myid].m_cxRandState, left, right, -1 );
//create matrix wrapper for 1 floating point value
CxMat mat = cxMat( 1, numElem, CX_64FC1, vec );
//generate value
cxRand( &g_RNG[myid].m_cxRandState, &mat );
}
double pnlRandNormal( double mean, double sigma )
{
int myid = PAR_OMP_NUM_CURR_THREAD;
//set distribution type
g_RNG[myid].m_cxRandState.disttype = CX_RAND_NORMAL;
//set range
cxRandSetRange( &g_RNG[myid].m_cxRandState, sqrt(sigma), mean, -1 );
//create matrix wrapper for 1 floating point value
double val = 0;
CxMat mat = cxMat( 1, 1, CX_32FC1, &val );
//generate value
cxRand( &g_RNG[myid].m_cxRandState, &mat );
return val;
}
double pnlRand(double left, double right)
{
int myid = PAR_OMP_NUM_CURR_THREAD;
//set distribution type
g_RNG[myid].m_cxRandState.disttype = CX_RAND_UNI;
//set range
cxRandSetRange( &g_RNG[myid].m_cxRandState, left, right, -1 );
//create matrix wrapper for 1 floating point value
double val = 0;
CxMat mat = cxMat( 1, 1, CX_64FC1, &val );
//generate value
cxRand( &g_RNG[myid].m_cxRandState, &mat );
return val;
}
//generate uniform integer distribution in the range [left,right]
int pnlRand(int left, int right)
{
int myid = PAR_OMP_NUM_CURR_THREAD;
PNL_CHECK_LEFT_BORDER( right, left );
g_RNG[myid].m_cxRandState.disttype = CX_RAND_UNI;
//set range
cxRandSetRange( &g_RNG[myid].m_cxRandState, left, right, -1 );
//create matrix wrapper for 1 floating point value
float val = 0;
CxMat mat = cxMat( 1, 1, CX_32FC1, &val );
//generate value
cxRand( &g_RNG[myid].m_cxRandState, &mat );
double x1;
double ip = modf(val, &x1);
return (int)(x1 >= 0 ? ( ip > 0.5 ? ++x1 : x1 ) : ( ip > -0.5 ? --x1 : x1 ));
}
void
PatchSample::findSamples(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
MatrixXd &mx, MatrixXd &my, MatrixXd &mz,
double cx, double cy, double cz)
{
// reset the cloud
cloud->points.resize(0);
// transform input data to local frame if necessary
if (world_frame_)
{
pcl::Xform3 pose;
pose.transform(mx, my, mz, p_.rexp(p_.getR()), p_.getC(), true, true); // invert, rotonly
MatrixXd cxMat(1,1), cyMat(1,1), czMat(1,1);
cxMat << cx;
cyMat << cy;
czMat << cz;
//TBD: wrong p.c or p.r
pose.transform(cxMat, cyMat, czMat, p_.rexp(p_.getR()), p_.getC(), true, false); // invert
cx = cxMat(0,0);
cy = cyMat(0,0);
cz = czMat(0,0);
}
// calculate ray intersections
r_ = p_.ri(mx, my, mz, cx, cy, cz);
// create cloud
MatrixXd x (mx.rows(),mx.cols());
x = (mx.array() * r_.array()) + cx;
MatrixXd y (my.rows(),my.cols());
y = (my.array() * r_.array()) + cy;
MatrixXd z (mz.rows(),mz.cols());
z = (mz.array() * r_.array()) + cz;
// clean them up
MatrixXd bl_mat;
p_.bl(x, y, bl_mat);
for (int i=0; i<x.rows(); i++)
{
for (int j=0; j<x.cols(); j++)
{
if (bl_mat(i,j)>0.0)
{
x(i,j) = std::numeric_limits<float>::quiet_NaN();
y(i,j) = std::numeric_limits<float>::quiet_NaN();
z(i,j) = std::numeric_limits<float>::quiet_NaN();
}
}
}
pcl::PointXYZ point;
for (int i=0; i<x.rows(); i++)
{
for (int j=0; j<x.cols(); j++)
{
point.x = x(i,j);
point.y = y(i,j);
point.z = z(i,j);
cloud->points.push_back(point);
}
}
cloud->width = x.cols();
cloud->height = x.rows();
cloud->is_dense = false;
}
