scanRegistration.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 double initTime;
00028 double timeStart;
00029 double timeLasted;
00030 bool systemInited = false;
00031 
00032 double timeScanCur = 0;
00033 double timeScanLast = 0;
00034 
00035 int laserRotDir = 1;
00036 float laserAngleLast = 0;
00037 float laserAngleCur = 0;
00038 
00039 int skipFrameNum = 3;
00040 int skipFrameCount = 0;
00041 
00042 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudExtreCur(new pcl::PointCloud<pcl::PointXYZHSV>());
00043 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudLessExtreCur(new pcl::PointCloud<pcl::PointXYZHSV>());
00044 
00045 sensor_msgs::PointCloud2 laserCloudExtreCur2;
00046 sensor_msgs::PointCloud2 laserCloudLast2;
00047 
00048 ros::Publisher* pubLaserCloudExtreCurPointer;
00049 ros::Publisher* pubLaserCloudLastPointer;
00050 
00051 int cloudSortInd[800];
00052 int cloudNeighborPicked[800];
00053 
00054 int imuPointerFront = 0;
00055 int imuPointerLast = -1;
00056 const int imuQueLength = 400;
00057 bool imuInited = false;
00058 
00059 float imuRollStart, imuPitchStart, imuYawStart;
00060 float imuRollCur, imuPitchCur, imuYawCur;
00061 
00062 float imuVeloXStart, imuVeloYStart, imuVeloZStart;
00063 float imuShiftXStart, imuShiftYStart, imuShiftZStart;
00064 float imuVeloXCur, imuVeloYCur, imuVeloZCur;
00065 float imuShiftXCur, imuShiftYCur, imuShiftZCur;
00066 
00067 float imuShiftFromStartXCur, imuShiftFromStartYCur, imuShiftFromStartZCur;
00068 float imuVeloFromStartXCur, imuVeloFromStartYCur, imuVeloFromStartZCur;
00069 
00070 double imuTime[imuQueLength] = {0};
00071 float imuRoll[imuQueLength] = {0};
00072 float imuPitch[imuQueLength] = {0};
00073 float imuYaw[imuQueLength] = {0};
00074 
00075 float imuAccX[imuQueLength] = {0};
00076 float imuAccY[imuQueLength] = {0};
00077 float imuAccZ[imuQueLength] = {0};
00078 
00079 float imuVeloX[imuQueLength] = {0};
00080 float imuVeloY[imuQueLength] = {0};
00081 float imuVeloZ[imuQueLength] = {0};
00082 
00083 float imuShiftX[imuQueLength] = {0};
00084 float imuShiftY[imuQueLength] = {0};
00085 float imuShiftZ[imuQueLength] = {0};
00086 
00087 void ShiftToStartIMU()
00088 {
00089   float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;
00090   float y1 = imuShiftFromStartYCur;
00091   float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;
00092 
00093   float x2 = x1;
00094   float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
00095   float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
00096 
00097   imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
00098   imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
00099   imuShiftFromStartZCur = z2;
00100 }
00101 
00102 void VeloToStartIMU()
00103 {
00104   float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
00105   float y1 = imuVeloFromStartYCur;
00106   float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;
00107 
00108   float x2 = x1;
00109   float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
00110   float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
00111 
00112   imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
00113   imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
00114   imuVeloFromStartZCur = z2;
00115 }
00116 
00117 void TransformToStartIMU(pcl::PointXYZHSV *p)
00118 {
00119   float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;
00120   float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
00121   float z1 = p->z;
00122 
00123   float x2 = x1;
00124   float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
00125   float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;
00126 
00127   float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
00128   float y3 = y2;
00129   float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;
00130 
00131   float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;
00132   float y4 = y3;
00133   float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;
00134 
00135   float x5 = x4;
00136   float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
00137   float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;
00138 
00139   p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;
00140   p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
00141   p->z = z5 + imuShiftFromStartZCur;
00142 }
00143 
00144 void AccumulateIMUShift()
00145 {
00146   float roll = imuRoll[imuPointerLast];
00147   float pitch = imuPitch[imuPointerLast];
00148   float yaw = imuYaw[imuPointerLast];
00149   float accX = imuAccX[imuPointerLast];
00150   float accY = imuAccY[imuPointerLast];
00151   float accZ = imuAccZ[imuPointerLast];
00152 
00153   float x1 = cos(roll) * accX - sin(roll) * accY;
00154   float y1 = sin(roll) * accX + cos(roll) * accY;
00155   float z1 = accZ;
00156 
00157   float x2 = x1;
00158   float y2 = cos(pitch) * y1 - sin(pitch) * z1;
00159   float z2 = sin(pitch) * y1 + cos(pitch) * z1;
00160 
00161   accX = cos(yaw) * x2 + sin(yaw) * z2;
00162   accY = y2;
00163   accZ = -sin(yaw) * x2 + cos(yaw) * z2;
00164 
00165   int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
00166   double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
00167   if (timeDiff < 0.1) {
00168 
00169     imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff 
00170                               + accX * timeDiff * timeDiff / 2;
00171     imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff 
00172                               + accY * timeDiff * timeDiff / 2;
00173     imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff 
00174                               + accZ * timeDiff * timeDiff / 2;
00175 
00176     imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
00177     imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
00178     imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
00179   }
00180 }
00181 
00182 void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn2)
00183 {
00184   if (!systemInited) {
00185     initTime = laserCloudIn2->header.stamp.toSec();
00186     imuPointerFront = (imuPointerLast + 1) % imuQueLength;
00187     systemInited = true;
00188   }
00189 
00190   timeScanLast = timeScanCur;
00191   timeScanCur = laserCloudIn2->header.stamp.toSec();
00192   timeLasted = timeScanCur - initTime;
00193   pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>());
00194   pcl::fromROSMsg(*laserCloudIn2, *laserCloudIn);
00195   int cloudSize = laserCloudIn->points.size();
00196 
00197   pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZHSV>(cloudSize, 1));
00198   for (int i = 0; i < cloudSize; i++) {
00199     laserCloud->points[i].x = laserCloudIn->points[i].x;
00200     laserCloud->points[i].y = laserCloudIn->points[i].y;
00201     laserCloud->points[i].z = laserCloudIn->points[i].z;
00202     laserCloud->points[i].h = timeLasted;
00203     laserCloud->points[i].v = 0;
00204     cloudSortInd[i] = i;
00205     cloudNeighborPicked[i] = 0;
00206   }
00207 
00208   bool newSweep = false;
00209   laserAngleLast = laserAngleCur;
00210   laserAngleCur = atan2(laserCloud->points[cloudSize - 1].y - laserCloud->points[0].y, 
00211                         laserCloud->points[cloudSize - 1].x - laserCloud->points[0].x);
00212 
00213   if (laserAngleLast > 0 && laserRotDir == 1 && laserAngleCur < laserAngleLast) {
00214     laserRotDir = -1;
00215     newSweep = true;
00216   } else if (laserAngleLast < 0 && laserRotDir == -1 && laserAngleCur > laserAngleLast) {
00217     laserRotDir = 1;
00218     newSweep = true;
00219   }
00220 
00221   if (newSweep) {
00222     timeStart = timeScanLast - initTime;
00223 
00224     pcl::PointCloud<pcl::PointXYZHSV>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZHSV>(4, 1));
00225     imuTrans->points[0].x = imuPitchStart;
00226     imuTrans->points[0].y = imuYawStart;
00227     imuTrans->points[0].z = imuRollStart;
00228     imuTrans->points[0].v = 10;
00229 
00230     imuTrans->points[1].x = imuPitchCur;
00231     imuTrans->points[1].y = imuYawCur;
00232     imuTrans->points[1].z = imuRollCur;
00233     imuTrans->points[1].v = 11;
00234 
00235     imuTrans->points[2].x = imuShiftFromStartXCur;
00236     imuTrans->points[2].y = imuShiftFromStartYCur;
00237     imuTrans->points[2].z = imuShiftFromStartZCur;
00238     imuTrans->points[2].v = 12;
00239 
00240     imuTrans->points[3].x = imuVeloFromStartXCur;
00241     imuTrans->points[3].y = imuVeloFromStartYCur;
00242     imuTrans->points[3].z = imuVeloFromStartZCur;
00243     imuTrans->points[3].v = 13;
00244 
00245     *laserCloudExtreCur += *laserCloudLessExtreCur;
00246     pcl::toROSMsg(*laserCloudExtreCur + *imuTrans, laserCloudLast2);
00247     laserCloudLast2.header.stamp = ros::Time().fromSec(timeScanLast);
00248     laserCloudLast2.header.frame_id = "/camera";
00249     laserCloudExtreCur->clear();
00250     laserCloudLessExtreCur->clear();
00251     imuTrans->clear();
00252 
00253     imuRollStart = imuRollCur;
00254     imuPitchStart = imuPitchCur;
00255     imuYawStart = imuYawCur;
00256 
00257     imuVeloXStart = imuVeloXCur;
00258     imuVeloYStart = imuVeloYCur;
00259     imuVeloZStart = imuVeloZCur;
00260 
00261     imuShiftXStart = imuShiftXCur;
00262     imuShiftYStart = imuShiftYCur;
00263     imuShiftZStart = imuShiftZCur;
00264   }
00265 
00266   imuRollCur = 0; imuPitchCur = 0; imuYawCur = 0;
00267   imuVeloXCur = 0; imuVeloYCur = 0; imuVeloZCur = 0;
00268   imuShiftXCur = 0; imuShiftYCur = 0; imuShiftZCur = 0;
00269   if (imuPointerLast >= 0) {
00270     while (imuPointerFront != imuPointerLast) {
00271       if (timeScanCur < imuTime[imuPointerFront]) {
00272         break;
00273       }
00274       imuPointerFront = (imuPointerFront + 1) % imuQueLength;
00275     }
00276 
00277     if (timeScanCur > imuTime[imuPointerFront]) {
00278       imuRollCur = imuRoll[imuPointerFront];
00279       imuPitchCur = imuPitch[imuPointerFront];
00280       imuYawCur = imuYaw[imuPointerFront];
00281 
00282       imuVeloXCur = imuVeloX[imuPointerFront];
00283       imuVeloYCur = imuVeloY[imuPointerFront];
00284       imuVeloZCur = imuVeloZ[imuPointerFront];
00285 
00286       imuShiftXCur = imuShiftX[imuPointerFront];
00287       imuShiftYCur = imuShiftY[imuPointerFront];
00288       imuShiftZCur = imuShiftZ[imuPointerFront];
00289     } else {
00290       int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
00291       float ratioFront = (timeScanCur - imuTime[imuPointerBack]) 
00292                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
00293       float ratioBack = (imuTime[imuPointerFront] - timeScanCur) 
00294                       / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
00295 
00296       imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
00297       imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
00298       if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) {
00299         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack;
00300       } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) {
00301         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack;
00302       } else {
00303         imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
00304       }
00305 
00306       imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
00307       imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
00308       imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
00309 
00310       imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
00311       imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
00312       imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
00313     }
00314   }
00315 
00316   if (!imuInited) {
00317     imuRollStart = imuRollCur;
00318     imuPitchStart = imuPitchCur;
00319     imuYawStart = imuYawCur;
00320 
00321     imuVeloXStart = imuVeloXCur;
00322     imuVeloYStart = imuVeloYCur;
00323     imuVeloZStart = imuVeloZCur;
00324 
00325     imuShiftXStart = imuShiftXCur;
00326     imuShiftYStart = imuShiftYCur;
00327     imuShiftZStart = imuShiftZCur;
00328 
00329     imuInited = true;
00330   }
00331 
00332   imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * (timeLasted - timeStart);
00333   imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * (timeLasted - timeStart);
00334   imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * (timeLasted - timeStart);
00335 
00336   ShiftToStartIMU();
00337 
00338   imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
00339   imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
00340   imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
00341 
00342   VeloToStartIMU();
00343 
00344   for (int i = 0; i < cloudSize; i++) {
00345     TransformToStartIMU(&laserCloud->points[i]);
00346   }
00347 
00348   for (int i = 5; i < cloudSize - 5; i++) {
00349     float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x 
00350                 + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x 
00351                 + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x 
00352                 + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
00353                 + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
00354                 + laserCloud->points[i + 5].x;
00355     float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y 
00356                 + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y 
00357                 + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y 
00358                 + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
00359                 + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
00360                 + laserCloud->points[i + 5].y;
00361     float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z 
00362                 + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z 
00363                 + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z 
00364                 + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
00365                 + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
00366                 + laserCloud->points[i + 5].z;
00367     
00368     laserCloud->points[i].s = diffX * diffX + diffY * diffY + diffZ * diffZ;
00369   }
00370   
00371   for (int i = 5; i < cloudSize - 6; i++) {
00372     float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
00373     float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
00374     float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
00375     float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
00376 
00377     if (diff > 0.05) {
00378 
00379       float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 
00380                      laserCloud->points[i].y * laserCloud->points[i].y +
00381                      laserCloud->points[i].z * laserCloud->points[i].z);
00382 
00383       float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 
00384                      laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
00385                      laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
00386 
00387       if (depth1 > depth2) {
00388         diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
00389         diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
00390         diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
00391 
00392         if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
00393           cloudNeighborPicked[i - 5] = 1;
00394           cloudNeighborPicked[i - 4] = 1;
00395           cloudNeighborPicked[i - 3] = 1;
00396           cloudNeighborPicked[i - 2] = 1;
00397           cloudNeighborPicked[i - 1] = 1;
00398           cloudNeighborPicked[i] = 1;
00399         }
00400       } else {
00401         diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
00402         diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
00403         diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
00404 
00405         if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
00406           cloudNeighborPicked[i + 1] = 1;
00407           cloudNeighborPicked[i + 2] = 1;
00408           cloudNeighborPicked[i + 3] = 1;
00409           cloudNeighborPicked[i + 4] = 1;
00410           cloudNeighborPicked[i + 5] = 1;
00411           cloudNeighborPicked[i + 6] = 1;
00412         }
00413       }
00414     }
00415 
00416     float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
00417     float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
00418     float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
00419     float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
00420 
00421     float dis = laserCloud->points[i].x * laserCloud->points[i].x
00422               + laserCloud->points[i].y * laserCloud->points[i].y
00423               + laserCloud->points[i].z * laserCloud->points[i].z;
00424 
00425     if (diff > (0.25 * 0.25) / (20 * 20) * dis && diff2 > (0.25 * 0.25) / (20 * 20) * dis) {
00426       cloudNeighborPicked[i] = 1;
00427     }
00428   }
00429 
00430   pcl::PointCloud<pcl::PointXYZHSV>::Ptr cornerPointsSharp(new pcl::PointCloud<pcl::PointXYZHSV>());
00431   pcl::PointCloud<pcl::PointXYZHSV>::Ptr cornerPointsLessSharp(new pcl::PointCloud<pcl::PointXYZHSV>());
00432   pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsFlat(new pcl::PointCloud<pcl::PointXYZHSV>());
00433   pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsLessFlat(new pcl::PointCloud<pcl::PointXYZHSV>());
00434 
00435   int startPoints[4] = {5, 6 + int((cloudSize - 10) / 4.0), 
00436                         6 + int((cloudSize - 10) / 2.0), 6 + int(3 * (cloudSize - 10) / 4.0)};
00437   int endPoints[4] = {5 + int((cloudSize - 10) / 4.0), 5 + int((cloudSize - 10) / 2.0), 
00438                       5 + int(3 * (cloudSize - 10) / 4.0), cloudSize - 6};
00439 
00440   for (int i = 0; i < 4; i++) {
00441     int sp = startPoints[i];
00442     int ep = endPoints[i];
00443 
00444     for (int j = sp + 1; j <= ep; j++) {
00445       for (int k = j; k >= sp + 1; k--) {
00446         if (laserCloud->points[cloudSortInd[k]].s < laserCloud->points[cloudSortInd[k - 1]].s) {
00447           int temp = cloudSortInd[k - 1];
00448           cloudSortInd[k - 1] = cloudSortInd[k];
00449           cloudSortInd[k] = temp;
00450         }
00451       }
00452     }
00453 
00454     int largestPickedNum = 0;
00455     for (int j = ep; j >= sp; j--) {
00456       if (cloudNeighborPicked[cloudSortInd[j]] == 0 &&
00457           laserCloud->points[cloudSortInd[j]].s > 0.1 &&
00458           (fabs(laserCloud->points[cloudSortInd[j]].x) > 0.3 || 
00459           fabs(laserCloud->points[cloudSortInd[j]].y) > 0.3 || 
00460           fabs(laserCloud->points[cloudSortInd[j]].z) > 0.3) && 
00461           fabs(laserCloud->points[cloudSortInd[j]].x) < 30 && 
00462           fabs(laserCloud->points[cloudSortInd[j]].y) < 30 && 
00463           fabs(laserCloud->points[cloudSortInd[j]].z) < 30) {
00464         
00465         largestPickedNum++;
00466         if (largestPickedNum <= 2) {
00467           laserCloud->points[cloudSortInd[j]].v = 2;
00468           cornerPointsSharp->push_back(laserCloud->points[cloudSortInd[j]]);
00469         } else if (largestPickedNum <= 20) {
00470           laserCloud->points[cloudSortInd[j]].v = 1;
00471           cornerPointsLessSharp->push_back(laserCloud->points[cloudSortInd[j]]);
00472         } else {
00473           break;
00474         }
00475 
00476         cloudNeighborPicked[cloudSortInd[j]] = 1;
00477         for (int k = 1; k <= 5; k++) {
00478           float diffX = laserCloud->points[cloudSortInd[j] + k].x 
00479                       - laserCloud->points[cloudSortInd[j] + k - 1].x;
00480           float diffY = laserCloud->points[cloudSortInd[j] + k].y 
00481                       - laserCloud->points[cloudSortInd[j] + k - 1].y;
00482           float diffZ = laserCloud->points[cloudSortInd[j] + k].z 
00483                       - laserCloud->points[cloudSortInd[j] + k - 1].z;
00484           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
00485             break;
00486           }
00487 
00488           cloudNeighborPicked[cloudSortInd[j] + k] = 1;
00489         }
00490         for (int k = -1; k >= -5; k--) {
00491           float diffX = laserCloud->points[cloudSortInd[j] + k].x 
00492                       - laserCloud->points[cloudSortInd[j] + k + 1].x;
00493           float diffY = laserCloud->points[cloudSortInd[j] + k].y 
00494                       - laserCloud->points[cloudSortInd[j] + k + 1].y;
00495           float diffZ = laserCloud->points[cloudSortInd[j] + k].z 
00496                       - laserCloud->points[cloudSortInd[j] + k + 1].z;
00497           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
00498             break;
00499           }
00500 
00501           cloudNeighborPicked[cloudSortInd[j] + k] = 1;
00502         }
00503       }
00504     }
00505 
00506     int smallestPickedNum = 0;
00507     for (int j = sp; j <= ep; j++) {
00508       if (cloudNeighborPicked[cloudSortInd[j]] == 0 &&
00509           laserCloud->points[cloudSortInd[j]].s < 0.1 &&
00510           (fabs(laserCloud->points[cloudSortInd[j]].x) > 0.3 || 
00511           fabs(laserCloud->points[cloudSortInd[j]].y) > 0.3 || 
00512           fabs(laserCloud->points[cloudSortInd[j]].z) > 0.3) && 
00513           fabs(laserCloud->points[cloudSortInd[j]].x) < 30 && 
00514           fabs(laserCloud->points[cloudSortInd[j]].y) < 30 && 
00515           fabs(laserCloud->points[cloudSortInd[j]].z) < 30) {
00516 
00517         laserCloud->points[cloudSortInd[j]].v = -1;
00518         surfPointsFlat->push_back(laserCloud->points[cloudSortInd[j]]);
00519 
00520         smallestPickedNum++;
00521         if (smallestPickedNum >= 4) {
00522           break;
00523         }
00524 
00525         cloudNeighborPicked[cloudSortInd[j]] = 1;
00526         for (int k = 1; k <= 5; k++) {
00527           float diffX = laserCloud->points[cloudSortInd[j] + k].x 
00528                       - laserCloud->points[cloudSortInd[j] + k - 1].x;
00529           float diffY = laserCloud->points[cloudSortInd[j] + k].y 
00530                       - laserCloud->points[cloudSortInd[j] + k - 1].y;
00531           float diffZ = laserCloud->points[cloudSortInd[j] + k].z 
00532                       - laserCloud->points[cloudSortInd[j] + k - 1].z;
00533           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
00534             break;
00535           }
00536 
00537           cloudNeighborPicked[cloudSortInd[j] + k] = 1;
00538         }
00539         for (int k = -1; k >= -5; k--) {
00540           float diffX = laserCloud->points[cloudSortInd[j] + k].x 
00541                       - laserCloud->points[cloudSortInd[j] + k + 1].x;
00542           float diffY = laserCloud->points[cloudSortInd[j] + k].y 
00543                       - laserCloud->points[cloudSortInd[j] + k + 1].y;
00544           float diffZ = laserCloud->points[cloudSortInd[j] + k].z 
00545                       - laserCloud->points[cloudSortInd[j] + k + 1].z;
00546           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
00547             break;
00548           }
00549 
00550           cloudNeighborPicked[cloudSortInd[j] + k] = 1;
00551         }
00552       }
00553     }
00554   }
00555 
00556   for (int i = 0; i < cloudSize; i++) {
00557     if (laserCloud->points[i].v == 0) {
00558       surfPointsLessFlat->push_back(laserCloud->points[i]);
00559     }
00560   }
00561 
00562   pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsLessFlatDS(new pcl::PointCloud<pcl::PointXYZHSV>());
00563   pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter;
00564   downSizeFilter.setInputCloud(surfPointsLessFlat);
00565   downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
00566   downSizeFilter.filter(*surfPointsLessFlatDS);
00567 
00568   *laserCloudExtreCur += *cornerPointsSharp;
00569   *laserCloudExtreCur += *surfPointsFlat;
00570   *laserCloudLessExtreCur += *cornerPointsLessSharp;
00571   *laserCloudLessExtreCur += *surfPointsLessFlatDS;
00572 
00573   laserCloudIn->clear();
00574   laserCloud->clear();
00575   cornerPointsSharp->clear();
00576   cornerPointsLessSharp->clear();
00577   surfPointsFlat->clear();
00578   surfPointsLessFlat->clear();
00579   surfPointsLessFlatDS->clear();
00580 
00581   if (skipFrameCount >= skipFrameNum) {
00582     skipFrameCount = 0;
00583 
00584     pcl::PointCloud<pcl::PointXYZHSV>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZHSV>(4, 1));
00585     imuTrans->points[0].x = imuPitchStart;
00586     imuTrans->points[0].y = imuYawStart;
00587     imuTrans->points[0].z = imuRollStart;
00588     imuTrans->points[0].v = 10;
00589 
00590     imuTrans->points[1].x = imuPitchCur;
00591     imuTrans->points[1].y = imuYawCur;
00592     imuTrans->points[1].z = imuRollCur;
00593     imuTrans->points[1].v = 11;
00594 
00595     imuTrans->points[2].x = imuShiftFromStartXCur;
00596     imuTrans->points[2].y = imuShiftFromStartYCur;
00597     imuTrans->points[2].z = imuShiftFromStartZCur;
00598     imuTrans->points[2].v = 12;
00599 
00600     imuTrans->points[3].x = imuVeloFromStartXCur;
00601     imuTrans->points[3].y = imuVeloFromStartYCur;
00602     imuTrans->points[3].z = imuVeloFromStartZCur;
00603     imuTrans->points[3].v = 13;
00604 
00605     sensor_msgs::PointCloud2 laserCloudExtreCur2;
00606     pcl::toROSMsg(*laserCloudExtreCur + *imuTrans, laserCloudExtreCur2);
00607     laserCloudExtreCur2.header.stamp = ros::Time().fromSec(timeScanCur);
00608     laserCloudExtreCur2.header.frame_id = "/camera";
00609     pubLaserCloudExtreCurPointer->publish(laserCloudExtreCur2);
00610     imuTrans->clear();
00611 
00612     pubLaserCloudLastPointer->publish(laserCloudLast2);
00613 
00614     //ROS_INFO ("%d %d", laserCloudLast2.width, laserCloudExtreCur2.width);
00615   }
00616   skipFrameCount++;
00617 }
00618 
00619 void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
00620 {
00621   double roll, pitch, yaw;
00622   tf::Quaternion orientation;
00623   tf::quaternionMsgToTF(imuIn->orientation, orientation);
00624   tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
00625 
00626   float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
00627   float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
00628   float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
00629 
00630   imuPointerLast = (imuPointerLast + 1) % imuQueLength;
00631 
00632   imuTime[imuPointerLast] = imuIn->header.stamp.toSec() - 0.1068;
00633   imuRoll[imuPointerLast] = roll;
00634   imuPitch[imuPointerLast] = pitch;
00635   imuYaw[imuPointerLast] = yaw;
00636   imuAccX[imuPointerLast] = accX;
00637   imuAccY[imuPointerLast] = accY;
00638   imuAccZ[imuPointerLast] = accZ;
00639 
00640   AccumulateIMUShift();
00641 }
00642 
00643 int main(int argc, char** argv)
00644 {
00645   ros::init(argc, argv, "scanRegistration");
00646   ros::NodeHandle nh;
00647 
00648   ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> 
00649                                   ("/sync_scan_cloud_filtered", 2, laserCloudHandler);
00650 
00651   ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> 
00652                            ("/imu/data", 5, imuHandler);
00653 
00654   ros::Publisher pubLaserCloudExtreCur = nh.advertise<sensor_msgs::PointCloud2> 
00655                                          ("/laser_cloud_extre_cur", 2);
00656 
00657   ros::Publisher pubLaserCloudLast = nh.advertise<sensor_msgs::PointCloud2> 
00658                                      ("/laser_cloud_last", 2);
00659 
00660   pubLaserCloudExtreCurPointer = &pubLaserCloudExtreCur;
00661   pubLaserCloudLastPointer = &pubLaserCloudLast;
00662 
00663   ros::spin();
00664 
00665   return 0;
00666 }


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