Skip to content

Commit

Permalink
Merge pull request laboshinl#50 from KitKat7/github/new-master
Browse files Browse the repository at this point in the history
Bugs of eigenvalues and eigenvectors order
  • Loading branch information
laboshinl authored Mar 29, 2018
2 parents d146f4b + 2402c4f commit f87b08d
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 9 deletions.
16 changes: 8 additions & 8 deletions src/lib/LaserMapping.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -823,17 +823,17 @@ void LaserMapping::optimizeTransformTobeMapped()
matD1 = esolver.eigenvalues().real();
matV1 = esolver.eigenvectors().real();

if (matD1(0, 0) > 3 * matD1(0, 1)) {
if (matD1(0, 2) > 3 * matD1(0, 1)) {

float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = vc.x() + 0.1 * matV1(0, 0);
float y1 = vc.y() + 0.1 * matV1(0, 1);
float z1 = vc.z() + 0.1 * matV1(0, 2);
float x2 = vc.x() - 0.1 * matV1(0, 0);
float y2 = vc.y() - 0.1 * matV1(0, 1);
float z2 = vc.z() - 0.1 * matV1(0, 2);
float x1 = vc.x() + 0.1 * matV1(0, 2);
float y1 = vc.y() + 0.1 * matV1(1, 2);
float z1 = vc.z() + 0.1 * matV1(2, 2);
float x2 = vc.x() - 0.1 * matV1(0, 2);
float y2 = vc.y() - 0.1 * matV1(1, 2);
float z2 = vc.z() - 0.1 * matV1(2, 2);

float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
Expand Down Expand Up @@ -997,7 +997,7 @@ void LaserMapping::optimizeTransformTobeMapped()

isDegenerate = false;
float eignThre[6] = {100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
for (int i = 0; i < 6; i++) {
if (matE(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2(i, j) = 0;
Expand Down
2 changes: 1 addition & 1 deletion src/lib/LaserOdometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -790,7 +790,7 @@ void LaserOdometry::process()

isDegenerate = false;
float eignThre[6] = {10, 10, 10, 10, 10, 10};
for (int i = 5; i >= 0; i--) {
for (int i = 0; i < 6; i++) {
if (matE(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2(i, j) = 0;
Expand Down

0 comments on commit f87b08d

Please sign in to comment.