void Button::Draw(App &app, float drawAreax, float drawAreay, int drawAreaWidth, int drawAreaHeight)
{
// define a 120x50 rectangle
/*sf::RectangleShape rectangle(sf::Vector2f(m_X(app), m_Y(app)));
rectangle.setPosition(sf::Vector2f(m_X(app), m_Y(app)));
// change the size to 100x100
rectangle.setSize(sf::Vector2f(m_width, m_height));
rectangle.setFillColor(sf::Color(64, 64, 64, 192));
rectangle.setOutlineColor(sf::Color(192, 192, 192, 255));
app.draw(rectangle);
*/
getText()->setPosition(m_X(app), m_Y(app));
app.draw(*getText());
}
void icp(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target,
const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,
float* h_R, float* h_t,
const registrationParameters ¶m)
{
int Xsize, Ysize;
boost::shared_array<float> h_X, h_Y;
cloud2data(cloud_target, h_X, Xsize);
cloud2data(cloud_source, h_Y, Ysize);
//
// initialize paramters
//
int maxIteration = param.maxIteration;
//
// memory allocation
//
float* h_Xx = h_X.get() + Xsize*0;
float* h_Xy = h_X.get() + Xsize*1;
float* h_Xz = h_X.get() + Xsize*2;
float* h_Yx = h_Y.get() + Ysize*0;
float* h_Yy = h_Y.get() + Ysize*1;
float* h_Yz = h_Y.get() + Ysize*2;
boost::shared_array<float> h_Xcorr ( new float[Ysize*3] ); // points in X corresponding to Y
float h_S[9];
float h_Xc[3];
float h_Yc[3];
findCenter(h_Y.get(), Ysize, h_Yc);
// building flann index
boost::shared_array<float> m_X ( new float [Xsize*3] );
for (int i = 0; i < Xsize; i++)
{
m_X[i*3 + 0] = h_Xx[i];
m_X[i*3 + 1] = h_Xy[i];
m_X[i*3 + 2] = h_Xz[i];
}
flann::Matrix<float> mat_X(m_X.get(), Xsize, 3); // Xsize rows and 3 columns
flann::Index< flann::L2<float> > index( mat_X, flann::KDTreeIndexParams() );
index.buildIndex();
boost::shared_array<float> m_Y ( new float [Ysize*3] );
float* h_Xcorrx = h_Xcorr.get() + Ysize*0;
float* h_Xcorry = h_Xcorr.get() + Ysize*1;
float* h_Xcorrz = h_Xcorr.get() + Ysize*2;
boost::shared_array<float> h_Ycentered ( new float [Ysize*3] );
{
float* h_Ycenteredx = h_Ycentered.get() + Ysize*0;
float* h_Ycenteredy = h_Ycentered.get() + Ysize*1;
float* h_Ycenteredz = h_Ycentered.get() + Ysize*2;
for(int i = 0; i < Ysize; i++){
h_Ycenteredx[i] = h_Yx[i] - h_Yc[0];
h_Ycenteredy[i] = h_Yy[i] - h_Yc[1];
h_Ycenteredz[i] = h_Yz[i] - h_Yc[2];
}
}
// ICP main loop
for(int iter=0; iter < maxIteration; iter++){
// find closest points
#pragma omp parallel for
for (int i = 0; i < Ysize; i++)
{
m_Y[i*3 + 0] = (h_R[0]*h_Yx[i] + h_R[1]*h_Yy[i] + h_R[2]*h_Yz[i]) + h_t[0];
m_Y[i*3 + 1] = (h_R[3]*h_Yx[i] + h_R[4]*h_Yy[i] + h_R[5]*h_Yz[i]) + h_t[1];
m_Y[i*3 + 2] = (h_R[6]*h_Yx[i] + h_R[7]*h_Yy[i] + h_R[8]*h_Yz[i]) + h_t[2];
}
flann::Matrix<float> mat_Y(m_Y.get(), Ysize, 3); // Ysize rows and 3 columns
std::vector< std::vector<size_t> > indices(Ysize);
std::vector< std::vector<float> > dists(Ysize);
index.knnSearch(mat_Y,
indices,
dists,
1, // k of knn
flann::SearchParams() );
#pragma omp parallel for
for(int i = 0; i < Ysize; i++){
// put the closest point to Ycorr
h_Xcorrx[i] = h_Xx[indices[i][0]];
h_Xcorry[i] = h_Xy[indices[i][0]];
h_Xcorrz[i] = h_Xz[indices[i][0]];
}
findCenter(h_Xcorr.get(), Ysize, h_Xc);
#pragma omp parallel for
for(int i = 0; i < Ysize; i++){
// put the closest point to Ycorr
h_Xcorrx[i] -= h_Xc[0];
h_Xcorry[i] -= h_Xc[1];
h_Xcorrz[i] -= h_Xc[2];
}
// compute S
{
// SGEMM(TRANSA,TRANSB,M,N,K,ALPHA,A,LDA,B,LDB,BETA,C,LDC)
// C := alpha*op( A )*op( B ) + beta*C
//
// h_X^T * h_Y => S
// m*k k*n m*n
int three = 3;
float one = 1.0f, zero = 0.0f;
sgemm_((char*)"t", (char*)"n",
&three, &three, &Ysize, // m,n,k
&one, h_Xcorr.get(), &Ysize, // alpha, op(A), lda
h_Ycentered.get(), &Ysize, // op(B), ldb
&zero, h_S, &three); // beta, C, ldc
}
// find RT from S
findRTfromS(h_Xc, h_Yc, h_S, h_R, h_t);
#ifndef NOVIEWER
if(!param.noviewer){
Eigen::Matrix4f transformation;
transformation <<
h_R[0], h_R[1], h_R[2], h_t[0],
h_R[3], h_R[4], h_R[5], h_t[1],
h_R[6], h_R[7], h_R[8], h_t[2],
0, 0, 0, 1;
pcl::transformPointCloud ( *param.cloud_source, *param.cloud_source_trans, transformation );
param.viewer->updatePointCloud ( param.cloud_source_trans, *param.source_trans_color, "source trans" );
param.viewer->spinOnce();
}
#endif
}
}
