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


demo_rgbd
Author(s): Ji Zhang
autogenerated on Mon Jan 6 2014 11:16:08