laserOdometry.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <time.h>
00003 #include <stdio.h>
00004 #include <stdlib.h>
00005 #include <ros/ros.h>
00006 
00007 #include <nav_msgs/Odometry.h>
00008 #include <sensor_msgs/Imu.h>
00009 #include <sensor_msgs/PointCloud2.h>
00010 
00011 #include <tf/transform_datatypes.h>
00012 #include <tf/transform_broadcaster.h>
00013 
00014 #include <opencv/cv.h>
00015 #include <opencv2/highgui/highgui.hpp>
00016 
00017 #include <pcl/ros/conversions.h>
00018 #include <pcl/point_cloud.h>
00019 #include <pcl/point_types.h>
00020 #include <pcl/filters/voxel_grid.h>
00021 #include <pcl/kdtree/kdtree_flann.h>
00022 
00023 const double PI = 3.1415926;
00024 const double rad2deg = 180 / PI;
00025 const double deg2rad = PI / 180;
00026 
00027 bool systemInited = false;
00028 
00029 double initTime;
00030 double timeLasted;
00031 double timeLastedRec;
00032 double startTimeCur;
00033 double startTimeLast;
00034 
00035 double timeLaserCloudExtreCur = 0;
00036 double timeLaserCloudLast = 0;
00037 
00038 bool newLaserCloudExtreCur = false;
00039 bool newLaserCloudLast = false;
00040 
00041 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreCur(new pcl::PointCloud<pcl::PointXYZHSV>());
00042 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00043 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreOri(new pcl::PointCloud<pcl::PointXYZHSV>());
00044 //pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreSel(new pcl::PointCloud<pcl::PointXYZHSV>());
00045 //pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreUnsel(new pcl::PointCloud<pcl::PointXYZHSV>());
00046 //pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreProj(new pcl::PointCloud<pcl::PointXYZHSV>());
00047 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00048 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCornerLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00049 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSurfLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00050 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCornerLLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00051 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSurfLLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00052 //pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSel(new pcl::PointCloud<pcl::PointXYZHSV>());
00053 pcl::PointCloud<pcl::PointXYZHSV>::Ptr coeffSel(new pcl::PointCloud<pcl::PointXYZHSV>());
00054 pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZHSV>());
00055 pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<pcl::PointXYZHSV>());
00056 pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreeCornerLLast(new pcl::KdTreeFLANN<pcl::PointXYZHSV>());
00057 pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreeSurfLLast(new pcl::KdTreeFLANN<pcl::PointXYZHSV>());
00058 
00059 float transform[6] = {0};
00060 float transformRec[6] = {0};
00061 float transformSum[6] = {0};
00062 
00063 float imuRollStartCur = 0, imuPitchStartCur = 0, imuYawStartCur = 0;
00064 float imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0;
00065 float imuShiftFromStartXCur = 0, imuShiftFromStartYCur = 0, imuShiftFromStartZCur = 0;
00066 float imuVeloFromStartXCur = 0, imuVeloFromStartYCur = 0, imuVeloFromStartZCur = 0;
00067 
00068 float imuRollStartLast = 0, imuPitchStartLast = 0, imuYawStartLast = 0;
00069 float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
00070 float imuShiftFromStartXLast = 0, imuShiftFromStartYLast = 0, imuShiftFromStartZLast = 0;
00071 float imuVeloFromStartXLast = 0, imuVeloFromStartYLast = 0, imuVeloFromStartZLast = 0;
00072 
00073 bool imuInited = false;
00074 
00075 void TransformReset()
00076 {
00077   for (int i = 0; i < 6; i++) {
00078     transformRec[i] = transform[i];
00079     transform[i] = 0;
00080   }
00081 
00082   transformRec[3] -= imuVeloFromStartXLast * (startTimeCur - startTimeLast);
00083   transformRec[4] -= imuVeloFromStartYLast * (startTimeCur - startTimeLast);
00084   transformRec[5] -= imuVeloFromStartZLast * (startTimeCur - startTimeLast);
00085 }
00086 
00087 void TransformToStart(pcl::PointXYZHSV *pi, pcl::PointXYZHSV *po, double startTime, double endTime)
00088 {
00089   float s = (pi->h - startTime) / (endTime - startTime);
00090 
00091   float rx = s * transform[0];
00092   float ry = s * transform[1];
00093   float rz = s * transform[2];
00094   float tx = s * transform[3];
00095   float ty = s * transform[4];
00096   float tz = s * transform[5];
00097 
00098   float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
00099   float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
00100   float z1 = (pi->z - tz);
00101 
00102   float x2 = x1;
00103   float y2 = cos(rx) * y1 + sin(rx) * z1;
00104   float z2 = -sin(rx) * y1 + cos(rx) * z1;
00105 
00106   po->x = cos(ry) * x2 - sin(ry) * z2;
00107   po->y = y2;
00108   po->z = sin(ry) * x2 + cos(ry) * z2;
00109   po->h = pi->h;
00110   po->s = pi->s;
00111   po->v = pi->v;
00112 }
00113 
00114 void TransformToEnd(pcl::PointXYZHSV *pi, pcl::PointXYZHSV *po, double startTime, double endTime)
00115 {
00116   float s = (pi->h - startTime) / (endTime - startTime);
00117 
00118   float rx = s * transform[0];
00119   float ry = s * transform[1];
00120   float rz = s * transform[2];
00121   float tx = s * transform[3];
00122   float ty = s * transform[4];
00123   float tz = s * transform[5];
00124 
00125   float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
00126   float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
00127   float z1 = (pi->z - tz);
00128 
00129   float x2 = x1;
00130   float y2 = cos(rx) * y1 + sin(rx) * z1;
00131   float z2 = -sin(rx) * y1 + cos(rx) * z1;
00132 
00133   float x3 = cos(ry) * x2 - sin(ry) * z2;
00134   float y3 = y2;
00135   float z3 = sin(ry) * x2 + cos(ry) * z2;
00136 
00137   rx = transform[0];
00138   ry = transform[1];
00139   rz = transform[2];
00140   tx = transform[3];
00141   ty = transform[4];
00142   tz = transform[5];
00143 
00144   float x4 = cos(ry) * x3 + sin(ry) * z3;
00145   float y4 = y3;
00146   float z4 = -sin(ry) * x3 + cos(ry) * z3;
00147 
00148   float x5 = x4;
00149   float y5 = cos(rx) * y4 - sin(rx) * z4;
00150   float z5 = sin(rx) * y4 + cos(rx) * z4;
00151 
00152   float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;
00153   float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;
00154   float z6 = z5 + tz;
00155 
00156   float x7 = cos(imuRollStartLast) * (x6 - imuShiftFromStartXLast) 
00157            - sin(imuRollStartLast) * (y6 - imuShiftFromStartYLast);
00158   float y7 = sin(imuRollStartLast) * (x6 - imuShiftFromStartXLast) 
00159            + cos(imuRollStartLast) * (y6 - imuShiftFromStartYLast);
00160   float z7 = z6 - imuShiftFromStartZLast;
00161 
00162   float x8 = x7;
00163   float y8 = cos(imuPitchStartLast) * y7 - sin(imuPitchStartLast) * z7;
00164   float z8 = sin(imuPitchStartLast) * y7 + cos(imuPitchStartLast) * z7;
00165 
00166   float x9 = cos(imuYawStartLast) * x8 + sin(imuYawStartLast) * z8;
00167   float y9 = y8;
00168   float z9 = -sin(imuYawStartLast) * x8 + cos(imuYawStartLast) * z8;
00169 
00170   float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;
00171   float y10 = y9;
00172   float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;
00173 
00174   float x11 = x10;
00175   float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;
00176   float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;
00177 
00178   po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;
00179   po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;
00180   po->z = z11;
00181   po->h = pi->h;
00182   po->s = pi->s;
00183   po->v = pi->v;
00184 }
00185 
00186 void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, 
00187                         float &ox, float &oy, float &oz)
00188 {
00189   float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
00190   ox = -asin(srx);
00191 
00192   float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
00193                + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
00194   float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
00195                - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
00196   oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
00197 
00198   float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
00199                + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
00200   float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
00201                - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
00202   oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
00203 }
00204 
00205 void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, 
00206                        float alx, float aly, float alz, float &acx, float &acy, float &acz)
00207 {
00208   float sbcx = sin(bcx);
00209   float cbcx = cos(bcx);
00210   float sbcy = sin(bcy);
00211   float cbcy = cos(bcy);
00212   float sbcz = sin(bcz);
00213   float cbcz = cos(bcz);
00214 
00215   float sblx = sin(blx);
00216   float cblx = cos(blx);
00217   float sbly = sin(bly);
00218   float cbly = cos(bly);
00219   float sblz = sin(blz);
00220   float cblz = cos(blz);
00221 
00222   float salx = sin(alx);
00223   float calx = cos(alx);
00224   float saly = sin(aly);
00225   float caly = cos(aly);
00226   float salz = sin(alz);
00227   float calz = cos(alz);
00228 
00229   float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 
00230             - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00231             - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00232             - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00233             - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
00234   acx = -asin(srx);
00235 
00236   float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00237                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00238                - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00239                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00240                + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00241   float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00242                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00243                - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00244                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00245                + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00246   acy = atan2(srycrx / cos(acx), crycrx / cos(acx));
00247   
00248   float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 
00249                - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 
00250                - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 
00251                + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 
00252                - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 
00253                + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 
00254                + calx*cblx*salz*sblz);
00255   float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 
00256                - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 
00257                + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 
00258                + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 
00259                + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 
00260                - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 
00261                - calx*calz*cblx*sblz);
00262   acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx));
00263 }
00264 
00265 void laserCloudExtreCurHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudExtreCur2)
00266 {
00267   if (!systemInited) {
00268     initTime = laserCloudExtreCur2->header.stamp.toSec();
00269     systemInited = true;
00270   }
00271   timeLaserCloudExtreCur = laserCloudExtreCur2->header.stamp.toSec();
00272   timeLasted = timeLaserCloudExtreCur - initTime;
00273 
00274   pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreCur3(new pcl::PointCloud<pcl::PointXYZHSV>());
00275   pcl::fromROSMsg(*laserCloudExtreCur2, *laserCloudExtreCur3);
00276   int laserCloudExtreCur3Size = laserCloudExtreCur3->points.size();
00277 
00278   laserCloudExtreCur->clear();
00279   for (int i = 0; i < laserCloudExtreCur3Size; i++) {
00280     if (fabs(laserCloudExtreCur3->points[i].v - 10) < 0.005) {
00281       imuPitchStartCur = laserCloudExtreCur3->points[i].x;
00282       imuYawStartCur = laserCloudExtreCur3->points[i].y;
00283       imuRollStartCur = laserCloudExtreCur3->points[i].z;
00284     } else if (fabs(laserCloudExtreCur3->points[i].v - 11) < 0.005) {
00285       imuPitchCur = laserCloudExtreCur3->points[i].x;
00286       imuYawCur = laserCloudExtreCur3->points[i].y;
00287       imuRollCur = laserCloudExtreCur3->points[i].z;
00288     } else if (fabs(laserCloudExtreCur3->points[i].v - 12) < 0.005) {
00289       imuShiftFromStartXCur = laserCloudExtreCur3->points[i].x;
00290       imuShiftFromStartYCur = laserCloudExtreCur3->points[i].y;
00291       imuShiftFromStartZCur = laserCloudExtreCur3->points[i].z;
00292     } else if (fabs(laserCloudExtreCur3->points[i].v - 13) < 0.005) {
00293       imuVeloFromStartXCur = laserCloudExtreCur3->points[i].x;
00294       imuVeloFromStartYCur = laserCloudExtreCur3->points[i].y;
00295       imuVeloFromStartZCur = laserCloudExtreCur3->points[i].z;
00296     } else {
00297       laserCloudExtreCur->push_back(laserCloudExtreCur3->points[i]);
00298     }
00299   }
00300   laserCloudExtreCur3->clear();
00301 
00302   if (!imuInited) {
00303     transformSum[0] += imuPitchStartCur;
00304     //transformSum[1] += imuYawStartCur;
00305     transformSum[2] += imuRollStartCur;
00306 
00307     imuInited = true;
00308   }
00309 
00310   if (timeLasted > 4.0) {
00311     newLaserCloudExtreCur = true;
00312   }
00313 }
00314 
00315 void laserCloudLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudLast2)
00316 {
00317   if (laserCloudLast2->header.stamp.toSec() > timeLaserCloudLast + 0.005) {
00318     timeLaserCloudLast = laserCloudLast2->header.stamp.toSec();
00319     startTimeLast = startTimeCur;
00320     startTimeCur = timeLaserCloudLast - initTime;
00321 
00322     pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudPointer = laserCloudCornerLLast;
00323     laserCloudCornerLLast = laserCloudCornerLast;
00324     laserCloudCornerLast = laserCloudPointer;
00325 
00326     laserCloudPointer = laserCloudSurfLLast;
00327     laserCloudSurfLLast = laserCloudSurfLast;
00328     laserCloudSurfLast = laserCloudPointer;
00329 
00330     laserCloudLast->clear();
00331     pcl::fromROSMsg(*laserCloudLast2, *laserCloudLast);
00332     int laserCloudLastSize = laserCloudLast->points.size();
00333 
00334     laserCloudExtreLast->clear();
00335     laserCloudCornerLast->clear();
00336     laserCloudSurfLast->clear();
00337     for (int i = 0; i < laserCloudLastSize; i++) {
00338       if (fabs(laserCloudLast->points[i].v - 2) < 0.005 || fabs(laserCloudLast->points[i].v + 1) < 0.005) {
00339         laserCloudExtreLast->push_back(laserCloudLast->points[i]);
00340       } 
00341       if (fabs(laserCloudLast->points[i].v - 2) < 0.005 || fabs(laserCloudLast->points[i].v - 1) < 0.005) {
00342         laserCloudCornerLast->push_back(laserCloudLast->points[i]);
00343       } 
00344       if (fabs(laserCloudLast->points[i].v) < 0.005 || fabs(laserCloudLast->points[i].v + 1) < 0.005) {
00345         laserCloudSurfLast->push_back(laserCloudLast->points[i]);
00346       }
00347       if (fabs(laserCloudLast->points[i].v - 10) < 0.005) {
00348         imuPitchStartLast = laserCloudLast->points[i].x;
00349         imuYawStartLast = laserCloudLast->points[i].y;
00350         imuRollStartLast = laserCloudLast->points[i].z;
00351       }
00352       if (fabs(laserCloudLast->points[i].v - 11) < 0.005) {
00353         imuPitchLast = laserCloudLast->points[i].x;
00354         imuYawLast = laserCloudLast->points[i].y;
00355         imuRollLast = laserCloudLast->points[i].z;
00356       }
00357       if (fabs(laserCloudLast->points[i].v - 12) < 0.005) {
00358         imuShiftFromStartXLast = laserCloudLast->points[i].x;
00359         imuShiftFromStartYLast = laserCloudLast->points[i].y;
00360         imuShiftFromStartZLast = laserCloudLast->points[i].z;
00361       }
00362       if (fabs(laserCloudLast->points[i].v - 13) < 0.005) {
00363         imuVeloFromStartXLast = laserCloudLast->points[i].x;
00364         imuVeloFromStartYLast = laserCloudLast->points[i].y;
00365         imuVeloFromStartZLast = laserCloudLast->points[i].z;
00366       }
00367     }
00368 
00369     pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreePointer = kdtreeCornerLLast;
00370     kdtreeCornerLLast = kdtreeCornerLast;
00371     kdtreeCornerLast = kdtreePointer;
00372     kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
00373 
00374     kdtreePointer = kdtreeSurfLLast;
00375     kdtreeSurfLLast = kdtreeSurfLast;
00376     kdtreeSurfLast = kdtreePointer;
00377     kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
00378 
00379     if (timeLasted > 4.0) {
00380       newLaserCloudLast = true;
00381     }
00382   }
00383 }
00384 
00385 int main(int argc, char** argv)
00386 {
00387   ros::init(argc, argv, "laserOdometry");
00388   ros::NodeHandle nh;
00389 
00390   ros::Subscriber subLaserCloudExtreCur = nh.subscribe<sensor_msgs::PointCloud2> 
00391                                           ("/laser_cloud_extre_cur", 2, laserCloudExtreCurHandler);
00392 
00393   ros::Subscriber subLaserCloudLast = nh.subscribe<sensor_msgs::PointCloud2> 
00394                                       ("/laser_cloud_last", 2, laserCloudLastHandler);
00395 
00396   ros::Publisher pubLaserCloudLast2 = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_last_2", 2);
00397 
00398   //ros::Publisher pub1 = nh.advertise<sensor_msgs::PointCloud2> ("/pc1", 1);
00399 
00400   //ros::Publisher pub2 = nh.advertise<sensor_msgs::PointCloud2> ("/pc2", 1);
00401 
00402   //ros::Publisher pub3 = nh.advertise<sensor_msgs::PointCloud2> ("/pc3", 1);
00403 
00404   //ros::Publisher pub4 = nh.advertise<sensor_msgs::PointCloud2> ("/pc4", 1);
00405 
00406   //ros::Publisher pub5 = nh.advertise<sensor_msgs::PointCloud2> ("/pc5", 1);
00407 
00408   //ros::Publisher pub6 = nh.advertise<sensor_msgs::PointCloud2> ("/pc6", 1);
00409 
00410   ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/cam_to_init", 5);
00411   nav_msgs::Odometry laserOdometry;
00412   laserOdometry.header.frame_id = "/camera_init";
00413   laserOdometry.child_frame_id = "/camera";
00414 
00415   tf::TransformBroadcaster tfBroadcaster;
00416   tf::StampedTransform laserOdometryTrans;
00417   laserOdometryTrans.frame_id_ = "/camera_init";
00418   laserOdometryTrans.child_frame_id_ = "/camera";
00419 
00420   std::vector<int> pointSearchInd;
00421   std::vector<float> pointSearchSqDis;
00422   std::vector<int> pointSelInd;
00423 
00424   pcl::PointXYZHSV extreOri, extreSel, extreProj, tripod1, tripod2, tripod3, coeff;
00425 
00426   ros::Rate rate(100);
00427   bool status = ros::ok();
00428   while (status) {
00429     ros::spinOnce();
00430 
00431     bool sweepEnd = false;
00432     bool newLaserPoints = false;
00433     bool sufficientPoints = false;
00434     double startTime, endTime;
00435     pcl::PointCloud<pcl::PointXYZHSV>::Ptr extrePointsPtr, laserCloudCornerPtr, laserCloudSurfPtr;
00436     pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreeCornerPtr, kdtreeSurfPtr;
00437     if (newLaserCloudExtreCur && newLaserCloudLast) {
00438 
00439       startTime = startTimeLast;
00440       endTime = startTimeCur;
00441 
00442       extrePointsPtr = laserCloudExtreLast;
00443       laserCloudCornerPtr = laserCloudCornerLLast;
00444       laserCloudSurfPtr = laserCloudSurfLLast;
00445       kdtreeCornerPtr = kdtreeCornerLLast;
00446       kdtreeSurfPtr = kdtreeSurfLLast;
00447 
00448       laserOdometry.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00449       laserOdometryTrans.stamp_ = ros::Time().fromSec(timeLaserCloudLast);
00450 
00451       sweepEnd = true;
00452       newLaserPoints = true;
00453 
00454       if (laserCloudSurfLLast->points.size() >= 100) {
00455         sufficientPoints = true;
00456       }
00457 
00458     } else if (newLaserCloudExtreCur) {
00459 
00460       startTime = startTimeCur;
00461       endTime = timeLasted;
00462 
00463       extrePointsPtr = laserCloudExtreCur;
00464       laserCloudCornerPtr = laserCloudCornerLast;
00465       laserCloudSurfPtr = laserCloudSurfLast;
00466       kdtreeCornerPtr = kdtreeCornerLast;
00467       kdtreeSurfPtr = kdtreeSurfLast;
00468 
00469       laserOdometry.header.stamp = ros::Time().fromSec(timeLaserCloudExtreCur);
00470       laserOdometryTrans.stamp_ = ros::Time().fromSec(timeLaserCloudExtreCur);
00471 
00472       float s = (timeLasted - timeLastedRec) / (startTimeCur - startTimeLast);
00473       for (int i = 0; i < 6; i++) {
00474         transform[i] += s * transformRec[i];        
00475       }
00476       timeLastedRec = timeLasted;
00477 
00478       newLaserPoints = true;
00479 
00480       if (laserCloudSurfLast->points.size() >= 100) {
00481         sufficientPoints = true;
00482       }
00483     }
00484 
00485     if (newLaserPoints && sufficientPoints) {
00486       newLaserCloudExtreCur = false;
00487       newLaserCloudLast = false;
00488 
00489       int extrePointNum = extrePointsPtr->points.size();
00490       int laserCloudCornerNum = laserCloudCornerPtr->points.size();
00491       int laserCloudSurfNum = laserCloudSurfPtr->points.size();
00492 
00493       float st = 1;
00494       if (!sweepEnd) {
00495         st = (timeLasted - startTime) / (startTimeCur - startTimeLast);
00496       }
00497       int iterNum = st * 50;
00498 
00499       int pointSelSkipNum = 2;
00500       for (int iterCount = 0; iterCount < iterNum; iterCount++) {
00501 
00502         laserCloudExtreOri->clear();
00503         //laserCloudExtreSel->clear();
00504         //laserCloudExtreUnsel->clear();
00505         //laserCloudExtreProj->clear();
00506         //laserCloudSel->clear();
00507         coeffSel->clear();
00508 
00509         bool isPointSel = false;
00510         if (iterCount % (pointSelSkipNum + 1) == 0) {
00511           isPointSel = true;
00512           pointSelInd.clear();
00513         }
00514 
00515         for (int i = 0; i < extrePointNum; i++) {
00516           extreOri = extrePointsPtr->points[i];
00517           TransformToStart(&extreOri, &extreSel, startTime, endTime);
00518 
00519           if (isPointSel) {
00520             pointSelInd.push_back(-1);
00521             pointSelInd.push_back(-1);
00522             pointSelInd.push_back(-1);
00523           }
00524 
00525           if (fabs(extreOri.v + 1) < 0.05) {
00526 
00527             int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
00528             if (isPointSel) {
00529               kdtreeSurfPtr->nearestKSearch(extreSel, 1, pointSearchInd, pointSearchSqDis);
00530               if (pointSearchSqDis[0] > 1.0) {
00531                 continue;
00532               }
00533 
00534               closestPointInd = pointSearchInd[0];
00535               float closestPointTime = laserCloudSurfPtr->points[closestPointInd].h;
00536 
00537               float pointSqDis, minPointSqDis2 = 1, minPointSqDis3 = 1;
00538               for (int j = closestPointInd + 1; j < laserCloudSurfNum; j++) {
00539                 if (laserCloudSurfPtr->points[j].h > closestPointTime + 0.07) {
00540                   break;
00541                 }
00542 
00543                 pointSqDis = (laserCloudSurfPtr->points[j].x - extreSel.x) * 
00544                              (laserCloudSurfPtr->points[j].x - extreSel.x) + 
00545                              (laserCloudSurfPtr->points[j].y - extreSel.y) * 
00546                              (laserCloudSurfPtr->points[j].y - extreSel.y) + 
00547                              (laserCloudSurfPtr->points[j].z - extreSel.z) * 
00548                              (laserCloudSurfPtr->points[j].z - extreSel.z);
00549 
00550                 if (laserCloudSurfPtr->points[j].h < closestPointTime + 0.005) {
00551                    if (pointSqDis < minPointSqDis2) {
00552                      minPointSqDis2 = pointSqDis;
00553                      minPointInd2 = j;
00554                    }
00555                 } else {
00556                    if (pointSqDis < minPointSqDis3) {
00557                      minPointSqDis3 = pointSqDis;
00558                      minPointInd3 = j;
00559                    }
00560                 }
00561               }
00562               for (int j = closestPointInd - 1; j >= 0; j--) {
00563                 if (laserCloudSurfPtr->points[j].h < closestPointTime - 0.07) {
00564                   break;
00565                 }
00566 
00567                 pointSqDis = (laserCloudSurfPtr->points[j].x - extreSel.x) * 
00568                              (laserCloudSurfPtr->points[j].x - extreSel.x) + 
00569                              (laserCloudSurfPtr->points[j].y - extreSel.y) * 
00570                              (laserCloudSurfPtr->points[j].y - extreSel.y) + 
00571                              (laserCloudSurfPtr->points[j].z - extreSel.z) * 
00572                              (laserCloudSurfPtr->points[j].z - extreSel.z);
00573 
00574                 if (laserCloudSurfPtr->points[j].h > closestPointTime - 0.005) {
00575                    if (pointSqDis < minPointSqDis2) {
00576                      minPointSqDis2 = pointSqDis;
00577                      minPointInd2 = j;
00578                    }
00579                 } else {
00580                    if (pointSqDis < minPointSqDis3) {
00581                      minPointSqDis3 = pointSqDis;
00582                      minPointInd3 = j;
00583                    }
00584                 }
00585               }
00586             } else {
00587               if (pointSelInd[3 * i] >= 0) {
00588                 closestPointInd = pointSelInd[3 * i];
00589                 minPointInd2 = pointSelInd[3 * i + 1];
00590                 minPointInd3 = pointSelInd[3 * i + 2];
00591 
00592                 float distX = extreSel.x - laserCloudSurfPtr->points[closestPointInd].x;
00593                 float distY = extreSel.y - laserCloudSurfPtr->points[closestPointInd].y;
00594                 float distZ = extreSel.z - laserCloudSurfPtr->points[closestPointInd].z;
00595                 if (distX * distX + distY * distY + distZ * distZ > 1.0) {
00596                   continue;
00597                 }
00598               } else {
00599                 continue;
00600               }
00601             }
00602 
00603             if (minPointInd2 >= 0 && minPointInd3 >= 0) {
00604               tripod1 = laserCloudSurfPtr->points[closestPointInd];
00605               tripod2 = laserCloudSurfPtr->points[minPointInd2];
00606               tripod3 = laserCloudSurfPtr->points[minPointInd3];
00607 
00608               float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) 
00609                        - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
00610               float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) 
00611                        - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
00612               float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) 
00613                        - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
00614               float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);
00615 
00616               float ps = sqrt(pa * pa + pb * pb + pc * pc);
00617               pa /= ps;
00618               pb /= ps;
00619               pc /= ps;
00620               pd /= ps;
00621 
00622               float pd2 = pa * extreSel.x + pb * extreSel.y + pc * extreSel.z + pd;
00623 
00624               extreProj = extreSel;
00625               extreProj.x -= pa * pd2;
00626               extreProj.y -= pb * pd2;
00627               extreProj.z -= pc * pd2;
00628 
00629               float s = 1;
00630               if (iterCount >= 30) {
00631                 s = 1 - 8 * fabs(pd2) / sqrt(sqrt(extreSel.x * extreSel.x
00632                   + extreSel.y * extreSel.y + extreSel.z * extreSel.z));
00633               }
00634 
00635               coeff.x = s * pa;
00636               coeff.y = s * pb;
00637               coeff.z = s * pc;
00638               coeff.h = s * pd2;
00639 
00640               if (s > 0.2 || iterNum < 30) {
00641                 laserCloudExtreOri->push_back(extreOri);
00642                 //laserCloudExtreSel->push_back(extreSel);
00643                 //laserCloudExtreProj->push_back(extreProj);
00644                 //laserCloudSel->push_back(tripod1);
00645                 //laserCloudSel->push_back(tripod2);
00646                 //laserCloudSel->push_back(tripod3);
00647                 coeffSel->push_back(coeff);
00648 
00649                 if (isPointSel) {
00650                   pointSelInd[3 * i] = closestPointInd;
00651                   pointSelInd[3 * i + 1] = minPointInd2;
00652                   pointSelInd[3 * i + 2] = minPointInd3;
00653                 }
00654               } else {
00655                 //laserCloudExtreUnsel->push_back(extreSel);
00656               }
00657             }
00658           } else if (fabs(extreOri.v - 2) < 0.05) {
00659 
00660             int closestPointInd = -1, minPointInd2 = -1;
00661             if (isPointSel) {
00662               kdtreeCornerPtr->nearestKSearch(extreSel, 1, pointSearchInd, pointSearchSqDis);
00663               if (pointSearchSqDis[0] > 1.0) {
00664                 continue;
00665               }
00666 
00667               closestPointInd = pointSearchInd[0];
00668               float closestPointTime = laserCloudCornerPtr->points[closestPointInd].h;
00669 
00670               float pointSqDis, minPointSqDis2 = 1;
00671               for (int j = closestPointInd + 1; j < laserCloudCornerNum; j++) {
00672                 if (laserCloudCornerPtr->points[j].h > closestPointTime + 0.07) {
00673                   break;
00674                 }
00675 
00676                 pointSqDis = (laserCloudCornerPtr->points[j].x - extreSel.x) * 
00677                              (laserCloudCornerPtr->points[j].x - extreSel.x) + 
00678                              (laserCloudCornerPtr->points[j].y - extreSel.y) * 
00679                              (laserCloudCornerPtr->points[j].y - extreSel.y) + 
00680                              (laserCloudCornerPtr->points[j].z - extreSel.z) * 
00681                              (laserCloudCornerPtr->points[j].z - extreSel.z);
00682 
00683                 if (laserCloudCornerPtr->points[j].h > closestPointTime + 0.005) {
00684                    if (pointSqDis < minPointSqDis2) {
00685                      minPointSqDis2 = pointSqDis;
00686                      minPointInd2 = j;
00687                    }
00688                 }
00689               }
00690               for (int j = closestPointInd - 1; j >= 0; j--) {
00691                 if (laserCloudCornerPtr->points[j].h < closestPointTime - 0.07) {
00692                   break;
00693                 }
00694 
00695                 pointSqDis = (laserCloudCornerPtr->points[j].x - extreSel.x) * 
00696                              (laserCloudCornerPtr->points[j].x - extreSel.x) + 
00697                              (laserCloudCornerPtr->points[j].y - extreSel.y) * 
00698                              (laserCloudCornerPtr->points[j].y - extreSel.y) + 
00699                              (laserCloudCornerPtr->points[j].z - extreSel.z) * 
00700                              (laserCloudCornerPtr->points[j].z - extreSel.z);
00701 
00702                 if (laserCloudCornerPtr->points[j].h < closestPointTime - 0.005) {
00703                    if (pointSqDis < minPointSqDis2) {
00704                      minPointSqDis2 = pointSqDis;
00705                      minPointInd2 = j;
00706                    }
00707                 }
00708               }
00709             } else {
00710               if (pointSelInd[3 * i] >= 0) {
00711                 closestPointInd = pointSelInd[3 * i];
00712                 minPointInd2 = pointSelInd[3 * i + 1];
00713 
00714                 float distX = extreSel.x - laserCloudCornerPtr->points[closestPointInd].x;
00715                 float distY = extreSel.y - laserCloudCornerPtr->points[closestPointInd].y;
00716                 float distZ = extreSel.z - laserCloudCornerPtr->points[closestPointInd].z;
00717                 if (distX * distX + distY * distY + distZ * distZ > 1.0) {
00718                   continue;
00719                 }
00720               } else {
00721                 continue;
00722               }
00723             }
00724 
00725             if (minPointInd2 >= 0) {
00726               tripod1 = laserCloudCornerPtr->points[closestPointInd];
00727               tripod2 = laserCloudCornerPtr->points[minPointInd2];
00728 
00729               float x0 = extreSel.x;
00730               float y0 = extreSel.y;
00731               float z0 = extreSel.z;
00732               float x1 = tripod1.x;
00733               float y1 = tripod1.y;
00734               float z1 = tripod1.z;
00735               float x2 = tripod2.x;
00736               float y2 = tripod2.y;
00737               float z2 = tripod2.z;
00738 
00739               float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
00740                         * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
00741                         + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
00742                         * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
00743                         + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
00744                         * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
00745 
00746               float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
00747 
00748               float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
00749                        + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
00750 
00751               float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
00752                        - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
00753 
00754               float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
00755                        + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
00756 
00757               float ld2 = a012 / l12;
00758 
00759               extreProj = extreSel;
00760               extreProj.x -= la * ld2;
00761               extreProj.y -= lb * ld2;
00762               extreProj.z -= lc * ld2;
00763 
00764               float s = 2 * (1 - 8 * fabs(ld2));
00765 
00766               coeff.x = s * la;
00767               coeff.y = s * lb;
00768               coeff.z = s * lc;
00769               coeff.h = s * ld2;
00770 
00771               if (s > 0.4) {
00772                 laserCloudExtreOri->push_back(extreOri);
00773                 //laserCloudExtreSel->push_back(extreSel);
00774                 //laserCloudExtreProj->push_back(extreProj);
00775                 //laserCloudSel->push_back(tripod1);
00776                 //laserCloudSel->push_back(tripod2);
00777                 coeffSel->push_back(coeff);
00778 
00779                 if (isPointSel) {
00780                   pointSelInd[3 * i] = closestPointInd;
00781                   pointSelInd[3 * i + 1] = minPointInd2;
00782                 }
00783               } else {
00784                 //laserCloudExtreUnsel->push_back(extreSel);
00785               }
00786             }
00787           }
00788         }
00789         int extrePointSelNum = laserCloudExtreOri->points.size();
00790 
00791         if (extrePointSelNum < 10) {
00792           continue;
00793         }
00794 
00795         cv::Mat matA(extrePointSelNum, 6, CV_32F, cv::Scalar::all(0));
00796         cv::Mat matAt(6, extrePointSelNum, CV_32F, cv::Scalar::all(0));
00797         cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
00798         cv::Mat matB(extrePointSelNum, 1, CV_32F, cv::Scalar::all(0));
00799         cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
00800         cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
00801         for (int i = 0; i < extrePointSelNum; i++) {
00802           extreOri = laserCloudExtreOri->points[i];
00803           coeff = coeffSel->points[i];
00804 
00805           float s = (extreOri.h - startTime) / (endTime - startTime);
00806 
00807           float srx = sin(s * transform[0]);
00808           float crx = cos(s * transform[0]);
00809           float sry = sin(s * transform[1]);
00810           float cry = cos(s * transform[1]);
00811           float srz = sin(s * transform[2]);
00812           float crz = cos(s * transform[2]);
00813           float tx = s * transform[3];
00814           float ty = s * transform[4];
00815           float tz = s * transform[5];
00816 
00817           float arx = (-s*crx*sry*srz*extreOri.x + s*crx*crz*sry*extreOri.y + s*srx*sry*extreOri.z 
00818                     + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x
00819                     + (s*srx*srz*extreOri.x - s*crz*srx*extreOri.y + s*crx*extreOri.z
00820                     + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y
00821                     + (s*crx*cry*srz*extreOri.x - s*crx*cry*crz*extreOri.y - s*cry*srx*extreOri.z
00822                     + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;
00823 
00824           float ary = ((-s*crz*sry - s*cry*srx*srz)*extreOri.x 
00825                     + (s*cry*crz*srx - s*sry*srz)*extreOri.y - s*crx*cry*extreOri.z 
00826                     + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) 
00827                     + s*tz*crx*cry) * coeff.x
00828                     + ((s*cry*crz - s*srx*sry*srz)*extreOri.x 
00829                     + (s*cry*srz + s*crz*srx*sry)*extreOri.y - s*crx*sry*extreOri.z
00830                     + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) 
00831                     - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;
00832 
00833           float arz = ((-s*cry*srz - s*crz*srx*sry)*extreOri.x + (s*cry*crz - s*srx*sry*srz)*extreOri.y
00834                     + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x
00835                     + (-s*crx*crz*extreOri.x - s*crx*srz*extreOri.y
00836                     + s*ty*crx*srz + s*tx*crx*crz) * coeff.y
00837                     + ((s*cry*crz*srx - s*sry*srz)*extreOri.x + (s*crz*sry + s*cry*srx*srz)*extreOri.y
00838                     + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;
00839 
00840           float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y 
00841                     - s*(crz*sry + cry*srx*srz) * coeff.z;
00842   
00843           float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y 
00844                     - s*(sry*srz - cry*crz*srx) * coeff.z;
00845   
00846           float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;
00847   
00848           float d2 = coeff.h;
00849 
00850           matA.at<float>(i, 0) = arx;
00851           matA.at<float>(i, 1) = ary;
00852           matA.at<float>(i, 2) = arz;
00853           matA.at<float>(i, 3) = atx;
00854           matA.at<float>(i, 4) = aty;
00855           matA.at<float>(i, 5) = atz;
00856           matB.at<float>(i, 0) = -0.015 * st * d2;
00857         }
00858         cv::transpose(matA, matAt);
00859         matAtA = matAt * matA; //+ 0.1 * cv::Mat::eye(6, 6, CV_32F);
00860         matAtB = matAt * matB;
00861         cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
00862         //cv::solve(matA, matB, matX, cv::DECOMP_SVD);
00863 
00864         if (fabs(matX.at<float>(0, 0)) < 0.005 &&
00865             fabs(matX.at<float>(1, 0)) < 0.005 &&
00866             fabs(matX.at<float>(2, 0)) < 0.005 &&
00867             fabs(matX.at<float>(3, 0)) < 0.01 &&
00868             fabs(matX.at<float>(4, 0)) < 0.01 &&
00869             fabs(matX.at<float>(5, 0)) < 0.01) {
00870 
00871           //transform[0] += 0.7 * matX.at<float>(0, 0);
00872           //transform[1] += 0.7 * matX.at<float>(1, 0);
00873           //transform[2] += 0.7 * matX.at<float>(2, 0);
00874           transform[0] += 0.1 * matX.at<float>(0, 0);
00875           transform[1] += 0.1 * matX.at<float>(1, 0);
00876           transform[2] += 0.1 * matX.at<float>(2, 0);
00877           transform[3] += matX.at<float>(3, 0);
00878           transform[4] += matX.at<float>(4, 0);
00879           transform[5] += matX.at<float>(5, 0);
00880         } else {
00881           //ROS_INFO ("Odometry update out of bound");
00882         }
00883         
00884         float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
00885                      + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
00886                      + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
00887         float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100
00888                      + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100
00889                      + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100);
00890 
00891         if (deltaR < 0.02 && deltaT < 0.02) {
00892           break;
00893         }
00894 
00895         //ROS_INFO ("iter: %d, deltaR: %f, deltaT: %f", iterCount, deltaR, deltaT);
00896       }
00897 
00898       /*sensor_msgs::PointCloud2 pc12;
00899       pcl::toROSMsg(*laserCloudCornerPtr, pc12);
00900       pc12.header.stamp = ros::Time().fromSec(timeLaserCloudExtreCur);
00901       pc12.header.frame_id = "/camera";
00902       pub1.publish(pc12);
00903 
00904       sensor_msgs::PointCloud2 pc22;
00905       pcl::toROSMsg(*laserCloudSurfPtr, pc22);
00906       pc22.header.stamp = ros::Time().fromSec(timeLaserCloudExtreCur);
00907       pc22.header.frame_id = "/camera";
00908       pub2.publish(pc22);
00909 
00910       sensor_msgs::PointCloud2 pc32;
00911       pcl::toROSMsg(*laserCloudExtreSel, pc32);
00912       pc32.header.stamp = ros::Time().fromSec(timeLaserCloudExtreCur);
00913       pc32.header.frame_id = "/camera";
00914       pub3.publish(pc32);
00915 
00916       sensor_msgs::PointCloud2 pc42;
00917       pcl::toROSMsg(*laserCloudExtreUnsel, pc42);
00918       pc42.header.stamp = ros::Time().fromSec(timeLaserCloudExtreCur);
00919       pc42.header.frame_id = "/camera";
00920       pub4.publish(pc42);
00921 
00922       sensor_msgs::PointCloud2 pc52;
00923       pcl::toROSMsg(*laserCloudExtreProj, pc52);
00924       pc52.header.stamp = ros::Time().fromSec(timeLaserCloudExtreCur);
00925       pc52.header.frame_id = "/camera";
00926       pub5.publish(pc52);
00927 
00928       sensor_msgs::PointCloud2 pc62;
00929       pcl::toROSMsg(*laserCloudSel, pc62);
00930       pc62.header.stamp = ros::Time().fromSec(timeLaserCloudExtreCur);
00931       pc62.header.frame_id = "/camera";
00932       pub6.publish(pc62);*/
00933     }
00934 
00935     if (newLaserPoints) {
00936       float rx, ry, rz, tx, ty, tz;
00937       if (sweepEnd) {
00938         AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
00939                            -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);
00940 
00941         float x1 = cos(rz) * (transform[3] - imuShiftFromStartXLast) 
00942                  - sin(rz) * (transform[4] - imuShiftFromStartYLast);
00943         float y1 = sin(rz) * (transform[3] - imuShiftFromStartXLast) 
00944                  + cos(rz) * (transform[4] - imuShiftFromStartYLast);
00945         float z1 = transform[5] * 1.05 - imuShiftFromStartZLast;
00946 
00947         float x2 = x1;
00948         float y2 = cos(rx) * y1 - sin(rx) * z1;
00949         float z2 = sin(rx) * y1 + cos(rx) * z1;
00950 
00951         tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
00952         ty = transformSum[4] - y2;
00953         tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
00954 
00955         PluginIMURotation(rx, ry, rz, imuPitchStartLast, imuYawStartLast, imuRollStartLast, 
00956                           imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
00957 
00958         int laserCloudCornerLastNum = laserCloudCornerLast->points.size();
00959         for (int i = 0; i < laserCloudCornerLastNum; i++) {
00960           TransformToEnd(&laserCloudCornerLast->points[i], &laserCloudCornerLast->points[i], 
00961                          startTimeLast, startTimeCur);
00962         }
00963 
00964         int laserCloudSurfLastNum = laserCloudSurfLast->points.size();
00965         for (int i = 0; i < laserCloudSurfLastNum; i++) {
00966           TransformToEnd(&laserCloudSurfLast->points[i], &laserCloudSurfLast->points[i], 
00967                          startTimeLast, startTimeCur);
00968         }
00969 
00970         TransformReset();
00971 
00972         transformSum[0] = rx;
00973         transformSum[1] = ry;
00974         transformSum[2] = rz;
00975         transformSum[3] = tx;
00976         transformSum[4] = ty;
00977         transformSum[5] = tz;
00978 
00979         sensor_msgs::PointCloud2 laserCloudLast2;
00980         pcl::toROSMsg(*laserCloudCornerLast + *laserCloudSurfLast, laserCloudLast2);
00981         laserCloudLast2.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00982         laserCloudLast2.header.frame_id = "/camera";
00983         pubLaserCloudLast2.publish(laserCloudLast2);
00984 
00985       } else {
00986         AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
00987                            -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);
00988 
00989         float x1 = cos(rz) * (transform[3] - imuShiftFromStartXCur) 
00990                  - sin(rz) * (transform[4] - imuShiftFromStartYCur);
00991         float y1 = sin(rz) * (transform[3] - imuShiftFromStartXCur) 
00992                  + cos(rz) * (transform[4] - imuShiftFromStartYCur);
00993         float z1 = transform[5] * 1.05 - imuShiftFromStartZCur;
00994 
00995         float x2 = x1;
00996         float y2 = cos(rx) * y1 - sin(rx) * z1;
00997         float z2 = sin(rx) * y1 + cos(rx) * z1;
00998 
00999         tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
01000         ty = transformSum[4] - y2;
01001         tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
01002 
01003         PluginIMURotation(rx, ry, rz, imuPitchStartCur, imuYawStartCur, imuRollStartCur, 
01004                           imuPitchCur, imuYawCur, imuRollCur, rx, ry, rz);
01005       }
01006 
01007       geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);
01008 
01009       laserOdometry.pose.pose.orientation.x = -geoQuat.y;
01010       laserOdometry.pose.pose.orientation.y = -geoQuat.z;
01011       laserOdometry.pose.pose.orientation.z = geoQuat.x;
01012       laserOdometry.pose.pose.orientation.w = geoQuat.w;
01013       laserOdometry.pose.pose.position.x = tx;
01014       laserOdometry.pose.pose.position.y = ty;
01015       laserOdometry.pose.pose.position.z = tz;
01016       pubLaserOdometry.publish(laserOdometry);
01017 
01018       laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
01019       laserOdometryTrans.setOrigin(tf::Vector3(tx, ty, tz));
01020       tfBroadcaster.sendTransform(laserOdometryTrans);
01021 
01022       //ROS_INFO ("%f %f %f %f %f %f", transformSum[0], transformSum[1], transformSum[2], 
01023       //                               transformSum[3], transformSum[4], transformSum[5]);
01024     }
01025 
01026     status = ros::ok();
01027     rate.sleep();
01028   }
01029 
01030   return 0;
01031 }


loam_back_and_forth
Author(s): Ji Zhang
autogenerated on Mon Oct 6 2014 01:51:39