static sval psm_l_to_r(struct logical_chunk_info *lci, uval laddr, uval size) { struct page_share_mgr *psm = PARENT_OBJ(typeof(*psm), psm_lci, lci); uval idx = (laddr - lci->lci_laddr) >> LOG_PGSIZE; uval raddr = INVALID_PHYSICAL_ADDRESS; lock_acquire(&psm->psm_lock); struct logical_chunk_info *next_lci = psm->psm_entries[idx]; if (next_lci) { assert(next_lci->lci_raddr != INVALID_PHYSICAL_ADDRESS, "PSM managed LCI's must use simple translation\n"); assert(range_subset(next_lci->lci_laddr, next_lci->lci_size, laddr, size), "range mismatch\n"); raddr = laddr - next_lci->lci_laddr + next_lci->lci_raddr; } lock_release(&psm->psm_lock); return raddr; }
SparseRangeScan* RangeScanner::optical_triangulation() { // first construct image... this->image_sensor(); RangeOctree range_octree(&sparse_scan); range_octree.construct_tree(); // first determine starting and ending points int mid_y = sensor.resy()/2, start_x = sensor.resx(), end_x = 0; int mid_x = sensor.resx()/2, start_y = sensor.resy(), end_y = 0; Vector3 start_pos, dir_start, end_pos, dir_end; sensor.generatePerspectiveRay(start_x, mid_y, start_pos, dir_start); sensor.generatePerspectiveRay(end_x, mid_y, end_pos, dir_end); //sensor.generatePerspectiveRay(mid_x, start_y, start_pos, dir_start); //sensor.generatePerspectiveRay(mid_x, end_y, end_pos, dir_end); Vector3 start_pt = start_pos + dir_start*max_range; Vector3 end_pt = end_pos + dir_end*min_range; // the start and end points determine a frustum for the laser - determine frustum angle Vector3 laser_dir_start = start_pt-laser_pos; laser_dir_start.normalize(); Vector3 laser_dir_end = end_pt-laser_pos; laser_dir_end.normalize(); double laser_frustum = acos(laser_dir_start.dotProduct(laser_dir_end)); cout << "laser frustum: " << ((laser_frustum/acos(-1))*180.0) << endl; // from the laser frustum and number of stripes, determine angle increment double angle_increment = laser_frustum / (double)num_stripes; double laser_shift = angle_increment/2.0; stripes_processed = 0; // populate true image double** r_scan = new double*[sensor.resy()]; double** g_scan = new double*[sensor.resy()]; double** b_scan = new double*[sensor.resy()]; for(int y = 0; y < sensor.resy(); y++) { r_scan[y] = new double[sensor.resx()]; g_scan[y] = new double[sensor.resx()]; b_scan[y] = new double[sensor.resx()]; for(int x = 0; x < sensor.resx(); x++) { r_scan[y][x] = 1; g_scan[y][x] = 1; b_scan[y][x] = 1; } } // compute analytical normals, while also populating original range scan image Vector3* pt_normals = new Vector3[sparse_scan.size()]; for(int p = 0; p < sparse_scan.size(); p++) { Vector3 scan_pt = sparse_scan.getPt(p); Vector3 shape_normal = shape->normal(scan_pt); pt_normals[p] = shape_normal; PixelCoord pix = sparse_scan.getPixel(p); Vector3 light_vec = sensor.camera_pos()-scan_pt; light_vec.normalize(); double dot = fabs(light_vec.dotProduct(shape_normal)); r_scan[pix.y][pix.x] = (236.0/255.0)*dot; g_scan[pix.y][pix.x] = (211.0/255.0)*dot; b_scan[pix.y][pix.x] = (143.0/255.0)*dot; } // ... add points which sensor saw, but laser didn't for(int p = 0; p < image_scan.size(); p++) { Vector3 scan_pt = image_scan.getPt(p); Vector3 shape_normal = shape->normal(scan_pt); PixelCoord pix = image_scan.getPixel(p); Vector3 light_vec = sensor.camera_pos()-scan_pt; light_vec.normalize(); double dot = fabs(light_vec.dotProduct(shape_normal)); r_scan[pix.y][pix.x] = (236.0/255.0)*dot; g_scan[pix.y][pix.x] = (211.0/255.0)*dot; b_scan[pix.y][pix.x] = (143.0/255.0)*dot; } // initialize gaussian fits GaussianFit*** gaussian_fits = new GaussianFit**[sensor.resy()]; for(int y = 0; y < sensor.resy(); y++) { gaussian_fits[y] = new GaussianFit*[sensor.resx()]; for(int x = 0; x < sensor.resx(); x++) gaussian_fits[y][x] = new GaussianFit(); } int fraction = num_stripes/5; // now go through each laser shift, and perform optical triangulation for(int s = 0; s < num_stripes; s++) { if((s % fraction) == 0) cout << ((double)s / (double)num_stripes) << " done with stripe... " << endl; laser_shift += angle_increment; LaserStripe stripe; this->construct_laser_stripe(start_pt, laser_shift, laser_fov, stripe); // gather all points which the laser stripe intersects vector<int> intersected_geometry; range_octree.intersect_stripe(stripe, &intersected_geometry); // construct image subset, first determine area coverage RangeSubset range_subset(sensor.resx(), sensor.resy(), laser_smoother); for(unsigned g = 0; g < intersected_geometry.size(); g++) { int pt_ind = intersected_geometry[g]; PixelCoord pix = sparse_scan.getPixel(pt_ind); range_subset.expand_range(pix.x); } range_subset.finalize_range(); // populate image with laser for(unsigned g = 0; g < intersected_geometry.size(); g++) { int pt_ind = intersected_geometry[g]; PixelCoord pix = sparse_scan.getPixel(pt_ind); Vector3 pt = sparse_scan.getPt(pt_ind); double light_sigma = pt.length(laser_pos)*tan(0.5*laser_fov); double pulse_dist = stripe.pulse_abs_distance(pt); double luminance = this->gaussian_beam(pulse_dist, 0.5*light_sigma); luminance = luminance < 0 ? 0 : luminance; double noise_scale = (1.0-luminance); double noise_magnitude = luminance < additive_noise ? luminance : additive_noise; double gaussian_noise = this->sampleNormalDistribution(noise_scale, noise_magnitude); Vector3 laser_ray = laser_pos-pt; laser_ray.normalize(); Vector3 pt_normal = pt_normals[pt_ind]; double cos_falloff = laser_ray.dotProduct(pt_normal); double radiance = luminance*cos_falloff+gaussian_noise; radiance = radiance > 1 ? 1 : radiance; radiance = radiance < 0 ? 0 : radiance; int quantized_radiance = 255*radiance; radiance = quantized_radiance / 255.0; range_subset.contribute_point(pix.x, pix.y, radiance); } if(range_subset.containsNothing()) continue; // smooth image range_subset.smooth(); if(dump_stripes) { char* file_char = new char[300]; int s_f = stripe_start+stripes_processed+1; if(s_f < 10) sprintf(file_char, "%s0000%u.png", base_stripe.c_str(), s_f); else if(s_f < 100) sprintf(file_char, "%s000%u.png", base_stripe.c_str(), s_f); else if(s_f < 1000) sprintf(file_char, "%s00%u.png", base_stripe.c_str(), s_f); else if(s_f < 10000) sprintf(file_char, "%s0%u.png", base_stripe.c_str(), s_f); else sprintf(file_char, "%s%u.png", base_stripe.c_str(), s_f); string filename = file_char; range_subset.write_to_file(filename, r_scan, g_scan, b_scan); delete [] file_char; } stripes_processed++; // gather center-of-gravity times for(int y = 0; y < sensor.resy(); y++) { for(int x = range_subset.startX(); x <= range_subset.endX(); x++) { double intensity = range_subset.get_intensity(x,y); if(intensity > 1e-6) gaussian_fits[y][x]->add_point(laser_shift, intensity); } } } for(int y = 0; y < sensor.resy(); y++) { delete [] r_scan[y]; delete [] g_scan[y]; delete [] b_scan[y]; } delete [] r_scan; delete [] g_scan; delete [] b_scan; double outlier_threshold = 4.0; SparseRangeScan* depth_scan = new SparseRangeScan(); int num_retained = 0, num_peak_rejections = 0, num_std_rejections = 0; double ave_weight = 0; for(int p = 0; p < sparse_scan.size(); p++) { Vector3 scan_pt = sparse_scan.getPt(p); PixelCoord pix = sparse_scan.getPixel(p); if(gaussian_fits[pix.y][pix.x]->do_fit()) { double peak = gaussian_fits[pix.y][pix.x]->get_peak(); double std = gaussian_fits[pix.y][pix.x]->get_std(); if(peak < peak_threshold) { //cout << "rejected " << pix.x << " " << pix.y << " due to peak threshold..." << endl; num_peak_rejections++; continue; } if(std > (0.5*std_threshold*laser_fov)) { //cout << "rejected " << pix.x << " " << pix.y << " due to std threshold..." << endl; num_std_rejections++; continue; } double proper_time = gaussian_fits[pix.y][pix.x]->get_mean(); num_retained++; Vector3 ray_pos, ray_dir; sensor.generatePerspectiveRay(pix.x, pix.y, ray_pos, ray_dir); double reference_depth = ray_pos.length(scan_pt); LaserStripe stripe; this->construct_laser_stripe(start_pt, proper_time, laser_fov, stripe); double local_depth = -(ray_pos.dotProduct(stripe.n)+stripe.d) / ray_dir.dotProduct(stripe.n); if(fabs(local_depth-reference_depth) > outlier_threshold) continue; Vector3 depth_pt = ray_pos + ray_dir*local_depth; depth_scan->addPoint(pix, depth_pt); } } cout << "retained " << num_retained << " / " << sparse_scan.size() << " peak rejections: " << num_peak_rejections << " ; std rejections: " << num_std_rejections << endl; for(int y = 0; y < sensor.resy(); y++) { for(int x = 0; x < sensor.resx(); x++) delete gaussian_fits[y][x]; } for(int y = 0; y < sensor.resy(); y++) delete [] gaussian_fits[y]; delete [] gaussian_fits; delete [] pt_normals; return depth_scan; }