divide by valid point count only in ScanMatcher::GetResponse

Otherwise the returned response value is too low for loop closure
when used with a half-blocked lidar.

lib/karto_sdk/src/Mapper.cpp

@@ -1181,6 +1181,7 @@ kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionI
 
   // get number of points in offset list
   kt_int32u nPoints = pOffsets->GetSize();
+  kt_int32u nValidPoints = 0;
   if (nPoints == 0) {
     return response;
   }
@@ -1198,10 +1199,11 @@ kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionI
 
     // uses index offsets to efficiently find location of point in the grid
     response += pByte[pAngleIndexPointer[i]];
+    nValidPoints++;
   }
 
   // normalize response
-  response /= (nPoints * GridStates_Occupied);
+  response /= (nValidPoints * GridStates_Occupied);
   assert(fabs(response) <= 1.0);
 
   return response;