Merge pull request #1 from jizhang-cmu/fuerte-groovy

Recommit

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)

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);
 

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
-

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;
 }
-

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;