From b9ef1f268c64106cc41b55282b36d1f0ba4d44ae Mon Sep 17 00:00:00 2001 From: Ji Zhang Date: Sat, 1 Mar 2014 15:56:29 -0500 Subject: [PATCH 1/7] Recommit --- src/featureTracking.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/featureTracking.cpp b/src/featureTracking.cpp index 4de02ac..c1a18a3 100644 --- a/src/featureTracking.cpp +++ b/src/featureTracking.cpp @@ -26,8 +26,8 @@ CvSize showSize = cvSize(imageWidth / showDSRate, imageHeight / showDSRate); IplImage *imageShowMono = cvCreateImage(showSize, IPL_DEPTH_8U, 1); IplImage *imageShowRGB = cvCreateImage(showSize, IPL_DEPTH_8U, 3); -CvMat kMat = cvMat(3, 3, CV_64FC1, kArray); -CvMat dMat = cvMat(4, 1, CV_64FC1, dArray); +CvMat kMat = cvMat(3, 3, CV_64FC1, kImage); +CvMat dMat = cvMat(4, 1, CV_64FC1, dImage); IplImage *mapx, *mapy; @@ -159,19 +159,19 @@ void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) featuresLast[featureCount].y = featuresLast[i].y; featuresInd[featureCount] = featuresInd[i]; - point.u = -(featuresCur[featureCount].x - kArray[2]) / kArray[0]; - point.v = -(featuresCur[featureCount].y - kArray[5]) / kArray[4]; + point.u = -(featuresCur[featureCount].x - kImage[2]) / kImage[0]; + point.v = -(featuresCur[featureCount].y - kImage[5]) / kImage[4]; point.ind = featuresInd[featureCount]; imagePointsCur->push_back(point); if (i >= recordFeatureNum) { - point.u = -(featuresLast[featureCount].x - kArray[2]) / kArray[0]; - point.v = -(featuresLast[featureCount].y - kArray[5]) / kArray[4]; + point.u = -(featuresLast[featureCount].x - kImage[2]) / kImage[0]; + point.v = -(featuresLast[featureCount].y - kImage[5]) / kImage[4]; imagePointsLast->push_back(point); } - meanShiftX += fabs((featuresCur[featureCount].x - featuresLast[featureCount].x) / kArray[0]); - meanShiftY += fabs((featuresCur[featureCount].y - featuresLast[featureCount].y) / kArray[4]); + meanShiftX += fabs((featuresCur[featureCount].x - featuresLast[featureCount].x) / kImage[0]); + meanShiftY += fabs((featuresCur[featureCount].y - featuresLast[featureCount].y) / kImage[4]); featureCount++; subregionFeatureNum[ind]++; @@ -231,4 +231,3 @@ int main(int argc, char** argv) return 0; } - From df097a4d5e35210513f8603a72723cc431c0cf3e Mon Sep 17 00:00:00 2001 From: Ji Zhang Date: Sat, 1 Mar 2014 15:57:20 -0500 Subject: [PATCH 2/7] Recommit --- src/cameraParameters.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/cameraParameters.h b/src/cameraParameters.h index 803b1a8..1407409 100644 --- a/src/cameraParameters.h +++ b/src/cameraParameters.h @@ -16,11 +16,10 @@ const int imageWidth = 744; const int imageHeight = 480; -double kArray[9] = {4.177343016733e+002, 0, 3.715643918956e+002, +double kImage[9] = {4.177343016733e+002, 0, 3.715643918956e+002, 0, 4.177970397634e+002, 1.960688121183e+002, 0, 0, 1}; -double dArray[4] = {-3.396867101163e-001, 1.309347902588e-001, -2.346791258754e-004, 2.209387016957e-004}; +double dImage[4] = {-3.396867101163e-001, 1.309347902588e-001, -2.346791258754e-004, 2.209387016957e-004}; #endif - From d9916ead2aed2584ca9cdf5decf54a5c01b82afd Mon Sep 17 00:00:00 2001 From: Ji Zhang Date: Sat, 1 Mar 2014 15:58:03 -0500 Subject: [PATCH 3/7] Recommit --- src/transformMaintenance.cpp | 58 ++++++++++++++++++++++++++++++++++-- 1 file changed, 55 insertions(+), 3 deletions(-) diff --git a/src/transformMaintenance.cpp b/src/transformMaintenance.cpp index 731bdd1..0481f1f 100644 --- a/src/transformMaintenance.cpp +++ b/src/transformMaintenance.cpp @@ -45,9 +45,61 @@ void transformAssociateToBA() tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2; tzDiff = z2; - pitchRec += transformAftBA[0] - transformBefBA[0]; - yawRec += transformAftBA[1] - transformBefBA[1]; - rollRec += transformAftBA[2] - transformBefBA[2]; + float sbcx = sin(-pitchRec); + float cbcx = cos(-pitchRec); + float sbcy = sin(-yawRec); + float cbcy = cos(-yawRec); + float sbcz = sin(rollRec); + float cbcz = cos(rollRec); + + float sblx = sin(-transformBefBA[0]); + float cblx = cos(-transformBefBA[0]); + float sbly = sin(-transformBefBA[1]); + float cbly = cos(-transformBefBA[1]); + float sblz = sin(transformBefBA[2]); + float cblz = cos(transformBefBA[2]); + + float salx = sin(-transformAftBA[0]); + float calx = cos(-transformAftBA[0]); + float saly = sin(-transformAftBA[1]); + float caly = cos(-transformAftBA[1]); + float salz = sin(transformAftBA[2]); + float calz = cos(transformAftBA[2]); + + float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) + - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); + pitchRec = asin(srx); + + float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + yawRec = -atan2(srycrx / cos(-pitchRec), crycrx / cos(-pitchRec)); + + float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) + - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) + - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) + - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + + calx*cblx*salz*sblz); + float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) + - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly + - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) + - calx*calz*cblx*sblz); + rollRec = atan2(srzcrx / cos(-pitchRec), crzcrx / cos(-pitchRec)); x1 = cos(rollRec) * txDiff - sin(rollRec) * tyDiff; y1 = sin(rollRec) * txDiff + cos(rollRec) * tyDiff; From 78f6b41c96d24387f11e7734ae6f063d7f464291 Mon Sep 17 00:00:00 2001 From: Ji Zhang Date: Sat, 1 Mar 2014 15:58:36 -0500 Subject: [PATCH 4/7] Recommit --- src/visualOdometry.cpp | 136 +++++++++++++++++------------------------ 1 file changed, 56 insertions(+), 80 deletions(-) diff --git a/src/visualOdometry.cpp b/src/visualOdometry.cpp index 8e91f5f..77852f1 100644 --- a/src/visualOdometry.cpp +++ b/src/visualOdometry.cpp @@ -52,8 +52,6 @@ std::vector pointSearchSqrDis; float transformSum[6] = {0}; float angleSum[3] = {0}; -pcl::PointXYZ origin(0, 0, 0), xAxis(1, 0, 0), yAxis(0, 1, 0), zAxis(0, 0, 1); - int imuPointerFront = 0; int imuPointerLast = -1; const int imuQueLength = 200; @@ -79,48 +77,44 @@ const int showDSRate = 2; IplImage *image; sensor_msgs::CvBridge bridge; -void transformPoint(pcl::PointXYZ *point, float *r) +void accumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, + float &ox, float &oy, float &oz) { - double cosrx = cos(r[0]); - double sinrx = sin(r[0]); - double cosry = cos(r[1]); - double sinry = sin(r[1]); - double cosrz = cos(r[2]); - double sinrz = sin(r[2]); - - double x1 = cosry * point->x + sinry * point->z; - double y1 = point->y; - double z1 = -sinry * point->x + cosry * point->z; - - double x2 = x1; - double y2 = cosrx * y1 - sinrx * z1; - double z2 = sinrx * y1 + cosrx * z1; - - point->x = cosrz * x2 - sinrz * y2 + r[3]; - point->y = sinrz * x2 + cosrz * y2 + r[4]; - point->z = z2 + r[5]; + float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx); + ox = -asin(srx); + + float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) + + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy); + float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) + - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx)); + oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); + + float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) + + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz); + float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) + - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)); + oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); } -void transformBack(pcl::PointXYZ *point, float *r) +void diffRotation(float cx, float cy, float cz, float lx, float ly, float lz, + float &ox, float &oy, float &oz) { - double cosrx = cos(r[0]); - double sinrx = sin(r[0]); - double cosry = cos(r[1]); - double sinry = sin(r[1]); - double cosrz = cos(r[2]); - double sinrz = sin(r[2]); - - double x1 = cosrz * (point->x - r[3]) + sinrz * (point->y - r[4]); - double y1 = -sinrz * (point->x - r[3]) + cosrz * (point->y - r[4]); - double z1 = point->z - r[5]; - - double x2 = x1; - double y2 = cosrx * y1 + sinrx * z1; - double z2 = -sinrx * y1 + cosrx * z1; - - point->x = cosry * x2 - sinry * z2; - point->y = y2; - point->z = sinry * x2 + cosry * z2; + float srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) + - cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx); + ox = -asin(srx); + + float srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + - cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz); + float crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly); + oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); + + float srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy) + - cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) + - (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)); + float crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz) + + cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz) + - cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz); + oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); } void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2) @@ -544,44 +538,26 @@ void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2) imuInited = true; } - transformPoint(&origin, transform); - transformPoint(&xAxis, transform); - transformPoint(&yAxis, transform); - transformPoint(&zAxis, transform); - - /*imagePointsProj->push_back(origin); - imagePointsProj->push_back(xAxis); - imagePointsProj->push_back(yAxis); - imagePointsProj->push_back(zAxis);*/ - - float rx = -asin(yAxis.z - origin.z); - float ry = -atan2(-(xAxis.z - origin.z) / cos(rx), (zAxis.z - origin.z) / cos(rx)); - float rz = -asin(-(yAxis.x - origin.x) / cos(rx)); + float rx, ry, rz; + accumulateRotation(transformSum[0], transformSum[1], transformSum[2], + -transform[0], -transform[1], -transform[2], rx, ry, rz); if (imuPointerLast >= 0) { - transformBack(&origin, transform); - transformBack(&xAxis, transform); - transformBack(&yAxis, transform); - transformBack(&zAxis, transform); - - transform[0] -= 0.1 * (imuPitchCur - rx); - /*if (imuYawCur - imuYawInit - ry > PI) { - transform[1] -= 0.1 * (imuYawCur - imuYawInit - ry - 2 * PI); + float drx, dry, drz; + diffRotation(imuPitchCur, imuYawCur - imuYawInit, imuRollCur, rx, ry, rz, drx, dry, drz); + + transform[0] -= 0.1 * drx; + /*if (dry > PI) { + transform[1] -= 0.1 * (dry - 2 * PI); } else if (imuYawCur - imuYawInit - ry < -PI) { - transform[1] -= 0.1 * (imuYawCur - imuYawInit - ry + 2 * PI); + transform[1] -= 0.1 * (dry + 2 * PI); } else { - transform[1] -= 0.1 * (imuYawCur - imuYawInit - ry); + transform[1] -= 0.1 * dry; }*/ - transform[2] -= 0.1 * (imuRollCur - rz); - - transformPoint(&origin, transform); - transformPoint(&xAxis, transform); - transformPoint(&yAxis, transform); - transformPoint(&zAxis, transform); + transform[2] -= 0.1 * drz; - rx = -asin(yAxis.z - origin.z); - ry = -atan2(-(xAxis.z - origin.z) / cos(rx), (zAxis.z - origin.z) / cos(rx)); - rz = -asin(-(yAxis.x - origin.x) / cos(rx)); + accumulateRotation(transformSum[0], transformSum[1], transformSum[2], + -transform[0], -transform[1], -transform[2], rx, ry, rz); } float x1 = cos(rz) * transform[3] - sin(rz) * transform[4]; @@ -779,17 +755,17 @@ void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) int ipRelationsNum = ipRelations->points.size(); for (int i = 0; i < ipRelationsNum; i++) { if (fabs(ipRelations->points[i].v) < 0.5) { - cvCircle(image, cvPoint((kArray[2] - ipRelations->points[i].z * kArray[0]) / showDSRate, - (kArray[5] - ipRelations->points[i].h * kArray[4]) / showDSRate), 1, CV_RGB(0, 0, 255), 2); + cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, + (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 255), 2); } else if (fabs(ipRelations->points[i].v - 1) < 0.5) { - cvCircle(image, cvPoint((kArray[2] - ipRelations->points[i].z * kArray[0]) / showDSRate, - (kArray[5] - ipRelations->points[i].h * kArray[4]) / showDSRate), 1, CV_RGB(0, 255, 0), 2); + cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, + (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 255, 0), 2); } else if (fabs(ipRelations->points[i].v - 2) < 0.5) { - cvCircle(image, cvPoint((kArray[2] - ipRelations->points[i].z * kArray[0]) / showDSRate, - (kArray[5] - ipRelations->points[i].h * kArray[4]) / showDSRate), 1, CV_RGB(0,255,0), 2); + cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, + (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0,255,0), 2); } else { - cvCircle(image, cvPoint((kArray[2] - ipRelations->points[i].z * kArray[0]) / showDSRate, - (kArray[5] - ipRelations->points[i].h * kArray[4]) / showDSRate), 1, CV_RGB(255, 0, 0), 2); + cvCircle(image, cvPoint((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, + (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(255, 0, 0), 2); } } From 03e5628539b51b92d87af2fc0e06a4376e98243b Mon Sep 17 00:00:00 2001 From: Ji Zhang Date: Sat, 1 Mar 2014 15:59:05 -0500 Subject: [PATCH 5/7] Recommit --- src/bundleAdjust.cpp | 84 +++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 80 insertions(+), 4 deletions(-) diff --git a/src/bundleAdjust.cpp b/src/bundleAdjust.cpp index d0d7ec9..ce36468 100644 --- a/src/bundleAdjust.cpp +++ b/src/bundleAdjust.cpp @@ -39,6 +39,27 @@ double robust_cost_function(double d) { return cost_pseudo_huber(d, .5); } +void diffRotation(float cx, float cy, float cz, float lx, float ly, float lz, + float &ox, float &oy, float &oz) +{ + float srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) + - cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx); + ox = -asin(srx); + + float srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + - cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz); + float crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly); + oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); + + float srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy) + - cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) + - (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)); + float crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz) + + cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz) + - cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz); + oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); +} + void transformAssociateToBA() { float txDiff = txRec - transformBefBA[3]; @@ -57,9 +78,61 @@ void transformAssociateToBA() tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2; tzDiff = z2; - pitchRec += transformAftBA[0] - transformBefBA[0]; - yawRec += transformAftBA[1] - transformBefBA[1]; - rollRec += transformAftBA[2] - transformBefBA[2]; + float sbcx = sin(pitchRec); + float cbcx = cos(pitchRec); + float sbcy = sin(yawRec); + float cbcy = cos(yawRec); + float sbcz = sin(rollRec); + float cbcz = cos(rollRec); + + float sblx = sin(transformBefBA[0]); + float cblx = cos(transformBefBA[0]); + float sbly = sin(transformBefBA[1]); + float cbly = cos(transformBefBA[1]); + float sblz = sin(transformBefBA[2]); + float cblz = cos(transformBefBA[2]); + + float salx = sin(transformAftBA[0]); + float calx = cos(transformAftBA[0]); + float saly = sin(transformAftBA[1]); + float caly = cos(transformAftBA[1]); + float salz = sin(transformAftBA[2]); + float calz = cos(transformAftBA[2]); + + float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) + - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); + pitchRec = -asin(srx); + + float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + yawRec = atan2(srycrx / cos(pitchRec), crycrx / cos(pitchRec)); + + float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) + - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) + - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) + - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + + calx*cblx*salz*sblz); + float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) + - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly + - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) + - calx*calz*cblx*sblz); + rollRec = atan2(srzcrx / cos(pitchRec), crzcrx / cos(pitchRec)); x1 = cos(rollRec) * txDiff - sin(rollRec) * tyDiff; y1 = sin(rollRec) * txDiff + cos(rollRec) * tyDiff; @@ -218,9 +291,12 @@ int main(int argc, char** argv) tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2; tzDiff = z2; + float rollDiff, pitchDiff, yawDiff; + diffRotation(pitch, yaw, roll, pitchRec, yawRec, rollRec, pitchDiff, yawDiff, rollDiff); + Pose3d_Pose3d_Factor* poseposeFactorn = new Pose3d_Pose3d_Factor (poses[i - 1], posen, Pose3d(tzDiff, txDiff, tyDiff, - yaw - yawRec, pitch - pitchRec, roll - rollRec), noise3); + yawDiff, pitchDiff, rollDiff), noise3); posePoseFactors.push_back(poseposeFactorn); ba.add_factor(poseposeFactorn); From 2b1e2f41445d689e500f80ebd1a3d88a8e09fd04 Mon Sep 17 00:00:00 2001 From: Ji Zhang Date: Sat, 1 Mar 2014 15:59:26 -0500 Subject: [PATCH 6/7] Recommit --- src/scanConverter.cpp | 52 ------------------------------------------- 1 file changed, 52 deletions(-) delete mode 100644 src/scanConverter.cpp diff --git a/src/scanConverter.cpp b/src/scanConverter.cpp deleted file mode 100644 index 034e6df..0000000 --- a/src/scanConverter.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include -#include -#include -#include - -#include - -#include "pointDefinition.h" - -const double PI = 3.1415926; - -const double minAngle = -1.57079637051; -const double increAngle = 0.00436332309619; -const int increNum = 720; - -pcl::PointCloud::Ptr syncCloud(new pcl::PointCloud()); -ros::Publisher *syncCloudPubPointer = NULL; - -void laserDataHandler(const sensor_msgs::LaserScan::ConstPtr& laserData) -{ - pcl::PointXYZ point; - syncCloud->clear(); - for (int i = 0; i < increNum; i++) { - double angle = minAngle + increAngle * i; - point.x = laserData->ranges[i] * sin(angle); - point.y = 0; - point.z = laserData->ranges[i] * cos(angle); - - syncCloud->push_back(point); - } - - syncCloud->header.frame_id = "camera2"; - syncCloud->header.stamp = laserData->header.stamp; - sensor_msgs::PointCloud2 syncCloud2; - pcl::toROSMsg(*syncCloud, syncCloud2); - syncCloudPubPointer->publish(syncCloud2); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "scanConverter"); - ros::NodeHandle nh; - - ros::Subscriber laserDataSub = nh.subscribe ("/scan", 5, laserDataHandler); - - ros::Publisher syncCloudPub = nh.advertise ("/sync_scan_cloud_filtered", 5); - syncCloudPubPointer = &syncCloudPub; - - ros::spin(); - - return 0; -} From b8e0d92a878a27f939e37a70b74839e821ced8d5 Mon Sep 17 00:00:00 2001 From: Ji Zhang Date: Sat, 1 Mar 2014 15:59:56 -0500 Subject: [PATCH 7/7] Recommit --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 431dcfe..e1951b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,6 @@ link_libraries(isamlib) rosbuild_add_executable(featureTracking src/featureTracking.cpp) rosbuild_add_executable(visualOdometry src/visualOdometry.cpp) rosbuild_add_executable(bundleAdjust src/bundleAdjust.cpp) -rosbuild_add_executable(scanConverter src/scanConverter.cpp) rosbuild_add_executable(processDepthmap src/processDepthmap.cpp) rosbuild_add_executable(stackDepthPoint src/stackDepthPoint.cpp) rosbuild_add_executable(transformMaintenance src/transformMaintenance.cpp)