bundleAdjust.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 
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.01;
00209   Noise noise0 = Information(mpNoise);
00210 
00211   MatrixXd dpNoise = eye(3);
00212   //dpNoise(2, 2) = 1.;
00213   Noise noise1 = Information(dpNoise);
00214 
00215   MatrixXd ssNoise = 10000. * eye(6);
00216   //ssNoise(3, 3) = 100;
00217   //ssNoise(4, 4) = 100;
00218   //ssNoise(5, 5) = 100;
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 /*&& dpPointer->points[j].label == 1*/) {
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                 //continue;
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 /*&& dpPointer->points[j].label == 1*/) {
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                 //continue;
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       //transformAftBA[1] = (1 - 0.1) * transformAftBA[1] + 0.1 * transformBefBA[1];
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 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:22:43