registerPointCloud.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::PointXYZ>::Ptr surroundCloud(new pcl::PointCloud<pcl::PointXYZ>());
00028 pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloud(new pcl::PointCloud<pcl::PointXYZ>());
00029 pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZ>());
00030 
00031 int startCount = -1;
00032 const int startSkipNum = 5;
00033 
00034 int showCount = -1;
00035 const int showSkipNum = 15;
00036 
00037 ros::Publisher *surroundCloudPubPointer = NULL;
00038 
00039 void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
00040 {
00041   double time = voData->header.stamp.toSec();
00042 
00043   double rx, ry, rz;
00044   geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
00045   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(rz, rx, ry);
00046 
00047   double tx = voData->pose.pose.position.x;
00048   double ty = voData->pose.pose.position.y;
00049   double tz = voData->pose.pose.position.z;
00050 
00051   voDataInd = (voDataInd + 1) % keepVoDataNum;
00052   voDataTime[voDataInd] = time;
00053   voRx[voDataInd] = rx;
00054   voRy[voDataInd] = ry;
00055   voRz[voDataInd] = rz;
00056   voTx[voDataInd] = tx;
00057   voTy[voDataInd] = ty;
00058   voTz[voDataInd] = tz;
00059 }
00060 
00061 void syncCloudHandler(const sensor_msgs::PointCloud2ConstPtr& syncCloud2)
00062 {
00063   if (startCount < startSkipNum) {
00064     startCount++;
00065     return;
00066   }
00067 
00068   double time = syncCloud2->header.stamp.toSec();
00069 
00070   syncCloud->clear();
00071   pcl::fromROSMsg(*syncCloud2, *syncCloud);
00072 
00073   double scaleCur = 1;
00074   double scaleLast = 0;
00075   int voPreInd = keepVoDataNum - 1;
00076   if (voDataInd >= 0) {
00077     while (voDataTime[voRegInd] <= time && voRegInd != voDataInd) {
00078       voRegInd = (voRegInd + 1) % keepVoDataNum;
00079     }
00080 
00081     voPreInd = (voRegInd + keepVoDataNum - 1) % keepVoDataNum;
00082     double voTimePre = voDataTime[voPreInd];
00083     double voTimeReg = voDataTime[voRegInd];
00084 
00085     if (voTimeReg - voTimePre < 0.5) {
00086       double scaleLast =  (voTimeReg - time) / (voTimeReg - voTimePre);
00087       double scaleCur =  (time - voTimePre) / (voTimeReg - voTimePre);
00088       if (scaleLast > 1) {
00089         scaleLast = 1;
00090       } else if (scaleLast < 0) {
00091         scaleLast = 0;
00092       }
00093       if (scaleCur > 1) {
00094         scaleCur = 1;
00095       } else if (scaleCur < 0) {
00096         scaleCur = 0;
00097       }
00098     }
00099   }
00100 
00101   double rx2 = voRx[voRegInd] * scaleCur + voRx[voPreInd] * scaleLast;
00102   double ry2;
00103   if (voRy[voRegInd] - voRy[voPreInd] > PI) {
00104     ry2 = voRy[voRegInd] * scaleCur + (voRy[voPreInd] + 2 * PI) * scaleLast;
00105   } else if (voRy[voRegInd] - voRy[voPreInd] < -PI) {
00106     ry2 = voRy[voRegInd] * scaleCur + (voRy[voPreInd] - 2 * PI) * scaleLast;
00107   } else {
00108     ry2 = voRy[voRegInd] * scaleCur + voRy[voPreInd] * scaleLast;
00109   }
00110   double rz2 = voRz[voRegInd] * scaleCur + voRz[voPreInd] * scaleLast;
00111 
00112   double tx2 = voTx[voRegInd] * scaleCur + voTx[voPreInd] * scaleLast;
00113   double ty2 = voTy[voRegInd] * scaleCur + voTy[voPreInd] * scaleLast;
00114   double tz2 = voTz[voRegInd] * scaleCur + voTz[voPreInd] * scaleLast;
00115 
00116   double cosrx2 = cos(rx2);
00117   double sinrx2 = sin(rx2);
00118   double cosry2 = cos(ry2);
00119   double sinry2 = sin(ry2);
00120   double cosrz2 = cos(rz2);
00121   double sinrz2 = sin(rz2);
00122 
00123   pcl::PointXYZ point;
00124   int syncCloudNum = syncCloud->points.size();
00125   for (int i = 0; i < syncCloudNum; i++) {
00126     point = syncCloud->points[i];
00127     double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
00128     if (pointDis > 0.3 && pointDis < 5) {
00129       double x1 = cosrz2 * point.x - sinrz2 * point.y;
00130       double y1 = sinrz2 * point.x + cosrz2 * point.y;
00131       double z1 = point.z;
00132 
00133       double x2 = x1;
00134       double y2 = cosrx2 * y1 + sinrx2 * z1;
00135       double z2 = -sinrx2 * y1 + cosrx2 * z1;
00136 
00137       point.x = cosry2 * x2 - sinry2 * z2 + tx2;
00138       point.y = y2 + ty2;
00139       point.z = sinry2 * x2 + cosry2 * z2 + tz2;
00140 
00141       surroundCloud->push_back(point);
00142     }
00143   }
00144 
00145   showCount = (showCount + 1) % (showSkipNum + 1);
00146   if (showCount != showSkipNum) {
00147     return;
00148   }
00149 
00150   tempCloud->clear();
00151   int surroundCloudNum = surroundCloud->points.size();
00152   for (int i = 0; i < surroundCloudNum; i++) {
00153     point = surroundCloud->points[i];
00154 
00155     double xDiff = point.x - voTx[voRegInd];
00156     double yDiff = point.y - voTy[voRegInd];
00157     double zDiff = point.z - voTz[voRegInd];
00158 
00159     double pointDis = sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff);
00160     if (pointDis < 50) {
00161       tempCloud->push_back(point);
00162     }
00163   }
00164 
00165   surroundCloud->clear();
00166   pcl::VoxelGrid<pcl::PointXYZ> downSizeFilter;
00167   downSizeFilter.setInputCloud(tempCloud);
00168   downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
00169   downSizeFilter.filter(*surroundCloud);
00170 
00171   sensor_msgs::PointCloud2 surroundCloud2;
00172   pcl::toROSMsg(*surroundCloud, surroundCloud2);
00173   surroundCloud2.header.frame_id = "/camera_init";
00174   surroundCloud2.header.stamp = syncCloud2->header.stamp;
00175   surroundCloudPubPointer->publish(surroundCloud2);
00176 }
00177 
00178 int main(int argc, char** argv)
00179 {
00180   ros::init(argc, argv, "registerPointCloud");
00181   ros::NodeHandle nh;
00182 
00183   ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> ("/cam2_to_init", 5, voDataHandler);
00184 
00185   ros::Subscriber syncCloudSub = nh.subscribe<sensor_msgs::PointCloud2>
00186                                  ("/sync_scan_cloud_filtered", 5, syncCloudHandler);
00187 
00188   ros::Publisher surroundCloudPub = nh.advertise<sensor_msgs::PointCloud2> ("/surround_cloud", 1);
00189   surroundCloudPubPointer = &surroundCloudPub;
00190 
00191   ros::spin();
00192 
00193   return 0;
00194 }


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