template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computeRGBPairFeatures (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
int p_idx, int q_idx,
float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
{
Eigen::Vector4i colors1 (cloud.points[p_idx].r, cloud.points[p_idx].g, cloud.points[p_idx].b, 0),
colors2 (cloud.points[q_idx].r, cloud.points[q_idx].g, cloud.points[q_idx].b, 0);
pcl::computeRGBPairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (),
colors1,
cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (),
colors2,
f1, f2, f3, f4, f5, f6, f7);
return (true);
}
int main(int argc, char** argv){
ros::init(argc, argv, "wkmeans_test");
ros::NodeHandle node;
ros::Rate rate(100);
std::size_t K = 10;
arma::vec pi = arma::randu<arma::vec>(3);
pi = arma::normalise(pi);
std::vector<arma::vec> Mu(K);
std::vector<arma::mat> Sigma(K);
float a = -2;
float b = 2;
for(std::size_t k = 0; k < K;k++){
Mu[k] = (b - a) * arma::randu<arma::vec>(3) + a;
Sigma[k].zeros(3,3);
arma::vec tmp = 0.3 * arma::randu<arma::vec>(3) + 0.2;
Sigma[k](0,0) =tmp(0);
Sigma[k](1,1) =tmp(1);
Sigma[k](2,2) =tmp(2);
}
GMM gmm;
gmm.setParam(pi,Mu,Sigma);
std::size_t nb_samples = 10000;
arma::colvec weights;
arma::mat X(nb_samples,3);
gmm.sample(X);
arma::vec L(nb_samples);
gmm.P(X,L);
L = L / arma::max(L);
// L.elem(q1).zeros();
unsigned char rgb[3];
std::vector<std::array<float,3> > colors(nb_samples);
for(std::size_t i = 0 ; i < L.n_elem;i++){
ColorMap::jetColorMap(rgb,L(i),0,1);
colors[i][0] = ((float)rgb[0])/255;
colors[i][1] = ((float)rgb[1])/255;
colors[i][2] = ((float)rgb[2])/255;
}
arma::uvec q1 = arma::find(L > 0.6*arma::max(L));
std::vector<std::array<float,3> > colors2(q1.n_elem);
arma::mat X_W(q1.n_elem,3);
for(std::size_t i = 0; i < q1.n_elem;i++){
assert(q1(i) < colors.size());
assert(q1(i) < X.n_rows);
colors2[i] = colors[q1(i)];
X_W.row(i) = X.row(q1(i));
}
Weighted_Kmeans<double> weighted_kmeans;
weighted_kmeans.cluster(X_W.st(),L);
weighted_kmeans.centroids.print("centroids");
opti_rviz::Vis_points points(node,"centroids");
points.scale = 0.1;
points.r = 1;
points.b = 1;
arma::fmat c = arma::conv_to<arma::fmat>::from(weighted_kmeans.centroids.st());
points.initialise("world",c);
opti_rviz::Vis_point_cloud vis_point(node,"samples");
vis_point.set_display_type(opti_rviz::Vis_point_cloud::DEFAULT);
vis_point.initialise("world",X_W);
opti_rviz::Vis_gmm vis_gmm(node,"gmm");
vis_gmm.initialise("world",pi,Mu,Sigma);
while(node.ok()){
vis_gmm.update(pi,Mu,Sigma);
vis_gmm.publish();
vis_point.update(X_W,colors2,weights);
vis_point.publish();
points.update(c);
points.publish();
ros::spinOnce();
rate.sleep();
}
}
