int scanMatchingLumilios(Tsc *sistemaSalida, Tscan *ptosNew, Tscan *ptosRef,
Tsc *estimacion, TSMparams *params) {
Tsc solucion;
static Tscan ptosNewRef;
static TLines lin;
static TAsoc cp_associations[361];
static TAsoc mr_associations[361];
/* static TAsoc cp_associations2[361]; */
/* static TAsoc mr_associations2[361]; */
int cntAssociationsR = 0;
int cntAssociationsT = 0;
int cntAssociations = 0;
Tsc estim_mr;
Tsc estim_cp;
int iter, L, R, Io;
float dist;
float cp_ass_ptX, cp_ass_ptY, cp_ass_ptD;
Tpfp tmp_mr_ind;
float theta_int, dist_k1, interx, intery, a1, b1, c1, a2, b2, c2, den;
float tmp_cp_indX, tmp_cp_indY, tmp_cp_indD, error1, error2;
Tpfp mr_ass_pt;
Tpf mr;
float mediaR, mediaT;
for (int i = 0; i < ptosRef->numPuntos; i++)
car2pol(&ptosRef->laserC[i], &ptosRef->laserP[i]);
/* Initialise solution with the a priori estimation */
solucion.x = estimacion->x;
solucion.y = estimacion->y;
solucion.tita = estimacion->tita;
ptosNewRef.numPuntos = ptosNew->numPuntos;
for (int i = 0; i < ptosNew->numPuntos; i++) {
transfor_directa_p(ptosNew->laserC[i].x, ptosNew->laserC[i].y,
estimacion, &ptosNewRef.laserC[i]);
car2pol(&ptosNewRef.laserC[i], &ptosNewRef.laserP[i]); /** ojo **/
}
/* Filtro de proyeccion */
/* elimina los puntos que no puedes ver */
/* Ademas ordena los datos segun el angulo */
int cnt = 1; /* ojo con la inicializacion del filtro a der o izq (si rot>90) */
for (int i = 1; i < ptosNew->numPuntos; i++) {
if (ptosNewRef.laserP[i].t >= ptosNewRef.laserP[i - 1].t) { /* punto visible */
ptosNewRef.laserP[cnt] = ptosNewRef.laserP[i];
ptosNewRef.laserC[cnt] = ptosNewRef.laserC[i];
cnt++;
}
}
ptosNewRef.numPuntos = cnt;
/* TBI filtrar para puntos muy cercanos */ /* !!!! Ayudara !!!*/
/* TBI filtro de resampleado */
/* Creamos las lineas entre puntos */
computeLines(ptosRef, &lin);
/* Bucle de iteracion hasta convergencia */
iter = 0;
while (iter <= params->MaxIter) {
/* printf("%d\n"); */
/* Alineamos indices */
L = 0;
R = 0; /* indices de la ventana para recorrer ptoRef */
Io = 0; /* indices para recorrer ptoNewRef */
if (ptosNewRef.laserP[Io].t < ptosRef->laserP[L].t) /* elimino de los nuevos */
if (ptosNewRef.laserP[Io].t + params->Bw < ptosRef->laserP[L].t)
while (Io < ptosNewRef.numPuntos - 1 && ptosNewRef.laserP[Io].t
+ params->Bw < ptosRef->laserP[L].t)
Io++;
else
while (R < ptosRef->numPuntos - 1 && ptosNewRef.laserP[Io].t
+ params->Bw > ptosRef->laserP[R + 1].t)
R++;
else {
while (L < ptosRef->numPuntos - 1 && ptosNewRef.laserP[Io].t
- params->Bw > ptosRef->laserP[L].t)
L++;
R = L;
while (R < ptosRef->numPuntos - 1 && ptosNewRef.laserP[Io].t
+ params->Bw > ptosRef->laserP[R + 1].t)
R++;
}
/**********************************/
/* Look for correspondences */
/**********************************/
cnt = 0;
for (int i = Io; i < ptosNewRef.numPuntos; i++) {
while (L < ptosRef->numPuntos - 1 && ptosNewRef.laserP[i].t
- params->Bw > ptosRef->laserP[L].t)
L = L + 1;
while (R < ptosRef->numPuntos - 1 && ptosNewRef.laserP[i].t
+ params->Bw > ptosRef->laserP[R + 1].t)
R = R + 1;
if (L == R) {
dist = (ptosNewRef.laserC[i].x - ptosRef->laserC[R].x)
* (ptosNewRef.laserC[i].x - ptosRef->laserC[R].x)
+ (ptosNewRef.laserC[i].y - ptosRef->laserC[R].y)
* (ptosNewRef.laserC[i].y
- ptosRef->laserC[R].y);
if (dist < params->Br) {
mr_associations[cnt].nx = ptosNewRef.laserC[i].x;
mr_associations[cnt].ny = ptosNewRef.laserC[i].y;
mr_associations[cnt].rx = ptosRef->laserC[R].x;
mr_associations[cnt].ry = ptosRef->laserC[R].y;
mr_associations[cnt].dist = dist;
cp_associations[cnt].nx = ptosNewRef.laserC[i].x;
cp_associations[cnt].ny = ptosNewRef.laserC[i].y;
cp_associations[cnt].rx = ptosRef->laserC[R].x;
cp_associations[cnt].ry = ptosRef->laserC[R].y;
cp_associations[cnt].dist = dist;
cnt++;
}
}
else if (L < R) {
mr_ass_pt.t = 0;
mr_ass_pt.r = 100000;/* % [theta ro] */
cp_ass_ptX = 0;
cp_ass_ptY = 0;
cp_ass_ptD = 100000;
/* Look for the associated point */
/* ------------------------------ */
/* Matching-Range rule */
for (int J = L + 1; J <= R; J++) {
if ((ptosRef->laserP[J - 1].r <= ptosNewRef.laserP[i].r
&& ptosRef->laserP[J].r <= ptosNewRef.laserP[i].r)
|| (ptosRef->laserP[J - 1].r
>= ptosNewRef.laserP[i].r
&& ptosRef->laserP[J].r
>= ptosNewRef.laserP[i].r))
if (fabs(ptosRef->laserP[J - 1].r
- ptosNewRef.laserP[i].r) < fabs(
ptosRef->laserP[J].r - ptosNewRef.laserP[i].r))
tmp_mr_ind = ptosRef->laserP[J - 1];
else
tmp_mr_ind = ptosRef->laserP[J];
else {
theta_int = (ptosNewRef.laserP[i].r
* (ptosRef->laserP[J - 1].r * ptosRef->laserP[J
- 1].t - ptosRef->laserP[J].r
* ptosRef->laserP[J].t)
+ ptosRef->laserP[J - 1].r
* ptosRef->laserP[J].r
* (ptosRef->laserP[J].t
- ptosRef->laserP[J - 1].t))
/ (ptosNewRef.laserP[i].r * (ptosRef->laserP[J
- 1].r - ptosRef->laserP[J].r));
tmp_mr_ind.t = theta_int;
tmp_mr_ind.r = ptosNewRef.laserP[i].r;
}
if ((fabs(tmp_mr_ind.r - ptosNewRef.laserP[i].r) < fabs(
mr_ass_pt.r - ptosNewRef.laserP[i].r))
|| (((int) (0.5F + 1000.0F * fabs(tmp_mr_ind.r
- ptosNewRef.laserP[i].r)) == (int) (0.5F
+ 1000.0F * fabs(mr_ass_pt.r
- ptosNewRef.laserP[i].r)))
&& (fabs(tmp_mr_ind.t
- ptosNewRef.laserP[i].t) < fabs(
mr_ass_pt.t
- ptosNewRef.laserP[i].t)))) {
mr_ass_pt.t = tmp_mr_ind.t;
mr_ass_pt.r = tmp_mr_ind.r;
}
}
dist_k1 = pow2(ptosNewRef.laserC[i].x-ptosRef->laserC[L].x)
+ pow2(ptosNewRef.laserC[i].y-ptosRef->laserC[L].y);
/* Closest point rule */
for (int J = L + 1; J <= R; J++) {
dist
= pow2(ptosNewRef.laserC[i].x-ptosRef->laserC[J].x)
+ pow2(ptosNewRef.laserC[i].y-ptosRef->laserC[J].y);
if (lin.lines[J - 1].B == 0) {
interx = ptosRef->laserC[J].x;
intery = ptosNewRef.laserC[i].y;
} else {
a1 = lin.lines[J - 1].A;
b1 = lin.lines[J - 1].B;
c1 = lin.lines[J - 1].C;
if (ptosRef->laserC[J].y == ptosRef->laserC[J - 1].y) {
a2 = 1;
b2 = 0;
c2 = ptosRef->laserC[J].y;
interx = ptosNewRef.laserC[i].x;
intery = ptosRef->laserC[J].y;
} else {
a2 = -1 / a1;
b2 = -1;
c2 = -a2 * ptosNewRef.laserC[i].x
+ ptosNewRef.laserC[i].y;
den = -a1 + a2;
interx = (-c2 + c1) / den;
intery = (c1 * a2 - c2 * a1) / den;
}
}
if (((interx < ptosRef->laserC[J].x && interx
> ptosRef->laserC[J - 1].x) || (interx
> ptosRef->laserC[J].x && interx
< ptosRef->laserC[J - 1].x)) && ((intery
< ptosRef->laserC[J].y && intery
> ptosRef->laserC[J - 1].y) || (intery
> ptosRef->laserC[J].y && intery
< ptosRef->laserC[J - 1].y))) {
/* take the intersection point between the two straight lines */
dist = pow2(ptosNewRef.laserC[i].x-interx)
+ pow2(ptosNewRef.laserC[i].y-intery);
tmp_cp_indX = interx;
tmp_cp_indY = intery;
tmp_cp_indD = dist;
} else {
/* take the nearest end-point of the segment */
if (dist < dist_k1) {
tmp_cp_indX = ptosRef->laserC[J].x;
tmp_cp_indY = ptosRef->laserC[J].y;
tmp_cp_indD = dist;
} else {
tmp_cp_indX = ptosRef->laserC[J - 1].x;
tmp_cp_indY = ptosRef->laserC[J - 1].y;
tmp_cp_indD = dist_k1;
}
}
if (tmp_cp_indD < cp_ass_ptD) {
cp_ass_ptX = tmp_cp_indX;
cp_ass_ptY = tmp_cp_indY;
cp_ass_ptD = tmp_cp_indD;
}
}
pol2car(&mr_ass_pt, &mr);
dist = pow2(ptosNewRef.laserC[i].x-mr.x)
+ pow2(ptosNewRef.laserC[i].y-mr.y);
if (dist < params->Br || cp_ass_ptD < params->Br) {
mr_associations[cnt].nx = ptosNewRef.laserC[i].x;
mr_associations[cnt].ny = ptosNewRef.laserC[i].y;
mr_associations[cnt].rx = mr.x;
mr_associations[cnt].ry = mr.y;
mr_associations[cnt].dist = dist;
cp_associations[cnt].nx = ptosNewRef.laserC[i].x;
cp_associations[cnt].ny = ptosNewRef.laserC[i].y;
cp_associations[cnt].rx = cp_ass_ptX;
cp_associations[cnt].ry = cp_ass_ptY;
cp_associations[cnt].dist = cp_ass_ptD;
cnt++;
}
}
}
cntAssociations = cnt;
/* --------------------- */
/* ----- Resampling ---- */
/* --------------------- */
mediaT = 0;
mediaR = 0;
for (int i = 0; i < cntAssociations; i++) {
mediaT = mediaT + cp_associations[i].dist;
mediaR = mediaR + mr_associations[i].dist;
}
mediaT = mediaT / cntAssociations;
mediaR = mediaR / cntAssociations;
cntAssociationsT = 0;
cntAssociationsR = 0;
mediaT = mediaT * 3 / 2; // ojo !!
mediaR = mediaR * 3 / 2;
for (int i = 0; i < cntAssociations; i++) {
if (cp_associations[i].dist < mediaT) {
cp_associations[cntAssociationsT] = cp_associations[i];
cntAssociationsT++;
}
if (mr_associations[i].dist < mediaR) {
mr_associations[cntAssociationsR] = mr_associations[i];
cntAssociationsR++;
}
}
// printf("<asocT,asocR> = <%d,%d>\n",cntAssociationsT,cntAssociationsR);
/* --------------------- */
/* ---------MINIMOS ------------ */
/* --------------------- */
/* motionLMS(cp_associations2,cntAssociationsT,&estim_cp); */
/* motionLMS(mr_associations2,cntAssociationsR,&estim_mr); */
motionLMS(cp_associations, cntAssociationsT, &estim_cp);
motionLMS(mr_associations, cntAssociationsR, &estim_mr);
error1 = fabs(estim_cp.x) + fabs(estim_cp.y);
error2 = fabs(estim_mr.tita);
//printf("<eT,eR>=<%f,%f>\n",error1,error2);
if (error1 < params->error_r && error2 < params->error_th)
break;
solucion.x = solucion.x + estim_cp.x;
solucion.y = solucion.y + estim_cp.y;
solucion.tita = solucion.tita + estim_mr.tita;
for (int i = 0; i < ptosNew->numPuntos; i++) {
transfor_directa_p(ptosNew->laserC[i].x, ptosNew->laserC[i].y,
&solucion, &ptosNewRef.laserC[i]);
car2pol(&ptosNewRef.laserC[i], &ptosNewRef.laserP[i]); /** ojo **/
}
iter++;
}
*sistemaSalida = solucion;
if (iter < params->MaxIter)
return 1;
else {
/* printf("Ey! Er=<%f,%f>\n",error1,error2); */
return 0;
}
}
static int EStep()
{
int cnt;
int i,J;
static Tscan ptosNewRef;
static int indexPtosNewRef[MAXLASERPOINTS];
int L,R,Io;
float dist;
float cp_ass_ptX,cp_ass_ptY,cp_ass_ptD;
float tmp_cp_indD;
float q1x, q1y, q2x,q2y,p2x,p2y, dqx, dqy, dqpx, dqpy, qx, qy,dx,dy;
float landaMin;
float A,B,C,D;
float LMET2;
LMET2=params.LMET*params.LMET;
// Transform the points according to the current pose estimation
ptosNewRef.numPuntos=0;
for (i=0; i<ptosNew.numPuntos; i++){
transfor_directa_p ( ptosNew.laserC[i].x, ptosNew.laserC[i].y,
&motion2, &ptosNewRef.laserC[ptosNewRef.numPuntos]);
car2pol(&ptosNewRef.laserC[ptosNewRef.numPuntos],&ptosNewRef.laserP[ptosNewRef.numPuntos]);
ptosNewRef.numPuntos++;
}
// ----
/* Projection Filter */
/* Eliminate the points that cannot be seen */
/* Furthermore it orders the points with the angle */
cnt = 1; /* Becarefull with this filter (order) when the angles are big >90 */
ptosNoView.numPuntos=0;
if (params.ProjectionFilter==1){
for (i=1;i<ptosNewRef.numPuntos;i++){
if (ptosNewRef.laserP[i].t>=ptosNewRef.laserP[cnt-1].t){
ptosNewRef.laserP[cnt]=ptosNewRef.laserP[i];
ptosNewRef.laserC[cnt]=ptosNewRef.laserC[i];
cnt++;
}
else{
ptosNoView.laserP[ptosNoView.numPuntos]=ptosNewRef.laserP[i];
ptosNoView.laserC[ptosNoView.numPuntos]=ptosNewRef.laserC[i];
ptosNoView.numPuntos++;
}
}
ptosNewRef.numPuntos=cnt;
}
// ----
/* Build the index for the windows (this is the role of the Bw parameter */
/* The correspondences are searched between windows in both scans */
/* Like this you speed up the algorithm */
L=0; R=0; /* index of the window for ptoRef */
Io=0; /* index of the window for ptoNewRef */
if (ptosNewRef.laserP[Io].t<ptosRef.laserP[L].t) {
if (ptosNewRef.laserP[Io].t + params.Bw < ptosRef.laserP[L].t){
while (Io<ptosNewRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw < ptosRef.laserP[L].t) {
Io++;
}
}
else{
while (R<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw > ptosRef.laserP[R+1].t)
R++;
}
}
else{
while (L<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t - params.Bw > ptosRef.laserP[L].t)
L++;
R=L;
while (R<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw > ptosRef.laserP[R+1].t)
R++;
}
// ----
/* Look for potential correspondences between the scans */
/* Here is where we use the windows */
cnt=0;
for (i=Io;i<ptosNewRef.numPuntos;i++){
// Keep the index of the original scan ordering
cp_associations[cnt].index=indexPtosNewRef[i];
// Move the window
while (L < ptosRef.numPuntos-1 && ptosNewRef.laserP[i].t - params.Bw > ptosRef.laserP[L].t)
L = L + 1;
while (R <ptosRef.numPuntos-1 && ptosNewRef.laserP[i].t + params.Bw > ptosRef.laserP[R+1].t)
R = R + 1;
cp_associations[cnt].L=L;
cp_associations[cnt].R=R;
if (L==R){
// Just one possible correspondence
// precompute stuff to speed up
qx=ptosRef.laserC[R].x; qy=ptosRef.laserC[R].y;
p2x=ptosNewRef.laserC[i].x; p2y=ptosNewRef.laserC[i].y;
dx=p2x-qx; dy=p2y-qy;
dist=dx*dx+dy*dy-(dx*qy-dy*qx)*(dx*qy-dy*qx)/(qx*qx+qy*qy+LMET2);
if (dist<params.Br){
cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
cp_associations[cnt].rx=ptosRef.laserC[R].x;
cp_associations[cnt].ry=ptosRef.laserC[R].y;
cp_associations[cnt].dist=dist;
cnt++;
}
}
else if (L<R)
{
// More possible correspondences
cp_ass_ptX=0;
cp_ass_ptY=0;
cp_ass_ptD=100000;
/* Metric based Closest point rule */
for (J=L+1;J<=R;J++){
// Precompute stuff to speed up
q1x=ptosRef.laserC[J-1].x; q1y=ptosRef.laserC[J-1].y;
q2x=ptosRef.laserC[J].x; q2y=ptosRef.laserC[J].y;
p2x=ptosNewRef.laserC[i].x; p2y=ptosNewRef.laserC[i].y;
dqx=refdqx[J-1]; dqy=refdqy[J-1];
dqpx=q1x-p2x; dqpy=q1y-p2y;
A=1/(p2x*p2x+p2y*p2y+LMET2);
B=(1-A*p2y*p2y);
C=(1-A*p2x*p2x);
D=A*p2x*p2y;
landaMin=(D*(dqx*dqpy+dqy*dqpx)+B*dqx*dqpx+C*dqy*dqpy)/(B*refdqx2[J-1]+C*refdqy2[J-1]+2*D*refdqxdqy[J-1]);
if (landaMin<0){ // Out of the segment on one side
qx=q1x; qy=q1y;}
else if (landaMin>1){ // Out of the segment on the other side
qx=q2x; qy=q2y;}
else if (distref[J-1]<params.MaxDistInter) { // Within the segment and interpotation OK
qx=(1-landaMin)*q1x+landaMin*q2x;
qy=(1-landaMin)*q1y+landaMin*q2y;
}
else{ // Segment too big do not interpolate
if (landaMin<0.5){
qx=q1x; qy=q1y;}
else{
qx=q2x; qy=q2y;}
}
// Precompute stuff to see if we save the association
dx=p2x-qx;
dy=p2y-qy;
tmp_cp_indD=dx*dx+dy*dy-(dx*qy-dy*qx)*(dx*qy-dy*qx)/(qx*qx+qy*qy+LMET2);
// Check if the association is the best up to now
if (tmp_cp_indD < cp_ass_ptD){
cp_ass_ptX=qx;
cp_ass_ptY=qy;
cp_ass_ptD=tmp_cp_indD;
}
}
// Association compatible in distance (Br parameter)
if (cp_ass_ptD< params.Br){
// 20110620: had some problems with these, so I commented them out
cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
cp_associations[cnt].rx=cp_ass_ptX;
cp_associations[cnt].ry=cp_ass_ptY;
cp_associations[cnt].dist=cp_ass_ptD;
cnt++;
}
}
else { // This cannot happen but just in case ...
cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
cp_associations[cnt].rx=0;
cp_associations[cnt].ry=0;
cp_associations[cnt].dist=params.Br;
cnt++;
}
} // End for (i=Io;i<ptosNewRef.numPuntos;i++){
cntAssociationsT=cnt;
// Check if the number of associations is ok
if (cntAssociationsT<ptosNewRef.numPuntos*params.AsocError){
#ifdef INTMATSM_DEB
printf("Number of associations too low <%d out of %f>\n",
cntAssociationsT,ptosNewRef.numPuntos*params.AsocError);
#endif
return 0;
}
return 1;
}
