/// <summary>
/// Creates projection planes (2D) for image display
/// </summary>
public void CalculateProjectedLines()
{
List <Vector2d> acceptablePoints = new List <Vector2d>(4);
Vector2d start, stop;
limited2dLines.Clear();
const double epsilon = .0001;
foreach (Line3D line in line3DCollection)
{
if (Math.Abs(line.Direction * depthFOVPlaneLeft - 1) > epsilon)
{
acceptablePoints.Add(CalculateProjectedPoint(FeatureExtractorHelper.CalculateLinePlaneIntersectionPoint(line, depthFOVPlaneLeft)));
}
if (Math.Abs(line.Direction * depthFOVPlaneTop - 1) > epsilon)
{
acceptablePoints.Add(CalculateProjectedPoint(FeatureExtractorHelper.CalculateLinePlaneIntersectionPoint(line, depthFOVPlaneTop)));
}
if (Math.Abs(line.Direction * depthFOVPlaneRight - 1) > epsilon)
{
acceptablePoints.Add(CalculateProjectedPoint(FeatureExtractorHelper.CalculateLinePlaneIntersectionPoint(line, depthFOVPlaneRight)));
}
if (Math.Abs(line.Direction * depthFOVPlaneBottom - 1) > epsilon)
{
acceptablePoints.Add(CalculateProjectedPoint(FeatureExtractorHelper.CalculateLinePlaneIntersectionPoint(line, depthFOVPlaneBottom)));
}
if (FeatureExtractorHelper.FindBestLine(acceptablePoints, imageSize, out start, out stop))
{
limited2dLines.Add(new LimitedLine2d(start, stop));
}
}
}
/// <summary>
/// Extracts intersection lines after the plane extraction has been done
/// </summary>
public void ExtractIntersectionLines()
{
line3DCollection.Clear();
Line3D line;
foreach (PlaneCluster plane1 in planeCollection)
{
foreach (PlaneCluster plane2 in planeCollection)
{
if (plane1 == plane2)
{
continue;
}
if (FeatureExtractorHelper.CalculateIntersection(plane1, plane2, out line))
{
line3DCollection.Add(line);
}
}
}
}
/// <summary>
/// Extract features from given data
/// </summary>
/// <param name="depthData">Pointer to depth data</param>
public void ExtractPlanes(IntPtr depthData)
{
coordinates = new Vector3d[ImageSize.Width * ImageSize.Height];
coordinateMatcher.CalculateCoordinateTranslation(depthData, coordinates);
int cellId;
int realTopLeftIndex, realTopRightIndex, realBottomLeftIndex, realBottomRightIndex;
List <Plane> planeCollection = new List <Plane>(gridDivision.Horizontal * gridDivision.Vertical);
Vector3d planePoint, planeNormal;
int horizontalGridDivision = gridDivision.Horizontal;
int verticalGridDivision = gridDivision.Vertical;
double pointEpsilon = this.pointEpsilon;
double normalEpsilon = this.normalEpsilon;
int marginLeft = margin.Left;
int marginRight = margin.Right;
int marginTop = margin.Top;
int marginBottom = margin.Bottom;
int imageWidth = imageSize.Width;
int imageHeight = imageSize.Height;
double xsize = imageWidth - marginLeft - marginRight;
double ysize = imageHeight - marginTop - marginBottom;
double xgridsize = xsize / horizontalGridDivision;
double ygridsize = ysize / verticalGridDivision;
double xpos = marginLeft;
double ypos = marginTop;
/*
* Get the detected (and allowed) planes from the given data
*/
for (int x = 0; x < horizontalGridDivision; x++, xpos += xgridsize)
{
ypos = marginTop;
for (int y = 0; y < verticalGridDivision; y++, ypos += ygridsize)
{
cellId = x + y * gridDivision.Horizontal;
realTopLeftIndex = (int)xpos + ((int)ypos) * imageWidth;
realTopRightIndex = (int)(xpos + xgridsize) + ((int)ypos) * imageWidth;
realBottomLeftIndex = (int)xpos + ((int)(ypos + ygridsize)) * imageWidth;
realBottomRightIndex = (int)(xpos + xgridsize) + ((int)(ypos + ygridsize)) * imageWidth;
if (FeatureExtractorHelper.DepthDataValid(depthData, ref depthDataFormat, ref realTopLeftIndex, ref realTopRightIndex, ref realBottomLeftIndex, ref realBottomRightIndex))
{
if (FeatureExtractorHelper.IsPlane(coordinates, ref realTopLeftIndex, ref realTopRightIndex, ref realBottomLeftIndex, ref realBottomRightIndex, ref pointEpsilon, out planePoint, out planeNormal))
{
planeCollection.Add(FeatureExtractorHelper.CreatePlane(ref planePoint, ref planeNormal, ref cellId));
}
}
}
}
/*
* Cluster the plane cells to bigger planes
*/
this.planeCollection.Clear();
int index = 0;
foreach (Plane plane in planeCollection)
{
this.planeCollection.AddPlane(plane);
index++;
}
this.planeCollection.RemoveSmallClusters(MinimumPlaneClusterSize);
}
