processDepthmap.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <ros/ros.h>
00005 
00006 #include <nav_msgs/Odometry.h>
00007 
00008 #include <tf/transform_datatypes.h>
00009 #include <tf/transform_listener.h>
00010 #include <tf/transform_broadcaster.h>
00011 
00012 #include "pointDefinition.h"
00013 
00014 const double PI = 3.1415926;
00015 
00016 const int keepVoDataNum = 30;
00017 double voDataTime[keepVoDataNum] = {0};
00018 double voRx[keepVoDataNum] = {0};
00019 double voRy[keepVoDataNum] = {0};
00020 double voRz[keepVoDataNum] = {0};
00021 double voTx[keepVoDataNum] = {0};
00022 double voTy[keepVoDataNum] = {0};
00023 double voTz[keepVoDataNum] = {0};
00024 int voDataInd = -1;
00025 int voRegInd = 0;
00026 
00027 pcl::PointCloud<pcl::PointXYZI>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZI>());
00028 pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloud(new pcl::PointCloud<pcl::PointXYZ>());
00029 pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZI>());
00030 pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud2(new pcl::PointCloud<pcl::PointXYZI>());
00031 
00032 double timeRec = 0;
00033 double rxRec = 0, ryRec = 0, rzRec = 0;
00034 double txRec = 0, tyRec = 0, tzRec = 0;
00035 
00036 bool systemInited = false;
00037 double initTime;
00038 
00039 int startCount = -1;
00040 const int startSkipNum = 5;
00041 
00042 ros::Publisher *depthCloudPubPointer = NULL;
00043 
00044 void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
00045 {
00046   double time = voData->header.stamp.toSec();
00047 
00048   double roll, pitch, yaw;
00049   geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
00050   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00051 
00052   double rx = voData->twist.twist.angular.x - rxRec;
00053   double ry = voData->twist.twist.angular.y - ryRec;
00054   double rz = voData->twist.twist.angular.z - rzRec;
00055 
00056   if (ry < -PI) {
00057     ry += 2 * PI;
00058   } else if (ry > PI) {
00059     ry -= 2 * PI;
00060   }
00061 
00062   double tx = voData->pose.pose.position.x - txRec;
00063   double ty = voData->pose.pose.position.y - tyRec;
00064   double tz = voData->pose.pose.position.z - tzRec;
00065 
00066   rxRec = voData->twist.twist.angular.x;
00067   ryRec = voData->twist.twist.angular.y;
00068   rzRec = voData->twist.twist.angular.z;
00069 
00070   txRec = voData->pose.pose.position.x;
00071   tyRec = voData->pose.pose.position.y;
00072   tzRec = voData->pose.pose.position.z;
00073 
00074   double x1 = cos(yaw) * tx + sin(yaw) * tz;
00075   double y1 = ty;
00076   double z1 = -sin(yaw) * tx + cos(yaw) * tz;
00077 
00078   double x2 = x1;
00079   double y2 = cos(pitch) * y1 - sin(pitch) * z1;
00080   double z2 = sin(pitch) * y1 + cos(pitch) * z1;
00081 
00082   tx = cos(roll) * x2 + sin(roll) * y2;
00083   ty = -sin(roll) * x2 + cos(roll) * y2;
00084   tz = z2;
00085 
00086   voDataInd = (voDataInd + 1) % keepVoDataNum;
00087   voDataTime[voDataInd] = time;
00088   voRx[voDataInd] = rx;
00089   voRy[voDataInd] = ry;
00090   voRz[voDataInd] = rz;
00091   voTx[voDataInd] = tx;
00092   voTy[voDataInd] = ty;
00093   voTz[voDataInd] = tz;
00094 
00095   double cosrx = cos(rx);
00096   double sinrx = sin(rx);
00097   double cosry = cos(ry);
00098   double sinry = sin(ry);
00099   double cosrz = cos(rz);
00100   double sinrz = sin(rz);
00101 
00102   if (time - timeRec < 0.5) {
00103     pcl::PointXYZI point;
00104     tempCloud->clear();
00105     double x1, y1, z1, x2, y2, z2;
00106     int depthCloudNum = depthCloud->points.size();
00107     for (int i = 0; i < depthCloudNum; i++) {
00108       point = depthCloud->points[i];
00109 
00110       x1 = cosry * point.x - sinry * point.z;
00111       y1 = point.y;
00112       z1 = sinry * point.x + cosry * point.z;
00113 
00114       x2 = x1;
00115       y2 = cosrx * y1 + sinrx * z1;
00116       z2 = -sinrx * y1 + cosrx * z1;
00117 
00118       point.x = cosrz * x2 + sinrz * y2 - tx;
00119       point.y = -sinrz * x2 + cosrz * y2 - ty;
00120       point.z = z2 - tz;
00121 
00122       double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
00123       double timeDis = time - initTime - point.intensity;
00124       if (fabs(point.x / point.z) < 2 && fabs(point.y / point.z) < 1 && point.z > 0.5 && pointDis < 15 &&
00125           timeDis < 5.0) {
00126         tempCloud->push_back(point);
00127       }
00128     }
00129 
00130     depthCloud->clear();
00131     pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
00132     downSizeFilter.setInputCloud(tempCloud);
00133     downSizeFilter.setLeafSize(0.05, 0.05, 0.05);
00134     downSizeFilter.filter(*depthCloud);
00135     depthCloudNum = depthCloud->points.size();
00136 
00137     tempCloud->clear();
00138     for (int i = 0; i < depthCloudNum; i++) {
00139       point = depthCloud->points[i];
00140 
00141       if (fabs(point.x / point.z) < 1 && fabs(point.y / point.z) < 0.6) {
00142         point.intensity = depthCloud->points[i].z;
00143         point.x *= 10 / depthCloud->points[i].z;
00144         point.y *= 10 / depthCloud->points[i].z;
00145         point.z = 10;
00146 
00147         tempCloud->push_back(point);
00148       }
00149     }
00150 
00151     tempCloud2->clear();
00152     downSizeFilter.setInputCloud(tempCloud);
00153     downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
00154     downSizeFilter.filter(*tempCloud2);
00155     int tempCloud2Num = tempCloud2->points.size();
00156 
00157     for (int i = 0; i < tempCloud2Num; i++) {
00158       tempCloud2->points[i].z = tempCloud2->points[i].intensity;
00159       tempCloud2->points[i].x *= tempCloud2->points[i].z / 10;
00160       tempCloud2->points[i].y *= tempCloud2->points[i].z / 10;
00161       tempCloud2->points[i].intensity = 10;
00162     }
00163 
00164     sensor_msgs::PointCloud2 depthCloud2;
00165     pcl::toROSMsg(*tempCloud2, depthCloud2);
00166     depthCloud2.header.frame_id = "camera2";
00167     depthCloud2.header.stamp = voData->header.stamp;
00168     depthCloudPubPointer->publish(depthCloud2);
00169   }
00170 
00171   timeRec = time;
00172 }
00173 
00174 void syncCloudHandler(const sensor_msgs::PointCloud2ConstPtr& syncCloud2)
00175 {
00176   if (startCount < startSkipNum) {
00177     startCount++;
00178     return;
00179   }
00180 
00181   if (!systemInited) {
00182     initTime = syncCloud2->header.stamp.toSec();
00183     systemInited = true;
00184   }
00185 
00186   double time = syncCloud2->header.stamp.toSec();
00187   double timeLasted = time - initTime;
00188 
00189   syncCloud->clear();
00190   pcl::fromROSMsg(*syncCloud2, *syncCloud);
00191 
00192   double scale = 0;
00193   int voPreInd = keepVoDataNum - 1;
00194   if (voDataInd >= 0) {
00195     while (voDataTime[voRegInd] <= time && voRegInd != voDataInd) {
00196       voRegInd = (voRegInd + 1) % keepVoDataNum;
00197     }
00198 
00199     voPreInd = (voRegInd + keepVoDataNum - 1) % keepVoDataNum;
00200     double voTimePre = voDataTime[voPreInd];
00201     double voTimeReg = voDataTime[voRegInd];
00202 
00203     if (voTimeReg - voTimePre < 0.5) {
00204       double scale =  (voTimeReg - time) / (voTimeReg - voTimePre);
00205       if (scale > 1) {
00206         scale = 1;
00207       } else if (scale < 0) {
00208         scale = 0;
00209       }
00210     }
00211   }
00212 
00213   double rx2 = voRx[voRegInd] * scale;
00214   double ry2 = voRy[voRegInd] * scale;
00215   double rz2 = voRz[voRegInd] * scale;
00216 
00217   double tx2 = voTx[voRegInd] * scale;
00218   double ty2 = voTy[voRegInd] * scale;
00219   double tz2 = voTz[voRegInd] * scale;
00220 
00221   double cosrx2 = cos(rx2);
00222   double sinrx2 = sin(rx2);
00223   double cosry2 = cos(ry2);
00224   double sinry2 = sin(ry2);
00225   double cosrz2 = cos(rz2);
00226   double sinrz2 = sin(rz2);
00227 
00228   pcl::PointXYZI point;
00229   double x1, y1, z1, x2, y2, z2;
00230   int syncCloudNum = syncCloud->points.size();
00231   for (int i = 0; i < syncCloudNum; i++) {
00232     point.x = syncCloud->points[i].x;
00233     point.y = syncCloud->points[i].y;
00234     point.z = syncCloud->points[i].z;
00235     point.intensity = timeLasted;
00236 
00237     x1 = cosry2 * point.x - sinry2 * point.z;
00238     y1 = point.y;
00239     z1 = sinry2 * point.x + cosry2 * point.z;
00240 
00241     x2 = x1;
00242     y2 = cosrx2 * y1 + sinrx2 * z1;
00243     z2 = -sinrx2 * y1 + cosrx2 * z1;
00244 
00245     point.x = cosrz2 * x2 + sinrz2 * y2 - tx2;
00246     point.y = -sinrz2 * x2 + cosrz2 * y2 - ty2;
00247     point.z = z2 - tz2;
00248 
00249     if (voDataInd >= 0) {
00250       int voAftInd = (voRegInd + 1) % keepVoDataNum;
00251       while (voAftInd != (voDataInd + 1) % keepVoDataNum) {
00252         double rx = voRx[voAftInd];
00253         double ry = voRy[voAftInd];
00254         double rz = voRz[voAftInd];
00255 
00256         double tx = voTx[voAftInd];
00257         double ty = voTy[voAftInd];
00258         double tz = voTz[voAftInd];
00259 
00260         double cosrx = cos(rx);
00261         double sinrx = sin(rx);
00262         double cosry = cos(ry);
00263         double sinry = sin(ry);
00264         double cosrz = cos(rz);
00265         double sinrz = sin(rz);
00266 
00267         x1 = cosry * point.x - sinry * point.z;
00268         y1 = point.y;
00269         z1 = sinry * point.x + cosry * point.z;
00270 
00271         x2 = x1;
00272         y2 = cosrx * y1 + sinrx * z1;
00273         z2 = -sinrx * y1 + cosrx * z1;
00274 
00275         point.x = cosrz * x2 + sinrz * y2 - tx;
00276         point.y = -sinrz * x2 + cosrz * y2 - ty;
00277         point.z = z2 - tz;
00278 
00279         voAftInd = (voAftInd + 1) % keepVoDataNum;
00280       }
00281     }
00282 
00283     double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
00284     if (fabs(point.x / point.z) < 2 && fabs(point.y / point.z) < 1.5 && point.z > 0.5 && pointDis < 15) {
00285       depthCloud->push_back(point);
00286     }
00287   }
00288 }
00289 
00290 int main(int argc, char** argv)
00291 {
00292   ros::init(argc, argv, "processDepthmap");
00293   ros::NodeHandle nh;
00294 
00295   ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> ("/cam_to_init", 5, voDataHandler);
00296 
00297   ros::Subscriber syncCloudSub = nh.subscribe<sensor_msgs::PointCloud2>
00298                                  ("/sync_scan_cloud_filtered", 5, syncCloudHandler);
00299 
00300   ros::Publisher depthCloudPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_cloud", 5);
00301   depthCloudPubPointer = &depthCloudPub;
00302 
00303   ros::spin();
00304 
00305   return 0;
00306 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:22:43