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_conversions/pcl_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
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 }