From b7db61e1b9d36cd79eae65a97e058f6d25c403b3 Mon Sep 17 00:00:00 2001 From: Matthew McGill Date: Mon, 27 May 2013 18:16:02 +1000 Subject: [PATCH] Importing crosbot_mbicp --- crosbot_mbicp/CMakeLists.txt | 122 +++ crosbot_mbicp/Makefile | 5 + crosbot_mbicp/package.xml | 62 ++ crosbot_mbicp/src/mbicpStd.cpp | 1155 +++++++++++++++++++++++++++++ crosbot_mbicp/src/mbicpStd.h | 361 +++++++++ crosbot_mbicp/src/nodes/mbicp.cpp | 305 ++++++++ 6 files changed, 2010 insertions(+) create mode 100644 crosbot_mbicp/CMakeLists.txt create mode 100644 crosbot_mbicp/Makefile create mode 100644 crosbot_mbicp/package.xml create mode 100644 crosbot_mbicp/src/mbicpStd.cpp create mode 100644 crosbot_mbicp/src/mbicpStd.h create mode 100644 crosbot_mbicp/src/nodes/mbicp.cpp diff --git a/crosbot_mbicp/CMakeLists.txt b/crosbot_mbicp/CMakeLists.txt new file mode 100644 index 0000000..04d38d2 --- /dev/null +++ b/crosbot_mbicp/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 2.8.3) +project(crosbot_mbicp) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS crosbot roscpp tf sensor_msgs) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and scripts declared therein get installed +# catkin_python_setup() + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################### +## Declare things to be passed to other projects ## +################################################### + +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES crosbot_mbicp +# CATKIN_DEPENDS crosbot roscpp tf +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(crosbot_mbicp +# src/${PROJECT_NAME}/crosbot_mbicp.cpp +# ) + +## Declare a cpp executable +add_executable(mbicp src/nodes/mbicp.cpp src/mbicpStd.cpp) + +## Add dependencies to the executable +add_dependencies(mbicp ${PROJECT_NAME} roscpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(mbicp + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +## Mark executable scripts (Python etc.) for installation +## not required for python when using catkin_python_setup() +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS mbicp + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_crosbot_mbicp.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/crosbot_mbicp/Makefile b/crosbot_mbicp/Makefile new file mode 100644 index 0000000..5004622 --- /dev/null +++ b/crosbot_mbicp/Makefile @@ -0,0 +1,5 @@ +default: all + +all: + make -C ../.. + diff --git a/crosbot_mbicp/package.xml b/crosbot_mbicp/package.xml new file mode 100644 index 0000000..52d0c63 --- /dev/null +++ b/crosbot_mbicp/package.xml @@ -0,0 +1,62 @@ + + + crosbot_mbicp + 0.0.0 + The crosbot_mbicp package + + + + + mmcgill + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + crosbot + roscpp + tf + sensor_msgs + + crosbot + roscpp + tf + sensor_msgs + + + + + + + + + + + \ No newline at end of file diff --git a/crosbot_mbicp/src/mbicpStd.cpp b/crosbot_mbicp/src/mbicpStd.cpp new file mode 100644 index 0000000..7032f81 --- /dev/null +++ b/crosbot_mbicp/src/mbicpStd.cpp @@ -0,0 +1,1155 @@ +#include "mbicpStd.h" +#include +#include +#include + +/*************************************************************************************/ +/* */ +/* File: MbICP.h */ +/* Authors: Luis Montesano and Javier Minguez */ +/* Modified: 1/3/2006 */ +/* */ +/* This library implements the: */ +/* */ +/* J. Minguez, F. Lamiraux and L. Montesano */ +/* Metric-Based Iterative Closest Point, */ +/* Scan Matching for Mobile Robot Displacement Estimation */ +/* IEEE Transactions on Roboticics (2006) */ +/* */ +/*************************************************************************************/ + + +// #include "MbICP.h" +// #include "MBICP/MbICP2.h" +// #include "MBICP/calcul.h" +// #include "MBICP/sp_matrix.h" +#include +#include +// #include "MBICP/percolate.h" + +namespace mbicp +{ + +// Initial error to compute error ratio +#define BIG_INITIAL_ERROR 1000000.0F + +#define MROWS(m) ((m).rows) +#define MCOLS(m) ((m).cols) +#define MDATA(m,i,j) ((m).data[i][j]) + +#define VELEMENTS(v) ((v).elements) +#define VDATA(v,i) ((v).data[i]) + +#define M_SQUARE(m) ((m).rows == (m).cols) +#define M_COMPAT_DIM(m, n) ((m).cols == (n).rows) +#define M_EQUAL_DIM(m, n) (((m).rows == (n).rows) && ((m).cols == (n).cols)) +#define V_EQUAL_DIM(v, w) (((v).elements == (w).elements)) +#define MV_COMPAT_DIM(m, v) ((m).cols == (v).elements) + +#define FIRST(b) ((b).mat[0]) +#define SECOND(b) ((b).mat[1]) +#define THIRD(b) ((b).mat[2]) +#define RANGE(b) ((b).range) + +#define SQUARE(x) ((x)*(x)) + +// Debugging flag. Print sm info in the screen. +// #define INTMATSM_DEB + +// ************************ +// Function that initializes the SM parameters +// ************************ + +void MBICPStd::Init_MbICP_ScanMatching(float max_laser_range,float Bw, float Br, + float L, int laserStep, + float MaxDistInter, + float filter, + int ProjectionFilter, + float AsocError, + int MaxIter, float error_ratio, + float error_x, float error_y, float error_t, int IterSmoothConv, + float xmin, float xmax, float ymin, float ymax){ + + #ifdef INTMATSM_DEB + printf("-- Init EM params . . "); + #endif + + MAXLASERRANGE = max_laser_range; + MINLASERRANGE = 0.4; + params.Bw = Bw; + params.Br = Br*Br; + params.error_th=error_ratio; + params.MaxIter=MaxIter; + params.LMET=L; + params.laserStep=laserStep; + params.MaxDistInter=MaxDistInter; + params.filter=filter; + params.ProjectionFilter=ProjectionFilter; + params.AsocError=AsocError; + params.errx_out=error_x; + params.erry_out=error_y; + params.errt_out=error_t; + params.IterSmoothConv=IterSmoothConv; + params.xmin = xmin; + params.xmax = xmax; + params.ymin = ymin; + params.ymax = ymax; + xmin_ = xmin; + xmax_ = xmax; + ymin_ = ymin; + ymax_ = ymax; + + useAdvanced_ = false; + + #ifdef INTMATSM_DEB + printf(". OK!\n"); + #endif + +} + + +// ************************ +// Function that initializes the SM parameters +// ************************ + +int MBICPStd::MbICPmatcher(Tpfp *laserK, Tpfp *laserK1, + Tsc *sensorMotion, Tsc *solution){ + double x, y, th; + preProcessingCreate(laserK,laserK1,sensorMotion); + + return MbICPMatchScans(x, y, th); + +} + +int MBICPStd::MbICPMatchScans(double &x, double &y, double &th) { + int resEStep=1; + int resMStep=1; + int numIteration=0; + Tsc solution; + +// motion2.x = 0.0; +// motion2.y = 0.0; + motion2.tita = 0.0; + + // Preprocess both scans + preProcessingCalc(); + x = 0.0; + y = 0.0; + th = 0.0; + solution.x = 0.0; + solution.y = 0.0; + solution.tita = 0.0; + + while (numIteration90 */ + ptosNoView.numPuntos=0; + if (params.ProjectionFilter==1){ + for (i=1;i=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; + } +// printf("projection: %d\n",ptosNewRef.numPuntos); + + // ---- + /* 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[R+1].t) + R++; + } + } + else{ + while (L ptosRef.laserP[L].t) + L++; + R=L; + while (R 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 ptosRef.laserP[L].t) + L = L + 1; + while (R 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 (dist1){ // Out of the segment on the other side + qx=q2x; qy=q2y;} + else if (distref[J-1]\n", cntAssociationsT,ptosNewRef.numPuntos, 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; +} + + +// ************************ +// Function that does the minimization step of the MbICP +// ************************ + +int MBICPStd::MStep(Tsc *solucion){ + + Tsc estim_cp; + int i,cnt,res; + float error_ratio, error; + float cosw, sinw, dtx, dty, tmp1, tmp2; + static TAsoc cp_tmp[MAXLASERPOINTS+1]; + + // Filtering of the spurious data + // Used the trimmed versions that orders the point by distance between associations + + if (params.filter<1){ + + // Add Null element in array position 0 (this is because heapsort requirement) + for (i=0;i\n",estim_cp.x, estim_cp.y,estim_cp.tita); + printf("New impl: <%f %f %f>\n",estim_cp.x, estim_cp.y,estim_cp.tita); + #endif + + cosw=(float)cos(estim_cp.tita); sinw=(float)sin(estim_cp.tita); + dtx=estim_cp.x; dty=estim_cp.y; + + + // ------ + /* Compute the error of the associations */ + + error=0; + for (i = 0; i=<%f,%f,%f>\n estim=<%f,%f,%f>\n", + error,error_k1,error_ratio, estim_cp.x,estim_cp.y, estim_cp.tita); + #endif + + // ---- + /* Check the exit criteria */ + /* Error ratio */ + if (fabs(1.0-error_ratio)<=params.error_th || + (fabs(estim_cp.x)params.IterSmoothConv) + return 1; + else + return 0; +} + + +// ************************ +// Function to do the least-squares but optimized for the metric +// ************************ + +int MBICPStd::computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion) { + + int i; + float fac = 1.0; + float Ltmp; + float LMETRICA2; + float X1[MAXLASERPOINTS], Y1[MAXLASERPOINTS]; + float X2[MAXLASERPOINTS],Y2[MAXLASERPOINTS]; + float X2Y2[MAXLASERPOINTS],X1X2[MAXLASERPOINTS]; + float X1Y2[MAXLASERPOINTS], Y1X2[MAXLASERPOINTS]; + float Y1Y2[MAXLASERPOINTS]; + float K[MAXLASERPOINTS], DS[MAXLASERPOINTS]; + float DsD[MAXLASERPOINTS], X2DsD[MAXLASERPOINTS], Y2DsD[MAXLASERPOINTS]; + float Bs[MAXLASERPOINTS], BsD[MAXLASERPOINTS]; + float A1, A2, A3, B1, B2, B3, C1, C2, C3, D1, D2, D3; + MATRIX matA,invMatA; + VECTOR vecB,vecSol; + + A1=0;A2=0;A3=0;B1=0;B2=0;B3=0; + C1=0;C2=0;C3=0;D1=0;D2=0;D3=0; + + + LMETRICA2=params.LMET*params.LMET; + + for (i=0; i 4.0) fac = 4.0; +// Ltmp = params.LMET * (6.0 - fac) / 6.0; +// Ltmp = 2.0 * params.LMET * (fac) / 4.0; +// LMETRICA2 = Ltmp*Ltmp; + +// fac = 1.0; + } + + K[i]=X2[i]+Y2[i] + LMETRICA2; + DS[i]=Y1Y2[i] + X1X2[i]; + DsD[i]=DS[i]/K[i]; + X2DsD[i]=cp_ass[i].rx*DsD[i]; + Y2DsD[i]=cp_ass[i].ry*DsD[i]; + + Bs[i]=X1Y2[i]-Y1X2[i]; + BsD[i]=Bs[i]/K[i]; + + A1=A1 + fac * (1-Y2[i]/K[i]); + B1=B1 + fac * X2Y2[i]/K[i]; + C1=C1 + fac * (-cp_ass[i].ny + Y2DsD[i]); + D1=D1 + fac * (cp_ass[i].nx - cp_ass[i].rx -cp_ass[i].ry*BsD[i]); + + A2=B1; + B2=B2 + fac * (1-X2[i]/K[i]); + C2=C2 + fac * (cp_ass[i].nx-X2DsD[i]); + D2=D2 + fac * (cp_ass[i].ny -cp_ass[i].ry +cp_ass[i].rx*BsD[i]); + + A3=C1; + B3=C2; + C3=C3 + fac * (X1[i] + Y1[i] - DS[i]*DS[i]/K[i]); + D3=D3 + fac * (Bs[i]*(-1+DsD[i])); + } + + + initialize_matrix(&matA,3,3); + MDATA(matA,0,0)=A1; MDATA(matA,0,1)=B1; MDATA(matA,0,2)=C1; + MDATA(matA,1,0)=A2; MDATA(matA,1,1)=B2; MDATA(matA,1,2)=C2; + MDATA(matA,2,0)=A3; MDATA(matA,2,1)=B3; MDATA(matA,2,2)=C3; + + if (inverse_matrix (&matA, &invMatA)==-1) + return -1; + +#ifdef INTMATSM_DEB + print_matrix("inverted matrix", &invMatA); +#endif + + initialize_vector(&vecB,3); + VDATA(vecB,0)=D1; VDATA(vecB,1)=D2; VDATA(vecB,2)=D3; + multiply_matrix_vector (&invMatA, &vecB, &vecSol); + + estimacion->x=-VDATA(vecSol,0); + estimacion->y=-VDATA(vecSol,1); + estimacion->tita=-VDATA(vecSol,2); + + return 1; +} + + +void MBICPStd::preProcessingCreate(Tpfp *laserK, Tpfp *laserK1, + Tsc *initialMotion) +{ + int i; + double tx, ty; + + // ------------------------------------------------// + // Compute xy coordinates of the points in laserK1 + ptosNew.numPuntos=0; + for (i=0; itita); + CosT=(float)cos(sistema->tita); + + sol->x=x*CosT-y*SinT+sistema->x; + sol->y=x*SinT+y*CosT+sistema->y; + + //fprintf(stderr,"input:<%f,%f> sis:<%f %f %f> sol:<%f %f>\n",x,y,sistema->x, sistema->y, sistema->tita,sol->x, sol->y); + +} + +void MBICPStd::transfor_directa_pt0(float x, float y, + Tsc *sistema, Tpf *sol){ + + /* Esta funcion transforma el punto x,y en el sistema de coordenadas mas global*/ + /* Es decir las coordenadas x y son vistas desde el sistema de coordenadas sistema*/ + /* Y se las quiere transformar en el sistema de ref desde el que se sistema*/ + /* Es la transformacion directa */ + + sol->x=x+sistema->x; + sol->y=y+sistema->y; + +} + + +void MBICPStd::transfor_inversa_p(float x,float y, + Tsc *sistema, Tpf *sol){ + + /* Esta funcion transforma el punto x,y en el sistema de coordenadas que entra*/ + /* Las coordenadas x y se ven desde el sistema de coordenadas desde el que se tienen las */ + /* las coordenadas de sistema */ + /* Es la transformacion directa */ + + float a13, a23; + float SinT,CosT; + + SinT=(float)sin(sistema->tita); + CosT=(float)cos(sistema->tita); + + + a13=-sistema->y*SinT-sistema->x*CosT; + a23=-sistema->y*CosT+sistema->x*SinT; + + sol->x=x*CosT+y*SinT+a13; + sol->y=-x*SinT+y*CosT+a23; +} + +float MBICPStd::NormalizarPI(float ang){ + + return (float)(ang+(2*M_PI)*floor((M_PI-ang)/(2*M_PI))); +} + +void MBICPStd::inversion_sis(Tsc *sisIn, Tsc *sisOut){ + + float c,s; + + c=(float)cos(sisIn->tita); + s=(float)sin(sisIn->tita); + sisOut->x =-c*sisIn->x-s*sisIn->y; + sisOut->y = s*sisIn->x-c*sisIn->y; + sisOut->tita = NormalizarPI(-sisIn->tita); +} + +void MBICPStd::composicion_sis(Tsc *sis1,Tsc *sis2,Tsc *sisOut){ + + Tpf sol; + + transfor_directa_p(sis2->x, sis2->y, + sis1, &sol); + sisOut->x=sol.x; + sisOut->y=sol.y; + sisOut->tita = NormalizarPI(sis1->tita+sis2->tita); + +} + +void MBICPStd::car2pol(Tpf *in, Tpfp *out){ + + out->r=(float)sqrt(in->x*in->x+in->y*in->y); + out->t=(float)atan2(in->y,in->x); +// out->r=(float)sqrt(in->x*in->x+in->y*in->y); +// out->t=(float)atan2(in->x,in->y); +} + +void MBICPStd::pol2car(Tpfp *in, Tpf *out){ + + out->x=in->r*(float)cos(in->t); + out->y=in->r*(float)sin(in->t); +// out->y=in->r*(float)cos(in->t); +// out->x=in->r*(float)sin(in->t); +} + + + + +int MBICPStd::corte_segmentos(float x1,float y1,float x2,float y2, + float x3,float y3,float x4,float y4, + Tpf *sol){ +/* corte de segmentos */ +/* TE DEVUELVE EL PUNTO DE CORTE EN EL SISTEMA QUE ESTEN LOS SEGMENTOS */ + + float a1,a2,b1,b2,c1,c2,xm,ym,denominador,max1_x,max1_y,min1_x,min1_y; + float xerr,yerr; + int si1; + float error_redondeo; + + error_redondeo=(float)0.00001F; + + /* primera recta */ + a1=y2-y1; + b1=x1-x2; + c1=y1*(-b1)-x1*a1; + + /* segunda recta */ + a2=y4-y3; + b2=x3-x4; + c2=y3*(-b2)-x3*a2; + + + denominador=a1*b2-a2*b1; + if (denominador==0) + return 0; + else{ + xm=(b1*c2-b2*c1)/denominador; + ym=(c1*a2-c2*a1)/denominador; + + xerr=xm+error_redondeo; + yerr=ym+error_redondeo; + + /* Comprobamos que cae entre los segmantos */ + if (x1>x2){ + max1_x=x1; min1_x=x2; + } + else{ + max1_x=x2; min1_x=x1; + } + if (y1>y2){ + max1_y=y1; min1_y=y2; + } + else{ + max1_y=y2; min1_y=y1; + } + si1=0; + if (max1_x+error_redondeo>=xm && xerr>=min1_x && max1_y+error_redondeo>=ym && yerr>=min1_y) + si1=1; + + + if (si1){ + + if (x3>x4){ + max1_x=x3; min1_x=x4; + } + else{ + max1_x=x4; min1_x=x3; + } + if (y3>y4){ + max1_y=y3; min1_y=y4; + } + else{ + max1_y=y4; min1_y=y3; + } + + if (max1_x+error_redondeo>=xm && xerr>=min1_x && max1_y+error_redondeo>=ym && yerr>=min1_y){ + sol->x=xm; + sol->y=ym; + return 1; + } + } + return 0; + } +} + +void MBICPStd::swapItem(TAsoc *a, TAsoc *b){ + TAsoc c; + + c=*a; + *a=*b; + *b=c; +} + +void MBICPStd::perc_down(TAsoc a[], int i, int n) { + int child; TAsoc tmp; + for (tmp=a[i]; i*2 <= n; i=child) { + child = i*2; + if ((child != n) && (a[child+1].dist > a[child].dist)) + child++; + if (tmp.dist < a[child].dist) + a[i] = a[child]; + else + break; + } + a[i] = tmp; +} + +void MBICPStd::heapsort(TAsoc a[], int n) { + int i, j; + j = n; + for (i=n/2; i>0; i--) /* BuildHeap */ + perc_down(a,i,j); + i = 1; + for (j=n; j>=2; j--) { + swapItem(&a[i],&a[j]); /* DeleteMax */ + perc_down(a,i,j-1); + } +} + +/*****************************************************************************/ + MATRIX MBICPStd::create_matrix (int rows, int cols) + +/***************************************************************************** + Creates a MATRIX of dimensions (rows, cols) and initializaes it to zeros. +******************************************************************************/ +{ + MATRIX m; + + MROWS (m) = rows; + MCOLS (m) = cols; + + { + int i, j; + + for (i = 0; i < MROWS (m); i++) + for (j = 0; j < MCOLS (m); j++) + MDATA (m, i, j) = 0; + } + + return m; +} + +/*****************************************************************************/ + void MBICPStd::initialize_matrix (MATRIX *m, int rows, int cols) + +/***************************************************************************** + Initializes a MATRIX to dimensions (rows, cols) and content zeros. +******************************************************************************/ +{ + MROWS (*m) = rows; + MCOLS (*m) = cols; + + { + int i, j; + + for (i = 0; i < MROWS (*m); i++) + for (j = 0; j < MCOLS (*m); j++) + MDATA (*m, i, j) = 0; + } + +} + + +/*****************************************************************************/ + void MBICPStd::print_matrix (const char *message, MATRIX const *m) + +/***************************************************************************** + Print to stdout the contents of MATRIX m. +******************************************************************************/ +{ + int i, j; + + printf ("%s\n",message); + printf("%d %d \n",MROWS (*m),MCOLS (*m)); + if ((MROWS (*m) <= MAX_ROWS) && (MCOLS (*m) <= MAX_COLS)) + for (i = 0; i < MROWS (*m); i++) + { + for (j = 0; j < MCOLS (*m); j++) + printf ("%10.5f ", MDATA (*m, i, j)); + printf ("\n"); + } + else printf ("Dimension incorrecta!"); + printf ("\n"); +} + +/*****************************************************************************/ + VECTOR MBICPStd::create_vector (int elements) + +/***************************************************************************** + Initializes a VECTOR to dimension (elements) and its contents to zeros. +******************************************************************************/ +{ + VECTOR v; + + VELEMENTS (v) = elements; + + { + int i; + + for (i = 0; i < VELEMENTS (v); i++) + VDATA (v, i) = 0; + } + + return v; +} + +/*****************************************************************************/ + void MBICPStd::initialize_vector (VECTOR *v, int elements) + +/***************************************************************************** + Initializes a VECTOR to dimension (elements) and its contents to zeros. +******************************************************************************/ +{ + VELEMENTS (*v) = elements; + + { + int i; + + for (i = 0; i < VELEMENTS (*v); i++) + VDATA (*v, i) = 0; + } +} + +/*****************************************************************************/ + void MBICPStd::print_vector (const char *message, VECTOR const *v) + +/***************************************************************************** + Print to stdout the contents of VECTOR m. +******************************************************************************/ +{ + int i; + + printf ("%s\n",message); + if (VELEMENTS (*v) <= MAX_ROWS) + for (i = 0; i < VELEMENTS (*v); i++) + { + printf ("%f ", VDATA (*v, i)); + printf ("\n"); + } + else printf ("Dimension incorrecta!"); + printf ("\n"); +} + +/*****************************************************************************/ + float MBICPStd::cross_product (MATRIX const *m, int f1, int c1, int f2, int c2) + +/***************************************************************************** +******************************************************************************/ +{ + return MDATA (*m, f1, c1) * MDATA (*m, f2, c2) - MDATA (*m, f1, c2) * MDATA (*m, f2, c1); +} + +/*****************************************************************************/ +int MBICPStd::determinant (MATRIX const *m, float *result) +/***************************************************************************** +******************************************************************************/ +{ + if (!M_SQUARE (*m)) + { + printf ("ERROR (determinant): MATRIX must be square!\n"); + print_matrix ("MATRIX:", m); + return -1; + } + else + { + + if (MROWS (*m) == 1) + *result = MDATA (*m, 0, 0); + else if (MROWS (*m) == 2) + *result = cross_product (m, 0, 0, 1, 1); + else + *result = MDATA (*m, 0, 0) * cross_product (m, 1, 1, 2, 2) + - MDATA (*m, 0, 1) * cross_product (m, 1, 0, 2, 2) + + MDATA (*m, 0, 2) * cross_product (m, 1, 0, 2, 1); + + + return 1; + } +} + +/*****************************************************************************/ + int MBICPStd::inverse_matrix (MATRIX const *m, MATRIX *n) + +/***************************************************************************** +******************************************************************************/ +{ + if (!M_SQUARE (*m)) + { + printf ("ERROR (inverse_matrix): MATRIX must be square!\n"); + print_matrix ("MATRIX:", m); + n->cols=0; n->rows=0; + return -1; + } + else + { + float det; + int res; + + res = determinant (m,&det); + + if (res == -1) + { + printf ("ERROR (inverse_matrix): singular MATRIX!\n"); + print_matrix ("MATRIX:", m); + return -1; + } + else + { + initialize_matrix (n, MROWS (*m), MCOLS (*m)); + if (MROWS (*m) == 1) + { + MDATA (*n, 0, 0) = 1 / det ; + } + else if (MROWS (*m) == 2) + { + MDATA (*n, 0, 0) = MDATA (*m, 1, 1) / det ; + MDATA (*n, 0, 1) = -MDATA (*m, 0, 1) / det ; + MDATA (*n, 1, 0) = -MDATA (*m, 1, 0) / det ; + MDATA (*n, 1, 1) = MDATA (*m, 0, 0) / det ; + } + else + { + MDATA (*n, 0, 0) = cross_product (m, 1, 1, 2, 2) / det ; + MDATA (*n, 0, 1) = -cross_product (m, 0, 1, 2, 2) / det ; + MDATA (*n, 0, 2) = cross_product (m, 0, 1, 1, 2) / det ; + MDATA (*n, 1, 0) = -cross_product (m, 1, 0, 2, 2) / det ; + MDATA (*n, 1, 1) = cross_product (m, 0, 0, 2, 2) / det ; + MDATA (*n, 1, 2) = -cross_product (m, 0, 0, 1, 2) / det ; + MDATA (*n, 2, 0) = cross_product (m, 1, 0, 2, 1) / det ; + MDATA (*n, 2, 1) = -cross_product (m, 0, 0, 2, 1) / det ; + MDATA (*n, 2, 2) = cross_product (m, 0, 0, 1, 1) / det ; + } + } + } + return 1; +} + +/*****************************************************************************/ + int MBICPStd::multiply_matrix_vector (MATRIX const *m, VECTOR const *v, VECTOR *r) + +/***************************************************************************** + Returns the VECTOR-MATRIX product of m and v in r. +******************************************************************************/ +{ + if (! (MV_COMPAT_DIM (*m, *v))) + { + printf ("ERROR (multiply_matrix_vector): MATRIX and VECTOR dimensions incompatible!\n"); + print_matrix ("MATRIX:", m); + print_vector ("VECTOR:", v); + return -1; /*added 1996-07*/ + } + else + { + int i, j; + float datum; + + VELEMENTS (*r) = MROWS (*m); + + for (i = 0; i < MROWS (*m); i++) + { + datum = 0; + for (j = 0; j < VELEMENTS (*v); j++) + datum = datum + MDATA (*m, i, j) * VDATA (*v, j); + VDATA (*r, i) = datum; + } + } + return 1; +} + + +} // namespace mbicp diff --git a/crosbot_mbicp/src/mbicpStd.h b/crosbot_mbicp/src/mbicpStd.h new file mode 100644 index 0000000..a74e264 --- /dev/null +++ b/crosbot_mbicp/src/mbicpStd.h @@ -0,0 +1,361 @@ +#ifndef MBPOSTRACKSTD_H_ +#define MBPOSTRACKSTD_H_ + +// #include "MBICP/TData.h" + +using namespace std; + +namespace mbicp +{ + +// AHM: this must be greater than or equal to the max possible number of points. +// it must be equal if using preProcessingCreate +//#define MAXLASERPOINTS 682 +#define MAXLASERPOINTS 1081 + +#define MAX_ROWS (7) +#define MAX_COLS (7) + +typedef struct { + int rows; + int cols; + float data[MAX_ROWS][MAX_COLS]; +} MATRIX; + +typedef struct { + int elements; + float data[MAX_ROWS]; +} VECTOR; + +#define DOF (3) + +typedef struct { + int mat[DOF]; + int range; +} BMAT; + + +// #define RADIO 0.4F /* Radio del robot */ + +typedef struct { + float x; + float y; +}Tpf; + + +typedef struct { + float r; + float t; +}Tpfp; + +typedef struct { + int x; + int y; +}Tpi; + +typedef struct { + float x; + float y; + float tita; +}Tsc; + +typedef struct { + int numPuntos; + Tpf laserC[MAXLASERPOINTS]; // Cartesian coordinates + Tpfp laserP[MAXLASERPOINTS]; // Polar coordinates +}Tscan; + + + + +// Associations information +typedef struct{ + float rx,ry,nx,ny,dist; // Point (nx,ny), static corr (rx,ry), dist + int numDyn; // Number of dynamic associations + float unknown; // Unknown weight + int index; // Index within the original scan + int L,R; +}TAsoc; + + +// ************************ +// Scan inner matching parameters +typedef struct{ + /* --------------------- */ + /* --- Thresold parameters */ + /* Bw: maximum angle diference between points of different scans */ + /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */ + /* This is a speed up parameter */ + float Bw; + + /* Br: maximum distance difference between points of different scans */ + /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */ + float Br; + + /* --------------------- */ + /* --- Inner parameters */ + + /* L: value of the metric */ + /* When L tends to infinity you are using the standart ICP */ + /* When L tends to 0 you use the metric (more importance to rotation */ + float LMET; + + /* laserStep: selects points of each scan with an step laserStep */ + /* When laserStep=1 uses all the points of the scans */ + /* When laserStep=2 uses one each two ... */ + /* This is an speed up parameter */ + int laserStep; + + /* ProjectionFilter: */ + /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */ + /* It works well for angles < 45 \circ*/ + /* 1 : activates the filter */ + /* 0 : desactivates the filter */ + int ProjectionFilter; + + /* MaxDistInter: maximum distance to interpolate between points in the ref scan */ + /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */ + float MaxDistInter; + + /* filtrado: in [0,1] sets the % of asociations NOT considered spurious */ + float filter; + + /* AsocError: in [0,1] */ + /* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */ + /* When the number of associations is below AsocError, the main function will return error in associations step */ + float AsocError; + + /* --------------------- */ + /* --- Exit parameters */ + /* MaxIter: sets the maximum number of iterations for the algorithm to exit */ + /* More iterations more chance you give the algorithm to be more accurate */ + int MaxIter; + + /* error_th: in [0,1] sets the maximum error ratio between iterations to exit */ + /* In each iteration, the error is the residual of the minimization */ + /* When error_th tends to 1 more precise is the solution of the scan matching */ + float error_th; + + /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */ + /* In each iteration, the error is the residual of the minimization in each component */ + /* The condition is (lower than errx_out && lower than erry_out && lower than errt_out */ + /* When error_XXX tend to 0 more precise is the solution of the scan matching */ + float errx_out,erry_out, errt_out; + + /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria */ + /* (error_th) OR (errorx_out && errory_out && errt_out) */ + /* With this parameter >1 avoids random solutions */ + int IterSmoothConv; + + float xmin; + float xmax; + float ymin; + float ymax; + + +}TSMparams; + +class MBICPStd { +public: + void Init_MbICP_ScanMatching( + float max_laser_range, + float Bw, + float Br, + float L, + int laserStep, + float MaxDistInter, + float filter, + int ProjectionFilter, + float AsocError, + int MaxIter, + float errorRatio, + float errx_out, + float erry_out, + float errt_out, + int IterSmoothConv, + float xmin, + float xmax, + float ymin, + float ymax + ); + + int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1, + Tsc *sensorMotion, Tsc *solution); + + int MbICPMatchScans(double &x, double &y, double &th); + + // Original points to be aligned + Tscan ptosRef; + Tscan ptosNew; + // Current motion estimation + Tsc motion2; + + bool useAdvanced_; +protected: + + // ************************ + // Static structure to initialize the SM parameters + TSMparams params; + + // At each step:: + + // Those points removed by the projection filter (see Lu&Millios -- IDC) + Tscan ptosNoView; // Only with ProjectionFilter=1; + + // Structure of the associations before filtering + TAsoc cp_associations[MAXLASERPOINTS]; + int cntAssociationsT; + + // Filtered Associations + TAsoc cp_associationsTemp[MAXLASERPOINTS]; + int cntAssociationsTemp; + + float MAXLASERRANGE; + float MINLASERRANGE; + float xmin_; + float xmax_; + float ymin_; + float ymax_; + + // ************************ + // Some precomputations for each scan to speed up + float refdqx[MAXLASERPOINTS]; + float refdqx2[MAXLASERPOINTS]; + float refdqy[MAXLASERPOINTS]; + float refdqy2[MAXLASERPOINTS]; + float distref[MAXLASERPOINTS]; + float refdqxdqy[MAXLASERPOINTS]; + + + // value of errors + float error_k1; + int numConverged; + + // Function for compatibility with the scans + void preProcessingLib(Tpfp *laserK, Tpfp *laserK1, + Tsc *initialMotion); + // separated functionality to avoid multiple conversions, this way the structures can be initialized elsewhere + void preProcessingCreate(Tpfp *laserK, Tpfp *laserK1, Tsc *initialMotion); + void preProcessingCalc(); + + // Function that does the association step of the MbICP + int EStep(); + + // Function that does the minimization step of the MbICP + int MStep(Tsc *solucion); + + // Function to do the least-squares but optimized for the metric + int computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion); + + /* --------------------------------------------------------------------------------------- */ + /* TRANSFORMACIONES DE PUNTO DE UN SISTEMA DE REFERENCIA A OTRO */ + /* --------------------------------------------------------------------------------------- */ + + /* --------------------------------------------------------------------------------------- */ + /* transfor_directa_p */ + /* .... Hace la transformacion directa de un punto a un sistema a otro */ + /* .... In: (x,y) las coordenadas del punto, sistema es el sistema de referencia */ + /* .... Out: en sol se devuelve las coordenadas del punto en el nuevo sistema */ + + void transfor_directa_p ( float x, float y, Tsc *sistema, Tpf *sol ); + + /* --------------------------------------------------------------------------------------- */ + /* transfor_directa_p */ + /* .... Hace la transformacion directa de un punto a un sistema a otro */ + /* .... La diferencia es que aqui el punto de entrada es el (0,0) (optimiza la anterior) */ + /* .... In: (x,y) las coordenadas del punto, sistema es el sistema de referencia */ + /* .... Out: en sol se devuelve las coordenadas del punto en el nuevo sistema */ + + void transfor_directa_pt0(float x, float y, + Tsc *sistema, Tpf *sol); + + /* --------------------------------------------------------------------------------------- */ + /* transfor_inversa_p */ + /* .... Hace la transformacion inversa de un punto a un sistema a otro */ + /* .... In: (x,y) las coordenadas del punto, sistema es el sistema de referencia */ + /* .... Out: en sol se devuelve las coordenadas del punto en el nuevo sistema */ + + void transfor_inversa_p ( float x, float y, Tsc *sistema, Tpf *sol ); + + /* --------------------------------------------------------------------------------------- */ + /* TRANSFORMACIONES DE COMPOSICION E INVERSION DE SISTEMAS DE REFERENCIA */ + /* --------------------------------------------------------------------------------------- */ + + /* --------------------------------------------------------------------------------------- */ + /* composicion_sis */ + /* .... Realiza la composicion de sistemas de referencia en otro sistema */ + /* .... In: compone sis1 y sis2 */ + /* .... Out: la salida sisOut es el resultado de la composicion de los sistemas */ + /* .... Nota: resulta muy importante el orden de las entradas en la composicion */ + + void composicion_sis(Tsc *sis1,Tsc *sis2,Tsc *sisOut); + + /* --------------------------------------------------------------------------------------- */ + /* inversion_sis */ + /* .... Realiza la inversion de un sistema de referencia */ + /* .... In: sisIn es el sistema a invertir */ + /* .... Out: sisOut es el sistema invertido */ + + void inversion_sis(Tsc *sisIn, Tsc *sisOut); + + /* --------------------------------------------------------------------------------------- */ + /* TRANSFORMACIONES DE PUNTO DE UN SISTEMA DE REFERENCIA A OTRO */ + /* --------------------------------------------------------------------------------------- */ + + /* --------------------------------------------------------------------------------------- */ + /* car2pol */ + /* .... Transforma un punto de coordenadas cartesianas a polares */ + /* .... In: el punto en coordenadas cartesianas a transformar */ + /* .... Out: el punto salida en coordenadas polares */ + + void car2pol(Tpf *in, Tpfp *out); + + /* --------------------------------------------------------------------------------------- */ + /* pol2car */ + /* .... Transforma un punto de coordenadas polares a cartesianas */ + /* .... In: el punto entrada en coordenadas polares a transformar */ + /* .... Out: el punto en coordenadas cartesianas transformado */ + + void pol2car(Tpfp *in, Tpf *out); + + /* --------------------------------------------------------------------------------------- */ + /* TRANSFORMACIONES DE PUNTO DE UN SISTEMA DE REFERENCIA A OTRO */ + /* --------------------------------------------------------------------------------------- */ + + /* --------------------------------------------------------------------------------------- */ + /* corte_segmentos */ + /* .... Calcula el punto de corte entre dos segmentos */ + /* .... In: las coordenadas de los puntos extremos (x1,y1)-(x2,y2) y (x3,y3)-(x4,y4) */ + /* .... Out: sol son las coordenadas del punto de corte. return --> 1 si hay corte. -->0 no */ + + int corte_segmentos ( float x1, float y1, float x2, float y2, + float x3, float y3, float x4, float y4, + Tpf *sol ); + + + /* Normaliza el angulo entre [-PI, PI] */ + float NormalizarPI(float ang); + + void heapsort(TAsoc a[], int n); + void swapItem(TAsoc *a, TAsoc *b); + void perc_down(TAsoc a[], int i, int n); + + MATRIX create_matrix (int rows, int cols); + void initialize_matrix (MATRIX *m, int rows, int cols); + void diagonal_matrix (MATRIX *m, int dim, float el1, float el2, float el3); + void print_matrix (const char *message, MATRIX const *m); + VECTOR create_vector (int elements); + void initialize_vector (VECTOR *v, int elements); + void print_vector (const char *message, VECTOR const *v); + float cross_product (MATRIX const *m, int f1, int c1, int f2, int c2); + int determinant (MATRIX const *m, float *result); + int inverse_matrix (MATRIX const *m, MATRIX *n); + int multiply_matrix_vector (MATRIX const *m, VECTOR const *v, VECTOR *r); + +}; + +} // namespace mbicp + + + +#endif /*MBPOSTRACKSTD_H_*/ diff --git a/crosbot_mbicp/src/nodes/mbicp.cpp b/crosbot_mbicp/src/nodes/mbicp.cpp new file mode 100644 index 0000000..be92edb --- /dev/null +++ b/crosbot_mbicp/src/nodes/mbicp.cpp @@ -0,0 +1,305 @@ +/* + * mbicp.cpp + * + * Created on: 19/02/2013 + * Author: mmcgill + */ + +#include +#include "../mbicpStd.h" +#include +#include + +using namespace crosbot; + +#include +#include +#include + +#define DEFAULT_ICPFRAME "/icp" +#define DEFAULT_BASEFRAME "/base_link" +#define DEFAULT_ODOMFRAME "" +#define DEFAULT_MAXWAIT4TRANSFORM 2.0 // [s] + +class MBICPNode { + std::string icpFrame, baseFrame, odomFrame; + + ros::Subscriber scanSub; + tf::TransformListener tfListener; + + tf::TransformBroadcaster tfPub; + + Pose previousPose, previousMove, previousOdom; + bool useOdometry; + + mbicp::MBICPStd mbicpStd; + + + // MBICP Parameters + + // Configurable parameters + double maxAlignTheta, maxAlignDist; // MBICP: can't align cells further than this apart + double l; // MBICP: L + int laserSkip; // MBICP: iteration value for laser readings for fine alignment + int maxIterations; // MBICP: Maximum number of iterations + bool useProjectionFilter; // MBICP: Activate projection filter + int iterSmoothCount; // + + double z; // Height of the robot off the ground + + bool mbicpInitialized; +public: + MBICPNode() : + icpFrame(DEFAULT_ICPFRAME), baseFrame(DEFAULT_BASEFRAME), + odomFrame(DEFAULT_ODOMFRAME), tfListener(ros::Duration(100000000,0)), useOdometry(false), + mbicpInitialized(false) + { + + maxAlignTheta = 0.4; // MB: can't align cells further than this apart + maxAlignDist = 0.3; // MB: can't align cells further than this apart + l = 2.0; + laserSkip = 4; // MB: iteration value for laser readings for fine alignment + maxIterations = 200; + useProjectionFilter = false; + iterSmoothCount = 2; + + z = 0; + } + + void populateTscan(mbicp::Tscan& tscan, const sensor_msgs::LaserScanConstPtr& latestScan, const PointCloud& cloud) { + double laserMin = latestScan->range_min, + laserMax = latestScan->range_max; + + tscan.numPuntos = 0; + + double bearing = latestScan->angle_min; + for (size_t i = 0; i < latestScan->ranges.size(); ++i) { + const float& r = latestScan->ranges[i]; + if (r > laserMax || r < laserMin) { + bearing += latestScan->angle_increment; + continue; + } + + tscan.laserP[tscan.numPuntos].r = r; + tscan.laserP[tscan.numPuntos].t = bearing; + + tscan.laserC[tscan.numPuntos].x = cloud.cloud[i].x; + tscan.laserC[tscan.numPuntos].y = cloud.cloud[i].y; + + tscan.numPuntos++; + bearing += latestScan->angle_increment; + } + } + + void initializeICP(const sensor_msgs::LaserScanConstPtr& latestScan, const PointCloud& cloud) { + LOG("Initialising Standard MBICP.\n"); + + // Initialise MBICPStd + mbicpStd.Init_MbICP_ScanMatching(// maximum laser reading + latestScan->range_max, // float max_laser_range, + /* Bw: maximum angle diference between points of different scans */ + /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */ + /* This is a speed up parameter */ + (float)maxAlignTheta, // float Bw, + // config->getAttributeAsDouble("std_angular_window", 1.0), // float Bw, + /* Br: maximum distance difference between points of different scans */ + /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */ + (float)maxAlignDist, // float Br, + /* L: value of the metric */ + /* When L tends to infinity you are using the standard ICP */ + /* When L tends to 0 you use the metric (more importance to rotation + * This is the most important parameter */ + l, // float L, + /* laserStep: selects points of each scan with an step laserStep */ + /* When laserStep=1 uses all the points of the scans */ + /* When laserStep=2 uses one each two ... */ + /* This is an speed up parameter, although not as much as previously when it was using laserStep^2*/ + laserSkip, // int laserStep, + /* MaxDistInter: maximum distance to interpolate between points in the ref scan */ + /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */ + 0.5,// float MaxDistInter, + /* filtrado: in [0,1] sets the % of asociations NOT considered spurious + * probably don't change this */ + 0.95,// float filter, + /* ProjectionFilter: */ + /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */ + /* It works well for angles < 45 \circ*/ + /* 1 : activates the filter */ + /* 0 : deactivates the filter + * probably don't turn this off */ + (useProjectionFilter?1:0),// int ProjectionFilter, + /* AsocError: in [0,1] */ + /* One way to check if the algorithm diverges if to supervise if the number of associations goes below a threshold */ + /* When the number of associations is below AsocError, the main function will return error in associations step */ + 0.1,// float AsocError, + /* MaxIter: sets the maximum number of iterations for the algorithm to exit */ + /* More iterations more chance you give the algorithm to be more accurate */ + maxIterations,// int MaxIter, + /* error_th: in [0,1] sets the maximum error ratio between iterations to exit */ + /* In each iteration, the error is the residual of the minimization */ + /* When error_th tends to 1 more precise is the solution of the scan matching */ + 0.0001,// float errorRatio, + /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */ + /* In each iteration, the error is the residual of the minimization in each component */ + /* The condition is (lower than errx_out && lower than erry_out && lower than errt_out */ + /* When error_XXX tend to 0 more precise is the solution of the scan matching */ + 0.0001,// float errx_out, + 0.0001,// float erry_out, + 0.0001,// float errt_out, + /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria */ + /* (error_th) OR (errorx_out && errory_out && errt_out) */ + /* With this parameter >1 avoids random solutions */ + iterSmoothCount,// int IterSmoothConv, + // not using these since we cull the points before sending + 0.0, 0.0, 0.0, 0.0 + ); + + if (latestScan->ranges.size() > MAXLASERPOINTS) { + ROS_ERROR("Number of points in laser scan is larger than can be handled by MBICP.\n"); + return; + } + mbicpInitialized = true; + populateTscan(mbicpStd.ptosRef, latestScan, cloud); + } + + void callbackScan(const sensor_msgs::LaserScanConstPtr& latestScan) { + Pose odomPose, sensorPose; + + tf::StampedTransform transform; + bool haveOdometry = odomFrame != ""; + try { + tfListener.waitForTransform(baseFrame, latestScan->header.frame_id, latestScan->header.stamp, ros::Duration(1, 0)); + tfListener.lookupTransform(baseFrame, + latestScan->header.frame_id, latestScan->header.stamp, transform); + sensorPose = transform; + + if (haveOdometry) { + tfListener.waitForTransform(odomFrame, baseFrame, latestScan->header.stamp, ros::Duration(1, 0)); + tfListener.lookupTransform(odomFrame, baseFrame, + latestScan->header.stamp, transform); + odomPose = transform; + } + } catch (tf::TransformException& ex) { + ERROR("mbicp: Error getting transform. (%s) (%d.%d)\n", ex.what(), + latestScan->header.stamp.sec, latestScan->header.stamp.nsec); + return; + } + + PointCloud cloud(baseFrame, PointCloud(latestScan, true), sensorPose); + if (!mbicpInitialized) { + initializeICP(latestScan, cloud); + if (useOdometry) { + previousOdom = odomPose; + } + return; + } + + // Apply estimate to mbicp + double roll, pitch, yaw; + if (useOdometry) { + // odometry change estimate + btTransform prevOdomTInv, odomT; + prevOdomTInv = previousOdom.getTransform().inverse(); + odomT = odomPose.getTransform(); + + odomT = prevOdomTInv * odomT; + + Pose odomMotion = odomT; + odomMotion.getYPR(yaw, pitch, roll); + mbicpStd.motion2.x = odomMotion.position.x; + mbicpStd.motion2.y = odomMotion.position.y; + mbicpStd.motion2.tita = yaw; + + previousOdom = odomPose; + } else { + // Use previous mostion for motion estimate + previousMove.getYPR(yaw, pitch, roll); + mbicpStd.motion2.x = previousMove.position.x; + mbicpStd.motion2.y = previousMove.position.y; + mbicpStd.motion2.tita = yaw; + } + + populateTscan(mbicpStd.ptosNew, latestScan, cloud); + + /** + * + */ + double x = 0, y = 0, theta = 0; + int err = mbicpStd.MbICPMatchScans(x, y, theta); + if (err != 1) { + if (err == 2) { + ERROR("MBICP: MBICP failed(too many iterations (>%d)) ignoring current scan.\n", maxIterations); + } else { + ERROR("MBICP: MBICP failed(%d) ignoring current scan.\n", err); + } + memcpy(&mbicpStd.ptosRef, &mbicpStd.ptosNew, sizeof(mbicp::Tscan)); + } + + previousMove.position.x = x; previousMove.position.y = y; previousMove.position.z = 0; + previousMove.orientation.setYPR(theta, 0, 0); + + btTransform poseTransform, moveTransform; + previousMove.getTransform(moveTransform); + previousPose.getTransform(poseTransform); + poseTransform *= moveTransform; + previousPose = poseTransform; + + + if (haveOdometry) { + LOG("Publishing correction to odometry.\n"); + Pose p = previousPose.getTransform() * odomPose.getTransform().inverse(); + publishPose(p, odomFrame, latestScan->header.stamp); + } else { + LOG("Publishing pose.\n"); + publishPose(previousPose, baseFrame, latestScan->header.stamp); + } + + memcpy(&mbicpStd.ptosRef, &mbicpStd.ptosNew, sizeof(mbicp::Tscan)); + } + + void publishPose(const Pose& pose, std::string childFrame, ros::Time stamp = ros::Time::now()) { + geometry_msgs::TransformStamped ts; + ts.header.frame_id = icpFrame; + ts.header.stamp = stamp; + ts.child_frame_id = childFrame; + ts.transform.translation.x = pose.position.x; + ts.transform.translation.y = pose.position.y; + ts.transform.translation.z = pose.position.z; + ts.transform.rotation = pose.orientation.toROS(); + + tfPub.sendTransform(ts); + } + + void initalise(ros::NodeHandle& nh) { + // read configuration/parameters + ros::NodeHandle paramNH("~"); // Because ROS's search order is buggered + paramNH.param("icp_frame", icpFrame, DEFAULT_ICPFRAME); + paramNH.param("base_frame", baseFrame, DEFAULT_BASEFRAME); + paramNH.param("odom_frame", odomFrame, DEFAULT_ODOMFRAME); + + // TODO: read MBICP parameters + + scanSub = nh.subscribe("scan", 1, &MBICPNode::callbackScan, this); + } + + void shutdown() { + scanSub.shutdown(); + } +}; + +int main(int argc, char** argv) { + ros::init(argc, argv, "mbicp"); + + ros::NodeHandle nh; + + MBICPNode node; + node.initalise(nh); + + while (ros::ok()) { + ros::spin(); + } + node.shutdown(); + + return 0; +} + -- GitLab