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;
}
