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 double imagePointsCurTime;
00035 double imagePointsLastTime;
00036
00037 int imagePointsCurNum = 0;
00038 int imagePointsLastNum = 0;
00039
00040 int depthPointsCurNum = 0;
00041 int depthPointsLastNum = 0;
00042
00043 pcl::PointCloud<pcl::PointXYZI>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZI>());
00044 pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdTree(new pcl::KdTreeFLANN<pcl::PointXYZI>());
00045
00046 double depthCloudTime;
00047 int depthCloudNum = 0;
00048
00049 std::vector<int> pointSearchInd;
00050 std::vector<float> pointSearchSqrDis;
00051
00052 float transformSum[6] = {0};
00053 float angleSum[3] = {0};
00054
00055 pcl::PointXYZ origin(0, 0, 0), xAxis(1, 0, 0), yAxis(0, 1, 0), zAxis(0, 0, 1);
00056
00057 int imuPointerFront = 0;
00058 int imuPointerLast = -1;
00059 const int imuQueLength = 200;
00060 bool imuInited = false;
00061
00062 float imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0;
00063 float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
00064
00065 float imuYawInit = 0;
00066 double imuTime[imuQueLength] = {0};
00067 float imuRoll[imuQueLength] = {0};
00068 float imuPitch[imuQueLength] = {0};
00069 float imuYaw[imuQueLength] = {0};
00070
00071 ros::Publisher *voDataPubPointer = NULL;
00072 tf::TransformBroadcaster *tfBroadcasterPointer = NULL;
00073 ros::Publisher *depthPointsPubPointer = NULL;
00074 ros::Publisher *imagePointsProjPubPointer = NULL;
00075 ros::Publisher *imageShowPubPointer;
00076
00077 const int showDSRate = 2;
00078
00079 IplImage *image;
00080 sensor_msgs::CvBridge bridge;
00081
00082 void transformPoint(pcl::PointXYZ *point, float *r)
00083 {
00084 double cosrx = cos(r[0]);
00085 double sinrx = sin(r[0]);
00086 double cosry = cos(r[1]);
00087 double sinry = sin(r[1]);
00088 double cosrz = cos(r[2]);
00089 double sinrz = sin(r[2]);
00090
00091 double x1 = cosry * point->x + sinry * point->z;
00092 double y1 = point->y;
00093 double z1 = -sinry * point->x + cosry * point->z;
00094
00095 double x2 = x1;
00096 double y2 = cosrx * y1 - sinrx * z1;
00097 double z2 = sinrx * y1 + cosrx * z1;
00098
00099 point->x = cosrz * x2 - sinrz * y2 + r[3];
00100 point->y = sinrz * x2 + cosrz * y2 + r[4];
00101 point->z = z2 + r[5];
00102 }
00103
00104 void transformBack(pcl::PointXYZ *point, float *r)
00105 {
00106 double cosrx = cos(r[0]);
00107 double sinrx = sin(r[0]);
00108 double cosry = cos(r[1]);
00109 double sinry = sin(r[1]);
00110 double cosrz = cos(r[2]);
00111 double sinrz = sin(r[2]);
00112
00113 double x1 = cosrz * (point->x - r[3]) + sinrz * (point->y - r[4]);
00114 double y1 = -sinrz * (point->x - r[3]) + cosrz * (point->y - r[4]);
00115 double z1 = point->z - r[5];
00116
00117 double x2 = x1;
00118 double y2 = cosrx * y1 + sinrx * z1;
00119 double z2 = -sinrx * y1 + cosrx * z1;
00120
00121 point->x = cosry * x2 - sinry * z2;
00122 point->y = y2;
00123 point->z = sinry * x2 + cosry * z2;
00124 }
00125
00126 void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2)
00127 {
00128 imagePointsLastTime = imagePointsCurTime;
00129 imagePointsCurTime = imagePoints2->header.stamp.toSec();
00130
00131 imuRollLast = imuRollCur;
00132 imuPitchLast = imuPitchCur;
00133 imuYawLast = imuYawCur;
00134
00135 float transform[6] = {0};
00136 if (imuPointerLast >= 0) {
00137 while (imuPointerFront != imuPointerLast) {
00138 if (imagePointsCurTime < imuTime[imuPointerFront]) {
00139 break;
00140 }
00141 imuPointerFront = (imuPointerFront + 1) % imuQueLength;
00142 }
00143
00144 if (imagePointsCurTime > imuTime[imuPointerFront]) {
00145 imuRollCur = imuRoll[imuPointerFront];
00146 imuPitchCur = imuPitch[imuPointerFront];
00147 imuYawCur = imuYaw[imuPointerFront];
00148 } else {
00149 int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
00150 float ratioFront = (imagePointsCurTime - imuTime[imuPointerBack])
00151 / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
00152 float ratioBack = (imuTime[imuPointerFront] - imagePointsCurTime)
00153 / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
00154
00155 imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
00156 imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
00157 if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) {
00158 imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack;
00159 } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) {
00160 imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack;
00161 } else {
00162 imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
00163 }
00164 }
00165
00166 if (imuInited) {
00167
00168
00169
00170 }
00171 }
00172
00173 pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
00174 imagePointsLast = imagePointsCur;
00175 imagePointsCur = imagePointsTemp;
00176
00177 imagePointsCur->clear();
00178 pcl::fromROSMsg(*imagePoints2, *imagePointsCur);
00179
00180 imagePointsLastNum = imagePointsCurNum;
00181 imagePointsCurNum = imagePointsCur->points.size();
00182
00183 pcl::PointCloud<ImagePoint>::Ptr startPointsTemp = startPointsLast;
00184 startPointsLast = startPointsCur;
00185 startPointsCur = startPointsTemp;
00186
00187 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransTemp = startTransLast;
00188 startTransLast = startTransCur;
00189 startTransCur = startTransTemp;
00190
00191 int j = 0;
00192 pcl::PointXYZI ips;
00193 pcl::PointXYZHSV ipr;
00194 ipRelations->clear();
00195 ipInd.clear();
00196 for (int i = 0; i < imagePointsLastNum; i++) {
00197 bool ipFound = false;
00198 for (; j < imagePointsCurNum; j++) {
00199 if (imagePointsCur->points[j].ind == imagePointsLast->points[i].ind) {
00200 ipFound = true;
00201 }
00202 if (imagePointsCur->points[j].ind >= imagePointsLast->points[i].ind) {
00203 break;
00204 }
00205 }
00206
00207 if (ipFound) {
00208 ipr.x = imagePointsLast->points[i].u;
00209 ipr.y = imagePointsLast->points[i].v;
00210 ipr.z = imagePointsCur->points[j].u;
00211 ipr.h = imagePointsCur->points[j].v;
00212
00213 ips.x = 10 * ipr.x;
00214 ips.y = 10 * ipr.y;
00215 ips.z = 10;
00216
00217 if (depthCloudNum > 10) {
00218 kdTree->nearestKSearch(ips, 3, pointSearchInd, pointSearchSqrDis);
00219
00220 double minDepth, maxDepth;
00221 if (pointSearchSqrDis[0] < 0.5 && pointSearchInd.size() == 3) {
00222 pcl::PointXYZI depthPoint = depthCloud->points[pointSearchInd[0]];
00223 double x1 = depthPoint.x * depthPoint.intensity / 10;
00224 double y1 = depthPoint.y * depthPoint.intensity / 10;
00225 double z1 = depthPoint.intensity;
00226 minDepth = z1;
00227 maxDepth = z1;
00228
00229 depthPoint = depthCloud->points[pointSearchInd[1]];
00230 double x2 = depthPoint.x * depthPoint.intensity / 10;
00231 double y2 = depthPoint.y * depthPoint.intensity / 10;
00232 double z2 = depthPoint.intensity;
00233 minDepth = (z2 < minDepth)? z2 : minDepth;
00234 maxDepth = (z2 > maxDepth)? z2 : maxDepth;
00235
00236 depthPoint = depthCloud->points[pointSearchInd[2]];
00237 double x3 = depthPoint.x * depthPoint.intensity / 10;
00238 double y3 = depthPoint.y * depthPoint.intensity / 10;
00239 double z3 = depthPoint.intensity;
00240 minDepth = (z3 < minDepth)? z3 : minDepth;
00241 maxDepth = (z3 > maxDepth)? z3 : maxDepth;
00242
00243 double u = ipr.x;
00244 double v = ipr.y;
00245 ipr.s = (x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1)
00246 / (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2 + u*y1*z2 - u*y2*z1
00247 - v*x1*z2 + v*x2*z1 - u*y1*z3 + u*y3*z1 + v*x1*z3 - v*x3*z1 + u*y2*z3
00248 - u*y3*z2 - v*x2*z3 + v*x3*z2);
00249 ipr.v = 1;
00250
00251 if (maxDepth - minDepth > 2) {
00252 ipr.s = 0;
00253 ipr.v = 0;
00254 } else if (ipr.s - maxDepth > 0.2) {
00255 ipr.s = maxDepth;
00256 } else if (ipr.s - minDepth < -0.2) {
00257 ipr.s = minDepth;
00258 }
00259 } else {
00260 ipr.s = 0;
00261 ipr.v = 0;
00262 }
00263 } else {
00264 ipr.s = 0;
00265 ipr.v = 0;
00266 }
00267
00268 if (fabs(ipr.v) < 0.5) {
00269 double disX = transformSum[3] - startTransLast->points[i].h;
00270 double disY = transformSum[4] - startTransLast->points[i].s;
00271 double disZ = transformSum[5] - startTransLast->points[i].v;
00272
00273 if (sqrt(disX * disX + disY * disY + disZ * disZ) > 1) {
00274
00275 float u0 = startPointsLast->points[i].u;
00276 float v0 = startPointsLast->points[i].v;
00277 float u1 = ipr.x;
00278 float v1 = ipr.y;
00279
00280 float srx0 = sin(-startTransLast->points[i].x);
00281 float crx0 = cos(-startTransLast->points[i].x);
00282 float sry0 = sin(-startTransLast->points[i].y);
00283 float cry0 = cos(-startTransLast->points[i].y);
00284 float srz0 = sin(-startTransLast->points[i].z);
00285 float crz0 = cos(-startTransLast->points[i].z);
00286
00287 float srx1 = sin(-transformSum[0]);
00288 float crx1 = cos(-transformSum[0]);
00289 float sry1 = sin(-transformSum[1]);
00290 float cry1 = cos(-transformSum[1]);
00291 float srz1 = sin(-transformSum[2]);
00292 float crz1 = cos(-transformSum[2]);
00293
00294 float tx0 = -startTransLast->points[i].h;
00295 float ty0 = -startTransLast->points[i].s;
00296 float tz0 = -startTransLast->points[i].v;
00297
00298 float tx1 = -transformSum[3];
00299 float ty1 = -transformSum[4];
00300 float tz1 = -transformSum[5];
00301
00302 float top0 = (srx1*(ty0 - ty1) + crx1*cry1*(tz0 - tz1) - crx1*sry1*(tx0
00303 - tx1))*(v0*((cry0*srz0 + crz0*srx0*sry0)*(cry1*crz1 - srx1*sry1*srz1) + (sry0*srz0
00304 - cry0*crz0*srx0)*(crz1*sry1 + cry1*srx1*srz1) - crx0*crx1*crz0*srz1)
00305 + u0*((crz0*sry0 + cry0*srx0*srz0)*(crz1*sry1 + cry1*srx1*srz1) + (cry0*crz0
00306 - srx0*sry0*srz0)*(cry1*crz1 - srx1*sry1*srz1) + crx0*crx1*srz0*srz1)
00307 + crx0*cry0*(crz1*sry1 + cry1*srx1*srz1) - crx0*sry0*(cry1*crz1 - srx1*sry1*srz1)
00308 - crx1*srx0*srz1) - ((cry1*crz1 - srx1*sry1*srz1)*(tx0 - tx1) + (crz1*sry1
00309 + cry1*srx1*srz1)*(tz0 - tz1) - crx1*srz1*(ty0 - ty1))*(srx0*srx1
00310 + v0*(crx1*cry1*(sry0*srz0 - cry0*crz0*srx0) - crx1*sry1*(cry0*srz0
00311 + crz0*srx0*sry0) + crx0*crz0*srx1) - u0*(crx1*sry1*(cry0*crz0 - srx0*sry0*srz0)
00312 - crx1*cry1*(crz0*sry0 + cry0*srx0*srz0) + crx0*srx1*srz0) + crx0*crx1*sry0*sry1
00313 + crx0*crx1*cry0*cry1);
00314
00315 float down0 = u1*(srx0*srx1 + v0*(crx1*cry1*(sry0*srz0 - cry0*crz0*srx0)
00316 - crx1*sry1*(cry0*srz0 + crz0*srx0*sry0) + crx0*crz0*srx1)
00317 - u0*(crx1*sry1*(cry0*crz0 - srx0*sry0*srz0) - crx1*cry1*(crz0*sry0
00318 + cry0*srx0*srz0) + crx0*srx1*srz0) + crx0*crx1*sry0*sry1 + crx0*crx1*cry0*cry1)
00319 - v0*((cry0*srz0 + crz0*srx0*sry0)*(cry1*crz1 - srx1*sry1*srz1) + (sry0*srz0
00320 - cry0*crz0*srx0)*(crz1*sry1 + cry1*srx1*srz1) - crx0*crx1*crz0*srz1)
00321 - u0*((crz0*sry0 + cry0*srx0*srz0)*(crz1*sry1 + cry1*srx1*srz1) + (cry0*crz0
00322 - srx0*sry0*srz0)*(cry1*crz1 - srx1*sry1*srz1) + crx0*crx1*srz0*srz1)
00323 - crx0*cry0*(crz1*sry1 + cry1*srx1*srz1) + crx0*sry0*(cry1*crz1 - srx1*sry1*srz1)
00324 + crx1*srx0*srz1;
00325
00326 float top1 = (srx1*(ty0 - ty1) + crx1*cry1*(tz0 - tz1) - crx1*sry1*(tx0
00327 - tx1))*(v0*((cry0*srz0 + crz0*srx0*sry0)*(cry1*srz1 + crz1*srx1*sry1) + (sry0*srz0
00328 - cry0*crz0*srx0)*(sry1*srz1 - cry1*crz1*srx1) + crx0*crx1*crz0*crz1)
00329 + u0*((crz0*sry0 + cry0*srx0*srz0)*(sry1*srz1 - cry1*crz1*srx1) + (cry0*crz0
00330 - srx0*sry0*srz0)*(cry1*srz1 + crz1*srx1*sry1) - crx0*crx1*crz1*srz0)
00331 + crx0*cry0*(sry1*srz1 - cry1*crz1*srx1) - crx0*sry0*(cry1*srz1 + crz1*srx1*sry1)
00332 + crx1*crz1*srx0) - ((cry1*srz1 + crz1*srx1*sry1)*(tx0 - tx1) + (sry1*srz1
00333 - cry1*crz1*srx1)*(tz0 - tz1) + crx1*crz1*(ty0 - ty1))*(srx0*srx1
00334 + v0*(crx1*cry1*(sry0*srz0 - cry0*crz0*srx0) - crx1*sry1*(cry0*srz0
00335 + crz0*srx0*sry0) + crx0*crz0*srx1) - u0*(crx1*sry1*(cry0*crz0 - srx0*sry0*srz0)
00336 - crx1*cry1*(crz0*sry0 + cry0*srx0*srz0) + crx0*srx1*srz0) + crx0*crx1*sry0*sry1
00337 + crx0*crx1*cry0*cry1);
00338
00339 float down1 = v1*(srx0*srx1 + v0*(crx1*cry1*(sry0*srz0 - cry0*crz0*srx0)
00340 - crx1*sry1*(cry0*srz0 + crz0*srx0*sry0) + crx0*crz0*srx1)
00341 - u0*(crx1*sry1*(cry0*crz0 - srx0*sry0*srz0) - crx1*cry1*(crz0*sry0
00342 + cry0*srx0*srz0) + crx0*srx1*srz0) + crx0*crx1*sry0*sry1 + crx0*crx1*cry0*cry1)
00343 - v0*((cry0*srz0 + crz0*srx0*sry0)*(cry1*srz1 + crz1*srx1*sry1) + (sry0*srz0
00344 - cry0*crz0*srx0)*(sry1*srz1 - cry1*crz1*srx1) + crx0*crx1*crz0*crz1)
00345 - u0*((crz0*sry0 + cry0*srx0*srz0)*(sry1*srz1 - cry1*crz1*srx1) + (cry0*crz0
00346 - srx0*sry0*srz0)*(cry1*srz1 + crz1*srx1*sry1) - crx0*crx1*crz1*srz0)
00347 - crx0*cry0*(sry1*srz1 - cry1*crz1*srx1) + crx0*sry0*(cry1*srz1 + crz1*srx1*sry1)
00348 - crx1*crz1*srx0;
00349
00350 float depth0 = top0 / down0;
00351 float depth1 = top1 / down1;
00352 if (depth0 > 0.5 && depth0 < 100 && depth1 > 0.5 && depth1 < 100) {
00353 ipr.s = (depth0 + depth1) / 2;
00354 ipr.v = 2;
00355 } else if (depth0 > 0.5 && depth0 < 100) {
00356 ipr.s = depth0;
00357 ipr.v = 2;
00358 } else if (depth1 > 0.5 && depth1 < 100) {
00359 ipr.s = depth1;
00360 ipr.v = 2;
00361 }
00362 }
00363 }
00364
00365 ipRelations->push_back(ipr);
00366 ipInd.push_back(imagePointsLast->points[i].ind);
00367 }
00368 }
00369
00370 int iterNum = 50;
00371 pcl::PointXYZHSV ipr2, ipr3, ipr4;
00372 int ipRelationsNum = ipRelations->points.size();
00373 int ptNumNoDepthRec = 0;
00374 int ptNumWithDepthRec = 0;
00375 double meanValueWithDepthRec = 100000;
00376 for (int iterCount = 0; iterCount < iterNum; iterCount++) {
00377 ipRelations2->clear();
00378 ipy2.clear();
00379 int ptNumNoDepth = 0;
00380 int ptNumWithDepth = 0;
00381 double meanValueNoDepth = 0;
00382 double meanValueWithDepth = 0;
00383 for (int i = 0; i < ipRelationsNum; i++) {
00384 ipr = ipRelations->points[i];
00385
00386 float u0 = ipr.x;
00387 float v0 = ipr.y;
00388 float u1 = ipr.z;
00389 float v1 = ipr.h;
00390
00391 float srx = sin(transform[0]);
00392 float crx = cos(transform[0]);
00393 float sry = sin(transform[1]);
00394 float cry = cos(transform[1]);
00395 float srz = sin(transform[2]);
00396 float crz = cos(transform[2]);
00397 float tx = transform[3];
00398 float ty = transform[4];
00399 float tz = transform[5];
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 float 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 float 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 float 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 float 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 }
00538
00539 if (!imuInited) {
00540 imuYawInit = imuYawCur;
00541 transform[0] -= imuPitchCur;
00542 transform[2] -= imuRollCur;
00543
00544 imuInited = true;
00545 }
00546
00547 transformPoint(&origin, transform);
00548 transformPoint(&xAxis, transform);
00549 transformPoint(&yAxis, transform);
00550 transformPoint(&zAxis, transform);
00551
00552
00553
00554
00555
00556
00557 float rx = -asin(yAxis.z - origin.z);
00558 float ry = -atan2(-(xAxis.z - origin.z) / cos(rx), (zAxis.z - origin.z) / cos(rx));
00559 float rz = -asin(-(yAxis.x - origin.x) / cos(rx));
00560
00561 if (imuPointerLast >= 0) {
00562 transformBack(&origin, transform);
00563 transformBack(&xAxis, transform);
00564 transformBack(&yAxis, transform);
00565 transformBack(&zAxis, transform);
00566
00567 transform[0] -= 0.1 * (imuPitchCur - rx);
00568
00569
00570
00571
00572
00573
00574
00575 transform[2] -= 0.1 * (imuRollCur - rz);
00576
00577 transformPoint(&origin, transform);
00578 transformPoint(&xAxis, transform);
00579 transformPoint(&yAxis, transform);
00580 transformPoint(&zAxis, transform);
00581
00582 rx = -asin(yAxis.z - origin.z);
00583 ry = -atan2(-(xAxis.z - origin.z) / cos(rx), (zAxis.z - origin.z) / cos(rx));
00584 rz = -asin(-(yAxis.x - origin.x) / cos(rx));
00585 }
00586
00587 float x1 = cos(rz) * transform[3] - sin(rz) * transform[4];
00588 float y1 = sin(rz) * transform[3] + cos(rz) * transform[4];
00589 float z1 = transform[5];
00590
00591 float x2 = x1;
00592 float y2 = cos(rx + 0.035) * y1 - sin(rx + 0.035) * z1;
00593 float z2 = sin(rx + 0.035) * y1 + cos(rx + 0.035) * z1;
00594
00595 float tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
00596 float ty = transformSum[4] - y2;
00597 float tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
00598
00599 transformSum[0] = rx;
00600 transformSum[1] = ry;
00601 transformSum[2] = rz;
00602 transformSum[3] = tx;
00603 transformSum[4] = ty;
00604 transformSum[5] = tz;
00605
00606 pcl::PointXYZHSV spc;
00607 spc.x = transformSum[0];
00608 spc.y = transformSum[1];
00609 spc.z = transformSum[2];
00610 spc.h = transformSum[3];
00611 spc.s = transformSum[4];
00612 spc.v = transformSum[5];
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 } else {
00630 startPointsCur->push_back(imagePointsCur->points[i]);
00631 startTransCur->push_back(spc);
00632 }
00633 }
00634 startPointsLast->clear();
00635 startTransLast->clear();
00636
00637 angleSum[0] -= transform[0];
00638 angleSum[1] -= transform[1];
00639 angleSum[2] -= transform[2];
00640
00641 geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);
00642
00643 nav_msgs::Odometry voData;
00644 voData.header.frame_id = "/camera_init";
00645 voData.child_frame_id = "/camera";
00646 voData.header.stamp = imagePoints2->header.stamp;
00647 voData.pose.pose.orientation.x = -geoQuat.y;
00648 voData.pose.pose.orientation.y = -geoQuat.z;
00649 voData.pose.pose.orientation.z = geoQuat.x;
00650 voData.pose.pose.orientation.w = geoQuat.w;
00651 voData.pose.pose.position.x = tx;
00652 voData.pose.pose.position.y = ty;
00653 voData.pose.pose.position.z = tz;
00654 voData.twist.twist.angular.x = angleSum[0];
00655 voData.twist.twist.angular.y = angleSum[1];
00656 voData.twist.twist.angular.z = angleSum[2];
00657 voDataPubPointer->publish(voData);
00658
00659 tf::StampedTransform voTrans;
00660 voTrans.frame_id_ = "/camera_init";
00661 voTrans.child_frame_id_ = "/camera";
00662 voTrans.stamp_ = imagePoints2->header.stamp;
00663 voTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
00664 voTrans.setOrigin(tf::Vector3(tx, ty, tz));
00665 tfBroadcasterPointer->sendTransform(voTrans);
00666
00667 pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp = depthPointsLast;
00668 depthPointsLast = depthPointsCur;
00669 depthPointsCur = depthPointsTemp;
00670
00671 DepthPoint ipd;
00672 depthPointsCur->clear();
00673
00674 ipd.u = transformSum[0];
00675 ipd.v = transformSum[1];
00676 ipd.depth = transformSum[2];
00677 ipd.ind = -2;
00678 depthPointsCur->push_back(ipd);
00679
00680 ipd.u = transformSum[3];
00681 ipd.v = transformSum[4];
00682 ipd.depth = transformSum[5];
00683 ipd.ind = -1;
00684 depthPointsCur->push_back(ipd);
00685
00686 depthPointsLastNum = depthPointsCurNum;
00687 depthPointsCurNum = 2;
00688
00689 j = 0;
00690 pcl::PointXYZ ipp;
00691 depthPointsSend->clear();
00692 imagePointsProj->clear();
00693 for (int i = 0; i < ipRelationsNum; i++) {
00694 if (fabs(ipRelations->points[i].v - 1) < 0.5 || fabs(ipRelations->points[i].v - 2) < 0.5) {
00695
00696 ipd.u = ipRelations->points[i].z;
00697 ipd.v = ipRelations->points[i].h;
00698 ipd.depth = ipRelations->points[i].s + transform[5];
00699 ipd.label = ipRelations->points[i].v;
00700 ipd.ind = ipInd[i];
00701
00702 depthPointsCur->push_back(ipd);
00703 depthPointsCurNum++;
00704
00705 for (; j < depthPointsLastNum; j++) {
00706 if (depthPointsLast->points[j].ind < ipInd[i]) {
00707 depthPointsSend->push_back(depthPointsLast->points[j]);
00708 } else if (depthPointsLast->points[j].ind > ipInd[i]) {
00709 break;
00710 }
00711 }
00712
00713 ipd.u = ipRelations->points[i].x;
00714 ipd.v = ipRelations->points[i].y;
00715 ipd.depth = ipRelations->points[i].s;
00716
00717 depthPointsSend->push_back(ipd);
00718
00719 ipp.x = ipRelations->points[i].x * ipRelations->points[i].s;
00720 ipp.y = ipRelations->points[i].y * ipRelations->points[i].s;
00721 ipp.z = ipRelations->points[i].s;
00722
00723 imagePointsProj->push_back(ipp);
00724 }
00725 }
00726
00727 depthPointsSend->header.frame_id = "camera2";
00728 depthPointsSend->header.stamp = ros::Time().fromSec(imagePointsLastTime);
00729 sensor_msgs::PointCloud2 depthPoints2;
00730 pcl::toROSMsg(*depthPointsSend, depthPoints2);
00731 depthPointsPubPointer->publish(depthPoints2);
00732
00733 imagePointsProj->header.frame_id = "camera2";
00734 imagePointsProj->header.stamp = ros::Time().fromSec(imagePointsLastTime);
00735 sensor_msgs::PointCloud2 imagePointsProj2;
00736 pcl::toROSMsg(*imagePointsProj, imagePointsProj2);
00737 imagePointsProjPubPointer->publish(imagePointsProj2);
00738 }
00739
00740 void depthCloudHandler(const sensor_msgs::PointCloud2ConstPtr& depthCloud2)
00741 {
00742 depthCloudTime = depthCloud2->header.stamp.toSec();
00743
00744 depthCloud->clear();
00745 pcl::fromROSMsg(*depthCloud2, *depthCloud);
00746 depthCloudNum = depthCloud->points.size();
00747
00748 if (depthCloudNum > 10) {
00749 for (int i = 0; i < depthCloudNum; i++) {
00750 depthCloud->points[i].intensity = depthCloud->points[i].z;
00751 depthCloud->points[i].x *= 10 / depthCloud->points[i].z;
00752 depthCloud->points[i].y *= 10 / depthCloud->points[i].z;
00753 depthCloud->points[i].z = 10;
00754 }
00755
00756 kdTree->setInputCloud(depthCloud);
00757 }
00758 }
00759
00760 void imuDataHandler(const sensor_msgs::Imu::ConstPtr& imuData)
00761 {
00762 double roll, pitch, yaw;
00763 tf::Quaternion orientation;
00764 tf::quaternionMsgToTF(imuData->orientation, orientation);
00765 tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
00766
00767 imuPointerLast = (imuPointerLast + 1) % imuQueLength;
00768
00769 imuTime[imuPointerLast] = imuData->header.stamp.toSec();
00770 imuRoll[imuPointerLast] = roll;
00771 imuPitch[imuPointerLast] = pitch;
00772 imuYaw[imuPointerLast] = yaw;
00773 }
00774
00775 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
00776 {
00777 image = bridge.imgMsgToCv(imageData, "bgr8");
00778
00779 int ipRelationsNum = ipRelations->points.size();
00780 for (int i = 0; i < ipRelationsNum; i++) {
00781 if (fabs(ipRelations->points[i].v) < 0.5) {
00782 cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00783 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 255), 2);
00784 } else if (fabs(ipRelations->points[i].v - 1) < 0.5) {
00785 cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00786 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 255, 0), 2);
00787 } else if (fabs(ipRelations->points[i].v - 2) < 0.5) {
00788 cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00789 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0,255,0), 2);
00790 } else {
00791 cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
00792 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(255, 0, 0), 2);
00793 }
00794 }
00795
00796 sensor_msgs::Image::Ptr imageShow = bridge.cvToImgMsg(image, "bgr8");
00797 imageShowPubPointer->publish(imageShow);
00798 }
00799
00800 int main(int argc, char** argv)
00801 {
00802 ros::init(argc, argv, "visualOdometry");
00803 ros::NodeHandle nh;
00804
00805 ros::Subscriber imagePointsSub = nh.subscribe<sensor_msgs::PointCloud2>
00806 ("/image_points_last", 5, imagePointsHandler);
00807
00808 ros::Subscriber depthCloudSub = nh.subscribe<sensor_msgs::PointCloud2>
00809 ("/depth_cloud", 5, depthCloudHandler);
00810
00811 ros::Subscriber imuDataSub = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 5, imuDataHandler);
00812
00813 ros::Publisher voDataPub = nh.advertise<nav_msgs::Odometry> ("/cam_to_init", 5);
00814 voDataPubPointer = &voDataPub;
00815
00816 tf::TransformBroadcaster tfBroadcaster;
00817 tfBroadcasterPointer = &tfBroadcaster;
00818
00819 ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_last", 5);
00820 depthPointsPubPointer = &depthPointsPub;
00821
00822 ros::Publisher imagePointsProjPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_proj", 1);
00823 imagePointsProjPubPointer = &imagePointsProjPub;
00824
00825 ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/image/show", 1, imageDataHandler);
00826
00827 ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show_2", 1);
00828 imageShowPubPointer = &imageShowPub;
00829
00830 ros::spin();
00831
00832 return 0;
00833 }