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
00008 #include <tf/transform_datatypes.h>
00009 #include <tf/transform_listener.h>
00010 #include <tf/transform_broadcaster.h>
00011
00012 #include "isam/isam.h"
00013 #include "isam/slam_depthmono.h"
00014 #include "isam/robust.h"
00015
00016 #include "cameraParameters.h"
00017 #include "pointDefinition.h"
00018
00019 using namespace std;
00020 using namespace isam;
00021 using namespace Eigen;
00022
00023 const double PI = 3.1415926;
00024
00025 const int keyframeNum = 5;
00026 pcl::PointCloud<DepthPoint>::Ptr depthPoints[keyframeNum];
00027 pcl::PointCloud<DepthPoint>::Ptr depthPointsStacked(new pcl::PointCloud<DepthPoint>());
00028
00029 double depthPointsTime;
00030 bool newKeyframe = false;
00031
00032 double rollRec, pitchRec, yawRec;
00033 double txRec, tyRec, tzRec;
00034
00035 double transformBefBA[6] = {0};
00036 double transformAftBA[6] = {0};
00037
00038 void diffRotation(double cx, double cy, double cz, double lx, double ly, double lz,
00039 double &ox, double &oy, double &oz)
00040 {
00041 double srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx))
00042 - cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx);
00043 ox = -asin(srx);
00044
00045 double srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz))
00046 - cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz);
00047 double crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly);
00048 oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
00049
00050 double srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy)
00051 - cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx))
00052 - (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly));
00053 double crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz)
00054 + cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz)
00055 - cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz);
00056 oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
00057 }
00058
00059 void transformAssociateToBA()
00060 {
00061 double txDiff = txRec - transformBefBA[3];
00062 double tyDiff = tyRec - transformBefBA[4];
00063 double tzDiff = tzRec - transformBefBA[5];
00064
00065 double x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff;
00066 double y1 = tyDiff;
00067 double z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff;
00068
00069 double x2 = x1;
00070 double y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
00071 double z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;
00072
00073 txDiff = cos(rollRec) * x2 + sin(rollRec) * y2;
00074 tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2;
00075 tzDiff = z2;
00076
00077 double sbcx = sin(pitchRec);
00078 double cbcx = cos(pitchRec);
00079 double sbcy = sin(yawRec);
00080 double cbcy = cos(yawRec);
00081 double sbcz = sin(rollRec);
00082 double cbcz = cos(rollRec);
00083
00084 double sblx = sin(transformBefBA[0]);
00085 double cblx = cos(transformBefBA[0]);
00086 double sbly = sin(transformBefBA[1]);
00087 double cbly = cos(transformBefBA[1]);
00088 double sblz = sin(transformBefBA[2]);
00089 double cblz = cos(transformBefBA[2]);
00090
00091 double salx = sin(transformAftBA[0]);
00092 double calx = cos(transformAftBA[0]);
00093 double saly = sin(transformAftBA[1]);
00094 double caly = cos(transformAftBA[1]);
00095 double salz = sin(transformAftBA[2]);
00096 double calz = cos(transformAftBA[2]);
00097
00098 double srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly)
00099 - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
00100 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
00101 - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
00102 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
00103 pitchRec = -asin(srx);
00104
00105 double srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
00106 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
00107 - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
00108 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
00109 + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00110 double crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
00111 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
00112 - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
00113 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
00114 + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00115 yawRec = atan2(srycrx / cos(pitchRec), crycrx / cos(pitchRec));
00116
00117 double srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz)
00118 - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx)
00119 - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly)
00120 + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx)
00121 - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz
00122 + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz)
00123 + calx*cblx*salz*sblz);
00124 double crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly)
00125 - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx)
00126 + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
00127 + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly)
00128 + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly
00129 - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz)
00130 - calx*calz*cblx*sblz);
00131 rollRec = atan2(srzcrx / cos(pitchRec), crzcrx / cos(pitchRec));
00132
00133 x1 = cos(rollRec) * txDiff - sin(rollRec) * tyDiff;
00134 y1 = sin(rollRec) * txDiff + cos(rollRec) * tyDiff;
00135 z1 = tzDiff;
00136
00137 x2 = x1;
00138 y2 = cos(pitchRec) * y1 - sin(pitchRec) * z1;
00139 z2 = sin(pitchRec) * y1 + cos(pitchRec) * z1;
00140
00141 txDiff = cos(yawRec) * x2 + sin(yawRec) * z2;
00142 tyDiff = y2;
00143 tzDiff = -sin(yawRec) * x2 + cos(yawRec) * z2;
00144
00145 txRec = transformAftBA[3] + txDiff;
00146 tyRec = transformAftBA[4] + tyDiff;
00147 tzRec = transformAftBA[5] + tzDiff;
00148 }
00149
00150 void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2)
00151 {
00152 depthPointsTime = depthPoints2->header.stamp.toSec();
00153
00154 depthPointsStacked->clear();
00155 pcl::fromROSMsg(*depthPoints2, *depthPointsStacked);
00156 int depthPointsStackedNum = depthPointsStacked->points.size();
00157
00158 for (int i = 0; i < keyframeNum; i++) {
00159 depthPoints[i]->clear();
00160 }
00161
00162 int keyframeCount = -1;
00163 for (int i = 0; i < depthPointsStackedNum; i++) {
00164 if (depthPointsStacked->points[i].ind == -2) {
00165 keyframeCount++;
00166 }
00167
00168 if (keyframeCount >= 0 && keyframeCount < keyframeNum) {
00169 depthPoints[keyframeCount]->push_back(depthPointsStacked->points[i]);
00170 }
00171 }
00172
00173 bool enoughPoints = true;
00174 for (int i = 0; i < keyframeNum; i++) {
00175 if (depthPoints[i]->points.size() < 10) {
00176 enoughPoints = false;
00177 }
00178 }
00179
00180 if (enoughPoints) {
00181 newKeyframe = true;
00182 }
00183 }
00184
00185 int main(int argc, char** argv)
00186 {
00187 ros::init(argc, argv, "bundleAdjust");
00188 ros::NodeHandle nh;
00189
00190 for (int i = 0; i < keyframeNum; i++) {
00191 pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp(new pcl::PointCloud<DepthPoint>());
00192 depthPoints[i] = depthPointsTemp;
00193 }
00194
00195 ros::Subscriber depthPointsSub = nh.subscribe<sensor_msgs::PointCloud2>
00196 ("/depth_points_stacked", 1, depthPointsHandler);
00197
00198 ros::Publisher odomBefBAPub = nh.advertise<nav_msgs::Odometry> ("/bef_ba_to_init", 1);
00199
00200 ros::Publisher odomAftBAPub = nh.advertise<nav_msgs::Odometry> ("/aft_ba_to_init", 1);
00201
00202 tf::TransformBroadcaster tfBroadcaster;
00203
00204 Vector2d pp(0, 0);
00205 DepthmonoCamera camera(1, pp);
00206
00207 MatrixXd mpNoise = eye(3);
00208 mpNoise(2, 2) = 0.001;
00209 Noise noise0 = Information(mpNoise);
00210
00211 MatrixXd dpNoise = eye(3);
00212
00213 Noise noise1 = Information(dpNoise);
00214
00215 MatrixXd ssNoise = 10000. * eye(6);
00216
00217
00218
00219 Noise noise2 = Information(ssNoise);
00220
00221 MatrixXd psNoise = 10000. * eye(6);
00222 psNoise(3, 3) = 100;
00223 psNoise(4, 4) = 100;
00224 psNoise(5, 5) = 100;
00225 Noise noise3 = Information(psNoise);
00226
00227 bool status = ros::ok();
00228 while (status) {
00229 ros::spinOnce();
00230
00231 if (newKeyframe) {
00232 newKeyframe = false;
00233
00234 Slam ba;
00235
00236 vector<Pose3d_Node*> poses;
00237 Pose3d_Node* pose0 = new Pose3d_Node();
00238 poses.push_back(pose0);
00239 ba.add_node(pose0);
00240
00241 rollRec = depthPoints[0]->points[0].depth;
00242 pitchRec = depthPoints[0]->points[0].u;
00243 yawRec = depthPoints[0]->points[0].v;
00244 txRec = depthPoints[0]->points[1].u;
00245 tyRec = depthPoints[0]->points[1].v;
00246 tzRec = depthPoints[0]->points[1].depth;
00247
00248 transformAssociateToBA();
00249
00250 Pose3d_Factor* poseFactors0 = new Pose3d_Factor(pose0,
00251 Pose3d(tzRec, txRec, tyRec, yawRec, pitchRec, rollRec), noise2);
00252 ba.add_factor(poseFactors0);
00253
00254 rollRec = depthPoints[0]->points[0].depth;
00255 pitchRec = depthPoints[0]->points[0].u;
00256 yawRec = depthPoints[0]->points[0].v;
00257 txRec = depthPoints[0]->points[1].u;
00258 tyRec = depthPoints[0]->points[1].v;
00259 tzRec = depthPoints[0]->points[1].depth;
00260
00261 vector<Pose3d_Pose3d_Factor*> posePoseFactors;
00262 for (int i = 1; i < keyframeNum; i++) {
00263 Pose3d_Node* posen = new Pose3d_Node();
00264 poses.push_back(posen);
00265 ba.add_node(posen);
00266
00267 double roll = depthPoints[i]->points[0].depth;
00268 double pitch = depthPoints[i]->points[0].u;
00269 double yaw = depthPoints[i]->points[0].v;
00270 double tx = depthPoints[i]->points[1].u;
00271 double ty = depthPoints[i]->points[1].v;
00272 double tz = depthPoints[i]->points[1].depth;
00273
00274 double txDiff = tx - txRec;
00275 double tyDiff = ty - tyRec;
00276 double tzDiff = tz - tzRec;
00277
00278 double x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff;
00279 double y1 = tyDiff;
00280 double z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff;
00281
00282 double x2 = x1;
00283 double y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
00284 double z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;
00285
00286 txDiff = cos(rollRec) * x2 + sin(rollRec) * y2;
00287 tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2;
00288 tzDiff = z2;
00289
00290 double rollDiff, pitchDiff, yawDiff;
00291 diffRotation(pitch, yaw, roll, pitchRec, yawRec, rollRec, pitchDiff, yawDiff, rollDiff);
00292
00293 Pose3d_Pose3d_Factor* poseposeFactorn = new Pose3d_Pose3d_Factor
00294 (poses[i - 1], posen, Pose3d(tzDiff, txDiff, tyDiff,
00295 yawDiff, pitchDiff, rollDiff), noise3);
00296 posePoseFactors.push_back(poseposeFactorn);
00297 ba.add_factor(poseposeFactorn);
00298
00299 rollRec = roll; pitchRec = pitch; yawRec = yaw;
00300 txRec = tx; tyRec = ty; tzRec = tz;
00301 }
00302
00303 vector<Point3d_Node*> points;
00304 std::vector<int> pointsInd;
00305 vector<Depthmono_Factor*> depthmonoFactors;
00306 for (int i = 0; i < keyframeNum; i++) {
00307 pcl::PointCloud<DepthPoint>::Ptr dpPointer = depthPoints[i];
00308 int kfptNum = dpPointer->points.size();
00309 int ptRecNum = points.size();
00310
00311 if (i == 0) {
00312 pcl::PointCloud<DepthPoint>::Ptr dpPointerNext = depthPoints[i + 1];
00313 int kfptNumNext = dpPointerNext->points.size();
00314
00315 int ptCountNext = 2;
00316 for (int j = 2; j < kfptNum; j++) {
00317 bool ptFound = false;
00318 for (; ptCountNext < kfptNumNext; ptCountNext++) {
00319 if (dpPointerNext->points[ptCountNext].ind == dpPointer->points[j].ind) {
00320 ptFound = true;
00321 }
00322 if (dpPointerNext->points[ptCountNext].ind >= dpPointer->points[j].ind) {
00323 break;
00324 }
00325 }
00326
00327 if (ptFound && dpPointer->points[j].label == 1) {
00328 Point3d_Node* pointn = new Point3d_Node();
00329 points.push_back(pointn);
00330 pointsInd.push_back(dpPointer->points[j].ind);
00331 ba.add_node(pointn);
00332
00333 Depthmono_Factor* depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera,
00334 DepthmonoMeasurement(dpPointer->points[j].u,
00335 dpPointer->points[j].v, dpPointer->points[j].depth),
00336 noise1);
00337 depthmonoFactors.push_back(depthmonoFactorn);
00338 ba.add_factor(depthmonoFactorn);
00339 }
00340 }
00341 } else if (i == keyframeNum - 1) {
00342 pcl::PointCloud<DepthPoint>::Ptr dpPointerLast = depthPoints[i - 1];
00343 int kfptNumLast = dpPointerLast->points.size();
00344
00345 int ptCountLast = 2;
00346 int ptRecCount = 0;
00347 for (int j = 2; j < kfptNum; j++) {
00348 bool ptFound = false;
00349 for (; ptCountLast < kfptNumLast; ptCountLast++) {
00350 if (dpPointerLast->points[ptCountLast].ind == dpPointer->points[j].ind) {
00351 ptFound = true;
00352 }
00353 if (dpPointerLast->points[ptCountLast].ind >= dpPointer->points[j].ind) {
00354 break;
00355 }
00356 }
00357
00358 if (ptFound ) {
00359 bool prFound = false;
00360 for (; ptRecCount < ptRecNum; ptRecCount++) {
00361 if (pointsInd[ptRecCount] == dpPointer->points[j].ind) {
00362 prFound = true;
00363 }
00364 if (pointsInd[ptRecCount] >= dpPointer->points[j].ind) {
00365 break;
00366 }
00367 }
00368
00369 Point3d_Node* pointn;
00370 Depthmono_Factor* depthmonoFactorn;
00371 if (prFound) {
00372 pointn = points[ptRecCount];
00373
00374 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera,
00375 DepthmonoMeasurement(dpPointer->points[j].u,
00376 dpPointer->points[j].v, dpPointer->points[j].depth), noise0);
00377
00378 } else {
00379 pointn = new Point3d_Node();
00380 points.push_back(pointn);
00381 pointsInd.push_back(dpPointer->points[j].ind);
00382 ba.add_node(pointn);
00383
00384 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera,
00385 DepthmonoMeasurement(dpPointer->points[j].u,
00386 dpPointer->points[j].v, dpPointer->points[j].depth), noise1);
00387 }
00388
00389 depthmonoFactors.push_back(depthmonoFactorn);
00390 ba.add_factor(depthmonoFactorn);
00391 }
00392 }
00393 } else {
00394 pcl::PointCloud<DepthPoint>::Ptr dpPointerNext = depthPoints[i + 1];
00395 pcl::PointCloud<DepthPoint>::Ptr dpPointerLast = depthPoints[i - 1];
00396 int kfptNumNext = dpPointerNext->points.size();
00397 int kfptNumLast = dpPointerLast->points.size();
00398
00399 int ptCountNext = 2;
00400 int ptCountLast = 2;
00401 int ptRecCount = 0;
00402 for (int j = 2; j < kfptNum; j++) {
00403 bool ptFound = false;
00404 for (; ptCountNext < kfptNumNext; ptCountNext++) {
00405 if (dpPointerNext->points[ptCountNext].ind == dpPointer->points[j].ind) {
00406 ptFound = true;
00407 }
00408 if (dpPointerNext->points[ptCountNext].ind >= dpPointer->points[j].ind) {
00409 break;
00410 }
00411 }
00412
00413 if (!ptFound) {
00414 for (; ptCountLast < kfptNumLast; ptCountLast++) {
00415 if (dpPointerLast->points[ptCountLast].ind == dpPointer->points[j].ind) {
00416 ptFound = true;
00417 }
00418 if (dpPointerLast->points[ptCountLast].ind >= dpPointer->points[j].ind) {
00419 break;
00420 }
00421 }
00422 }
00423
00424 if (ptFound ) {
00425 bool prFound = false;
00426 for (; ptRecCount < ptRecNum; ptRecCount++) {
00427 if (pointsInd[ptRecCount] == dpPointer->points[j].ind) {
00428 prFound = true;
00429 }
00430 if (pointsInd[ptRecCount] >= dpPointer->points[j].ind) {
00431 break;
00432 }
00433 }
00434
00435 Point3d_Node* pointn;
00436 Depthmono_Factor* depthmonoFactorn;
00437 if (prFound) {
00438 pointn = points[ptRecCount];
00439
00440 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera,
00441 DepthmonoMeasurement(dpPointer->points[j].u,
00442 dpPointer->points[j].v, dpPointer->points[j].depth), noise0);
00443
00444 } else {
00445 pointn = new Point3d_Node();
00446 points.push_back(pointn);
00447 pointsInd.push_back(dpPointer->points[j].ind);
00448 ba.add_node(pointn);
00449
00450 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera,
00451 DepthmonoMeasurement(dpPointer->points[j].u,
00452 dpPointer->points[j].v, dpPointer->points[j].depth), noise1);
00453 }
00454
00455 depthmonoFactors.push_back(depthmonoFactorn);
00456 ba.add_factor(depthmonoFactorn);
00457 }
00458 }
00459 }
00460 }
00461
00462 Properties prop = ba.properties();
00463 prop.method = DOG_LEG;
00464 ba.set_properties(prop);
00465 ba.batch_optimization();
00466
00467 transformBefBA[0] = depthPoints[keyframeNum - 1]->points[0].u;
00468 transformBefBA[1] = depthPoints[keyframeNum - 1]->points[0].v;
00469 transformBefBA[2] = depthPoints[keyframeNum - 1]->points[0].depth;
00470 transformBefBA[3] = depthPoints[keyframeNum - 1]->points[1].u;
00471 transformBefBA[4] = depthPoints[keyframeNum - 1]->points[1].v;
00472 transformBefBA[5] = depthPoints[keyframeNum - 1]->points[1].depth;
00473
00474 transformAftBA[0] = poses[keyframeNum - 1]->value().pitch();
00475 transformAftBA[1] = poses[keyframeNum - 1]->value().yaw();
00476 transformAftBA[2] = poses[keyframeNum - 1]->value().roll();
00477 transformAftBA[3] = poses[keyframeNum - 1]->value().y();
00478 transformAftBA[4] = poses[keyframeNum - 1]->value().z();
00479 transformAftBA[5] = poses[keyframeNum - 1]->value().x();
00480
00481 transformAftBA[0] = (1 - 0.5) * transformAftBA[0] + 0.5 * transformBefBA[0];
00482
00483 transformAftBA[2] = (1 - 0.5) * transformAftBA[2] + 0.5 * transformBefBA[2];
00484
00485 int posesNum = poses.size();
00486 for (int i = 1; i < posesNum; i++) {
00487 delete poses[i];
00488 }
00489 poses.clear();
00490
00491 delete poseFactors0;
00492
00493 int posePoseFactorsNum = posePoseFactors.size();
00494 for (int i = 1; i < posePoseFactorsNum; i++) {
00495 delete posePoseFactors[i];
00496 }
00497 posePoseFactors.clear();
00498
00499 int pointsNum = points.size();
00500 for (int i = 1; i < pointsNum; i++) {
00501 delete points[i];
00502 }
00503 points.clear();
00504
00505 int depthmonoFactorsNum = depthmonoFactors.size();
00506 for (int i = 1; i < depthmonoFactorsNum; i++) {
00507 delete depthmonoFactors[i];
00508 }
00509 depthmonoFactors.clear();
00510
00511 geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
00512 (transformBefBA[2], -transformBefBA[0], -transformBefBA[1]);
00513
00514 nav_msgs::Odometry odomBefBA;
00515 odomBefBA.header.frame_id = "/camera_init";
00516 odomBefBA.child_frame_id = "/bef_ba";
00517 odomBefBA.header.stamp = ros::Time().fromSec(depthPointsTime);
00518 odomBefBA.pose.pose.orientation.x = -geoQuat.y;
00519 odomBefBA.pose.pose.orientation.y = -geoQuat.z;
00520 odomBefBA.pose.pose.orientation.z = geoQuat.x;
00521 odomBefBA.pose.pose.orientation.w = geoQuat.w;
00522 odomBefBA.pose.pose.position.x = transformBefBA[3];
00523 odomBefBA.pose.pose.position.y = transformBefBA[4];
00524 odomBefBA.pose.pose.position.z = transformBefBA[5];
00525 odomBefBAPub.publish(odomBefBA);
00526
00527 geoQuat = tf::createQuaternionMsgFromRollPitchYaw
00528 (transformAftBA[2], -transformAftBA[0], -transformAftBA[1]);
00529
00530 nav_msgs::Odometry odomAftBA;
00531 odomAftBA.header.frame_id = "/camera_init";
00532 odomAftBA.child_frame_id = "/aft_ba";
00533 odomAftBA.header.stamp = ros::Time().fromSec(depthPointsTime);
00534 odomAftBA.pose.pose.orientation.x = -geoQuat.y;
00535 odomAftBA.pose.pose.orientation.y = -geoQuat.z;
00536 odomAftBA.pose.pose.orientation.z = geoQuat.x;
00537 odomAftBA.pose.pose.orientation.w = geoQuat.w;
00538 odomAftBA.pose.pose.position.x = transformAftBA[3];
00539 odomAftBA.pose.pose.position.y = transformAftBA[4];
00540 odomAftBA.pose.pose.position.z = transformAftBA[5];
00541 odomAftBAPub.publish(odomAftBA);
00542
00543 tf::StampedTransform tfAftBA;
00544 tfAftBA.frame_id_ = "/camera_init";
00545 tfAftBA.child_frame_id_ = "/aft_ba";
00546 tfAftBA.stamp_ = ros::Time().fromSec(depthPointsTime);
00547 tfAftBA.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
00548 tfAftBA.setOrigin(tf::Vector3(transformAftBA[3],
00549 transformAftBA[4], transformAftBA[5]));
00550 tfBroadcaster.sendTransform(tfAftBA);
00551 }
00552
00553 status = ros::ok();
00554 cv::waitKey(10);
00555 }
00556
00557 return 0;
00558 }