ReturnSampledObject sampleLocation(KinectDepthData data, short[] depthImage, int index, Vector l1, Vector3D p,
int rMin, int height, int cMin, int width)
{
int retries = 0;
bool valid = false;
//initialize MAX RETRIES
while ((!valid) && retries <= MAXRETRIES)
{
Random rndm = new Random();
l1.Y = rMin + rndm.Next() % height;
l1.X = cMin + rndm.Next() % width;
index = (int)l1.Y * data.getWidth() + (int)l1.X;
if (index > 0)
{
valid = data.isValidDepthValue(index, depthImage);
}
retries++;
numSampleLocations++;
}
if (valid)
{
Vector3D v3D = data.depthPixelTo3d(index, depthImage);
p = new Vector3D(v3D.X, v3D.Y, v3D.Z);
}
ReturnSampledObject rsmo = new ReturnSampledObject(l1, p, index, valid);
return(rsmo);
}
public List <Vector3D> GenerateSampledPointCloud(KinectDepthData data, short[] depthData, List <Vector3D> pointCloud, int numPoints)
{
int ind = 0;
Vector v = new Vector();
Vector3D p = new Vector3D();
for (int i = 0; i < numPoints; i++)
{
ReturnSampledObject object1 = sampleLocation(data, depthData, ind, v, p, 0, data.height - 1, 0, data.width - 1);
if (object1.valid)
{
pointCloud.Add(object1.p);
}
}
return(pointCloud);
}
public void GenerateFilteredPointCloud(short[] depthImage, List <Vector3D> filteredPointCloud,
List <Vector> pixelLocs, List <Vector3D> pointCloudNormals, List <Vector3D> outlierCloud, KinectDepthData data)
{
filteredPointCloud.Clear();
pixelLocs.Clear();
pointCloudNormals.Clear();
outlierCloud.Clear();
double minInliers = this.numLocalSamples;
double maxOutliers = (1 - this.minInlierFraction) * this.numLocalSamples;
double planeSizeH = 0.5 * this.planeSize;
double PlanseSize = this.planeSize;
double w2 = data.width - this.planeSize;
double h2 = data.height - this.planeSize;
int numPlanes = 0;
int numPoints = 0;
int ind1 = 0, ind2 = 0, ind3 = 0;
// Call the math.tan function
double sampleRadiusHFactor = this.worldPlaneSize * data.width / (4.0 * Math.Tan(data.fieldOfViewHorizontal));
double sampleRadiusVFactor = this.worldPlaneSize * data.height / (4.0 * Math.Tan(data.fieldOfViewVertical));
Vector3D p1, p2, p3, p;
Vector l1, l2, l3, l;
p1 = new Vector3D();
p2 = new Vector3D();
p3 = new Vector3D();
p = new Vector3D();
l1 = new Vector();
l2 = new Vector();
l3 = new Vector();
l = new Vector();
List <Vector3D> neighborhoodInliers = new List <Vector3D>();
List <Vector> neighborhoodPixelLocs = new List <Vector>();
double d;
double sampleRadiusH, sampleRadiusV, rMin, rMax, cMin, cMax, dR, dC;
// numSamples not present
for (int i = 0; numPoints < this.maxPoints && i < this.numSamples; i++)
{
ReturnSampledObject object1 = sampleLocation(data, depthImage, ind1, l1, p1, (int)planeSizeH, (int)h2, (int)planeSizeH, (int)w2);
if (!object1.valid)
{
continue;
}
ReturnSampledObject object2 = sampleLocation(data, depthImage, ind2, l2, p2, (int)(object1.l.Y - planeSizeH),
planeSize, (int)(object1.l.X - planeSizeH), planeSize);
if (!object2.valid)
{
continue;
}
ReturnSampledObject object3 = sampleLocation(data, depthImage, ind3, l3, p3, (int)(object1.l.Y - planeSizeH),
planeSize, (int)(object1.l.X - planeSizeH), planeSize);
if (!object3.valid)
{
continue;
}
p1 = object1.p;
p2 = object2.p;
p3 = object3.p;
Vector3D n = Vector3D.CrossProduct((p1 - p2), (p3 - p2));
d = Vector3D.DotProduct(p1, n);
double meanDepth = (p1.X + p2.X + p3.X) * 0.33333333;
sampleRadiusH = Math.Ceiling(sampleRadiusHFactor / meanDepth * Math.Sqrt(1.0 - (n.Y * n.Y)));
sampleRadiusV = Math.Ceiling(sampleRadiusVFactor / meanDepth * Math.Sqrt(1.0 - (n.Z * n.Z)));
rMin = Math.Max(0, l1.Y - sampleRadiusV);
rMax = Math.Min(data.height - 1, l1.Y + sampleRadiusV);
cMin = Math.Max(0, l1.X - sampleRadiusH);
cMax = Math.Min(data.width - 1, l1.X + sampleRadiusH);
dR = rMax - rMin;
dC = cMax - cMin;
if (sampleRadiusH < 2.0 && sampleRadiusV < 2.0)
{
continue;
}
int inliers = 0, outliers = 0;
neighborhoodInliers.Clear();
neighborhoodPixelLocs.Clear();
for (int j = 0; outliers < maxOutliers && j < this.numLocalSamples; j++)
{
int r, c, ind = 0;
ReturnSampledObject object4 = sampleLocation(data, depthImage, ind, l, p, (int)rMin, (int)dR, (int)cMin, (int)dC);
if (!object4.valid)
{
continue;
}
double err = Math.Abs(Vector3D.DotProduct(n, object4.p) - d);
if (err < maxError && (p.X < meanDepth + maxDepthDiff) && (p.X > meanDepth - maxDepthDiff))
{
inliers++;
neighborhoodInliers.Add(new Vector3D(p.X, p.Y, p.Z));
neighborhoodPixelLocs.Add(new Vector(l.X, l.Y));
}
else
{
outliers++;
}
}
if (inliers >= minInliers && inliers > 3)
{
Vector3D ng = n;
//ng.w = 0;
for (int k = 0; k < neighborhoodInliers.Count; k++)
{
filteredPointCloud.Add(neighborhoodInliers.ElementAt(i));
pointCloudNormals.Add(ng);
pixelLocs.Add(neighborhoodPixelLocs.ElementAt(i));
}
filteredPointCloud.Add(new Vector3D(p1.X, p1.Y, p1.Z));
filteredPointCloud.Add(new Vector3D(p2.X, p2.Y, p2.Z));
filteredPointCloud.Add(new Vector3D(p3.X, p2.Y, p3.Z));
pointCloudNormals.Add(ng);
pointCloudNormals.Add(ng);
pointCloudNormals.Add(ng);
pixelLocs.Add(new Vector(l1.X, l1.Y));
pixelLocs.Add(new Vector(l2.X, l2.Y));
pixelLocs.Add(new Vector(l3.X, l3.Y));
numPoints += neighborhoodInliers.Count + 3;
numPlanes += 1;
}
else
{
for (int j = 0; j < neighborhoodInliers.Count; j++)
{
outlierCloud.Add(neighborhoodInliers.ElementAt(i));
}
outlierCloud.Add(p1);
outlierCloud.Add(p2);
outlierCloud.Add(p3);
}
}
}
ReturnSampledObject sampleLocation(KinectDepthData data, short[] depthImage, int index, Vector l1, Vector3D p,
int rMin, int height, int cMin, int width)
{
int retries = 0;
bool valid = false;
//initialize MAX RETRIES
while ((!valid) && retries <= MAXRETRIES)
{
Random rndm = new Random();
l1.Y = rMin + rndm.Next() % height;
l1.X = cMin + rndm.Next() % width;
index = (int)l1.Y * data.getWidth() + (int)l1.X;
if(index > 0)
valid = data.isValidDepthValue(index, depthImage);
retries++;
numSampleLocations++;
}
if (valid)
{
Vector3D v3D = data.depthPixelTo3d(index, depthImage);
p = new Vector3D(v3D.X, v3D.Y, v3D.Z);
}
ReturnSampledObject rsmo = new ReturnSampledObject(l1, p, index, valid);
return rsmo;
}
