From 21afa803147f749db00986d9e6e24af53960b18b Mon Sep 17 00:00:00 2001 From: Christoph Pech Date: Wed, 8 Mar 2017 15:49:09 +0100 Subject: - HeightMap grid extrapolation of missing probe points by linear least squares (#83) - reduced complexity of interpolation due to consistent grid --- src/GCodes/GCodes.cpp | 1 + src/Movement/Grid.cpp | 219 +++++++++++++++++++++++--------------------------- src/Movement/Grid.h | 7 +- 3 files changed, 103 insertions(+), 124 deletions(-) (limited to 'src') diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp index 5fd49c54..e4b734f8 100644 --- a/src/GCodes/GCodes.cpp +++ b/src/GCodes/GCodes.cpp @@ -620,6 +620,7 @@ void GCodes::Spin() { reply.printf("%u points probed, mean error %.3f, deviation %.3f\n", numPointsProbed, mean, deviation); error = SaveHeightMap(gb, reply); + reprap.GetMove()->AccessBedProbeGrid().ExtrapolateMissing(); reprap.GetMove()->AccessBedProbeGrid().UseHeightMap(true); } else diff --git a/src/Movement/Grid.cpp b/src/Movement/Grid.cpp index bb08fc9d..7e7bfbd1 100644 --- a/src/Movement/Grid.cpp +++ b/src/Movement/Grid.cpp @@ -284,6 +284,8 @@ bool HeightMap::LoadFromFile(FileStore *f, StringRef& r) } return false; // success! } + + ExtrapolateMissing(); return true; // an error occurred } @@ -327,6 +329,18 @@ float HeightMap::GetInterpolatedHeightError(float x, float y) const return 0.0; } + //last grid point + const float xLast = def.xMin + (def.numX-1)*def.spacing; + const float yLast = def.yMin + (def.numY-1)*def.spacing; + + //clamp to rectangle so InterpolateXY will always have valid parameters + const float fEPSILON = 0.01; + if (x < def.xMin) { x = def.xMin; } + if (y < def.yMin) { y = def.yMin; } + if (x > xLast -fEPSILON) { x = xLast -fEPSILON; } + if (y > yLast -fEPSILON) { y = yLast -fEPSILON; } + + const float xf = (x - def.xMin) * def.recipSpacing; const float xFloor = floor(xf); const int32_t xIndex = (int32_t)xFloor; @@ -334,141 +348,108 @@ float HeightMap::GetInterpolatedHeightError(float x, float y) const const float yFloor = floor(yf); const int32_t yIndex = (int32_t)yFloor; - if (xIndex < 0) - { - if (yIndex < 0) - { - // We are off the bottom left corner of the grid - return GetHeightError(0, 0); - } - else if (yIndex >= (int)def.numY - 1) - { - return GetHeightError(0, def.numY - 1); - } - else - { - return InterpolateY(0, yIndex, yf - yFloor); - } - } - else if (xIndex >= (int)def.numX - 1) + return InterpolateXY(xIndex, yIndex, xf - xFloor, yf - yFloor); +} + +float HeightMap::InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFrac, float yFrac) const +{ + const uint32_t indexX0Y0 = GetMapIndex(xIndex, yIndex); // (X0,Y0) + const uint32_t indexX1Y0 = indexX0Y0 + 1; // (X1,Y0) + const uint32_t indexX0Y1 = indexX0Y0 + def.numX; // (X0 Y1) + const uint32_t indexX1Y1 = indexX0Y1 + 1; // (X1,Y1) + + const float xyFrac = xFrac * yFrac; + return (gridHeights[indexX0Y0] * (1.0 - xFrac - yFrac + xyFrac)) + + (gridHeights[indexX1Y0] * (xFrac - xyFrac)) + + (gridHeights[indexX0Y1] * (yFrac - xyFrac)) + + (gridHeights[indexX1Y1] * xyFrac); +} + +void HeightMap::ExtrapolateMissing() +{ + //1: calculating the bed plane by least squares fit + //2: filling in missing points + + //algorithm: http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points + float sumX = 0, sumY = 0, sumZ = 0; + int n = 0; + for (int iY = 0; iY < def.numY; iY++) { - if (yIndex < 0) - { - // We are off the bottom left corner of the grid - return GetHeightError(def.numX - 1, 0); - } - else if (yIndex >= (int)def.numY - 1) + for (int iX = 0; iX < def.numX; iX++) { - return GetHeightError(def.numX - 1, def.numY - 1); - } - else - { - return InterpolateY(def.numX - 1, yIndex, yf - yFloor); + int index = GetMapIndex(iX, iY); + if (!IsHeightSet(index)) { continue; } + float fX = (def.spacing * iX) + def.xMin; + float fY = (def.spacing * iY) + def.yMin; + float fZ = gridHeights[index]; + + n++; + sumX += fX; sumY += fY; sumZ += fZ; } } - else + + float invN = 1.0 / float(n); + float centX = sumX * invN, centY = sumY * invN, centZ = sumZ * invN; + + // Calc full 3x3 covariance matrix, excluding symmetries: + float xx = 0.0; float xy = 0.0; float xz = 0.0; + float yy = 0.0; float yz = 0.0; float zz = 0.0; + + for (int iY = 0; iY < def.numY; iY++) { - if (yIndex < 0) - { - // We are off the bottom left corner of the grid - return InterpolateX(xIndex, 0, xf - xFloor); - } - else if (yIndex >= (int)def.numY - 1) - { - return InterpolateX(xIndex, def.numY - 1, xf - xFloor); - } - else + for (int iX = 0; iX < def.numX; iX++) { - return InterpolateXY(xIndex, yIndex, xf - xFloor, yf - yFloor); + int index = GetMapIndex(iX, iY); + if (!IsHeightSet(index)) { continue; } + float fX = (def.spacing * iX) + def.xMin; + float fY = (def.spacing * iY) + def.yMin; + float fZ = gridHeights[index]; + + float rX = fX - centX; + float rY = fY - centY; + float rZ = fZ - centZ; + + xx += rX * rX; + xy += rX * rY; + xz += rX * rZ; + yy += rY * rY; + yz += rY * rZ; + zz += rZ * rZ; } } -} -float HeightMap::GetHeightError(uint32_t xIndex, uint32_t yIndex) const -{ - const uint32_t index = GetMapIndex(xIndex, yIndex); - return (IsHeightSet(index)) ? gridHeights[index] : 0.0; -} + const float detZ = xx*yy - xy*xy; + if (detZ <= 0) { + //not a valid plane (or a vertical one) + return; + } -float HeightMap::InterpolateX(uint32_t xIndex, uint32_t yIndex, float xFrac) const -{ - const uint32_t index1 = GetMapIndex(xIndex, yIndex); - return Interpolate2(index1, index1 + 1, xFrac); -} + //plane equation: ax+by+cz=d -> z = (d-(ax+by))/c + float a = (yz*xy - xz*yy) / detZ; + float b = (xz*xy - yz*xx) / detZ; + float c = 1.0; + float normLenInv=1.0/sqrtf(a*a + b*b + 1); + a *= normLenInv; + b *= normLenInv; + c *= normLenInv; + float d = centX*a + centY*b + centZ*c; -float HeightMap::InterpolateY(uint32_t xIndex, uint32_t yIndex, float yFrac) const -{ - const uint32_t index1 = GetMapIndex(xIndex, yIndex); - return Interpolate2(index1, index1 + def.numX, yFrac); -} -float HeightMap::Interpolate2(uint32_t index1, uint32_t index2, float frac) const -{ - const bool b1 = IsHeightSet(index1); - const bool b2 = IsHeightSet(index2); - return (b1 && b2) ? ((1.0 - frac) * gridHeights[index1]) + (frac * gridHeights[index2]) - : (b1) ? gridHeights[index1] - : (b2) ? gridHeights[index2] - : 0.0; -} - -float HeightMap::InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFrac, float yFrac) const -{ - const uint32_t indexX0Y0 = GetMapIndex(xIndex, yIndex); // (X0,Y0) - const uint32_t indexX1Y0 = indexX0Y0 + 1; // (X1,Y0) - const uint32_t indexX0Y1 = indexX0Y0 + def.numX; // (X0 Y1) - const uint32_t indexX1Y1 = indexX0Y1 + 1; // (X1,Y1) - const unsigned int cc = ((unsigned int)IsHeightSet(indexX0Y0) << 0) - + ((unsigned int)IsHeightSet(indexX1Y0) << 1) - + ((unsigned int)IsHeightSet(indexX0Y1) << 2) - + ((unsigned int)IsHeightSet(indexX1Y1) << 3); - switch(cc) + //fill in the blanks + float invC = 1.0 / c; + for (int iY = 0; iY < def.numY; iY++) { - case 0: // no points defined - default: - return 0.0; - case 1: // (X0,Y0) defined - return gridHeights[indexX0Y0]; - case 2: // (X1,Y0) defined - return gridHeights[indexX1Y0]; - case 3: // (X0,Y0) and (X1,Y0) defined - return (xFrac * gridHeights[indexX1Y0]) + ((1.0 - xFrac) * gridHeights[indexX0Y0]); - case 4: // (X0,Y1) defined - return gridHeights[indexX0Y1]; - case 5: // (X0,Y0) and (X0,Y1) defined - return (yFrac * gridHeights[indexX0Y1]) + ((1.0 - yFrac) * gridHeights[indexX0Y0]); - case 6: // (X1,Y0) and (X0,Y1) defined - diagonal interpolation - return (((xFrac + 1.0 - yFrac) * gridHeights[indexX1Y0]) + ((yFrac + 1.0 - xFrac) * gridHeights[indexX0Y1]))/2; - case 7: // (X0,Y0), (X1,Y0) and (X0,Y1) defined - 3-way interpolation - return InterpolateCorner(indexX0Y0, indexX1Y0, indexX0Y1, xFrac, yFrac); - case 8: // (X1,Y1) defined - return gridHeights[indexX1Y1]; - case 9: // (X0,Y0) and (X1,Y1) defined - diagonal interpolation - return ((xFrac + yFrac) * gridHeights[indexX1Y1]) + ((2.0 - (xFrac + yFrac)) * gridHeights[indexX0Y0])/2; - case 10: // (X1,Y0) and (X1,Y1) defined - return (yFrac * gridHeights[indexX1Y1]) + ((1.0 - yFrac) * gridHeights[indexX1Y0]); - case 11: // (X0,Y0), (X1,Y0) and (X1,Y1) defined - 3-way interpolation - return InterpolateCorner(indexX1Y0, indexX0Y0, indexX1Y1, xFrac, yFrac); - case 12: // (X0,Y1) and (X1,Y1) defined - return (xFrac * gridHeights[indexX1Y1]) + ((1.0 - xFrac) * gridHeights[indexX0Y1]); - case 13: // (X0,Y0), (X0,Y1) and (X1,Y1) defined - 3-way interpolation - return InterpolateCorner(indexX0Y1, indexX1Y1, indexX0Y0, xFrac, 1.0 - yFrac); - case 14: // (X1,Y0), (X0,Y1) and (X1,Y1) defined - 3-way interpolation - return InterpolateCorner(indexX1Y1, indexX0Y1, indexX1Y0, 1.0 - xFrac, 1.0 - yFrac); - case 15: // All points defined + for (int iX = 0; iX < def.numX; iX++) { - const float xyFrac = xFrac * yFrac; - return (gridHeights[indexX0Y0] * (1.0 - xFrac - yFrac + xyFrac)) - + (gridHeights[indexX1Y0] * (xFrac - xyFrac)) - + (gridHeights[indexX0Y1] * (yFrac - xyFrac)) - + (gridHeights[indexX1Y1] * xyFrac); + int index = GetMapIndex(iX, iY); + if (IsHeightSet(index)) { continue; } + float fX = (def.spacing * iX) + def.xMin; + float fY = (def.spacing * iY) + def.yMin; + + float fZ = (d - (a * fX + b * fY)) * invC; + gridHeights[index] = fZ; //fill in Z but don't mark it as set so we can always differentiate between measured and extrapolated } } } -float HeightMap::InterpolateCorner(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const -{ - return ((xFrac * gridHeights[indexX]) + (yFrac * gridHeights[indexY]) + ((2.0 - xFrac - yFrac) * gridHeights[cornerIndex]))/2; -} - // End diff --git a/src/Movement/Grid.h b/src/Movement/Grid.h index 2de4e0ba..a1189018 100644 --- a/src/Movement/Grid.h +++ b/src/Movement/Grid.h @@ -81,6 +81,8 @@ public: unsigned int GetStatistics(float& mean, float& deviation) const; // Return number of points probed, mean and RMS deviation + void ExtrapolateMissing(); //extrapolate missing points to ensure consistency + private: static const char *HeightMapComment; // The start of the comment we write at the start of the height map file @@ -92,12 +94,7 @@ private: uint32_t GetMapIndex(uint32_t xIndex, uint32_t yIndex) const { return (yIndex * def.NumXpoints()) + xIndex; } bool IsHeightSet(uint32_t index) const { return (gridHeightSet[index/32] & (1 << (index & 31))) != 0; } - float GetHeightError(uint32_t xIndex, uint32_t yIndex) const; - float InterpolateX(uint32_t xIndex, uint32_t yIndex, float xFrac) const; - float InterpolateY(uint32_t xIndex, uint32_t yIndex, float yFrac) const; float InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFrac, float yFrac) const; - float Interpolate2(uint32_t index1, uint32_t index2, float frac) const; - float InterpolateCorner(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const; }; #endif /* SRC_MOVEMENT_GRID_H_ */ -- cgit v1.2.3