diff --git a/leg_detector/src/laser_processor.cpp b/leg_detector/src/laser_processor.cpp
index 582676e8..637b47f6 100644
--- a/leg_detector/src/laser_processor.cpp
+++ b/leg_detector/src/laser_processor.cpp
@@ -255,7 +255,7 @@ ScanProcessor::splitConnected(float thresh)
         while ((s_rest != (*c_iter)->end() &&
                 (*s_rest)->index < (*s_q)->index + expand))
         {
-          if ((*s_rest)->range - (*s_q)->range > thresh)
+          if (std::abs((*s_rest)->range - (*s_q)->range) > thresh)
           {
             break;
           }
diff --git a/leg_detector/src/leg_detector.cpp b/leg_detector/src/leg_detector.cpp
index 8a3cc50a..f15506dc 100644
--- a/leg_detector/src/leg_detector.cpp
+++ b/leg_detector/src/leg_detector.cpp
@@ -647,16 +647,16 @@ class LegDetector
             || (*leg1)->getReliability() < leg_reliability_limit_)
           continue;
 
-        for (leg2 = begin; leg2 != end; ++leg2)
+        for (leg2 = leg1,leg2++; leg2 != end; ++leg2)
         {
           if (((*leg2)->object_id != "")
-              || ((*leg2)->getReliability() < leg_reliability_limit_)
-              || (leg1 == leg2)) continue;
+              || ((*leg2)->getReliability() < leg_reliability_limit_)) continue;
           double d = distance(leg1, leg2);
           if (d < closest_dist)
           {
             best1 = leg1;
             best2 = leg2;
+            closest_dist = d;
           }
         }
       }