void Configuration::begin() { size = computeLines(); Serial.printf("size: %d\n", size); if (size > 0) { properties = new pair[size]; File f = SPIFFS.open(fileName, "r"); if (f) { for (int i = 0; i < size; i++) { String line = f.readStringUntil('\n'); int ind = line.indexOf('='); if (ind != -1) { properties[i].key = line.substring(0, ind); properties[i].key.trim(); properties[i].value = line.substring(ind + 1); properties[i].value.trim(); Serial.printf("read from file: |%s=%s|\n", properties[i].key.c_str(), properties[i].value.c_str()); } } f.close(); } } }
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; } }