diff --git a/crosbot_mbicp/CMakeLists.txt b/crosbot_mbicp/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..04d38d2a416495b60e0da877a04c038b40a668d4
--- /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 0000000000000000000000000000000000000000..500462201e45707c3ad6ccccdaa35878fe40ae6f
--- /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 0000000000000000000000000000000000000000..52d0c63e4a2096d36fdc1793bb47e54d49e1d6c5
--- /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 0000000000000000000000000000000000000000..7032f81477211d5f5d559e38e517a2c1e0c09c6e
--- /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 0000000000000000000000000000000000000000..a74e26437373ef09867fcf8482b05093f7b1be78
--- /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 0000000000000000000000000000000000000000..be92edb0c9bb2c3992fae1e8a4c8d20199b6f18b
--- /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;
+}
+