From 402b42e7de2afde4fc5970aa8971ad884194aabe Mon Sep 17 00:00:00 2001 From: haoala Date: Sat, 13 Aug 2016 17:39:37 +0800 Subject: [PATCH 1/2] Fixed clearing of laserCloudOri and coeffSel laserCloudOri and coeffSel, which are used to build the jacobian, should be cleared at the start of every iteration, not just once per round of odometry. --- src/laserOdometry.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/laserOdometry.cpp b/src/laserOdometry.cpp index 4442c691..4a48c891 100755 --- a/src/laserOdometry.cpp +++ b/src/laserOdometry.cpp @@ -455,9 +455,6 @@ int main(int argc, char** argv) continue; } - laserCloudOri->clear(); - coeffSel->clear(); - transform[3] -= imuVeloFromStartX * scanPeriod; transform[4] -= imuVeloFromStartY * scanPeriod; transform[5] -= imuVeloFromStartZ * scanPeriod; @@ -468,6 +465,9 @@ int main(int argc, char** argv) int cornerPointsSharpNum = cornerPointsSharp->points.size(); int surfPointsFlatNum = surfPointsFlat->points.size(); for (int iterCount = 0; iterCount < 25; iterCount++) { + laserCloudOri->clear(); + coeffSel->clear(); + for (int i = 0; i < cornerPointsSharpNum; i++) { TransformToStart(&cornerPointsSharp->points[i], &pointSel); From c2605be64771d3ccf9f0d89e0f1db2b8c506efcd Mon Sep 17 00:00:00 2001 From: haoala Date: Sat, 13 Aug 2016 17:59:18 +0800 Subject: [PATCH 2/2] Fixed transformAssociateToMap() transformAssociateToMap was buggy, assuming the computation we wanted to do is: H_transformToBeMapped = H_transformAftMapped * H_transformBefMapped^-1 * H_transformSum where H_t represents the 4x4 homogeneous transformation matrix of the 6-D array t --- src/laserMapping.cpp | 56 ++++++++++++++++---------------- src/transformMaintenance.cpp | 62 ++++++++++++++++++------------------ 2 files changed, 58 insertions(+), 60 deletions(-) diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 3113a592..472aa833 100755 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -144,40 +144,38 @@ void transformAssociateToMap() float salz = sin(transformAftMapped[2]); float calz = cos(transformAftMapped[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); + float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) + - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx); transformTobeMapped[0] = -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); + float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) + - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) + - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) + + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly) + + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly); + float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) + - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) + + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly) + - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly); transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), crycrx / cos(transformTobeMapped[0])); - 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); + float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); + float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) + + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), crzcrx / cos(transformTobeMapped[0])); diff --git a/src/transformMaintenance.cpp b/src/transformMaintenance.cpp index 65582b94..292ead13 100755 --- a/src/transformMaintenance.cpp +++ b/src/transformMaintenance.cpp @@ -94,40 +94,40 @@ void transformAssociateToMap() float salz = sin(transformAftMapped[2]); float calz = cos(transformAftMapped[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); + float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) + - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx); transformMapped[0] = -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); - transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0])); + float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) + - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) + - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) + + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly) + + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly); + float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) + - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) + + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly) + - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly); + transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), + crycrx / cos(transformMapped[0])); - 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); - transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0])); + float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); + float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) + + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); + transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), + crzcrx / cos(transformMapped[0])); x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4]; y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4];