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 "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 = 15;
00020 double syncCloudTime[keepSyncCloudNum] = {0};
00021 pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloudArray[keepSyncCloudNum];
00022 int syncCloudInd = -1;
00023 int cloudRegInd = 0;
00024 
00025 pcl::PointCloud<pcl::PointXYZ>::Ptr surroundCloud(new pcl::PointCloud<pcl::PointXYZ>());
00026 pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZ>());
00027 pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud2(new pcl::PointCloud<pcl::PointXYZ>());
00028 
00029 double timeRec = 0;
00030 double rxRec = 0, ryRec = 0, rzRec = 0;
00031 double txRec = 0, tyRec = 0, tzRec = 0;
00032 
00033 int cloudCount = -1;
00034 const int cloudSkipNum = 5;
00035 
00036 int showCount = -1;
00037 const int showSkipNum = 10;
00038 
00039 ros::Publisher *surroundCloudPubPointer = NULL;
00040 
00041 void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
00042 {
00043   double time = voData->header.stamp.toSec();
00044 
00045   double rx, ry, rz;
00046   geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
00047   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(rz, rx, ry);
00048 
00049   double tx = voData->pose.pose.position.x;
00050   double ty = voData->pose.pose.position.y;
00051   double tz = voData->pose.pose.position.z;
00052 
00053   if (time - timeRec < 0.5 && syncCloudInd >= 0) {
00054     pcl::PointXYZ point;
00055     while (syncCloudTime[cloudRegInd] <= time && cloudRegInd != (syncCloudInd + 1) % keepSyncCloudNum) {
00056       double scaleLast =  (time - syncCloudTime[cloudRegInd]) / (time - timeRec);
00057       double scaleCur =  (syncCloudTime[cloudRegInd] - timeRec) / (time - timeRec);
00058       if (scaleLast > 1) {
00059         scaleLast = 1;
00060       } else if (scaleLast < 0) {
00061         scaleLast = 0;
00062       }
00063       if (scaleCur > 1) {
00064         scaleCur = 1;
00065       } else if (scaleCur < 0) {
00066         scaleCur = 0;
00067       }
00068 
00069       double rx2 = rx * scaleCur + rxRec * scaleLast;
00070       double ry2;
00071       if (ry - ryRec > PI) {
00072         ry2 = ry * scaleCur + (ryRec + 2 * PI) * scaleLast;
00073       } else if (ry - ryRec < -PI) {
00074         ry2 = ry * scaleCur + (ryRec - 2 * PI) * scaleLast;
00075       } else {
00076         ry2 = ry * scaleCur + ryRec * scaleLast;
00077       }
00078       double rz2 = rz * scaleCur + rzRec * scaleLast;
00079 
00080       double tx2 = tx * scaleCur + txRec * scaleLast;
00081       double ty2 = ty * scaleCur + tyRec * scaleLast;
00082       double tz2 = tz * scaleCur + tzRec * scaleLast;
00083 
00084       double cosrx2 = cos(rx2);
00085       double sinrx2 = sin(rx2);
00086       double cosry2 = cos(ry2);
00087       double sinry2 = sin(ry2);
00088       double cosrz2 = cos(rz2);
00089       double sinrz2 = sin(rz2);
00090 
00091       pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloudPointer = syncCloudArray[cloudRegInd];
00092       int syncCloudNum = syncCloudPointer->points.size();
00093       for (int i = 0; i < syncCloudNum; i++) {
00094         point = syncCloudPointer->points[i];
00095         double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
00096         if (pointDis > 0.3 && pointDis < 5) {
00097           double x1 = cosrz2 * point.x - sinrz2 * point.y;
00098           double y1 = sinrz2 * point.x + cosrz2 * point.y;
00099           double z1 = point.z;
00100 
00101           double x2 = x1;
00102           double y2 = cosrx2 * y1 + sinrx2 * z1;
00103           double z2 = -sinrx2 * y1 + cosrx2 * z1;
00104 
00105           point.x = cosry2 * x2 - sinry2 * z2 + tx2;
00106           point.y = y2 + ty2;
00107           point.z = sinry2 * x2 + cosry2 * z2 + tz2;
00108 
00109           tempCloud->push_back(point);
00110         }
00111       }
00112 
00113       cloudRegInd = (cloudRegInd + 1) % keepSyncCloudNum;
00114     }
00115 
00116     showCount = (showCount + 1) % (showSkipNum + 1);
00117     if (showCount != 0) {
00118       rxRec = rx; ryRec = ry; rzRec = rz;
00119       txRec = tx; tyRec = ty; tzRec = tz;
00120       timeRec = time;
00121       return;
00122     }
00123 
00124     int surroundCloudNum = surroundCloud->points.size();
00125     for (int i = 0; i < surroundCloudNum; i++) {
00126       point = surroundCloud->points[i];
00127 
00128       double xDiff = point.x - tx;
00129       double yDiff = point.y - ty;
00130       double zDiff = point.z - tz;
00131 
00132       double pointDis = sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff);
00133       if (pointDis < 50) {
00134         tempCloud->push_back(point);
00135       }
00136     }
00137 
00138     surroundCloud->clear();
00139     pcl::VoxelGrid<pcl::PointXYZ> downSizeFilter;
00140     downSizeFilter.setInputCloud(tempCloud);
00141     downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
00142     downSizeFilter.filter(*surroundCloud);
00143     tempCloud->clear();
00144 
00145     sensor_msgs::PointCloud2 surroundCloud2;
00146     pcl::toROSMsg(*surroundCloud, surroundCloud2);
00147     surroundCloud2.header.frame_id = "/camera_init";
00148     surroundCloud2.header.stamp = voData->header.stamp;
00149     surroundCloudPubPointer->publish(surroundCloud2);
00150   }
00151 
00152   rxRec = rx; ryRec = ry; rzRec = rz;
00153   txRec = tx; tyRec = ty; tzRec = tz;
00154   timeRec = time;
00155 }
00156 
00157 void syncCloudHandler(const sensor_msgs::Image::ConstPtr& syncCloud2)
00158 {
00159   cloudCount = (cloudCount + 1) % (cloudSkipNum + 1);
00160   if (cloudCount != 0) {
00161     return;
00162   }
00163 
00164   double time = syncCloud2->header.stamp.toSec();
00165 
00166   syncCloudInd = (syncCloudInd + 1) % keepSyncCloudNum;
00167   syncCloudTime[syncCloudInd] = time;
00168 
00169   tempCloud2->clear();
00170   pcl::PointXYZ point;
00171   const float* syncCloud2Pointer = reinterpret_cast<const float*>(&syncCloud2->data[0]);
00172   for (int i = 0; i < imagePixelNum; i++) {
00173     float val = syncCloud2Pointer[i];
00174 
00175     int xd = i % imageWidth;
00176     int yd = int(i / imageWidth);
00177 
00178     double ud = (kDepth[2] - xd) / kDepth[0];
00179     double vd = (kDepth[5] - yd) / kDepth[4];
00180 
00181     point.z = val;
00182     point.x = ud * val;
00183     point.y = vd * val;
00184 
00185     if (point.z > 0.3 && point.z < 7) {
00186       tempCloud2->push_back(point);
00187     }
00188   }
00189 
00190   pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloudPointer = syncCloudArray[syncCloudInd];
00191   syncCloudPointer->clear();
00192 
00193   pcl::VoxelGrid<pcl::PointXYZ> downSizeFilter;
00194   downSizeFilter.setInputCloud(tempCloud2);
00195   downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
00196   downSizeFilter.filter(*syncCloudPointer);
00197 }
00198 
00199 int main(int argc, char** argv)
00200 {
00201   ros::init(argc, argv, "registerPointCloud");
00202   ros::NodeHandle nh;
00203 
00204   for (int i = 0; i < keepSyncCloudNum; i++) {
00205     pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloudTemp(new pcl::PointCloud<pcl::PointXYZ>());
00206     syncCloudArray[i] = syncCloudTemp;
00207   }
00208 
00209   ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> ("/cam2_to_init", 5, voDataHandler);
00210 
00211   ros::Subscriber syncCloudSub = nh.subscribe<sensor_msgs::Image>
00212                                  ("/camera/depth_registered/image", 1, syncCloudHandler);
00213 
00214   ros::Publisher surroundCloudPub = nh.advertise<sensor_msgs::PointCloud2> ("/surround_cloud", 1);
00215   surroundCloudPubPointer = &surroundCloudPub;
00216 
00217   ros::spin();
00218 
00219   return 0;
00220 }


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