laserMapping.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <time.h>
00003 #include <stdio.h>
00004 #include <stdlib.h>
00005 #include <ros/ros.h>
00006 
00007 #include <nav_msgs/Odometry.h>
00008 #include <sensor_msgs/Imu.h>
00009 #include <sensor_msgs/PointCloud2.h>
00010 
00011 #include <tf/transform_datatypes.h>
00012 #include <tf/transform_broadcaster.h>
00013 
00014 #include <opencv/cv.h>
00015 #include <opencv2/highgui/highgui.hpp>
00016 
00017 #include <pcl/ros/conversions.h>
00018 #include <pcl/point_cloud.h>
00019 #include <pcl/point_types.h>
00020 #include <pcl/filters/voxel_grid.h>
00021 #include <pcl/kdtree/kdtree_flann.h>
00022 
00023 const double PI = 3.1415926;
00024 const double rad2deg = 180 / PI;
00025 const double deg2rad = PI / 180;
00026 
00027 double timeLaserCloudLast;
00028 double timeLaserOdometry;
00029 
00030 bool newLaserCloudLast = false;
00031 bool newLaserOdometry = false;
00032 
00033 const int laserCloudCenWidth = 5;
00034 const int laserCloudCenHeight = 5;
00035 const int laserCloudCenDepth = 5;
00036 const int laserCloudWidth = 11;
00037 const int laserCloudHeight = 11;
00038 const int laserCloudDepth = 11;
00039 const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;
00040 
00041 int laserCloudValidInd[27];
00042 int laserCloudSurroundInd[27];
00043 
00044 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCorner(new pcl::PointCloud<pcl::PointXYZHSV>());
00045 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSurf(new pcl::PointCloud<pcl::PointXYZHSV>());
00046 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCorner2(new pcl::PointCloud<pcl::PointXYZHSV>());
00047 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSurf2(new pcl::PointCloud<pcl::PointXYZHSV>());
00048 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00049 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudOri(new pcl::PointCloud<pcl::PointXYZHSV>());
00050 //pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSel(new pcl::PointCloud<pcl::PointXYZHSV>());
00051 //pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCorr(new pcl::PointCloud<pcl::PointXYZHSV>());
00052 //pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudProj(new pcl::PointCloud<pcl::PointXYZHSV>());
00053 pcl::PointCloud<pcl::PointXYZHSV>::Ptr coeffSel(new pcl::PointCloud<pcl::PointXYZHSV>());
00054 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudFromMap(new pcl::PointCloud<pcl::PointXYZHSV>());
00055 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<pcl::PointXYZHSV>());
00056 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<pcl::PointXYZHSV>());
00057 pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurround(new pcl::PointCloud<pcl::PointXYZI>());
00058 pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudArray[laserCloudNum];
00059 
00060 pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<pcl::PointXYZHSV>());
00061 pcl::KdTreeFLANN<pcl::PointXYZHSV>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<pcl::PointXYZHSV>());
00062 
00063 float transformSum[6] = {0};
00064 float transformIncre[6] = {0};
00065 float transformTobeMapped[6] = {0};
00066 float transformBefMapped[6] = {0};
00067 float transformAftMapped[6] = {0};
00068 
00069 void transformAssociateToMap()
00070 {
00071   float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
00072            - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
00073   float y1 = transformBefMapped[4] - transformSum[4];
00074   float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
00075            + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
00076 
00077   float x2 = x1;
00078   float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
00079   float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
00080 
00081   transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
00082   transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
00083   transformIncre[5] = z2;
00084 
00085   float sbcx = sin(transformSum[0]);
00086   float cbcx = cos(transformSum[0]);
00087   float sbcy = sin(transformSum[1]);
00088   float cbcy = cos(transformSum[1]);
00089   float sbcz = sin(transformSum[2]);
00090   float cbcz = cos(transformSum[2]);
00091 
00092   float sblx = sin(transformBefMapped[0]);
00093   float cblx = cos(transformBefMapped[0]);
00094   float sbly = sin(transformBefMapped[1]);
00095   float cbly = cos(transformBefMapped[1]);
00096   float sblz = sin(transformBefMapped[2]);
00097   float cblz = cos(transformBefMapped[2]);
00098 
00099   float salx = sin(transformAftMapped[0]);
00100   float calx = cos(transformAftMapped[0]);
00101   float saly = sin(transformAftMapped[1]);
00102   float caly = cos(transformAftMapped[1]);
00103   float salz = sin(transformAftMapped[2]);
00104   float calz = cos(transformAftMapped[2]);
00105 
00106   float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 
00107             - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00108             - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00109             - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00110             - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
00111   transformTobeMapped[0] = -asin(srx);
00112 
00113   float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00114                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00115                - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00116                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00117                + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00118   float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00119                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00120                - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00121                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00122                + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00123   transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), 
00124                                  crycrx / cos(transformTobeMapped[0]));
00125   
00126   float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 
00127                - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 
00128                - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 
00129                + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 
00130                - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 
00131                + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 
00132                + calx*cblx*salz*sblz);
00133   float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 
00134                - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 
00135                + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 
00136                + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 
00137                + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 
00138                - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 
00139                - calx*calz*cblx*sblz);
00140   transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), 
00141                                  crzcrx / cos(transformTobeMapped[0]));
00142 
00143   x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];
00144   y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];
00145   z1 = transformIncre[5];
00146 
00147   x2 = x1;
00148   y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
00149   z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;
00150 
00151   transformTobeMapped[3] = transformAftMapped[3] 
00152                          - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
00153   transformTobeMapped[4] = transformAftMapped[4] - y2;
00154   transformTobeMapped[5] = transformAftMapped[5] 
00155                          - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);
00156 }
00157 
00158 void transformUpdate()
00159 {
00160   //for (int i = 0; i < 3; i++) {
00161   //  transformTobeMapped[i] = (1 - 0.1) * transformTobeMapped[i] + 0.1 * transformSum[i];
00162   //}
00163 
00164   for (int i = 0; i < 6; i++) {
00165     transformBefMapped[i] = transformSum[i];
00166     transformAftMapped[i] = transformTobeMapped[i];
00167   }
00168 }
00169 
00170 void pointAssociateToMap(pcl::PointXYZHSV *pi, pcl::PointXYZHSV *po)
00171 {
00172   float x1 = cos(transformTobeMapped[2]) * pi->x
00173            - sin(transformTobeMapped[2]) * pi->y;
00174   float y1 = sin(transformTobeMapped[2]) * pi->x
00175            + cos(transformTobeMapped[2]) * pi->y;
00176   float z1 = pi->z;
00177 
00178   float x2 = x1;
00179   float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
00180   float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;
00181 
00182   po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2
00183         + transformTobeMapped[3];
00184   po->y = y2 + transformTobeMapped[4];
00185   po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2
00186         + transformTobeMapped[5];
00187   po->h = pi->h;
00188   po->s = pi->s;
00189   po->v = pi->v;
00190 }
00191 
00192 void laserCloudLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudLast2)
00193 {
00194   timeLaserCloudLast = laserCloudLast2->header.stamp.toSec();
00195 
00196   laserCloudLast->clear();
00197   pcl::fromROSMsg(*laserCloudLast2, *laserCloudLast);
00198 
00199   newLaserCloudLast = true;
00200 }
00201 
00202 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
00203 {
00204   timeLaserOdometry = laserOdometry->header.stamp.toSec();
00205 
00206   double roll, pitch, yaw;
00207   geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
00208   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00209 
00210   transformSum[0] = -pitch;
00211   transformSum[1] = -yaw;
00212   transformSum[2] = roll;
00213 
00214   transformSum[3] = laserOdometry->pose.pose.position.x;
00215   transformSum[4] = laserOdometry->pose.pose.position.y;
00216   transformSum[5] = laserOdometry->pose.pose.position.z;
00217 
00218   newLaserOdometry = true;
00219 }
00220 
00221 int main(int argc, char** argv)
00222 {
00223   ros::init(argc, argv, "laserMapping");
00224   ros::NodeHandle nh;
00225 
00226   ros::Subscriber subLaserCloudLast2 = nh.subscribe<sensor_msgs::PointCloud2> 
00227                                        ("/laser_cloud_last_2", 2, laserCloudLastHandler);
00228 
00229   ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> 
00230                                      ("/cam_to_init", 5, laserOdometryHandler);
00231 
00232   ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2> 
00233                                          ("/laser_cloud_surround", 1);
00234 
00235   //ros::Publisher pub1 = nh.advertise<sensor_msgs::PointCloud2> ("/pc1", 1);
00236 
00237   //ros::Publisher pub2 = nh.advertise<sensor_msgs::PointCloud2> ("/pc2", 1);
00238 
00239   //ros::Publisher pub3 = nh.advertise<sensor_msgs::PointCloud2> ("/pc3", 1);
00240 
00241   //ros::Publisher pub4 = nh.advertise<sensor_msgs::PointCloud2> ("/pc4", 1);
00242 
00243   ros::Publisher pubOdomBefMapped = nh.advertise<nav_msgs::Odometry> ("/bef_mapped_to_init_2", 5);
00244   nav_msgs::Odometry odomBefMapped;
00245   odomBefMapped.header.frame_id = "/camera_init_2";
00246   odomBefMapped.child_frame_id = "/bef_mapped";
00247 
00248   ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init_2", 5);
00249   nav_msgs::Odometry odomAftMapped;
00250   odomAftMapped.header.frame_id = "/camera_init_2";
00251   odomAftMapped.child_frame_id = "/aft_mapped";
00252 
00253   std::vector<int> pointSearchInd;
00254   std::vector<float> pointSearchSqDis;
00255 
00256   pcl::PointXYZHSV pointOri, pointSel, pointProj, coeff;
00257   pcl::PointXYZI pointSurround;
00258 
00259   cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));
00260   cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));
00261   cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));
00262 
00263   cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
00264   cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
00265   cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
00266 
00267   for (int i = 0; i < laserCloudNum; i++) {
00268     laserCloudArray[i].reset(new pcl::PointCloud<pcl::PointXYZHSV>());
00269   }
00270 
00271   ros::Rate rate(100);
00272   bool status = ros::ok();
00273   while (status) {
00274     ros::spinOnce();
00275 
00276     if (newLaserCloudLast && newLaserOdometry && fabs(timeLaserCloudLast - timeLaserOdometry) < 0.005) {
00277       newLaserCloudLast = false;
00278       newLaserOdometry = false;
00279 
00280       transformAssociateToMap();
00281 
00282       pcl::PointXYZHSV pointOnZAxis;
00283       pointOnZAxis.x = 0.0;
00284       pointOnZAxis.y = 0.0;
00285       pointOnZAxis.z = 10.0;
00286       pointAssociateToMap(&pointOnZAxis, &pointOnZAxis);        
00287 
00288       int centerCubeI = int((transformTobeMapped[3] + 10.0) / 20.0) + laserCloudCenWidth;
00289       int centerCubeJ = int((transformTobeMapped[4] + 10.0) / 20.0) + laserCloudCenHeight;
00290       int centerCubeK = int((transformTobeMapped[5] + 10.0) / 20.0) + laserCloudCenDepth;
00291       
00292       if (transformTobeMapped[3] + 10.0 < 0) centerCubeI--;
00293       if (transformTobeMapped[4] + 10.0 < 0) centerCubeJ--;
00294       if (transformTobeMapped[5] + 10.0 < 0) centerCubeK--;
00295       
00296       if (centerCubeI < 0 || centerCubeI >= laserCloudWidth || 
00297           centerCubeJ < 0 || centerCubeJ >= laserCloudHeight || 
00298           centerCubeK < 0 || centerCubeK >= laserCloudDepth) {
00299 
00300         transformUpdate();
00301         continue;
00302       }
00303 
00304       int laserCloudValidNum = 0;
00305       int laserCloudSurroundNum = 0;
00306       for (int i = centerCubeI - 1; i <= centerCubeI + 1; i++) {
00307         for (int j = centerCubeJ - 1; j <= centerCubeJ + 1; j++) {
00308           for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++) {
00309             if (i >= 0 && i < laserCloudWidth && 
00310                 j >= 0 && j < laserCloudHeight && 
00311                 k >= 0 && k < laserCloudDepth) {
00312               
00313               float centerX = 20.0 * (i - laserCloudCenWidth);
00314               float centerY = 20.0 * (j - laserCloudCenHeight);
00315               float centerZ = 20.0 * (k - laserCloudCenDepth);
00316 
00317               bool isInLaserFOV = false;
00318               for (int ii = -1; ii <= 1; ii += 2) {
00319                 for (int jj = -1; jj <= 1; jj += 2) {
00320                   for (int kk = -1; kk <= 1; kk += 2) {
00321                     float cornerX = centerX + 10.0 * ii;
00322                     float cornerY = centerY + 10.0 * jj;
00323                     float cornerZ = centerZ + 10.0 * kk;
00324 
00325                     float check = 100.0 
00326                                 + (transformTobeMapped[3] - cornerX) * (transformTobeMapped[3] - cornerX) 
00327                                 + (transformTobeMapped[4] - cornerY) * (transformTobeMapped[4] - cornerY)
00328                                 + (transformTobeMapped[5] - cornerZ) * (transformTobeMapped[5] - cornerZ)
00329                                 - (pointOnZAxis.x - cornerX) * (pointOnZAxis.x - cornerX) 
00330                                 - (pointOnZAxis.y - cornerY) * (pointOnZAxis.y - cornerY)
00331                                 - (pointOnZAxis.z - cornerZ) * (pointOnZAxis.z - cornerZ);
00332 
00333                     if (check > 0) {
00334                       isInLaserFOV = true;
00335                     }
00336                   }
00337                 }
00338               }
00339 
00340               if (isInLaserFOV) {
00341                 laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j 
00342                                                        + laserCloudWidth * laserCloudHeight * k;
00343                 laserCloudValidNum++;
00344               }
00345               laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j 
00346                                                            + laserCloudWidth * laserCloudHeight * k;
00347               laserCloudSurroundNum++;
00348             }
00349           }
00350         }
00351       }
00352 
00353       laserCloudFromMap->clear();
00354       for (int i = 0; i < laserCloudValidNum; i++) {
00355         *laserCloudFromMap += *laserCloudArray[laserCloudValidInd[i]];
00356       }
00357       int laserCloudFromMapNum = laserCloudFromMap->points.size();
00358 
00359       laserCloudCornerFromMap->clear();
00360       laserCloudSurfFromMap->clear();
00361       for (int i = 0; i < laserCloudFromMapNum; i++) {
00362         if (fabs(laserCloudFromMap->points[i].v - 2) < 0.005 || 
00363             fabs(laserCloudFromMap->points[i].v - 1) < 0.005) {
00364           laserCloudCornerFromMap->push_back(laserCloudFromMap->points[i]);
00365         } else {
00366           laserCloudSurfFromMap->push_back(laserCloudFromMap->points[i]);
00367         }
00368       }
00369 
00370       laserCloudCorner->clear();
00371       laserCloudSurf->clear();
00372       int laserCloudLastNum = laserCloudLast->points.size();
00373       for (int i = 0; i < laserCloudLastNum; i++) {
00374         if (fabs(laserCloudLast->points[i].v - 2) < 0.005 || 
00375             fabs(laserCloudLast->points[i].v - 1) < 0.005) {
00376           laserCloudCorner->push_back(laserCloudLast->points[i]);
00377         } else {
00378           laserCloudSurf->push_back(laserCloudLast->points[i]);
00379         }
00380       }
00381 
00382       laserCloudCorner2->clear();
00383       pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter;
00384       downSizeFilter.setInputCloud(laserCloudCorner);
00385       downSizeFilter.setLeafSize(0.05, 0.05, 0.05);
00386       downSizeFilter.filter(*laserCloudCorner2);
00387 
00388       laserCloudSurf2->clear();
00389       downSizeFilter.setInputCloud(laserCloudSurf);
00390       downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
00391       downSizeFilter.filter(*laserCloudSurf2);
00392 
00393       laserCloudLast->clear();
00394       *laserCloudLast = *laserCloudCorner2 + *laserCloudSurf2;
00395       laserCloudLastNum = laserCloudLast->points.size();
00396 
00397       if (laserCloudSurfFromMap->points.size() > 500) {
00398 
00399         kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
00400         kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
00401 
00402         for (int iterCount = 0; iterCount < 20; iterCount++) {
00403           laserCloudOri->clear();
00404           //laserCloudSel->clear();
00405           //laserCloudCorr->clear();
00406           //laserCloudProj->clear();
00407           coeffSel->clear();
00408 
00409           for (int i = 0; i < laserCloudLastNum; i++) {
00410             if (fabs(laserCloudLast->points[i].x > 0.3) || fabs(laserCloudLast->points[i].y > 0.3) ||
00411                 fabs(laserCloudLast->points[i].z > 0.3)) {
00412 
00413               pointOri = laserCloudLast->points[i];
00414               pointAssociateToMap(&pointOri, &pointSel); 
00415               if (fabs(pointOri.v) < 0.05 || fabs(pointOri.v + 1) < 0.05) {
00416 
00417                 kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
00418 
00419                 if (pointSearchSqDis[4] < 1.0) {
00420                   for (int j = 0; j < 5; j++) {
00421                     matA0.at<float>(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
00422                     matA0.at<float>(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
00423                     matA0.at<float>(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
00424                   }
00425                   cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);
00426 
00427                   float pa = matX0.at<float>(0, 0);
00428                   float pb = matX0.at<float>(1, 0);
00429                   float pc = matX0.at<float>(2, 0);
00430                   float pd = 1;
00431  
00432                   float ps = sqrt(pa * pa + pb * pb + pc * pc);
00433                   pa /= ps;
00434                   pb /= ps;
00435                   pc /= ps;
00436                   pd /= ps;
00437 
00438                   bool planeValid = true;
00439                   for (int j = 0; j < 5; j++) {
00440                     if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
00441                         pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
00442                         pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.05) {
00443                       planeValid = false;
00444                       break;
00445                     }
00446                   }
00447 
00448                   if (planeValid) {
00449                     float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
00450 
00451                     pointProj = pointSel;
00452                     pointProj.x -= pa * pd2;
00453                     pointProj.y -= pb * pd2;
00454                     pointProj.z -= pc * pd2;
00455 
00456                     float s = 1;
00457                     if (iterCount >= 6) {
00458                       s = 1 - 8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
00459                         + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
00460                     }
00461 
00462                     coeff.x = s * pa;
00463                     coeff.y = s * pb;
00464                     coeff.z = s * pc;
00465                     coeff.h = s * pd2;
00466 
00467                     if (s > 0.2) {
00468                       laserCloudOri->push_back(pointOri);
00469                       //laserCloudSel->push_back(pointSel);
00470                       //laserCloudProj->push_back(pointProj);
00471                       //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[0]]);
00472                       //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[1]]);
00473                       //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[2]]);
00474                       //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[3]]);
00475                       //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[4]]);
00476                       coeffSel->push_back(coeff);
00477                     }
00478                   }
00479                 }
00480               } else {
00481 
00482                 kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
00483                 
00484                 if (pointSearchSqDis[4] < 1.0) {
00485                   float cx = 0;
00486                   float cy = 0; 
00487                   float cz = 0;
00488                   for (int j = 0; j < 5; j++) {
00489                     cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;
00490                     cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;
00491                     cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;
00492                   }
00493                   cx /= 5;
00494                   cy /= 5; 
00495                   cz /= 5;
00496 
00497                   float a11 = 0;
00498                   float a12 = 0; 
00499                   float a13 = 0;
00500                   float a22 = 0;
00501                   float a23 = 0; 
00502                   float a33 = 0;
00503                   for (int j = 0; j < 5; j++) {
00504                     float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;
00505                     float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;
00506                     float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;
00507 
00508                     a11 += ax * ax;
00509                     a12 += ax * ay;
00510                     a13 += ax * az;
00511                     a22 += ay * ay;
00512                     a23 += ay * az;
00513                     a33 += az * az;
00514                   }
00515                   a11 /= 5;
00516                   a12 /= 5; 
00517                   a13 /= 5;
00518                   a22 /= 5;
00519                   a23 /= 5; 
00520                   a33 /= 5;
00521 
00522                   matA1.at<float>(0, 0) = a11;
00523                   matA1.at<float>(0, 1) = a12;
00524                   matA1.at<float>(0, 2) = a13;
00525                   matA1.at<float>(1, 0) = a12;
00526                   matA1.at<float>(1, 1) = a22;
00527                   matA1.at<float>(1, 2) = a23;
00528                   matA1.at<float>(2, 0) = a13;
00529                   matA1.at<float>(2, 1) = a23;
00530                   matA1.at<float>(2, 2) = a33;
00531 
00532                   cv::eigen(matA1, matD1, matV1);
00533 
00534                   if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
00535 
00536                     float x0 = pointSel.x;
00537                     float y0 = pointSel.y;
00538                     float z0 = pointSel.z;
00539                     float x1 = cx + 0.1 * matV1.at<float>(0, 0);
00540                     float y1 = cy + 0.1 * matV1.at<float>(0, 1);
00541                     float z1 = cz + 0.1 * matV1.at<float>(0, 2);
00542                     float x2 = cx - 0.1 * matV1.at<float>(0, 0);
00543                     float y2 = cy - 0.1 * matV1.at<float>(0, 1);
00544                     float z2 = cz - 0.1 * matV1.at<float>(0, 2);
00545 
00546                     float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
00547                                * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
00548                                + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
00549                                * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
00550                                + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
00551                                * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
00552 
00553                     float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
00554 
00555                     float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
00556                              + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
00557 
00558                     float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
00559                              - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
00560 
00561                     float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
00562                              + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
00563 
00564                     float ld2 = a012 / l12;
00565 
00566                     pointProj = pointSel;
00567                     pointProj.x -= la * ld2;
00568                     pointProj.y -= lb * ld2;
00569                     pointProj.z -= lc * ld2;
00570 
00571                     float s = 2 * (1 - 8 * fabs(ld2));
00572 
00573                     coeff.x = s * la;
00574                     coeff.y = s * lb;
00575                     coeff.z = s * lc;
00576                     coeff.h = s * ld2;
00577 
00578                     if (s > 0.4) {
00579                       laserCloudOri->push_back(pointOri);
00580                       //laserCloudSel->push_back(pointSel);
00581                       //laserCloudProj->push_back(pointProj);
00582                       //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[0]]);
00583                       //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[1]]);
00584                       //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[2]]);
00585                       //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[3]]);
00586                       //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[4]]);
00587                       coeffSel->push_back(coeff);
00588                     }
00589                   }
00590                 }
00591               }
00592             }
00593           }
00594           int laserCloudSelNum = laserCloudOri->points.size();
00595 
00596           float srx = sin(transformTobeMapped[0]);
00597           float crx = cos(transformTobeMapped[0]);
00598           float sry = sin(transformTobeMapped[1]);
00599           float cry = cos(transformTobeMapped[1]);
00600           float srz = sin(transformTobeMapped[2]);
00601           float crz = cos(transformTobeMapped[2]);
00602 
00603           if (laserCloudSelNum < 50) {
00604             continue;
00605           }
00606 
00607           cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
00608           cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
00609           cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
00610           cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
00611           cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
00612           cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
00613           for (int i = 0; i < laserCloudSelNum; i++) {
00614             pointOri = laserCloudOri->points[i];
00615             coeff = coeffSel->points[i];
00616 
00617             float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
00618                       + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
00619                       + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
00620 
00621             float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
00622                       + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
00623                       + ((-cry*crz - srx*sry*srz)*pointOri.x 
00624                       + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
00625 
00626             float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz - srx*sry*srz)*pointOri.y)*coeff.x
00627                       + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
00628                       + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry - cry*srx*srz)*pointOri.y)*coeff.z;
00629 
00630             matA.at<float>(i, 0) = arx;
00631             matA.at<float>(i, 1) = ary;
00632             matA.at<float>(i, 2) = arz;
00633             matA.at<float>(i, 3) = coeff.x;
00634             matA.at<float>(i, 4) = coeff.y;
00635             matA.at<float>(i, 5) = coeff.z;
00636             matB.at<float>(i, 0) = -coeff.h;
00637           }
00638           cv::transpose(matA, matAt);
00639           matAtA = matAt * matA;
00640           matAtB = matAt * matB;
00641           cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
00642 
00643           if (fabs(matX.at<float>(0, 0)) < 0.5 &&
00644               fabs(matX.at<float>(1, 0)) < 0.5 &&
00645               fabs(matX.at<float>(2, 0)) < 0.5 &&
00646               fabs(matX.at<float>(3, 0)) < 1 &&
00647               fabs(matX.at<float>(4, 0)) < 1 &&
00648               fabs(matX.at<float>(5, 0)) < 1) {
00649 
00650             transformTobeMapped[0] += matX.at<float>(0, 0);
00651             transformTobeMapped[1] += matX.at<float>(1, 0);
00652             transformTobeMapped[2] += matX.at<float>(2, 0);
00653             transformTobeMapped[3] += matX.at<float>(3, 0);
00654             transformTobeMapped[4] += matX.at<float>(4, 0);
00655             transformTobeMapped[5] += matX.at<float>(5, 0);
00656           } else {
00657             //ROS_INFO ("Mapping update out of bound");
00658           }
00659           
00660           float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
00661                        + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
00662                        + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
00663           float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100
00664                        + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100
00665                        + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100);
00666 
00667           if (deltaR < 0.05 && deltaT < 0.05) {
00668             break;
00669           }
00670 
00671           //ROS_INFO ("iter: %d, deltaR: %f, deltaT: %f", iterCount, deltaR, deltaT);
00672         }
00673       }
00674 
00675       transformUpdate();
00676 
00677       for (int i = 0; i < laserCloudLastNum; i++) {
00678         if (fabs(laserCloudLast->points[i].x) > 0.3 || fabs(laserCloudLast->points[i].y) > 0.3 ||
00679             fabs(laserCloudLast->points[i].z) > 0.3) {
00680 
00681           pointAssociateToMap(&laserCloudLast->points[i], &pointSel);
00682 
00683           int cubeI = int((pointSel.x + 10.0) / 20.0) + laserCloudCenWidth;
00684           int cubeJ = int((pointSel.y + 10.0) / 20.0) + laserCloudCenHeight;
00685           int cubeK = int((pointSel.z + 10.0) / 20.0) + laserCloudCenDepth;
00686           
00687           if (pointSel.x + 10.0 < 0) cubeI--;
00688           if (pointSel.y + 10.0 < 0) cubeJ--;
00689           if (pointSel.z + 10.0 < 0) cubeK--;
00690           
00691           int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
00692           laserCloudArray[cubeInd]->push_back(pointSel);
00693         }
00694       }
00695 
00696       for (int i = 0; i < laserCloudValidNum; i++) {
00697         laserCloudCorner->clear();
00698         laserCloudSurf->clear();
00699         pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCubePointer = 
00700                                                laserCloudArray[laserCloudValidInd[i]];
00701         int laserCloudCubeNum = laserCloudCubePointer->points.size();
00702         for (int j = 0; j < laserCloudCubeNum; j++) {
00703           if (fabs(laserCloudCubePointer->points[j].v - 2) < 0.005 || 
00704               fabs(laserCloudCubePointer->points[j].v - 1) < 0.005) {
00705             laserCloudCorner->push_back(laserCloudCubePointer->points[j]);
00706           } else {
00707             laserCloudSurf->push_back(laserCloudCubePointer->points[j]);
00708           }
00709         }
00710 
00711         laserCloudCorner2->clear();
00712         pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter;
00713         downSizeFilter.setInputCloud(laserCloudCorner);
00714         downSizeFilter.setLeafSize(0.05, 0.05, 0.05);
00715         downSizeFilter.filter(*laserCloudCorner2);
00716 
00717         laserCloudSurf2->clear();
00718         downSizeFilter.setInputCloud(laserCloudSurf);
00719         downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
00720         downSizeFilter.filter(*laserCloudSurf2);
00721 
00722         laserCloudCubePointer->clear();
00723         *laserCloudCubePointer = *laserCloudCorner2 + *laserCloudSurf2;
00724       }
00725 
00726       laserCloudSurround->clear();
00727       for (int i = 0; i < laserCloudSurroundNum; i++) {
00728          pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCubePointer = 
00729                                                 laserCloudArray[laserCloudSurroundInd[i]];
00730          int laserCloudCubeNum = laserCloudCubePointer->points.size();
00731          for (int j = 0; j < laserCloudCubeNum; j++) {
00732            pointSurround.x = laserCloudCubePointer->points[j].x;
00733            pointSurround.y = laserCloudCubePointer->points[j].y;
00734            pointSurround.z = laserCloudCubePointer->points[j].z;
00735            pointSurround.intensity = laserCloudCubePointer->points[j].h;
00736            laserCloudSurround->push_back(pointSurround);
00737          }
00738       }
00739 
00740       sensor_msgs::PointCloud2 laserCloudSurround2;
00741       pcl::toROSMsg(*laserCloudSurround, laserCloudSurround2);
00742       laserCloudSurround2.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00743       laserCloudSurround2.header.frame_id = "/camera_init_2";
00744       pubLaserCloudSurround.publish(laserCloudSurround2);
00745 
00746       geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
00747                      (transformBefMapped[2], -transformBefMapped[0], -transformBefMapped[1]);
00748 
00749       odomBefMapped.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00750       odomBefMapped.pose.pose.orientation.x = -geoQuat.y;
00751       odomBefMapped.pose.pose.orientation.y = -geoQuat.z;
00752       odomBefMapped.pose.pose.orientation.z = geoQuat.x;
00753       odomBefMapped.pose.pose.orientation.w = geoQuat.w;
00754       odomBefMapped.pose.pose.position.x = transformBefMapped[3];
00755       odomBefMapped.pose.pose.position.y = transformBefMapped[4];
00756       odomBefMapped.pose.pose.position.z = transformBefMapped[5];
00757       pubOdomBefMapped.publish(odomBefMapped);
00758 
00759       geoQuat = tf::createQuaternionMsgFromRollPitchYaw
00760                 (transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);
00761 
00762       odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00763       odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
00764       odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
00765       odomAftMapped.pose.pose.orientation.z = geoQuat.x;
00766       odomAftMapped.pose.pose.orientation.w = geoQuat.w;
00767       odomAftMapped.pose.pose.position.x = transformAftMapped[3];
00768       odomAftMapped.pose.pose.position.y = transformAftMapped[4];
00769       odomAftMapped.pose.pose.position.z = transformAftMapped[5];
00770       pubOdomAftMapped.publish(odomAftMapped);
00771 
00772       /*sensor_msgs::PointCloud2 pc12;
00773       pcl::toROSMsg(*laserCloudCornerFromMap, pc12);
00774       pc12.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00775       pc12.header.frame_id = "/camera_init_2";
00776       pub1.publish(pc12);
00777 
00778       sensor_msgs::PointCloud2 pc22;
00779       pcl::toROSMsg(*laserCloudSel, pc22);
00780       pc22.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00781       pc22.header.frame_id = "/camera_init_2";
00782       pub2.publish(pc22);
00783 
00784       sensor_msgs::PointCloud2 pc32;
00785       pcl::toROSMsg(*laserCloudCorr, pc32);
00786       pc32.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00787       pc32.header.frame_id = "/camera_init_2";
00788       pub3.publish(pc32);
00789 
00790       sensor_msgs::PointCloud2 pc42;
00791       pcl::toROSMsg(*laserCloudProj, pc42);
00792       pc42.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
00793       pc42.header.frame_id = "/camera_init_2";
00794       pub4.publish(pc42);*/
00795     }
00796 
00797     status = ros::ok();
00798     rate.sleep();
00799   }
00800 
00801   return 0;
00802 }


loam_back_and_forth
Author(s): Ji Zhang
autogenerated on Mon Oct 6 2014 01:51:39