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 = 150;
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 (fabs(ipr.v) < 0.5) {
00397 
00398         ipr2.x = v0*(crz*srx*(tx - tz*u1) - crx*(ty*u1 - tx*v1) + srz*srx*(ty - tz*v1)) 
00399                - u0*(sry*srx*(ty*u1 - tx*v1) + crz*sry*crx*(tx - tz*u1) + sry*srz*crx*(ty - tz*v1)) 
00400                + cry*srx*(ty*u1 - tx*v1) + cry*crz*crx*(tx - tz*u1) + cry*srz*crx*(ty - tz*v1);
00401 
00402         ipr2.y = u0*((tx - tz*u1)*(srz*sry - crz*srx*cry) - (ty - tz*v1)*(crz*sry + srx*srz*cry) 
00403                + crx*cry*(ty*u1 - tx*v1)) - (tx - tz*u1)*(srz*cry + crz*srx*sry) 
00404                + (ty - tz*v1)*(crz*cry - srx*srz*sry) + crx*sry*(ty*u1 - tx*v1);
00405 
00406         ipr2.z = -u0*((tx - tz*u1)*(cry*crz - srx*sry*srz) + (ty - tz*v1)*(cry*srz + srx*sry*crz)) 
00407                - (tx - tz*u1)*(sry*crz + cry*srx*srz) - (ty - tz*v1)*(sry*srz - cry*srx*crz) 
00408                - v0*(crx*crz*(ty - tz*v1) - crx*srz*(tx - tz*u1));
00409 
00410         ipr2.h = cry*crz*srx - v0*(crx*crz - srx*v1) - u0*(cry*srz + crz*srx*sry + crx*sry*v1) 
00411                - sry*srz + crx*cry*v1;
00412 
00413         ipr2.s = crz*sry - v0*(crx*srz + srx*u1) + u0*(cry*crz + crx*sry*u1 - srx*sry*srz) 
00414                - crx*cry*u1 + cry*srx*srz;
00415 
00416         ipr2.v = u1*(sry*srz - cry*crz*srx) - v1*(crz*sry + cry*srx*srz) + u0*(u1*(cry*srz + crz*srx*sry) 
00417                - v1*(cry*crz - srx*sry*srz)) + v0*(crx*crz*u1 + crx*srz*v1);
00418 
00419         double y2 = (ty - tz*v1)*(crz*sry + cry*srx*srz) - (tx - tz*u1)*(sry*srz - cry*crz*srx) 
00420                   - v0*(srx*(ty*u1 - tx*v1) + crx*crz*(tx - tz*u1) + crx*srz*(ty - tz*v1)) 
00421                   + u0*((ty - tz*v1)*(cry*crz - srx*sry*srz) - (tx - tz*u1)*(cry*srz + crz*srx*sry) 
00422                   + crx*sry*(ty*u1 - tx*v1)) - crx*cry*(ty*u1 - tx*v1);
00423 
00424         if (ptNumNoDepthRec < 50 || iterCount < 25 || fabs(y2) < 2 * meanValueWithDepthRec / 10000) {
00425           double scale = 100;
00426           ipr2.x *= scale;
00427           ipr2.y *= scale;
00428           ipr2.z *= scale;
00429           ipr2.h *= scale;
00430           ipr2.s *= scale;
00431           ipr2.v *= scale;
00432           y2 *= scale;
00433 
00434           ipRelations2->push_back(ipr2);
00435           ipy2.push_back(y2);
00436 
00437           ptNumNoDepth++;
00438         } else {
00439           ipRelations->points[i].v = -1;
00440         }
00441       } else if (fabs(ipr.v - 1) < 0.5 || fabs(ipr.v - 2) < 0.5) {
00442 
00443         double d0 = ipr.s;
00444 
00445         ipr3.x = d0*(cry*srz*crx + cry*u1*srx) - d0*u0*(sry*srz*crx + sry*u1*srx) 
00446                - d0*v0*(u1*crx - srz*srx);
00447 
00448         ipr3.y = d0*(crz*cry + crx*u1*sry - srx*srz*sry) - d0*u0*(crz*sry - crx*u1*cry + srx*srz*cry);
00449 
00450         ipr3.z = -d0*(sry*srz - cry*srx*crz) - d0*u0*(cry*srz + srx*sry*crz) - crx*d0*v0*crz;
00451 
00452         ipr3.h = 1;
00453 
00454         ipr3.s = 0;
00455 
00456         ipr3.v = -u1;
00457 
00458         double y3 = tx - tz*u1 + d0*(crz*sry - crx*cry*u1 + cry*srx*srz) - d0*v0*(crx*srz + srx*u1) 
00459                   + d0*u0*(cry*crz + crx*sry*u1 - srx*sry*srz);
00460 
00461         ipr4.x = d0*(cry*v1*srx - cry*crz*crx) + d0*u0*(crz*sry*crx - sry*v1*srx) 
00462                - d0*v0*(crz*srx + v1*crx);
00463 
00464         ipr4.y = d0*(srz*cry + crz*srx*sry + crx*v1*sry) + d0*u0*(crz*srx*cry - srz*sry + crx*v1*cry);
00465 
00466         ipr4.z = d0*(sry*crz + cry*srx*srz) + d0*u0*(cry*crz - srx*sry*srz) - crx*d0*v0*srz;
00467 
00468         ipr4.h = 0;
00469 
00470         ipr4.s = 1;
00471 
00472         ipr4.v = -v1;
00473 
00474         double y4 = ty - tz*v1 - d0*(cry*crz*srx - sry*srz + crx*cry*v1) + d0*v0*(crx*crz - srx*v1) 
00475                   + d0*u0*(cry*srz + crz*srx*sry + crx*sry*v1);
00476 
00477         if (ptNumWithDepthRec < 50 || iterCount < 25 || 
00478             sqrt(y3 * y3 + y4 * y4) < 2 * meanValueWithDepthRec) {
00479           ipRelations2->push_back(ipr3);
00480           ipy2.push_back(y3);
00481 
00482           ipRelations2->push_back(ipr4);
00483           ipy2.push_back(y4);
00484 
00485           ptNumWithDepth++;
00486           meanValueWithDepth += sqrt(y3 * y3 + y4 * y4);
00487         } else {
00488           ipRelations->points[i].v = -1;
00489         }
00490       }
00491     }
00492     meanValueWithDepth /= (ptNumWithDepth + 0.01);
00493     ptNumNoDepthRec = ptNumNoDepth;
00494     ptNumWithDepthRec = ptNumWithDepth;
00495     meanValueWithDepthRec = meanValueWithDepth;
00496 
00497     int ipRelations2Num = ipRelations2->points.size();
00498     if (ipRelations2Num > 10) {
00499       cv::Mat matA(ipRelations2Num, 6, CV_32F, cv::Scalar::all(0));
00500       cv::Mat matAt(6, ipRelations2Num, CV_32F, cv::Scalar::all(0));
00501       cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
00502       cv::Mat matB(ipRelations2Num, 1, CV_32F, cv::Scalar::all(0));
00503       cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
00504       cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
00505 
00506       for (int i = 0; i < ipRelations2Num; i++) {
00507         ipr2 = ipRelations2->points[i];
00508 
00509         matA.at<float>(i, 0) = ipr2.x;
00510         matA.at<float>(i, 1) = ipr2.y;
00511         matA.at<float>(i, 2) = ipr2.z;
00512         matA.at<float>(i, 3) = ipr2.h;
00513         matA.at<float>(i, 4) = ipr2.s;
00514         matA.at<float>(i, 5) = ipr2.v;
00515         matB.at<float>(i, 0) = -0.1 * ipy2[i];
00516       }
00517       cv::transpose(matA, matAt);
00518       matAtA = matAt * matA;
00519       matAtB = matAt * matB;
00520       cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
00521 
00522       //if (fabs(matX.at<float>(0, 0)) < 0.1 && fabs(matX.at<float>(1, 0)) < 0.1 && 
00523       //    fabs(matX.at<float>(2, 0)) < 0.1) {
00524         transform[0] += matX.at<float>(0, 0);
00525         transform[1] += matX.at<float>(1, 0);
00526         transform[2] += matX.at<float>(2, 0);
00527         transform[3] += matX.at<float>(3, 0);
00528         transform[4] += matX.at<float>(4, 0);
00529         transform[5] += matX.at<float>(5, 0);
00530       //}
00531 
00532       float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
00533                    + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
00534                    + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
00535       float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100
00536                    + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100
00537                    + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100);
00538 
00539       if (deltaR < 0.0001 && deltaT < 0.0001) {
00540         break;
00541       }
00542 
00543       //ROS_INFO ("iter: %d, deltaR: %f, deltaT: %f", iterCount, deltaR, deltaT);
00544     }
00545   }
00546 
00547   if (!imuInited) {
00548     imuYawInit = imuYawCur;
00549     transform[0] -= imuPitchCur;
00550     transform[2] -= imuRollCur;
00551 
00552     imuInited = true;
00553   }
00554 
00555   double rx, ry, rz;
00556   accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
00557                     -transform[0], -transform[1], -transform[2], rx, ry, rz);
00558 
00559   if (imuPointerLast >= 0) {
00560     double drx, dry, drz;
00561     diffRotation(imuPitchCur, imuYawCur - imuYawInit, imuRollCur, rx, ry, rz, drx, dry, drz);
00562 
00563     transform[0] -= 0.1 * drx;
00564     /*if (dry > PI) {
00565       transform[1] -= 0.1 * (dry - 2 * PI);
00566     } else if (imuYawCur - imuYawInit - ry < -PI) {
00567       transform[1] -= 0.1 * (dry + 2 * PI);
00568     } else {
00569       transform[1] -= 0.1 * dry;
00570     }*/
00571     transform[2] -= 0.1 * drz;
00572 
00573     accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
00574                       -transform[0], -transform[1], -transform[2], rx, ry, rz);
00575   }
00576 
00577   double x1 = cos(rz) * transform[3] - sin(rz) * transform[4];
00578   double y1 = sin(rz) * transform[3] + cos(rz) * transform[4];
00579   double z1 = transform[5];
00580 
00581   double x2 = x1;
00582   double y2 = cos(rx + 0.025) * y1 - sin(rx + 0.025) * z1;
00583   double z2 = sin(rx + 0.025) * y1 + cos(rx + 0.025) * z1;
00584 
00585   double tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
00586   double ty = transformSum[4] - y2;
00587   double tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
00588 
00589   transformSum[0] = rx;
00590   transformSum[1] = ry;
00591   transformSum[2] = rz;
00592   transformSum[3] = tx;
00593   transformSum[4] = ty;
00594   transformSum[5] = tz;
00595 
00596   pcl::PointXYZHSV spc;
00597   spc.x = transformSum[0];
00598   spc.y = transformSum[1];
00599   spc.z = transformSum[2];
00600   spc.h = transformSum[3];
00601   spc.s = transformSum[4];
00602   spc.v = transformSum[5];
00603 
00604   double crx = cos(transform[0]);
00605   double srx = sin(transform[0]);
00606   double cry = cos(transform[1]);
00607   double sry = sin(transform[1]);
00608 
00609   j = 0;
00610   for (int i = 0; i < imagePointsCurNum; i++) {
00611     bool ipFound = false;
00612     for (; j < imagePointsLastNum; j++) {
00613       if (imagePointsLast->points[j].ind == imagePointsCur->points[i].ind) {
00614         ipFound = true;
00615       }
00616       if (imagePointsLast->points[j].ind >= imagePointsCur->points[i].ind) {
00617         break;
00618       }
00619     }
00620 
00621     if (ipFound) {
00622       startPointsCur->push_back(startPointsLast->points[j]);
00623       startTransCur->push_back(startTransLast->points[j]);
00624 
00625       if ((*ipDepthLast)[j] > 0) {
00626         double ipz = (*ipDepthLast)[j];
00627         double ipx = imagePointsLast->points[j].u * ipz;
00628         double ipy = imagePointsLast->points[j].v * ipz;
00629 
00630         x1 = cry * ipx + sry * ipz;
00631         y1 = ipy;
00632         z1 = -sry * ipx + cry * ipz;
00633 
00634         x2 = x1;
00635         y2 = crx * y1 - srx * z1;
00636         z2 = srx * y1 + crx * z1;
00637 
00638         ipDepthCur->push_back(z2 + transform[5]);
00639       } else {
00640         ipDepthCur->push_back(-1);
00641       }
00642     } else {
00643       startPointsCur->push_back(imagePointsCur->points[i]);
00644       startTransCur->push_back(spc);
00645       ipDepthCur->push_back(-1);
00646     }
00647   }
00648   startPointsLast->clear();
00649   startTransLast->clear();
00650   ipDepthLast->clear();
00651 
00652   angleSum[0] -= transform[0];
00653   angleSum[1] -= transform[1];
00654   angleSum[2] -= transform[2];
00655 
00656   geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);
00657 
00658   nav_msgs::Odometry voData;
00659   voData.header.frame_id = "/camera_init";
00660   voData.child_frame_id = "/camera";
00661   voData.header.stamp = imagePoints2->header.stamp;
00662   voData.pose.pose.orientation.x = -geoQuat.y;
00663   voData.pose.pose.orientation.y = -geoQuat.z;
00664   voData.pose.pose.orientation.z = geoQuat.x;
00665   voData.pose.pose.orientation.w = geoQuat.w;
00666   voData.pose.pose.position.x = tx;
00667   voData.pose.pose.position.y = ty;
00668   voData.pose.pose.position.z = tz;
00669   voData.twist.twist.angular.x = angleSum[0];
00670   voData.twist.twist.angular.y = angleSum[1];
00671   voData.twist.twist.angular.z = angleSum[2];
00672   voDataPubPointer->publish(voData);
00673 
00674   tf::StampedTransform voTrans;
00675   voTrans.frame_id_ = "/camera_init";
00676   voTrans.child_frame_id_ = "/camera";
00677   voTrans.stamp_ = imagePoints2->header.stamp;
00678   voTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
00679   voTrans.setOrigin(tf::Vector3(tx, ty, tz));
00680   tfBroadcasterPointer->sendTransform(voTrans);
00681 
00682   pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp = depthPointsLast;
00683   depthPointsLast = depthPointsCur;
00684   depthPointsCur = depthPointsTemp;
00685 
00686   DepthPoint ipd;
00687   depthPointsCur->clear();
00688 
00689   ipd.u = transformSum[0];
00690   ipd.v = transformSum[1];
00691   ipd.depth = transformSum[2];
00692   ipd.ind = -2;
00693   depthPointsCur->push_back(ipd);
00694 
00695   ipd.u = transformSum[3];
00696   ipd.v = transformSum[4];
00697   ipd.depth = transformSum[5];
00698   ipd.ind = -1;
00699   depthPointsCur->push_back(ipd);
00700 
00701   depthPointsLastNum = depthPointsCurNum;
00702   depthPointsCurNum = 2;
00703 
00704   j = 0;
00705   pcl::PointXYZ ipp;
00706   depthPointsSend->clear();
00707   imagePointsProj->clear();
00708   for (int i = 0; i < ipRelationsNum; i++) {
00709     if (fabs(ipRelations->points[i].v - 1) < 0.5 || fabs(ipRelations->points[i].v - 2) < 0.5) {
00710 
00711       ipd.u = ipRelations->points[i].z;
00712       ipd.v = ipRelations->points[i].h;
00713       ipd.depth = ipRelations->points[i].s + transform[5];
00714       ipd.label = ipRelations->points[i].v;
00715       ipd.ind = ipInd[i];
00716 
00717       depthPointsCur->push_back(ipd);
00718       depthPointsCurNum++;
00719 
00720       for (; j < depthPointsLastNum; j++) {
00721         if (depthPointsLast->points[j].ind < ipInd[i]) {
00722           depthPointsSend->push_back(depthPointsLast->points[j]);
00723         } else if (depthPointsLast->points[j].ind > ipInd[i]) {
00724           break;
00725         }
00726       }
00727 
00728       ipd.u = ipRelations->points[i].x;
00729       ipd.v = ipRelations->points[i].y;
00730       ipd.depth = ipRelations->points[i].s;
00731 
00732       depthPointsSend->push_back(ipd);
00733 
00734       ipp.x = ipRelations->points[i].x * ipRelations->points[i].s;
00735       ipp.y = ipRelations->points[i].y * ipRelations->points[i].s;
00736       ipp.z = ipRelations->points[i].s;
00737 
00738       imagePointsProj->push_back(ipp);
00739     }
00740   }
00741 
00742   sensor_msgs::PointCloud2 depthPoints2;
00743   pcl::toROSMsg(*depthPointsSend, depthPoints2);
00744   depthPoints2.header.frame_id = "camera2";
00745   depthPoints2.header.stamp = ros::Time().fromSec(imagePointsLastTime);
00746   depthPointsPubPointer->publish(depthPoints2);
00747 
00748   sensor_msgs::PointCloud2 imagePointsProj2;
00749   pcl::toROSMsg(*imagePointsProj, imagePointsProj2);
00750   imagePointsProj2.header.frame_id = "camera2";
00751   imagePointsProj2.header.stamp = ros::Time().fromSec(imagePointsLastTime);
00752   imagePointsProjPubPointer->publish(imagePointsProj2);
00753 }
00754 
00755 void depthCloudHandler(const sensor_msgs::PointCloud2ConstPtr& depthCloud2)
00756 {
00757   depthCloudTime = depthCloud2->header.stamp.toSec();
00758 
00759   depthCloud->clear();
00760   pcl::fromROSMsg(*depthCloud2, *depthCloud);
00761   depthCloudNum = depthCloud->points.size();
00762 
00763   if (depthCloudNum > 10) {
00764     for (int i = 0; i < depthCloudNum; i++) {
00765       depthCloud->points[i].intensity = depthCloud->points[i].z;
00766       depthCloud->points[i].x *= 10 / depthCloud->points[i].z;
00767       depthCloud->points[i].y *= 10 / depthCloud->points[i].z;
00768       depthCloud->points[i].z = 10;
00769     }
00770 
00771     kdTree->setInputCloud(depthCloud);
00772   }
00773 }
00774 
00775 void imuDataHandler(const sensor_msgs::Imu::ConstPtr& imuData)
00776 {
00777   double roll, pitch, yaw;
00778   tf::Quaternion orientation;
00779   tf::quaternionMsgToTF(imuData->orientation, orientation);
00780   tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
00781 
00782   imuPointerLast = (imuPointerLast + 1) % imuQueLength;
00783 
00784   imuTime[imuPointerLast] = imuData->header.stamp.toSec();
00785   imuRoll[imuPointerLast] = roll;
00786   imuPitch[imuPointerLast] = pitch;
00787   imuYaw[imuPointerLast] = yaw;
00788 }
00789 
00790 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 
00791 {
00792   cv_bridge::CvImagePtr bridge = cv_bridge::toCvCopy(imageData, "bgr8");
00793 
00794   int ipRelationsNum = ipRelations->points.size();
00795   for (int i = 0; i < ipRelationsNum; i++) {
00796     if (fabs(ipRelations->points[i].v) < 0.5) {
00797       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00798                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(255, 0, 0), 2);
00799     } else if (fabs(ipRelations->points[i].v - 1) < 0.5) {
00800       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00801                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 255, 0), 2);
00802     } else if (fabs(ipRelations->points[i].v - 2) < 0.5) {
00803       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00804                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 255), 2);
00805     } /*else {
00806       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00807                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 0), 2);
00808     }*/
00809   }
00810 
00811   sensor_msgs::Image::Ptr imagePointer = bridge->toImageMsg();
00812   imageShowPubPointer->publish(imagePointer);
00813 }
00814 
00815 int main(int argc, char** argv)
00816 {
00817   ros::init(argc, argv, "visualOdometry");
00818   ros::NodeHandle nh;
00819 
00820   ros::Subscriber imagePointsSub = nh.subscribe<sensor_msgs::PointCloud2>
00821                                    ("/image_points_last", 5, imagePointsHandler);
00822 
00823   ros::Subscriber depthCloudSub = nh.subscribe<sensor_msgs::PointCloud2> 
00824                                   ("/depth_cloud", 5, depthCloudHandler);
00825 
00826   ros::Subscriber imuDataSub = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 5, imuDataHandler);
00827 
00828   ros::Publisher voDataPub = nh.advertise<nav_msgs::Odometry> ("/cam_to_init", 5);
00829   voDataPubPointer = &voDataPub;
00830 
00831   tf::TransformBroadcaster tfBroadcaster;
00832   tfBroadcasterPointer = &tfBroadcaster;
00833 
00834   ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_last", 5);
00835   depthPointsPubPointer = &depthPointsPub;
00836 
00837   ros::Publisher imagePointsProjPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_proj", 1);
00838   imagePointsProjPubPointer = &imagePointsProjPub;
00839 
00840   ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/image/show", 1, imageDataHandler);
00841 
00842   ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show_2", 1);
00843   imageShowPubPointer = &imageShowPub;
00844 
00845   ros::spin();
00846 
00847   return 0;
00848 }


demo_rgbd
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:25:12