Esempio n. 1
0
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");
}
Esempio n. 2
0
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");
}