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


loam_continuous
Author(s): Ji Zhang
autogenerated on Mon Oct 6 2014 01:53:01