From: Jochen Sprickerhof <jochen@sprickerhof.de>
Date: Tue, 6 Nov 2012 20:42:43 +0100
Subject: Test for y value in registration

taken from https://kforge.ros.org/openni/drivers
---
 Source/XnDeviceSensorV2/Registration.cpp |   19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/Source/XnDeviceSensorV2/Registration.cpp b/Source/XnDeviceSensorV2/Registration.cpp
index 4f85eee..1ad0435 100644
--- a/Source/XnDeviceSensorV2/Registration.cpp
+++ b/Source/XnDeviceSensorV2/Registration.cpp
@@ -472,13 +472,28 @@ void XnRegistration::Apply1000(XnDepthPixel* pInput, XnDepthPixel* pOutput)
 			nNewX = (XnInt32)(XnDouble(*pRegTable)/XN_REG_X_SCALE + XnInt32(pDepth2ShiftTable[nValue]/XN_REG_PARAB_COEFF - nConstShift) * dShiftFactor);
 			nNewY = *(pRegTable+1);
 
-			if ((XnUInt32)nNewX-1 < (XnUInt32)nDepthXRes-1)
+			if ((XnUInt32)nNewX-1 < (XnUInt32)nDepthXRes-1 && (XnUInt32)nNewY <(XnUInt32) nDepthYRes)
 			{
 				nArrPos = nNewY * nDepthXRes + nNewX;
 				nOutValue = pOutput[nArrPos];
 
 				if (nOutValue == 0 || nOutValue > nValue)
 				{
+				  if ( nNewX > 0 && nNewY > 0 )
+					{
+						pOutput[nArrPos-nDepthXRes] = nValue;
+						pOutput[nArrPos-nDepthXRes-1] = nValue;
+						pOutput[nArrPos-1] = nValue;
+					}
+					else if( nNewY > 0 )
+					{
+						pOutput[nArrPos-nDepthXRes] = nValue;
+					}
+					else if( nNewX > 0 )
+					{
+						pOutput[nArrPos-1] = nValue;
+					}
+    
 					pOutput[nArrPos] = nValue;
 					pOutput[nArrPos-1] = nValue;
 					pOutput[nArrPos-nDepthXRes] = nValue;
@@ -523,7 +538,7 @@ void XnRegistration::Apply1080(XnDepthPixel* pInput, XnDepthPixel* pOutput)
 				nNewX = (XnUInt32)(*pRegTable + pRGBRegDepthToShiftTable[nValue]) / RGB_REG_X_VAL_SCALE;
 				nNewY = *(pRegTable+1);
 
-				if (nNewX < nDepthXRes)
+				if (nNewX < nDepthXRes && nNewY < nDepthYRes)
 				{
 					nArrPos = bMirror ? (nNewY+1)*nDepthXRes - nNewX : (nNewY*nDepthXRes) + nNewX;
 					nArrPos -= nConstOffset;
