/**
* The message from the point cloud go here.
*/
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn2)
{
// ROS_INFO_STREAM("Handler initiated: " << ros::Time::now());
if (!systemInited) {
initTime = laserCloudIn2->header.stamp.toSec(); // initialize the start of the point cloud
imuPointerFront = (imuPointerLast + 1) % imuQueLength;
systemInited = true;
}
timeScanLast = timeScanCur; // initially 0
timeScanCur = laserCloudIn2->header.stamp.toSec(); // timestamp of measurement
timeLasted = timeScanCur - initTime; // first measurement is 0, then record the time from the very first measurements (system init)
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>()); // PointXYZ - Euclidean xyz coordinates
pcl::fromROSMsg(*laserCloudIn2, *laserCloudIn); // convert from PointCloud2 to PointCloud
int cloudInSize = laserCloudIn->points.size();
int cloudSize = 0;
pcl::PointXYZHSV laserPointIn;
pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZHSV>()); // PointXYZHSV - xyz with hue, saturation and value
// fill the new pointcloud laserCloud (XYZHSV) with the laserCloudIn (PointCloud2) via the var laserPointIn (PointXYZHSV)
for (int i = 0; i < cloudInSize; i++) {
// adopt the x y z values
laserPointIn.x = laserCloudIn->points[i].x;
laserPointIn.y = laserCloudIn->points[i].y;
laserPointIn.z = laserCloudIn->points[i].z;
laserPointIn.h = timeLasted; // hue - time from the initialization of the system, saturation is calculated afterwards
laserPointIn.v = 0; // value is always 0, as this is the definition for obtained point
if (!(fabs(laserPointIn.x) < 1.5 && fabs(laserPointIn.y) < 1.5 && fabs(laserPointIn.z) < 1.5)) {
laserCloud->push_back(laserPointIn); // push a new point at the end of the container (?)
cloudSortInd[cloudSize] = cloudSize;
cloudNeighborPicked[cloudSize] = 0;
cloudSize++;
}
}
// ROS_INFO("The size of the cloud points incomming is: %d, new cloudSize is: %d", cloudInSize, cloudSize);
// get the first and last laser points
pcl::PointXYZ laserPointFirst = laserCloudIn->points[0];
pcl::PointXYZ laserPointLast = laserCloudIn->points[cloudInSize - 1];
// spherical coordinates
// first point range = sqrt(x^2 + y^2 + z^2)
float rangeFirst = sqrt(laserPointFirst.x * laserPointFirst.x + laserPointFirst.y * laserPointFirst.y
+ laserPointFirst.z * laserPointFirst.z);
// normalize (?) x y z
laserPointFirst.x /= rangeFirst;
laserPointFirst.y /= rangeFirst;
laserPointFirst.z /= rangeFirst;
// last point range = sqrt(x^2 + y^2 + z^2)
float rangeLast = sqrt(laserPointLast.x * laserPointLast.x + laserPointLast.y * laserPointLast.y
+ laserPointLast.z * laserPointLast.z);
laserPointLast.x /= rangeLast;
laserPointLast.y /= rangeLast;
laserPointLast.z /= rangeLast;
// DEBUG prupose
// show in topic the first and last points and then the angle that is being calculated
// ROS_INFO_STREAM("Cloud size: " << cloudSize);
// ROS_INFO_STREAM("last: " << laserPointLast);
// ROS_INFO_STREAM("first: " << laserPointFirst);
// push the first point to its own pointcloud
pcl::PointCloud<pcl::PointXYZ>::Ptr laserFirstPointCloud(new pcl::PointCloud<pcl::PointXYZ>());
laserFirstPointCloud->push_back(laserPointFirst);
sensor_msgs::PointCloud2 laserPointFirstCloud2;
pcl::toROSMsg(*laserFirstPointCloud, laserPointFirstCloud2);
laserPointFirstCloud2.header.stamp = laserCloudIn2->header.stamp;
laserPointFirstCloud2.header.frame_id = "/camera";
pubFirstPointPointer->publish(laserPointFirstCloud2);
// and the last point to its own pointcloud
pcl::PointCloud<pcl::PointXYZ>::Ptr laserLastPointCloud(new pcl::PointCloud<pcl::PointXYZ>());
laserLastPointCloud->push_back(laserPointLast);
sensor_msgs::PointCloud2 laserPointLastCloud2;
pcl::toROSMsg(*laserLastPointCloud, laserPointLastCloud2);
laserPointLastCloud2.header.stamp = laserCloudIn2->header.stamp;
laserPointLastCloud2.header.frame_id = "/camera";
pubLastPointPointer->publish(laserPointLastCloud2);
// the angle of the laser
// atan2(x,y) - angle b/w two coordinates x, y
// supposedly this is the angle the laser is showing
float laserAngle = atan2(laserPointLast.x - laserPointFirst.x, laserPointLast.y - laserPointFirst.y);
// DEBUG purpose
// publish the laser angle as a channel to be plotted
laserAngle2.data = laserAngle*rad2deg;
pubLaserAnglePointer->publish(laserAngle2);
// ROS_INFO("Laser angle: %f", (laserAngle * rad2deg));
bool newSweep = false;
if (laserAngle * laserRotDir < 0 && timeLasted - timeStart > 0.7) {
laserRotDir *= -1;
newSweep = true;
// ROS_INFO("New sweep!! Laser angle: %f", (laserAngle * rad2deg));
}
// reinitalise the values if there's a new sweep
if (newSweep) {
timeStart = timeScanLast - initTime; // compute the starting time of this particular scan
pcl::PointCloud<pcl::PointXYZHSV>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZHSV>(4, 1));
imuTrans->points[0].x = imuPitchStart;
imuTrans->points[0].y = imuYawStart;
imuTrans->points[0].z = imuRollStart;
imuTrans->points[0].v = 10;
imuTrans->points[1].x = imuPitchCur;
imuTrans->points[1].y = imuYawCur;
imuTrans->points[1].z = imuRollCur;
imuTrans->points[1].v = 11;
imuTrans->points[2].x = imuShiftFromStartXCur;
imuTrans->points[2].y = imuShiftFromStartYCur;
imuTrans->points[2].z = imuShiftFromStartZCur;
imuTrans->points[2].v = 12;
imuTrans->points[3].x = imuVeloFromStartXCur;
imuTrans->points[3].y = imuVeloFromStartYCur;
imuTrans->points[3].z = imuVeloFromStartZCur;
imuTrans->points[3].v = 13;
*laserCloudExtreCur += *laserCloudLessExtreCur; // add the laserCloudLessExtreCur to laserCloudExtreCur
pcl::toROSMsg(*laserCloudExtreCur + *imuTrans, laserCloudLast2); // translate laserCloudExtreCur + imuTrans (pcl:PointCloud) to sensorMsg::PointCloud2
laserCloudLast2.header.stamp = ros::Time().fromSec(timeScanLast);
laserCloudLast2.header.frame_id = "/camera";
laserCloudExtreCur->clear();
laserCloudLessExtreCur->clear();
imuTrans->clear();
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
}
// reset the imu values
imuRollCur = 0; imuPitchCur = 0; imuYawCur = 0;
imuVeloXCur = 0; imuVeloYCur = 0; imuVeloZCur = 0;
imuShiftXCur = 0; imuShiftYCur = 0; imuShiftZCur = 0;
if (imuPointerLast >= 0) {
while (imuPointerFront != imuPointerLast) {
if (timeScanCur < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
if (timeScanCur > imuTime[imuPointerFront]) {
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
} else {
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
}
// initiate the very first imu measurement
if (!imuInited) {
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
imuInited = true;
}
// calculate the shift from the starting imu
imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * (timeLasted - timeStart);
imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * (timeLasted - timeStart);
imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * (timeLasted - timeStart);
// float befImuShiftFromStartXCur = imuShiftFromStartXCur;
// float befImuShiftFromStartYCur = imuShiftFromStartYCur;
// float befImuShiftFromStartZCur = imuShiftFromStartZCur;
ShiftToStartIMU();
//ROS_INFO("Imu Shift X: (bef, %f), (aft, %f); Y: (bef, %f), (aft, %f), Z: (bef, %f), (aft, %f)", befImuShiftFromStartXCur, imuShiftFromStartXCur, befImuShiftFromStartYCur, imuShiftFromStartYCur, befImuShiftFromStartZCur, imuShiftFromStartZCur);
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
VeloToStartIMU();
for (int i = 0; i < cloudSize; i++) {
TransformToStartIMU(&laserCloud->points[i]);
}
// calculate smoothness value based on the 5 neighbours on each side
for (int i = 5; i < cloudSize - 5; i++) {
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x
+ laserCloud->points[i - 3].x + laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x
+ laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
+ laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y
+ laserCloud->points[i - 3].y + laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y
+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
+ laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z
+ laserCloud->points[i - 3].z + laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z
+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
+ laserCloud->points[i + 5].z;
// calculate smoothness
laserCloud->points[i].s = diffX * diffX + diffY * diffY + diffZ * diffZ;
}
float m = 0;
for (int i = 5; i < cloudSize - 6; i++) {
// calculate difference with the neighbour
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
// incremental average calculation of the diff
m = m + ((diff-m)/(cloudSize-12));
// if there's a big difference
if (diff > 0.05) {
// depth1 - current point's score
float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
// depth2 - neighbour point's score
float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
// filter out points that are picked based on depth
if (depth1 > depth2) {
diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}
} else {
diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
// ROS_INFO("Point %d:", i);
// for (int pp=0; pp < cloudSize; pp++) {
// ROS_INFO("N: %d", cloudNeighborPicked[pp]);
// }
// ros::Duration(5.0).sleep();
}
float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
float dis = laserCloud->points[i].x * laserCloud->points[i].x
+ laserCloud->points[i].y * laserCloud->points[i].y
+ laserCloud->points[i].z * laserCloud->points[i].z;
if (diff > (0.25 * 0.25) / (20 * 20) * dis && diff2 > (0.25 * 0.25) / (20 * 20) * dis) {
cloudNeighborPicked[i] = 1;
}
}
// ROS_INFO("avrg difference: %f", m);
// if(newSweep) {
// mean_sweep = mean_sweep + ((m-mean_sweep)/10);
// ROS_INFO("avrg per sweep: %f", mean_sweep);
// }
// define the edge and surface areas
pcl::PointCloud<pcl::PointXYZHSV>::Ptr cornerPointsSharp(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::PointCloud<pcl::PointXYZHSV>::Ptr cornerPointsLessSharp(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsFlat(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsLessFlat(new pcl::PointCloud<pcl::PointXYZHSV>());
int startPoints[4] = {5, 6 + int((cloudSize - 10) / 4.0),
6 + int((cloudSize - 10) / 2.0), 6 + int(3 * (cloudSize - 10) / 4.0)};
int endPoints[4] = {5 + int((cloudSize - 10) / 4.0), 5 + int((cloudSize - 10) / 2.0),
5 + int(3 * (cloudSize - 10) / 4.0), cloudSize - 6};
for (int i = 0; i < 4; i++) {
int sp = startPoints[i];
int ep = endPoints[i];
// sort based on the smoothness value
for (int j = sp + 1; j <= ep; j++) {
for (int k = j; k >= sp + 1; k--) {
if (laserCloud->points[cloudSortInd[k]].s < laserCloud->points[cloudSortInd[k - 1]].s) {
int temp = cloudSortInd[k - 1];
cloudSortInd[k - 1] = cloudSortInd[k];
cloudSortInd[k] = temp;
}
}
}
// select the maximum smoothness - edge points
int largestPickedNum = 0;
for (int j = ep; j >= sp; j--) {
if (cloudNeighborPicked[cloudSortInd[j]] == 0 &&
laserCloud->points[cloudSortInd[j]].s > 0.1 &&
(fabs(laserCloud->points[cloudSortInd[j]].x) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].y) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].z) > 0.3) &&
fabs(laserCloud->points[cloudSortInd[j]].x) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].y) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].z) < 30) {
largestPickedNum++;
if (largestPickedNum <= 2) {
// value of 2 means it's a an edge
laserCloud->points[cloudSortInd[j]].v = 2;
cornerPointsSharp->push_back(laserCloud->points[cloudSortInd[j]]);
} else if (largestPickedNum <= 20) {
laserCloud->points[cloudSortInd[j]].v = 1;
cornerPointsLessSharp->push_back(laserCloud->points[cloudSortInd[j]]);
} else {
break;
}
cloudNeighborPicked[cloudSortInd[j]] = 1;
for (int k = 1; k <= 5; k++) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k - 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k - 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
for (int k = -1; k >= -5; k--) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k + 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k + 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
}
}
// select the minimum smoothness - planar points
int smallestPickedNum = 0;
for (int j = sp; j <= ep; j++) {
if (cloudNeighborPicked[cloudSortInd[j]] == 0 &&
laserCloud->points[cloudSortInd[j]].s < 0.1 &&
(fabs(laserCloud->points[cloudSortInd[j]].x) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].y) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].z) > 0.3) &&
fabs(laserCloud->points[cloudSortInd[j]].x) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].y) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].z) < 30) {
laserCloud->points[cloudSortInd[j]].v = -1;
surfPointsFlat->push_back(laserCloud->points[cloudSortInd[j]]);
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[cloudSortInd[j]] = 1;
for (int k = 1; k <= 5; k++) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k - 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k - 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
for (int k = -1; k >= -5; k--) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k + 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k + 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
}
}
}
for (int i = 0; i < cloudSize; i++) {
if (laserCloud->points[i].v == 0) {
surfPointsLessFlat->push_back(laserCloud->points[i]);
}
}
// downsize filter of the planar areas (voxel in pcl)
pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsLessFlatDS(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlat);
downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
downSizeFilter.filter(*surfPointsLessFlatDS);
// DEBUG purpose
// publish only the corner/flat points
sensor_msgs::PointCloud2 cornerPointsSharp2;
pcl::toROSMsg(*cornerPointsSharp, cornerPointsSharp2);
cornerPointsSharp2.header.stamp = laserCloudIn2->header.stamp;
cornerPointsSharp2.header.frame_id = "/camera";
pubCornerPointsSharpPointer->publish(cornerPointsSharp2);
sensor_msgs::PointCloud2 cornerPointsLessSharp2;
pcl::toROSMsg(*cornerPointsLessSharp, cornerPointsLessSharp2);
cornerPointsLessSharp2.header.stamp = laserCloudIn2->header.stamp;
cornerPointsLessSharp2.header.frame_id = "/camera";
pubCornerPointsLessSharpPointer->publish(cornerPointsLessSharp2);
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(*surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudIn2->header.stamp;
surfPointsFlat2.header.frame_id = "/camera";
pubSurfPointsFlatPointer->publish(surfPointsFlat2);
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(*surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudIn2->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera";
pubSurfPointsLessFlatPointer->publish(surfPointsLessFlat2);
*laserCloudExtreCur += *cornerPointsSharp;
*laserCloudExtreCur += *surfPointsFlat;
*laserCloudLessExtreCur += *cornerPointsLessSharp;
*laserCloudLessExtreCur += *surfPointsLessFlatDS;
laserCloudIn->clear();
laserCloud->clear();
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();
surfPointsLessFlatDS->clear();
if (skipFrameCount >= skipFrameNum) {
skipFrameCount = 0;
pcl::PointCloud<pcl::PointXYZHSV>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZHSV>(4, 1));
imuTrans->points[0].x = imuPitchStart;
imuTrans->points[0].y = imuYawStart;
imuTrans->points[0].z = imuRollStart;
imuTrans->points[0].v = 10;
imuTrans->points[1].x = imuPitchCur;
imuTrans->points[1].y = imuYawCur;
imuTrans->points[1].z = imuRollCur;
imuTrans->points[1].v = 11;
imuTrans->points[2].x = imuShiftFromStartXCur;
imuTrans->points[2].y = imuShiftFromStartYCur;
imuTrans->points[2].z = imuShiftFromStartZCur;
imuTrans->points[2].v = 12;
imuTrans->points[3].x = imuVeloFromStartXCur;
imuTrans->points[3].y = imuVeloFromStartYCur;
imuTrans->points[3].z = imuVeloFromStartZCur;
imuTrans->points[3].v = 13;
// publish only the imu translation
sensor_msgs::PointCloud2 imuTrans2;
pcl::toROSMsg(*imuTrans, imuTrans2);
imuTrans2.header.stamp = laserCloudIn2->header.stamp;
imuTrans2.header.frame_id = "/camera";
pubImuTransPointer->publish(imuTrans2);
sensor_msgs::PointCloud2 laserCloudExtreCur2;
pcl::toROSMsg(*laserCloudExtreCur + *imuTrans, laserCloudExtreCur2);
laserCloudExtreCur2.header.stamp = ros::Time().fromSec(timeScanCur);
laserCloudExtreCur2.header.frame_id = "/camera";
pubLaserCloudExtreCurPointer->publish(laserCloudExtreCur2);
imuTrans->clear();
pubLaserCloudLastPointer->publish(laserCloudLast2); // this is tha last registration before the new sweep
// ROS_INFO ("%d %d", laserCloudLast2.width, laserCloudExtreCur2.width);
}
skipFrameCount++;
}
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn2)
{
if (!systemInited) {
initTime = laserCloudIn2->header.stamp.toSec();
imuPointerFront = (imuPointerLast + 1) % imuQueLength;
systemInited = true;
}
timeScanLast = timeScanCur;
timeScanCur = laserCloudIn2->header.stamp.toSec();
timeLasted = timeScanCur - initTime;
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(*laserCloudIn2, *laserCloudIn);
int cloudSize = laserCloudIn->points.size();
pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZHSV>(cloudSize, 1));
for (int i = 0; i < cloudSize; i++) {
laserCloud->points[i].x = laserCloudIn->points[i].x;
laserCloud->points[i].y = laserCloudIn->points[i].y;
laserCloud->points[i].z = laserCloudIn->points[i].z;
laserCloud->points[i].h = timeLasted;
laserCloud->points[i].v = 0;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
}
bool newSweep = false;
laserAngleLast = laserAngleCur;
laserAngleCur = atan2(laserCloud->points[cloudSize - 1].y - laserCloud->points[0].y,
laserCloud->points[cloudSize - 1].x - laserCloud->points[0].x);
if (laserAngleLast > 0 && laserRotDir == 1 && laserAngleCur < laserAngleLast) {
laserRotDir = -1;
newSweep = true;
} else if (laserAngleLast < 0 && laserRotDir == -1 && laserAngleCur > laserAngleLast) {
laserRotDir = 1;
newSweep = true;
}
if (newSweep) {
timeStart = timeScanLast - initTime;
pcl::PointCloud<pcl::PointXYZHSV>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZHSV>(4, 1));
imuTrans->points[0].x = imuPitchStart;
imuTrans->points[0].y = imuYawStart;
imuTrans->points[0].z = imuRollStart;
imuTrans->points[0].v = 10;
imuTrans->points[1].x = imuPitchCur;
imuTrans->points[1].y = imuYawCur;
imuTrans->points[1].z = imuRollCur;
imuTrans->points[1].v = 11;
imuTrans->points[2].x = imuShiftFromStartXCur;
imuTrans->points[2].y = imuShiftFromStartYCur;
imuTrans->points[2].z = imuShiftFromStartZCur;
imuTrans->points[2].v = 12;
imuTrans->points[3].x = imuVeloFromStartXCur;
imuTrans->points[3].y = imuVeloFromStartYCur;
imuTrans->points[3].z = imuVeloFromStartZCur;
imuTrans->points[3].v = 13;
*laserCloudExtreCur += *laserCloudLessExtreCur;
pcl::toROSMsg(*laserCloudExtreCur + *imuTrans, laserCloudLast2);
laserCloudLast2.header.stamp = ros::Time().fromSec(timeScanLast);
laserCloudLast2.header.frame_id = "/camera";
laserCloudExtreCur->clear();
laserCloudLessExtreCur->clear();
imuTrans->clear();
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
}
imuRollCur = 0; imuPitchCur = 0; imuYawCur = 0;
imuVeloXCur = 0; imuVeloYCur = 0; imuVeloZCur = 0;
imuShiftXCur = 0; imuShiftYCur = 0; imuShiftZCur = 0;
if (imuPointerLast >= 0) {
while (imuPointerFront != imuPointerLast) {
if (timeScanCur < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
if (timeScanCur > imuTime[imuPointerFront]) {
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
} else {
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
}
if (!imuInited) {
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
imuInited = true;
}
imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * (timeLasted - timeStart);
imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * (timeLasted - timeStart);
imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * (timeLasted - timeStart);
ShiftToStartIMU();
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
VeloToStartIMU();
for (int i = 0; i < cloudSize; i++) {
TransformToStartIMU(&laserCloud->points[i]);
}
for (int i = 5; i < cloudSize - 5; i++) {
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x
+ laserCloud->points[i - 3].x + laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x
+ laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
+ laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y
+ laserCloud->points[i - 3].y + laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y
+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
+ laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z
+ laserCloud->points[i - 3].z + laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z
+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
+ laserCloud->points[i + 5].z;
laserCloud->points[i].s = diffX * diffX + diffY * diffY + diffZ * diffZ;
}
for (int i = 5; i < cloudSize - 6; i++) {
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
if (diff > 0.05) {
float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
if (depth1 > depth2) {
diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}
} else {
diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
}
float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
float dis = laserCloud->points[i].x * laserCloud->points[i].x
+ laserCloud->points[i].y * laserCloud->points[i].y
+ laserCloud->points[i].z * laserCloud->points[i].z;
if (diff > (0.25 * 0.25) / (20 * 20) * dis && diff2 > (0.25 * 0.25) / (20 * 20) * dis) {
cloudNeighborPicked[i] = 1;
}
}
pcl::PointCloud<pcl::PointXYZHSV>::Ptr cornerPointsSharp(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::PointCloud<pcl::PointXYZHSV>::Ptr cornerPointsLessSharp(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsFlat(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsLessFlat(new pcl::PointCloud<pcl::PointXYZHSV>());
int startPoints[4] = {5, 6 + int((cloudSize - 10) / 4.0),
6 + int((cloudSize - 10) / 2.0), 6 + int(3 * (cloudSize - 10) / 4.0)};
int endPoints[4] = {5 + int((cloudSize - 10) / 4.0), 5 + int((cloudSize - 10) / 2.0),
5 + int(3 * (cloudSize - 10) / 4.0), cloudSize - 6};
for (int i = 0; i < 4; i++) {
int sp = startPoints[i];
int ep = endPoints[i];
for (int j = sp + 1; j <= ep; j++) {
for (int k = j; k >= sp + 1; k--) {
if (laserCloud->points[cloudSortInd[k]].s < laserCloud->points[cloudSortInd[k - 1]].s) {
int temp = cloudSortInd[k - 1];
cloudSortInd[k - 1] = cloudSortInd[k];
cloudSortInd[k] = temp;
}
}
}
int largestPickedNum = 0;
for (int j = ep; j >= sp; j--) {
if (cloudNeighborPicked[cloudSortInd[j]] == 0 &&
laserCloud->points[cloudSortInd[j]].s > 0.1 &&
(fabs(laserCloud->points[cloudSortInd[j]].x) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].y) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].z) > 0.3) &&
fabs(laserCloud->points[cloudSortInd[j]].x) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].y) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].z) < 30) {
largestPickedNum++;
if (largestPickedNum <= 2) {
laserCloud->points[cloudSortInd[j]].v = 2;
cornerPointsSharp->push_back(laserCloud->points[cloudSortInd[j]]);
} else if (largestPickedNum <= 20) {
laserCloud->points[cloudSortInd[j]].v = 1;
cornerPointsLessSharp->push_back(laserCloud->points[cloudSortInd[j]]);
} else {
break;
}
cloudNeighborPicked[cloudSortInd[j]] = 1;
for (int k = 1; k <= 5; k++) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k - 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k - 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
for (int k = -1; k >= -5; k--) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k + 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k + 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
}
}
int smallestPickedNum = 0;
for (int j = sp; j <= ep; j++) {
if (cloudNeighborPicked[cloudSortInd[j]] == 0 &&
laserCloud->points[cloudSortInd[j]].s < 0.1 &&
(fabs(laserCloud->points[cloudSortInd[j]].x) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].y) > 0.3 ||
fabs(laserCloud->points[cloudSortInd[j]].z) > 0.3) &&
fabs(laserCloud->points[cloudSortInd[j]].x) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].y) < 30 &&
fabs(laserCloud->points[cloudSortInd[j]].z) < 30) {
laserCloud->points[cloudSortInd[j]].v = -1;
surfPointsFlat->push_back(laserCloud->points[cloudSortInd[j]]);
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[cloudSortInd[j]] = 1;
for (int k = 1; k <= 5; k++) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k - 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k - 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
for (int k = -1; k >= -5; k--) {
float diffX = laserCloud->points[cloudSortInd[j] + k].x
- laserCloud->points[cloudSortInd[j] + k + 1].x;
float diffY = laserCloud->points[cloudSortInd[j] + k].y
- laserCloud->points[cloudSortInd[j] + k + 1].y;
float diffZ = laserCloud->points[cloudSortInd[j] + k].z
- laserCloud->points[cloudSortInd[j] + k + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[cloudSortInd[j] + k] = 1;
}
}
}
}
for (int i = 0; i < cloudSize; i++) {
if (laserCloud->points[i].v == 0) {
surfPointsLessFlat->push_back(laserCloud->points[i]);
}
}
pcl::PointCloud<pcl::PointXYZHSV>::Ptr surfPointsLessFlatDS(new pcl::PointCloud<pcl::PointXYZHSV>());
pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlat);
downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
downSizeFilter.filter(*surfPointsLessFlatDS);
*laserCloudExtreCur += *cornerPointsSharp;
*laserCloudExtreCur += *surfPointsFlat;
*laserCloudLessExtreCur += *cornerPointsLessSharp;
*laserCloudLessExtreCur += *surfPointsLessFlatDS;
laserCloudIn->clear();
laserCloud->clear();
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();
surfPointsLessFlatDS->clear();
if (skipFrameCount >= skipFrameNum) {
skipFrameCount = 0;
pcl::PointCloud<pcl::PointXYZHSV>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZHSV>(4, 1));
imuTrans->points[0].x = imuPitchStart;
imuTrans->points[0].y = imuYawStart;
imuTrans->points[0].z = imuRollStart;
imuTrans->points[0].v = 10;
imuTrans->points[1].x = imuPitchCur;
imuTrans->points[1].y = imuYawCur;
imuTrans->points[1].z = imuRollCur;
imuTrans->points[1].v = 11;
imuTrans->points[2].x = imuShiftFromStartXCur;
imuTrans->points[2].y = imuShiftFromStartYCur;
imuTrans->points[2].z = imuShiftFromStartZCur;
imuTrans->points[2].v = 12;
imuTrans->points[3].x = imuVeloFromStartXCur;
imuTrans->points[3].y = imuVeloFromStartYCur;
imuTrans->points[3].z = imuVeloFromStartZCur;
imuTrans->points[3].v = 13;
sensor_msgs::PointCloud2 laserCloudExtreCur2;
pcl::toROSMsg(*laserCloudExtreCur + *imuTrans, laserCloudExtreCur2);
laserCloudExtreCur2.header.stamp = ros::Time().fromSec(timeScanCur);
laserCloudExtreCur2.header.frame_id = "/camera";
pubLaserCloudExtreCurPointer->publish(laserCloudExtreCur2);
imuTrans->clear();
pubLaserCloudLastPointer->publish(laserCloudLast2);
//ROS_INFO ("%d %d", laserCloudLast2.width, laserCloudExtreCur2.width);
}
skipFrameCount++;
}