void projectEllipsoid(Ellipsoid& ell, double ell_height, const PCRGB& pc, PCRGB& pc_ell)
{
double lat, lon, height, x, y, z, h, s, l;
BOOST_FOREACH(PRGB const pt, pc.points) {
PRGB npt;
extractHSL(pt.rgb, h, s, l);
ell.cartToEllipsoidal(pt.x, pt.y, pt.z, lat, lon, height);
ell.ellipsoidalToCart(lat, lon, ell_height, x, y, z);
npt.x = x; npt.y = y; npt.z = z;
writeHSL(0, 0, l, npt.rgb);
pc_ell.points.push_back(npt);
}
pc_ell.header.frame_id = "base_link";
//pubLoop(pc_ell, "/proj_head");
}
void projectEllipsoidDense(Ellipsoid& ell, double ell_height, const PCRGB& pc,
int num_lat, int num_lon, double sigma, int k)
{
pcl::KdTreeFLANN<PRGB> kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree.setInputCloud(pc.makeShared());
PCRGB pc_dense;
vector<int> inds;
vector<float> dists;
double h, s, l, h_sum, s_sum, l_sum, normal, normal_sum;
double x, y, z;
double lat = 0, lon = 0;
for(int i=0;i<num_lat;i++) {
lat += PI / num_lat;
lon = 0;
for(int j=0;j<num_lon;j++) {
lon += 2 * PI / num_lon;
PRGB pt;
ell.ellipsoidalToCart(lat, lon, ell_height, x, y, z);
pt.x = x; pt.y = y; pt.z = z;
inds.clear();
dists.clear();
kd_tree.nearestKSearch(pt, k, inds, dists);
normal_sum = 0; h_sum = 0; s_sum = 0; l_sum = 0;
for(uint32_t inds_i=0;inds_i<inds.size();inds_i++) {
extractHSL(pc.points[inds[inds_i]].rgb, h, s, l);
normal = NORMAL(dists[inds_i], sigma);
normal_sum += normal;
h_sum += normal * h; s_sum += normal * s; l_sum += normal * l;
}
writeHSL(h_sum / normal_sum, s_sum / normal_sum, l_sum / normal_sum, pt.rgb);
pc_dense.points.push_back(pt);
}
}
pc_dense.header.frame_id = "/base_link";
pubLoop(pc_dense, "/proj_head");
}