visualOdometry.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 #include <sensor_msgs/Imu.h>
00008 
00009 #include <tf/transform_datatypes.h>
00010 #include <tf/transform_listener.h>
00011 #include <tf/transform_broadcaster.h>
00012 
00013 #include "cameraParameters.h"
00014 #include "pointDefinition.h"
00015 
00016 const double PI = 3.1415926;
00017 
00018 pcl::PointCloud<ImagePoint>::Ptr imagePointsCur(new pcl::PointCloud<ImagePoint>());
00019 pcl::PointCloud<ImagePoint>::Ptr imagePointsLast(new pcl::PointCloud<ImagePoint>());
00020 pcl::PointCloud<ImagePoint>::Ptr startPointsCur(new pcl::PointCloud<ImagePoint>());
00021 pcl::PointCloud<ImagePoint>::Ptr startPointsLast(new pcl::PointCloud<ImagePoint>());
00022 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransCur(new pcl::PointCloud<pcl::PointXYZHSV>());
00023 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransLast(new pcl::PointCloud<pcl::PointXYZHSV>());
00024 pcl::PointCloud<pcl::PointXYZHSV>::Ptr ipRelations(new pcl::PointCloud<pcl::PointXYZHSV>());
00025 pcl::PointCloud<pcl::PointXYZHSV>::Ptr ipRelations2(new pcl::PointCloud<pcl::PointXYZHSV>());
00026 pcl::PointCloud<pcl::PointXYZ>::Ptr imagePointsProj(new pcl::PointCloud<pcl::PointXYZ>());
00027 pcl::PointCloud<DepthPoint>::Ptr depthPointsCur(new pcl::PointCloud<DepthPoint>());
00028 pcl::PointCloud<DepthPoint>::Ptr depthPointsLast(new pcl::PointCloud<DepthPoint>());
00029 pcl::PointCloud<DepthPoint>::Ptr depthPointsSend(new pcl::PointCloud<DepthPoint>());
00030 
00031 std::vector<int> ipInd;
00032 std::vector<float> ipy2;
00033 
00034 std::vector<float>* ipDepthCur = new std::vector<float>();
00035 std::vector<float>* ipDepthLast = new std::vector<float>();
00036 
00037 double imagePointsCurTime;
00038 double imagePointsLastTime;
00039 
00040 int imagePointsCurNum = 0;
00041 int imagePointsLastNum = 0;
00042 
00043 int depthPointsCurNum = 0;
00044 int depthPointsLastNum = 0;
00045 
00046 pcl::PointCloud<pcl::PointXYZI>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZI>());
00047 pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdTree(new pcl::KdTreeFLANN<pcl::PointXYZI>());
00048 
00049 double depthCloudTime;
00050 int depthCloudNum = 0;
00051 
00052 std::vector<int> pointSearchInd;
00053 std::vector<float> pointSearchSqrDis;
00054 
00055 double transformSum[6] = {0};
00056 double angleSum[3] = {0};
00057 
00058 int imuPointerFront = 0;
00059 int imuPointerLast = -1;
00060 const int imuQueLength = 200;
00061 bool imuInited = false;
00062 
00063 double imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0;
00064 double imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
00065 
00066 double imuYawInit = 0;
00067 double imuTime[imuQueLength] = {0};
00068 double imuRoll[imuQueLength] = {0};
00069 double imuPitch[imuQueLength] = {0};
00070 double imuYaw[imuQueLength] = {0};
00071 
00072 ros::Publisher *voDataPubPointer = NULL;
00073 tf::TransformBroadcaster *tfBroadcasterPointer = NULL;
00074 ros::Publisher *depthPointsPubPointer = NULL;
00075 ros::Publisher *imagePointsProjPubPointer = NULL;
00076 ros::Publisher *imageShowPubPointer;
00077 
00078 const int showDSRate = 2;
00079 
00080 void accumulateRotation(double cx, double cy, double cz, double lx, double ly, double lz, 
00081                         double &ox, double &oy, double &oz)
00082 {
00083   double srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
00084   ox = -asin(srx);
00085 
00086   double srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
00087                 + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
00088   double crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
00089                 - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
00090   oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
00091 
00092   double srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
00093                 + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
00094   double crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
00095                 - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
00096   oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
00097 }
00098 
00099 void diffRotation(double cx, double cy, double cz, double lx, double ly, double lz, 
00100                   double &ox, double &oy, double &oz)
00101 {
00102   double srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 
00103              - cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx);
00104   ox = -asin(srx);
00105 
00106   double srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) 
00107                 - cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz);
00108   double crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly);
00109   oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
00110 
00111   double srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy) 
00112                 - cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 
00113                 - (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly));
00114   double crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz) 
00115                 + cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz) 
00116                 - cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz);
00117   oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
00118 }
00119 
00120 void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2)
00121 {
00122   imagePointsLastTime = imagePointsCurTime;
00123   imagePointsCurTime = imagePoints2->header.stamp.toSec();
00124 
00125   imuRollLast = imuRollCur;
00126   imuPitchLast = imuPitchCur;
00127   imuYawLast = imuYawCur;
00128 
00129   double transform[6] = {0};
00130   if (imuPointerLast >= 0) {
00131     while (imuPointerFront != imuPointerLast) {
00132       if (imagePointsCurTime < imuTime[imuPointerFront]) {
00133         break;
00134       }
00135       imuPointerFront = (imuPointerFront + 1) % imuQueLength;
00136     }
00137 
00138     if (imagePointsCurTime > imuTime[imuPointerFront]) {
00139       imuRollCur = imuRoll[imuPointerFront];
00140       imuPitchCur = imuPitch[imuPointerFront];
00141       imuYawCur = imuYaw[imuPointerFront];
00142     } else {
00143       int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
00144       double ratioFront = (imagePointsCurTime - imuTime[imuPointerBack]) 
00145                         / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
00146       double ratioBack = (imuTime[imuPointerFront] - imagePointsCurTime) 
00147                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
00148 
00149       imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
00150       imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
00151       if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) {
00152         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack;
00153       } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) {
00154         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack;
00155       } else {
00156         imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
00157       }
00158     }
00159 
00160     if (imuInited) {
00161       //transform[0] -= imuPitchCur - imuPitchLast;
00162       //transform[1] -= imuYawCur - imuYawLast;
00163       //transform[2] -= imuRollCur - imuRollLast;
00164     }
00165   }
00166 
00167   pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
00168   imagePointsLast = imagePointsCur;
00169   imagePointsCur = imagePointsTemp;
00170 
00171   imagePointsCur->clear();
00172   pcl::fromROSMsg(*imagePoints2, *imagePointsCur);
00173 
00174   imagePointsLastNum = imagePointsCurNum;
00175   imagePointsCurNum = imagePointsCur->points.size();
00176 
00177   pcl::PointCloud<ImagePoint>::Ptr startPointsTemp = startPointsLast;
00178   startPointsLast = startPointsCur;
00179   startPointsCur = startPointsTemp;
00180 
00181   pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransTemp = startTransLast;
00182   startTransLast = startTransCur;
00183   startTransCur = startTransTemp;
00184 
00185   std::vector<float>* ipDepthTemp = ipDepthLast;
00186   ipDepthLast = ipDepthCur;
00187   ipDepthCur = ipDepthTemp;
00188 
00189   int j = 0;
00190   pcl::PointXYZI ips;
00191   pcl::PointXYZHSV ipr;
00192   ipRelations->clear();
00193   ipInd.clear();
00194   for (int i = 0; i < imagePointsLastNum; i++) {
00195     bool ipFound = false;
00196     for (; j < imagePointsCurNum; j++) {
00197       if (imagePointsCur->points[j].ind == imagePointsLast->points[i].ind) {
00198         ipFound = true;
00199       }
00200       if (imagePointsCur->points[j].ind >= imagePointsLast->points[i].ind) {
00201         break;
00202       }
00203     }
00204 
00205     if (ipFound) {
00206       ipr.x = imagePointsLast->points[i].u;
00207       ipr.y = imagePointsLast->points[i].v;
00208       ipr.z = imagePointsCur->points[j].u;
00209       ipr.h = imagePointsCur->points[j].v;
00210 
00211       ips.x = 10 * ipr.x;
00212       ips.y = 10 * ipr.y;
00213       ips.z = 10;
00214       
00215       if (depthCloudNum > 10) {
00216         kdTree->nearestKSearch(ips, 3, pointSearchInd, pointSearchSqrDis);
00217 
00218         double minDepth, maxDepth;
00219         if (pointSearchSqrDis[0] < 0.5 && pointSearchInd.size() == 3) {
00220           pcl::PointXYZI depthPoint = depthCloud->points[pointSearchInd[0]];
00221           double x1 = depthPoint.x * depthPoint.intensity / 10;
00222           double y1 = depthPoint.y * depthPoint.intensity / 10;
00223           double z1 = depthPoint.intensity;
00224           minDepth = z1;
00225           maxDepth = z1;
00226 
00227           depthPoint = depthCloud->points[pointSearchInd[1]];
00228           double x2 = depthPoint.x * depthPoint.intensity / 10;
00229           double y2 = depthPoint.y * depthPoint.intensity / 10;
00230           double z2 = depthPoint.intensity;
00231           minDepth = (z2 < minDepth)? z2 : minDepth;
00232           maxDepth = (z2 > maxDepth)? z2 : maxDepth;
00233 
00234           depthPoint = depthCloud->points[pointSearchInd[2]];
00235           double x3 = depthPoint.x * depthPoint.intensity / 10;
00236           double y3 = depthPoint.y * depthPoint.intensity / 10;
00237           double z3 = depthPoint.intensity;
00238           minDepth = (z3 < minDepth)? z3 : minDepth;
00239           maxDepth = (z3 > maxDepth)? z3 : maxDepth;
00240 
00241           double u = ipr.x;
00242           double v = ipr.y;
00243           ipr.s = (x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1) 
00244                 / (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2 + u*y1*z2 - u*y2*z1
00245                 - v*x1*z2 + v*x2*z1 - u*y1*z3 + u*y3*z1 + v*x1*z3 - v*x3*z1 + u*y2*z3 
00246                 - u*y3*z2 - v*x2*z3 + v*x3*z2);
00247           ipr.v = 1;
00248 
00249           if (maxDepth - minDepth > 2) {
00250             ipr.s = 0;
00251             ipr.v = 0;
00252           } else if (ipr.s - maxDepth > 0.2) {
00253             ipr.s = maxDepth;
00254           } else if (ipr.s - minDepth < -0.2) {
00255             ipr.s = minDepth;
00256           }
00257         } else {
00258           ipr.s = 0;
00259           ipr.v = 0;
00260         }
00261       } else {
00262         ipr.s = 0;
00263         ipr.v = 0;
00264       }
00265 
00266       if (fabs(ipr.v) < 0.5) {
00267         double disX = transformSum[3] - startTransLast->points[i].h;
00268         double disY = transformSum[4] - startTransLast->points[i].s;
00269         double disZ = transformSum[5] - startTransLast->points[i].v;
00270 
00271         if (sqrt(disX * disX + disY * disY + disZ * disZ) > 1) {
00272 
00273           double u0 = startPointsLast->points[i].u;
00274           double v0 = startPointsLast->points[i].v;
00275           double u1 = ipr.x;
00276           double v1 = ipr.y;
00277 
00278           double srx0 = sin(-startTransLast->points[i].x);
00279           double crx0 = cos(-startTransLast->points[i].x);
00280           double sry0 = sin(-startTransLast->points[i].y);
00281           double cry0 = cos(-startTransLast->points[i].y);
00282           double srz0 = sin(-startTransLast->points[i].z);
00283           double crz0 = cos(-startTransLast->points[i].z);
00284 
00285           double srx1 = sin(-transformSum[0]);
00286           double crx1 = cos(-transformSum[0]);
00287           double sry1 = sin(-transformSum[1]);
00288           double cry1 = cos(-transformSum[1]);
00289           double srz1 = sin(-transformSum[2]);
00290           double crz1 = cos(-transformSum[2]);
00291 
00292           double tx0 = -startTransLast->points[i].h;
00293           double ty0 = -startTransLast->points[i].s;
00294           double tz0 = -startTransLast->points[i].v;
00295 
00296           double tx1 = -transformSum[3];
00297           double ty1 = -transformSum[4];
00298           double tz1 = -transformSum[5];
00299 
00300           double x1 = crz0 * u0 + srz0 * v0;
00301           double y1 = -srz0 * u0 + crz0 * v0;
00302           double z1 = 1;
00303 
00304           double x2 = x1;
00305           double y2 = crx0 * y1 + srx0 * z1;
00306           double z2 = -srx0 * y1 + crx0 * z1;
00307 
00308           double x3 = cry0 * x2 - sry0 * z2;
00309           double y3 = y2;
00310           double z3 = sry0 * x2 + cry0 * z2;
00311 
00312           double x4 = cry1 * x3 + sry1 * z3;
00313           double y4 = y3;
00314           double z4 = -sry1 * x3 + cry1 * z3;
00315 
00316           double x5 = x4;
00317           double y5 = crx1 * y4 - srx1 * z4;
00318           double z5 = srx1 * y4 + crx1 * z4;
00319 
00320           double x6 = crz1 * x5 - srz1 * y5;
00321           double y6 = srz1 * x5 + crz1 * y5;
00322           double z6 = z5;
00323 
00324           u0 = x6 / z6;
00325           v0 = y6 / z6;
00326 
00327           x1 = cry1 * (tx1 - tx0) + sry1 * (tz1 - tz0);
00328           y1 = ty1 - ty0;
00329           z1 = -sry1 * (tx1 - tx0) + cry1 * (tz1 - tz0);
00330 
00331           x2 = x1;
00332           y2 = crx1 * y1 - srx1 * z1;
00333           z2 = srx1 * y1 + crx1 * z1;
00334 
00335           double tx = crz1 * x2 - srz1 * y2;
00336           double ty = srz1 * x2 + crz1 * y2;
00337           double tz = z2;
00338 
00339           double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1))
00340                        * cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1));
00341           double depth = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta;
00342 
00343           if (depth > 0.5 && depth < 100) {
00344             ipr.s = depth;
00345             ipr.v = 2;
00346           }
00347         }
00348 
00349         if (ipr.v == 2) {
00350           if ((*ipDepthLast)[i] > 0) {
00351             ipr.s = 3 * ipr.s * (*ipDepthLast)[i] / (ipr.s + 2 * (*ipDepthLast)[i]);
00352           }
00353           (*ipDepthLast)[i] = ipr.s;
00354         } else if ((*ipDepthLast)[i] > 0) {
00355           ipr.s = (*ipDepthLast)[i];
00356           ipr.v = 2;
00357         }
00358       }
00359 
00360       ipRelations->push_back(ipr);
00361       ipInd.push_back(imagePointsLast->points[i].ind);
00362     }
00363   }
00364 
00365   int iterNum = 100;
00366   pcl::PointXYZHSV ipr2, ipr3, ipr4;
00367   int ipRelationsNum = ipRelations->points.size();
00368   int ptNumNoDepthRec = 0;
00369   int ptNumWithDepthRec = 0;
00370   double meanValueWithDepthRec = 100000;
00371   for (int iterCount = 0; iterCount < iterNum; iterCount++) {
00372     ipRelations2->clear();
00373     ipy2.clear();
00374     int ptNumNoDepth = 0;
00375     int ptNumWithDepth = 0;
00376     double meanValueNoDepth = 0;
00377     double meanValueWithDepth = 0;
00378     for (int i = 0; i < ipRelationsNum; i++) {
00379       ipr = ipRelations->points[i];
00380 
00381       double u0 = ipr.x;
00382       double v0 = ipr.y;
00383       double u1 = ipr.z;
00384       double v1 = ipr.h;
00385 
00386       double srx = sin(transform[0]);
00387       double crx = cos(transform[0]);
00388       double sry = sin(transform[1]);
00389       double cry = cos(transform[1]);
00390       double srz = sin(transform[2]);
00391       double crz = cos(transform[2]);
00392       double tx = transform[3];
00393       double ty = transform[4];
00394       double tz = transform[5];
00395 
00396       if (ipr.s < 0.1 || ipr.s > 500) {
00397         ipr.s = 0;
00398         ipr.v = 0;
00399       }
00400 
00401       if (fabs(ipr.v) < 0.5) {
00402 
00403         ipr2.x = v0*(crz*srx*(tx - tz*u1) - crx*(ty*u1 - tx*v1) + srz*srx*(ty - tz*v1)) 
00404                - u0*(sry*srx*(ty*u1 - tx*v1) + crz*sry*crx*(tx - tz*u1) + sry*srz*crx*(ty - tz*v1)) 
00405                + cry*srx*(ty*u1 - tx*v1) + cry*crz*crx*(tx - tz*u1) + cry*srz*crx*(ty - tz*v1);
00406 
00407         ipr2.y = u0*((tx - tz*u1)*(srz*sry - crz*srx*cry) - (ty - tz*v1)*(crz*sry + srx*srz*cry) 
00408                + crx*cry*(ty*u1 - tx*v1)) - (tx - tz*u1)*(srz*cry + crz*srx*sry) 
00409                + (ty - tz*v1)*(crz*cry - srx*srz*sry) + crx*sry*(ty*u1 - tx*v1);
00410 
00411         ipr2.z = -u0*((tx - tz*u1)*(cry*crz - srx*sry*srz) + (ty - tz*v1)*(cry*srz + srx*sry*crz)) 
00412                - (tx - tz*u1)*(sry*crz + cry*srx*srz) - (ty - tz*v1)*(sry*srz - cry*srx*crz) 
00413                - v0*(crx*crz*(ty - tz*v1) - crx*srz*(tx - tz*u1));
00414 
00415         ipr2.h = cry*crz*srx - v0*(crx*crz - srx*v1) - u0*(cry*srz + crz*srx*sry + crx*sry*v1) 
00416                - sry*srz + crx*cry*v1;
00417 
00418         ipr2.s = crz*sry - v0*(crx*srz + srx*u1) + u0*(cry*crz + crx*sry*u1 - srx*sry*srz) 
00419                - crx*cry*u1 + cry*srx*srz;
00420 
00421         ipr2.v = u1*(sry*srz - cry*crz*srx) - v1*(crz*sry + cry*srx*srz) + u0*(u1*(cry*srz + crz*srx*sry) 
00422                - v1*(cry*crz - srx*sry*srz)) + v0*(crx*crz*u1 + crx*srz*v1);
00423 
00424         double y2 = (ty - tz*v1)*(crz*sry + cry*srx*srz) - (tx - tz*u1)*(sry*srz - cry*crz*srx) 
00425                   - v0*(srx*(ty*u1 - tx*v1) + crx*crz*(tx - tz*u1) + crx*srz*(ty - tz*v1)) 
00426                   + u0*((ty - tz*v1)*(cry*crz - srx*sry*srz) - (tx - tz*u1)*(cry*srz + crz*srx*sry) 
00427                   + crx*sry*(ty*u1 - tx*v1)) - crx*cry*(ty*u1 - tx*v1);
00428 
00429         if (ptNumNoDepthRec < 50 || iterCount < 25 || fabs(y2) < 2 * meanValueWithDepthRec / 10000) {
00430           double scale = 100;
00431           ipr2.x *= scale;
00432           ipr2.y *= scale;
00433           ipr2.z *= scale;
00434           ipr2.h *= scale;
00435           ipr2.s *= scale;
00436           ipr2.v *= scale;
00437           y2 *= scale;
00438 
00439           ipRelations2->push_back(ipr2);
00440           ipy2.push_back(y2);
00441 
00442           ptNumNoDepth++;
00443         } else {
00444           ipRelations->points[i].v = -1;
00445         }
00446       } else if (fabs(ipr.v - 1) < 0.5 || fabs(ipr.v - 2) < 0.5) {
00447 
00448         double d0 = ipr.s;
00449 
00450         ipr3.x = d0*(cry*srz*crx + cry*u1*srx) - d0*u0*(sry*srz*crx + sry*u1*srx) 
00451                - d0*v0*(u1*crx - srz*srx);
00452 
00453         ipr3.y = d0*(crz*cry + crx*u1*sry - srx*srz*sry) - d0*u0*(crz*sry - crx*u1*cry + srx*srz*cry);
00454 
00455         ipr3.z = -d0*(sry*srz - cry*srx*crz) - d0*u0*(cry*srz + srx*sry*crz) - crx*d0*v0*crz;
00456 
00457         ipr3.h = 1;
00458 
00459         ipr3.s = 0;
00460 
00461         ipr3.v = -u1;
00462 
00463         double y3 = tx - tz*u1 + d0*(crz*sry - crx*cry*u1 + cry*srx*srz) - d0*v0*(crx*srz + srx*u1) 
00464                   + d0*u0*(cry*crz + crx*sry*u1 - srx*sry*srz);
00465 
00466         ipr4.x = d0*(cry*v1*srx - cry*crz*crx) + d0*u0*(crz*sry*crx - sry*v1*srx) 
00467                - d0*v0*(crz*srx + v1*crx);
00468 
00469         ipr4.y = d0*(srz*cry + crz*srx*sry + crx*v1*sry) + d0*u0*(crz*srx*cry - srz*sry + crx*v1*cry);
00470 
00471         ipr4.z = d0*(sry*crz + cry*srx*srz) + d0*u0*(cry*crz - srx*sry*srz) - crx*d0*v0*srz;
00472 
00473         ipr4.h = 0;
00474 
00475         ipr4.s = 1;
00476 
00477         ipr4.v = -v1;
00478 
00479         double y4 = ty - tz*v1 - d0*(cry*crz*srx - sry*srz + crx*cry*v1) + d0*v0*(crx*crz - srx*v1) 
00480                   + d0*u0*(cry*srz + crz*srx*sry + crx*sry*v1);
00481 
00482         if (ptNumWithDepthRec < 50 || iterCount < 25 || 
00483             sqrt(y3 * y3 + y4 * y4) < 2 * meanValueWithDepthRec) {
00484           ipRelations2->push_back(ipr3);
00485           ipy2.push_back(y3);
00486 
00487           ipRelations2->push_back(ipr4);
00488           ipy2.push_back(y4);
00489 
00490           ptNumWithDepth++;
00491           meanValueWithDepth += sqrt(y3 * y3 + y4 * y4);
00492         } else {
00493           ipRelations->points[i].v = -1;
00494         }
00495       }
00496     }
00497     meanValueWithDepth /= (ptNumWithDepth + 0.01);
00498     ptNumNoDepthRec = ptNumNoDepth;
00499     ptNumWithDepthRec = ptNumWithDepth;
00500     meanValueWithDepthRec = meanValueWithDepth;
00501 
00502     int ipRelations2Num = ipRelations2->points.size();
00503     if (ipRelations2Num > 10) {
00504       cv::Mat matA(ipRelations2Num, 6, CV_32F, cv::Scalar::all(0));
00505       cv::Mat matAt(6, ipRelations2Num, CV_32F, cv::Scalar::all(0));
00506       cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
00507       cv::Mat matB(ipRelations2Num, 1, CV_32F, cv::Scalar::all(0));
00508       cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
00509       cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
00510 
00511       for (int i = 0; i < ipRelations2Num; i++) {
00512         ipr2 = ipRelations2->points[i];
00513 
00514         matA.at<float>(i, 0) = ipr2.x;
00515         matA.at<float>(i, 1) = ipr2.y;
00516         matA.at<float>(i, 2) = ipr2.z;
00517         matA.at<float>(i, 3) = ipr2.h;
00518         matA.at<float>(i, 4) = ipr2.s;
00519         matA.at<float>(i, 5) = ipr2.v;
00520         matB.at<float>(i, 0) = -0.2 * ipy2[i];
00521       }
00522       cv::transpose(matA, matAt);
00523       matAtA = matAt * matA;
00524       matAtB = matAt * matB;
00525       cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
00526 
00527       //if (fabs(matX.at<float>(0, 0)) < 0.1 && fabs(matX.at<float>(1, 0)) < 0.1 && 
00528       //    fabs(matX.at<float>(2, 0)) < 0.1) {
00529         transform[0] += matX.at<float>(0, 0);
00530         transform[1] += matX.at<float>(1, 0);
00531         transform[2] += matX.at<float>(2, 0);
00532         transform[3] += matX.at<float>(3, 0);
00533         transform[4] += matX.at<float>(4, 0);
00534         transform[5] += matX.at<float>(5, 0);
00535       //}
00536 
00537       float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
00538                    + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
00539                    + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
00540       float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100
00541                    + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100
00542                    + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100);
00543 
00544       if (deltaR < 0.00001 && deltaT < 0.00001) {
00545         break;
00546       }
00547 
00548       //ROS_INFO ("iter: %d, deltaR: %f, deltaT: %f", iterCount, deltaR, deltaT);
00549     }
00550   }
00551 
00552   if (!imuInited) {
00553     imuYawInit = imuYawCur;
00554     transform[0] -= imuPitchCur;
00555     transform[2] -= imuRollCur;
00556 
00557     imuInited = true;
00558   }
00559 
00560   double rx, ry, rz;
00561   accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
00562                     -transform[0], -transform[1], -transform[2], rx, ry, rz);
00563 
00564   if (imuPointerLast >= 0) {
00565     double drx, dry, drz;
00566     diffRotation(imuPitchCur, imuYawCur - imuYawInit, imuRollCur, rx, ry, rz, drx, dry, drz);
00567 
00568     transform[0] -= 0.1 * drx;
00569     /*if (dry > PI) {
00570       transform[1] -= 0.1 * (dry - 2 * PI);
00571     } else if (imuYawCur - imuYawInit - ry < -PI) {
00572       transform[1] -= 0.1 * (dry + 2 * PI);
00573     } else {
00574       transform[1] -= 0.1 * dry;
00575     }*/
00576     transform[2] -= 0.1 * drz;
00577 
00578     accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
00579                       -transform[0], -transform[1], -transform[2], rx, ry, rz);
00580   }
00581 
00582   double x1 = cos(rz) * transform[3] - sin(rz) * transform[4];
00583   double y1 = sin(rz) * transform[3] + cos(rz) * transform[4];
00584   double z1 = transform[5];
00585 
00586   double x2 = x1;
00587   double y2 = cos(rx) * y1 - sin(rx) * z1;
00588   double z2 = sin(rx) * y1 + cos(rx) * z1;
00589 
00590   double tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
00591   double ty = transformSum[4] - y2;
00592   double tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
00593 
00594   transformSum[0] = rx;
00595   transformSum[1] = ry;
00596   transformSum[2] = rz;
00597   transformSum[3] = tx;
00598   transformSum[4] = ty;
00599   transformSum[5] = tz;
00600 
00601   pcl::PointXYZHSV spc;
00602   spc.x = transformSum[0];
00603   spc.y = transformSum[1];
00604   spc.z = transformSum[2];
00605   spc.h = transformSum[3];
00606   spc.s = transformSum[4];
00607   spc.v = transformSum[5];
00608 
00609   double crx = cos(transform[0]);
00610   double srx = sin(transform[0]);
00611   double cry = cos(transform[1]);
00612   double sry = sin(transform[1]);
00613 
00614   j = 0;
00615   for (int i = 0; i < imagePointsCurNum; i++) {
00616     bool ipFound = false;
00617     for (; j < imagePointsLastNum; j++) {
00618       if (imagePointsLast->points[j].ind == imagePointsCur->points[i].ind) {
00619         ipFound = true;
00620       }
00621       if (imagePointsLast->points[j].ind >= imagePointsCur->points[i].ind) {
00622         break;
00623       }
00624     }
00625 
00626     if (ipFound) {
00627       startPointsCur->push_back(startPointsLast->points[j]);
00628       startTransCur->push_back(startTransLast->points[j]);
00629 
00630       if ((*ipDepthLast)[j] > 0) {
00631         double ipz = (*ipDepthLast)[j];
00632         double ipx = imagePointsLast->points[j].u * ipz;
00633         double ipy = imagePointsLast->points[j].v * ipz;
00634 
00635         x1 = cry * ipx + sry * ipz;
00636         y1 = ipy;
00637         z1 = -sry * ipx + cry * ipz;
00638 
00639         x2 = x1;
00640         y2 = crx * y1 - srx * z1;
00641         z2 = srx * y1 + crx * z1;
00642 
00643         ipDepthCur->push_back(z2 + transform[5]);
00644       } else {
00645         ipDepthCur->push_back(-1);
00646       }
00647     } else {
00648       startPointsCur->push_back(imagePointsCur->points[i]);
00649       startTransCur->push_back(spc);
00650       ipDepthCur->push_back(-1);
00651     }
00652   }
00653   startPointsLast->clear();
00654   startTransLast->clear();
00655   ipDepthLast->clear();
00656 
00657   angleSum[0] -= transform[0];
00658   angleSum[1] -= transform[1];
00659   angleSum[2] -= transform[2];
00660 
00661   geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);
00662 
00663   nav_msgs::Odometry voData;
00664   voData.header.frame_id = "/camera_init";
00665   voData.child_frame_id = "/camera";
00666   voData.header.stamp = imagePoints2->header.stamp;
00667   voData.pose.pose.orientation.x = -geoQuat.y;
00668   voData.pose.pose.orientation.y = -geoQuat.z;
00669   voData.pose.pose.orientation.z = geoQuat.x;
00670   voData.pose.pose.orientation.w = geoQuat.w;
00671   voData.pose.pose.position.x = tx;
00672   voData.pose.pose.position.y = ty;
00673   voData.pose.pose.position.z = tz;
00674   voData.twist.twist.angular.x = angleSum[0];
00675   voData.twist.twist.angular.y = angleSum[1];
00676   voData.twist.twist.angular.z = angleSum[2];
00677   voDataPubPointer->publish(voData);
00678 
00679   tf::StampedTransform voTrans;
00680   voTrans.frame_id_ = "/camera_init";
00681   voTrans.child_frame_id_ = "/camera";
00682   voTrans.stamp_ = imagePoints2->header.stamp;
00683   voTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
00684   voTrans.setOrigin(tf::Vector3(tx, ty, tz));
00685   tfBroadcasterPointer->sendTransform(voTrans);
00686 
00687   pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp = depthPointsLast;
00688   depthPointsLast = depthPointsCur;
00689   depthPointsCur = depthPointsTemp;
00690 
00691   DepthPoint ipd;
00692   depthPointsCur->clear();
00693 
00694   ipd.u = transformSum[0];
00695   ipd.v = transformSum[1];
00696   ipd.depth = transformSum[2];
00697   ipd.ind = -2;
00698   depthPointsCur->push_back(ipd);
00699 
00700   ipd.u = transformSum[3];
00701   ipd.v = transformSum[4];
00702   ipd.depth = transformSum[5];
00703   ipd.ind = -1;
00704   depthPointsCur->push_back(ipd);
00705 
00706   depthPointsLastNum = depthPointsCurNum;
00707   depthPointsCurNum = 2;
00708 
00709   j = 0;
00710   pcl::PointXYZ ipp;
00711   depthPointsSend->clear();
00712   imagePointsProj->clear();
00713   for (int i = 0; i < ipRelationsNum; i++) {
00714     if (fabs(ipRelations->points[i].v - 1) < 0.5 || fabs(ipRelations->points[i].v - 2) < 0.5) {
00715 
00716       ipd.u = ipRelations->points[i].z;
00717       ipd.v = ipRelations->points[i].h;
00718       ipd.depth = ipRelations->points[i].s + transform[5];
00719       ipd.label = ipRelations->points[i].v;
00720       ipd.ind = ipInd[i];
00721 
00722       depthPointsCur->push_back(ipd);
00723       depthPointsCurNum++;
00724 
00725       for (; j < depthPointsLastNum; j++) {
00726         if (depthPointsLast->points[j].ind < ipInd[i]) {
00727           depthPointsSend->push_back(depthPointsLast->points[j]);
00728         } else if (depthPointsLast->points[j].ind > ipInd[i]) {
00729           break;
00730         }
00731       }
00732 
00733       ipd.u = ipRelations->points[i].x;
00734       ipd.v = ipRelations->points[i].y;
00735       ipd.depth = ipRelations->points[i].s;
00736 
00737       depthPointsSend->push_back(ipd);
00738 
00739       ipp.x = ipRelations->points[i].x * ipRelations->points[i].s;
00740       ipp.y = ipRelations->points[i].y * ipRelations->points[i].s;
00741       ipp.z = ipRelations->points[i].s;
00742 
00743       imagePointsProj->push_back(ipp);
00744     }
00745   }
00746 
00747   sensor_msgs::PointCloud2 depthPoints2;
00748   pcl::toROSMsg(*depthPointsSend, depthPoints2);
00749   depthPoints2.header.frame_id = "camera2";
00750   depthPoints2.header.stamp = ros::Time().fromSec(imagePointsLastTime);
00751   depthPointsPubPointer->publish(depthPoints2);
00752 
00753   sensor_msgs::PointCloud2 imagePointsProj2;
00754   pcl::toROSMsg(*imagePointsProj, imagePointsProj2);
00755   imagePointsProj2.header.frame_id = "camera2";
00756   imagePointsProj2.header.stamp = ros::Time().fromSec(imagePointsLastTime);
00757   imagePointsProjPubPointer->publish(imagePointsProj2);
00758 }
00759 
00760 void depthCloudHandler(const sensor_msgs::PointCloud2ConstPtr& depthCloud2)
00761 {
00762   depthCloudTime = depthCloud2->header.stamp.toSec();
00763 
00764   depthCloud->clear();
00765   pcl::fromROSMsg(*depthCloud2, *depthCloud);
00766   depthCloudNum = depthCloud->points.size();
00767 
00768   if (depthCloudNum > 10) {
00769     for (int i = 0; i < depthCloudNum; i++) {
00770       depthCloud->points[i].intensity = depthCloud->points[i].z;
00771       depthCloud->points[i].x *= 10 / depthCloud->points[i].z;
00772       depthCloud->points[i].y *= 10 / depthCloud->points[i].z;
00773       depthCloud->points[i].z = 10;
00774     }
00775 
00776     kdTree->setInputCloud(depthCloud);
00777   }
00778 }
00779 
00780 void imuDataHandler(const sensor_msgs::Imu::ConstPtr& imuData)
00781 {
00782   double roll, pitch, yaw;
00783   tf::Quaternion orientation;
00784   tf::quaternionMsgToTF(imuData->orientation, orientation);
00785   tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
00786 
00787   imuPointerLast = (imuPointerLast + 1) % imuQueLength;
00788 
00789   imuTime[imuPointerLast] = imuData->header.stamp.toSec() - 0.1068;
00790   imuRoll[imuPointerLast] = roll;
00791   imuPitch[imuPointerLast] = pitch;
00792   imuYaw[imuPointerLast] = yaw;
00793 }
00794 
00795 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 
00796 {
00797   cv_bridge::CvImagePtr bridge = cv_bridge::toCvCopy(imageData, "bgr8");
00798 
00799   int ipRelationsNum = ipRelations->points.size();
00800   for (int i = 0; i < ipRelationsNum; i++) {
00801     if (fabs(ipRelations->points[i].v) < 0.5) {
00802       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00803                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(255, 0, 0), 2);
00804     } else if (fabs(ipRelations->points[i].v - 1) < 0.5) {
00805       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00806                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 255, 0), 2);
00807     } else if (fabs(ipRelations->points[i].v - 2) < 0.5) {
00808       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00809                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 255), 2);
00810     } /*else {
00811       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00812                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 0), 2);
00813     }*/
00814   }
00815 
00816   sensor_msgs::Image::Ptr imagePointer = bridge->toImageMsg();
00817   imageShowPubPointer->publish(imagePointer);
00818 }
00819 
00820 int main(int argc, char** argv)
00821 {
00822   ros::init(argc, argv, "visualOdometry");
00823   ros::NodeHandle nh;
00824 
00825   ros::Subscriber imagePointsSub = nh.subscribe<sensor_msgs::PointCloud2>
00826                                    ("/image_points_last", 5, imagePointsHandler);
00827 
00828   ros::Subscriber depthCloudSub = nh.subscribe<sensor_msgs::PointCloud2> 
00829                                   ("/depth_cloud", 5, depthCloudHandler);
00830 
00831   ros::Subscriber imuDataSub = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 5, imuDataHandler);
00832 
00833   ros::Publisher voDataPub = nh.advertise<nav_msgs::Odometry> ("/cam_to_init", 5);
00834   voDataPubPointer = &voDataPub;
00835 
00836   tf::TransformBroadcaster tfBroadcaster;
00837   tfBroadcasterPointer = &tfBroadcaster;
00838 
00839   ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_last", 5);
00840   depthPointsPubPointer = &depthPointsPub;
00841 
00842   ros::Publisher imagePointsProjPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_proj", 1);
00843   imagePointsProjPubPointer = &imagePointsProjPub;
00844 
00845   ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/image/show", 1, imageDataHandler);
00846 
00847   ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show_2", 1);
00848   imageShowPubPointer = &imageShowPub;
00849 
00850   ros::spin();
00851 
00852   return 0;
00853 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Mar 1 2015 11:30:50