void sm_icp(struct sm_params*params, struct sm_result*res) {
res->valid = 0;
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
if(!ld_valid_fields(laser_ref) ||
!ld_valid_fields(laser_sens)) {
return;
}
sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n",
count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays);
/** Mark as invalid the rays outside of (min_reading, max_reading] */
ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading);
ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading);
sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n",
count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays,
params->min_reading, params->max_reading);
if(JJ) jj_context_enter("sm_icp");
egsl_push_named("sm_icp");
if(params->use_corr_tricks || params->debug_verify_tricks)
ld_create_jump_tables(laser_ref);
ld_compute_cartesian(laser_ref);
ld_compute_cartesian(laser_sens);
if(params->do_alpha_test) {
ld_simple_clustering(laser_ref, params->clustering_threshold);
ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
ld_simple_clustering(laser_sens, params->clustering_threshold);
ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
}
if(JJ) jj_add("laser_ref", ld_to_json(laser_ref));
if(JJ) jj_add("laser_sens", ld_to_json(laser_sens));
gsl_vector * x_new = gsl_vector_alloc(3);
gsl_vector * x_old = vector_from_array(3, params->first_guess);
if(params->do_visibility_test) {
sm_debug("laser_ref:\n");
visibilityTest(laser_ref, x_old);
sm_debug("laser_sens:\n");
gsl_vector * minus_x_old = gsl_vector_alloc(3);
ominus(x_old,minus_x_old);
visibilityTest(laser_sens, minus_x_old);
gsl_vector_free(minus_x_old);
}
double error;
int iterations;
int nvalid;
if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
sm_error("icp: ICP failed for some reason. \n");
res->valid = 0;
res->iterations = iterations;
res->nvalid = 0;
} else {
/* It was succesfull */
int restarted = 0;
double best_error = error;
gsl_vector * best_x = gsl_vector_alloc(3);
gsl_vector_memcpy(best_x, x_new);
if(params->restart &&
(error/nvalid)>(params->restart_threshold_mean_error) ) {
sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error));
restarted = 1;
double dt = params->restart_dt;
double dth = params->restart_dtheta;
sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth));
double perturb[6][3] = {
{dt,0,0}, {-dt,0,0},
{0,dt,0}, {0,-dt,0},
{0,0,dth}, {0,0,-dth}
};
int a; for(a=0;a<6;a++){
sm_debug("-- Restarting with perturbation #%d\n", a);
struct sm_params my_params = *params;
gsl_vector * start = gsl_vector_alloc(3);
gvs(start, 0, gvg(x_new,0)+perturb[a][0]);
gvs(start, 1, gvg(x_new,1)+perturb[a][1]);
gvs(start, 2, gvg(x_new,2)+perturb[a][2]);
gsl_vector * x_a = gsl_vector_alloc(3);
double my_error; int my_valid; int my_iterations;
if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){
sm_error("Error during restart #%d/%d. \n", a, 6);
break;
}
iterations+=my_iterations;
if(my_error < best_error) {
sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error);
gsl_vector_memcpy(best_x, x_a);
best_error = my_error;
}
gsl_vector_free(x_a); gsl_vector_free(start);
}
}
/* At last, we did it. */
res->valid = 1;
vector_to_array(best_x, res->x);
sm_debug("icp: final x = %s \n", gsl_friendly_pose(best_x));
if (restarted) { // recompute correspondences in case of restarts
ld_compute_world_coords(laser_sens, res->x);
if(params->use_corr_tricks)
find_correspondences_tricks(params);
else
find_correspondences(params);
}
if(params->do_compute_covariance) {
val cov0_x, dx_dy1, dx_dy2;
compute_covariance_exact(
laser_ref, laser_sens, best_x,
&cov0_x, &dx_dy1, &dx_dy2);
val cov_x = sc(square(params->sigma), cov0_x);
/* egsl_v2da(cov_x, res->cov_x); */
res->cov_x_m = egsl_v2gslm(cov_x);
res->dx_dy1_m = egsl_v2gslm(dx_dy1);
res->dx_dy2_m = egsl_v2gslm(dx_dy2);
if(0) {
egsl_print("cov0_x", cov0_x);
egsl_print_spectrum("cov0_x", cov0_x);
val fim = ld_fisher0(laser_ref);
val ifim = inv(fim);
egsl_print("fim", fim);
egsl_print_spectrum("ifim", ifim);
}
}
res->error = best_error;
res->iterations = iterations;
res->nvalid = nvalid;
gsl_vector_free(best_x);
}
gsl_vector_free(x_new);
gsl_vector_free(x_old);
egsl_pop_named("sm_icp");
if(JJ) jj_context_exit();
}
std::vector<vec3f> PathTracer::renderPixels(){
Camera &camera = getCamera();
uint width = camera.mResolution.x, height = camera.mResolution.y;
std::vector<vec3f> pixelColors(width * height, vec3f(0,0,0));
if(useNextEventEstimation)
prepareForLightSampling();
for(uint s = 0; s < spp; s++){
std::cout << "iteration : " << s << std::endl;
engine->scene.updateSceneForMotionBlur();
#pragma omp parallel for
for(int p = 0; p < pixelColors.size(); p++){
Path eyePath;
samplePath(eyePath, camera.generateRay(p));
pixelColors[p] *= s / float(s+1);
vec3f color = vec3f(1,1,1);
bool hasToConnect = eyePath[eyePath.size()-1].radiance.length() <= 0;
if(!useNextEventEstimation || !hasToConnect){
for(int i = 0; i < eyePath.size(); i++){
if(i != eyePath.size() - 1){
color *= eyePath[i].cosineTerm();
float dist = (eyePath[i+1].origin - eyePath[i].origin).length();
color *= eyePath[i].radianceDecay(dist);
}
color *= eyePath[i].radiance / eyePath[i].originProb / eyePath[i].directionProb;
}
}
else{
int endIndex = (eyePath.back().contactObj || eyePath.back().insideObj) ? -1 : eyePath.size()-2;
if(endIndex <= 0) continue;
Ray &endRay = eyePath[endIndex], lightRay = genLightSample();
if(endRay.contactObj && endRay.contactObj->isEmissive()) continue;
if(endRay.contactObj && !endRay.contactObj->nonSpecular()) continue;
endRay.direction = lightRay.origin - endRay.origin;
endRay.direction.normalize();
lightRay.direction = -endRay.direction;
float connectDist = MAX((lightRay.origin - endRay.origin).length(), EPSILON);
if(endRay.direction.dot(lightRay.contactNormal()) >= 0) continue;
if(!visibilityTest(endRay, lightRay)) continue;
for(int i = 0; i < endIndex; i++){
color *= eyePath[i].radiance / eyePath[i].originProb / eyePath[i].directionProb;
color *= eyePath[i].cosineTerm();
float dist = (eyePath[i+1].origin - eyePath[i].origin).length();
color *= eyePath[i].radianceDecay(dist);
}
color *= eyePath[endIndex-1].evalBSDF(endRay) * lightRay.radiance * endRay.radianceDecay(connectDist)
* lightRay.cosineTerm() * endRay.cosineTerm() / (connectDist * connectDist);
color /= eyePath[endIndex].originProb * lightRay.originProb;
}
if(!isLegalColor(color))
color = vec3f(0,0,0);
pixelColors[p] += camera.fixVignetting(color, p) / (s+1);
}
camera.mFilm.setBuffer(pixelColors);
std::string filename = engine->renderer->name + engine->scene.name + ".pfm";
camera.mFilm.savePFM(filename);
}
return pixelColors;
}
