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


demo_rgbd
Author(s): Ji Zhang
autogenerated on Mon Mar 2 2015 12:20:33