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
00162
00163
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 (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.1 * 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
00528
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.0001 && deltaT < 0.0001) {
00545 break;
00546 }
00547
00548
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
00570
00571
00572
00573
00574
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 + 0.025) * y1 - sin(rx + 0.025) * z1;
00588 double z2 = sin(rx + 0.025) * y1 + cos(rx + 0.025) * 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();
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 }
00811
00812
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 }