Eigen::Vector3d PointToLineICP::getBestRototranslationVector(
const Scan &refScan,
const Scan &rotatedScan,
const unsigned int queryToReferenceMapping[][2],
const unsigned int queryIndices[],
unsigned int queryIndexCount) {
Eigen::Vector3d q;
struct gpc_corr c[queryIndexCount];
double x[3] = {0, 0, 0}; // give the solution
double n[2]; // normal to segment
double normN;
for (unsigned int k = 0; k < queryIndexCount; k++){
unsigned int h = queryIndices[k];
double rx0 = refScan[queryToReferenceMapping[h][0]].getX();
double ry0 = refScan[queryToReferenceMapping[h][0]].getY();
double rx1 = refScan[queryToReferenceMapping[h][1]].getX();
double ry1 = refScan[queryToReferenceMapping[h][1]].getY();
// first scan [use the nearest point]
c[k].p[0] = rx0;
c[k].p[1] = ry0;
// second scan
c[k].q[0] = rotatedScan[h].getX();
c[k].q[1] = rotatedScan[h].getY();
// point to line metric is wi*ni*ni' where ni is the normal to the segment
//TODO check segno (MODIFICATO SEGNO DI n[0])
n[0] = -(ry0 - ry1);
//n[0] = (yyR[idxRef[h][0]] - yyR[idxRef[h][1]]);
n[1] = rx0 - rx1;
// You need to normalize the vector?
normN = std::sqrt(n[0] * n[0] + n[1] * n[1]);
n[0] = n[0] / normN;
n[1] = n[1] / normN;
//n[0]*=-1;
//n[1]*=-1;
c[k].C[0][0] = n[0] * n[0];
c[k].C[0][1] = c[k].C[1][0] = n[0] * n[1];
c[k].C[1][1] = n[1] * n[1];
}
gpc_solve(queryIndexCount, c, x);
q << x[0], x[1], x[2];
return q;
}
int compute_next_estimate(struct sm_params*params,
const double x_old[3], double x_new[3])
{
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
struct gpc_corr c[laser_sens->nrays];
int i; int k=0;
for(i=0;i<laser_sens->nrays;i++) {
if(!laser_sens->valid[i])
continue;
if(!ld_valid_corr(laser_sens,i))
continue;
int j1 = laser_sens->corr[i].j1;
int j2 = laser_sens->corr[i].j2;
c[k].valid = 1;
if(laser_sens->corr[i].type == corr_pl) {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
c[k].q[0] = laser_ref->points[j1].p[0];
c[k].q[1] = laser_ref->points[j1].p[1];
/** TODO: here we could use the estimated alpha */
double diff[2];
diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
double normal[2];
normal[0] = +diff[1] * one_on_norm;
normal[1] = -diff[0] * one_on_norm;
double cos_alpha = normal[0];
double sin_alpha = normal[1];
c[k].C[0][0] = cos_alpha*cos_alpha;
c[k].C[1][0] =
c[k].C[0][1] = cos_alpha*sin_alpha;
c[k].C[1][1] = sin_alpha*sin_alpha;
/* sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d, alpha_seg=%f, cos=%f sin=%f \n", k,i,
rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/
#if 0
/* Note: it seems that because of numerical errors this matrix might be
not semidef positive. */
double det = c[k].C[0][0] * c[k].C[1][1] - c[k].C[0][1] * c[k].C[1][0];
double trace = c[k].C[0][0] + c[k].C[1][1];
int semidef = (det >= 0) && (trace>0);
if(!semidef) {
/* printf("%d: Adjusting correspondence weights\n",i);*/
double eps = -det;
c[k].C[0][0] += 2*sqrt(eps);
c[k].C[1][1] += 2*sqrt(eps);
}
#endif
} else {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
projection_on_segment_d(
laser_ref->points[j1].p,
laser_ref->points[j2].p,
laser_sens->points_w[i].p,
c[k].q);
/* Identity matrix */
c[k].C[0][0] = 1;
c[k].C[1][0] = 0;
c[k].C[0][1] = 0;
c[k].C[1][1] = 1;
}
double factor = 1;
/* Scale the correspondence weight by a factor concerning the
information in this reading. */
if(params->use_ml_weights) {
int have_alpha = 0;
double alpha = 0;
if(!is_nan(laser_ref->true_alpha[j1])) {
alpha = laser_ref->true_alpha[j1];
have_alpha = 1;
} else if(laser_ref->alpha_valid[j1]) {
alpha = laser_ref->alpha[j1];;
have_alpha = 1;
} else have_alpha = 0;
if(have_alpha) {
double pose_theta = x_old[2];
/** Incidence of the ray
Note that alpha is relative to the first scan (not the world)
and that pose_theta is the angle of the second scan with
respect to the first, hence it's ok. */
double beta = alpha - (pose_theta + laser_sens->theta[i]);
factor = 1 / square(cos(beta));
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]."
"Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");
sm_error("Writing laser_ref: \n");
ld_write_as_json(laser_ref, stderr);
warned_before = 1;
}
}
}
/* Weight the points by the sigma in laser_sens */
if(params->use_sigma_weights) {
if(!is_nan(laser_sens->readings_sigma[i])) {
factor *= 1 / square(laser_sens->readings_sigma[i]);
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
sm_error("Writing laser_sens: \n");
ld_write_as_json(laser_sens, stderr);
}
}
}
c[k].C[0][0] *= factor;
c[k].C[1][0] *= factor;
c[k].C[0][1] *= factor;
c[k].C[1][1] *= factor;
k++;
}
/* TODO: use prior for odometry */
double std = 0.11;
const double inv_cov_x0[9] =
{1/(std*std), 0, 0,
0, 1/(std*std), 0,
0, 0, 0};
int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
if(!ok) {
sm_error("gpc_solve_valid failed\n");
return 0;
}
double old_error = gpc_total_error(c, k, x_old);
double new_error = gpc_total_error(c, k, x_new);
sm_debug("\tcompute_next_estimate: old error: %f x_old= %s \n", old_error, friendly_pose(x_old));
sm_debug("\tcompute_next_estimate: new error: %f x_new= %s \n", new_error, friendly_pose(x_new));
sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);
double epsilon = 0.000001;
if(new_error > old_error + epsilon) {
sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf new error: %lf x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
}
return 1;
}
