protected List <TypeField> GetPathFrom(TypeField pFieldFrom, TypeField pFieldTo, float pMaxPathLength = int.MaxValue) { //todo: check if generated path is valid TypeField current = pFieldFrom; List <TypeField> path = new List <TypeField>() { current }; float pathLength = 0; while (!current.Equals(pFieldTo) && pathLength < pMaxPathLength) { EDirection dir = CField.GetDirection(current, pFieldTo); TypeField newCurrent = (TypeField)current.GetNeighbour(dir); pathLength += CUtils.Get2DDistance(current, newCurrent); current = newCurrent; path.Add(current); } return(path); }
public List <CField> GetFieldsInDistance(int pSteps) { List <CField> fields = new List <CField>(); for (int x = -pSteps; x <= pSteps; x++) { for (int y = -pSteps; y <= pSteps; y++) { CField field = GetFieldWithOffset(x, y); if (field != null) { fields.Add(field); } } } fields.Sort((a, b) => CUtils.Get2DDistance(Center, a.Center).CompareTo( CUtils.Get2DDistance(Center, b.Center))); return(fields); }
/// <summary> /// Get info from preprocessed file using lasinfo and set it to main header /// </summary> private static void SetMainHeader(string pPreprocessedFilePath) { //FileInfo fi = new FileInfo(pPreprocessedFilePath); string infoFilePath = CPreprocessController.currentTmpFolder + "\\" + CUtils.GetFileName(pPreprocessedFilePath) + "_i.txt"; string[] headerLines = CPreprocessController.GetHeaderLines(pPreprocessedFilePath, infoFilePath); if (headerLines == null) { CDebug.Error("header lines are null"); //todo: is it ok to leave it as null?? //CProjectData.mainHeader = new CHeaderInfo(); } else { CProjectData.mainHeader = new CHeaderInfo(headerLines); } //can be inited only after main header is set CBitmapExporter.Init(); }
public static bool IsAtBufferZone(Vector3 pPoint) { Vector3 mainBotLeft = CProjectData.mainHeader.BotLeftCorner; Vector3 mainTopRight = CProjectData.mainHeader.TopRightCorner; float distanceFromWholeForestBorder = CUtils.GetDistanceFromBorder(pPoint, mainBotLeft, mainTopRight); Vector3 tileBotLeft = CProjectData.currentTileHeader.BotLeftCorner; Vector3 tileTopRight = CProjectData.currentTileHeader.TopRightCorner; float distanceFromCurrentTileBorder = CUtils.GetDistanceFromBorder(pPoint, tileBotLeft, tileTopRight); //is close at tile border if (distanceFromCurrentTileBorder < CProjectData.bufferSize) { //is close at whole forest border - only corners are considered a buffer zone if (distanceFromWholeForestBorder < CProjectData.bufferSize) { float distanceXFromCurrentTileBorder = CUtils.GetDistanceFromBorderX(pPoint, tileBotLeft, tileTopRight); float distanceYFromCurrentTileBorder = CUtils.GetDistanceFromBorderY(pPoint, tileBotLeft, tileTopRight); float distanceXFromForest = CUtils.GetDistanceFromBorderX(pPoint, mainBotLeft, mainTopRight); float distanceYFromForest = CUtils.GetDistanceFromBorderY(pPoint, mainBotLeft, mainTopRight); bool isAtTileCorner = distanceXFromCurrentTileBorder < CProjectData.bufferSize && distanceYFromCurrentTileBorder < CProjectData.bufferSize; bool isNotAtForestCorner = distanceXFromForest > CProjectData.bufferSize || distanceYFromForest > CProjectData.bufferSize; return(isAtTileCorner && isNotAtForestCorner); } return(true); } return(false); }
/// <summary> /// /// </summary> private static Feature GetTreeBorder(CTree pTree) { List <Vector3> furthestPoints = pTree.GetFurthestPoints(); List <Coordinate> pointsCoords = new List <Coordinate>(); foreach (Vector3 p in furthestPoints) { CVector3D globalP = CUtils.GetGlobalPosition(p); pointsCoords.Add(new Coordinate(globalP.X, globalP.Y)); } pointsCoords.Add(pointsCoords[0]); //to close polygon IPolygon polygon = factory.CreatePolygon(pointsCoords.ToArray()); //id AttributesTable attributesTable = new AttributesTable(); attributesTable.Add(ATTR_ID, pTree.treeIndex); //position CVector3D globalTreepos = CUtils.GetGlobalPosition(pTree.peak.Center); attributesTable.Add(ATTR_X, globalTreepos.X.ToString(NUM_FORMAT)); attributesTable.Add(ATTR_Y, globalTreepos.Y.ToString(NUM_FORMAT)); //area attributesTable.Add(ATTR_AREA, pTree.GetArea()); //tree height float treeHeight = pTree.GetTreeHeight(); attributesTable.Add(ATTR_HEIGHT, treeHeight.ToString(NUM_FORMAT)); //reftree type //attributesTable.Add(ATTR_TYPE, pTree.assignedRefTree.RefTreeTypeName); Feature feature = new Feature(polygon, attributesTable); return(feature); }
/// <summary> /// Calculates similarity with other branch. /// Range = [0,1]. 1 = Best match. /// </summary> public float GetSimilarityWith(CBranch pOtherBranch, Vector3 offsetToThisTree, Matrix4x4 scaleMatrix) { if (pOtherBranch.TreePoints.Count == 0) { if (TreePoints.Count == 0) { return(1); } //situation when other branch has no points. //this can mean that data in this part of tree are just missing therefore it should return(UNDEFINED_SIMILARITY); } float similarity = 0; //CreateRotationY rotates point counter-clockwise => -pAngleOffset //rotation has to be calculated in each branch float angleOffsetRadians = CUtils.ToRadians(-(angleFrom - pOtherBranch.angleFrom)); Matrix4x4 rotationMatrix = Matrix4x4.CreateRotationY(angleOffsetRadians, tree.peak.Center); foreach (CTreePoint p in pOtherBranch.TreePoints) { Vector3 movedPoint = p.Center + offsetToThisTree; Vector3 scaledPoint = Vector3.Transform(movedPoint, scaleMatrix); Vector3 rotatedPoint = Vector3.Transform(scaledPoint, rotationMatrix); const int branchToleranceMultiply = 2; if (Contains(rotatedPoint, branchToleranceMultiply)) { similarity += 1f / pOtherBranch.TreePoints.Count; } } if (similarity - 1 > 0.1f) //similarity can be > 1 due to float imprecision { CDebug.Error("Similarity rounding error too big. " + similarity); } similarity = Math.Min(1, similarity); return(similarity); }
/// <summary> /// Add points from given class to the output and main output /// </summary> private static void AddPointsTo(ref StringBuilder pOutput, EClass pClass, ref DateTime start) { List <Vector3> points = CProjectData.Points.GetPoints(pClass); string res; DateTime lastDebug = DateTime.Now; for (int i = 0; i < points.Count; i++) { if (CProjectData.backgroundWorker.CancellationPending) { return; } CDebug.Progress(i, points.Count, DEBUG_FREQUENCY, ref lastDebug, start, "Export las (ground points)"); Vector3 p = points[i]; CVector3D globalP = CUtils.GetGlobalPosition(p); res = GetPointLine(globalP, 1, 0, GetClassColor(pClass)) + newLine; pOutput.Append(res); } //mainOutput.Append(pOutput); }
/// <summary> /// Merge trees with similar height which have peaks too close /// </summary> private static float GetMergeValidFacor(CTree pTreeToMerge, CTree pPossibleTree) { float factor = 0; float peakHeightDiff = pPossibleTree.peak.Z - pTreeToMerge.peak.Z; float treeExtent = CParameterSetter.treeExtent * CParameterSetter.treeExtentMultiply; float similarHeightFactor = (treeExtent - peakHeightDiff) / treeExtent; bool isSimilarHeight = similarHeightFactor > 0.5f; if (!isSimilarHeight) { return(0); } float peakDist2D = CUtils.Get2DDistance(pTreeToMerge.peak, pPossibleTree.peak); if (peakDist2D < treeExtent) { return(1); } return(factor); }
/// <summary> /// Is point closer than the "furthestPoint" of this or one /// of neighbouring branches /// </summary> public bool IsPointInExtent(Vector3 pPoint) { float pointDistToPeak = CUtils.Get2DDistance(pPoint, tree.peak); bool thisBranchInExtent = furthestPointDistance > pointDistToPeak; Vector3 closestPoint = GetClosestPointTo(pPoint); float distToClosestPoint = Vector3.Distance(pPoint, closestPoint); if (thisBranchInExtent && distToClosestPoint < 1) { return(true); } CBranch branch1 = GetNeigbourBranch(-1); float dist1 = branch1.furthestPointDistance; bool leftBranchInExtent = dist1 > pointDistToPeak; float distToLeftPoint = CUtils.Get2DDistance(pPoint, branch1.furthestPoint); //restrict only to points close to the furthest point. //for too large branches it was including points way too far bool leftBranchPointClose = distToLeftPoint < 1; if (leftBranchInExtent && leftBranchPointClose) { return(true); } CBranch branch2 = GetNeigbourBranch(1); float dist2 = branch2.furthestPointDistance; bool rightBranchInExtent = dist2 > pointDistToPeak; float distToRightPoint = CUtils.Get2DDistance(pPoint, branch2.furthestPoint); bool rightBranchPointClose = distToRightPoint < 1; if (rightBranchInExtent && rightBranchPointClose) { return(true); } return(false); }
/// <summary> /// Sets position, scale and rotation of tree obj to match given pTargetTree /// </summary> private static void SetRefTreeObjTransform(ref CRefTree pRefTree, CTree pTargetTree, int pAngleOffset) { Vector3 arrayCenter = CProjectData.GetArrayCenter(); float minHeight = CProjectData.GetMinHeight(); //float treeHeight = pTargetTree.peak.maxHeight.Y - (float)groundHeight; float treeHeight = pTargetTree.GetTreeHeight(); float heightRatio = treeHeight / pRefTree.GetTreeHeight(); pRefTree.Obj.Scale = heightRatio * Vector3.One; //align position to tree pRefTree.Obj.Position = pTargetTree.peak.Center; pRefTree.Obj.Position.Z -= pRefTree.GetTreeHeight() * heightRatio; //move obj so it is at 0,0,0 pRefTree.Obj.Position -= arrayCenter; pRefTree.Obj.Position -= Vector3.UnitZ * minHeight; pRefTree.Obj.Rotation = new Vector3(0, -pAngleOffset, 0); //in OBJ format Y = height CUtils.SwapYZ(ref pRefTree.Obj.Position); }
public float GetDistanceTo(Vector3 pPoint) { return(CUtils.Get2DDistance(this.Center, pPoint)); }
/// <summary> /// Merges all trees, whose peak has good AddFactor to another tree /// </summary> private static void MergeGoodAddFactorTrees(bool pOnlyInvalid) { DateTime mergeStart = DateTime.Now; DateTime previousMergeStart = DateTime.Now; int iteration = 0; int maxIterations = Trees.Count; for (int i = Trees.Count - 1; i >= 0; i--) { if (CProjectData.backgroundWorker.CancellationPending) { return; } if (i >= Trees.Count) { //CDebug.WriteLine("Tree was deleted"); continue; } CTree treeToMerge = Trees[i]; if (treeToMerge.treeIndex == 48 || treeToMerge.treeIndex == 1001) { CDebug.WriteLine(""); } if (pOnlyInvalid && !treeToMerge.isValid && treeToMerge.IsAtBorder()) { //CDebug.Warning(treeToMerge + " is at border"); continue; } //dont merge tree if it is local maximum //if some tree is very close (in neighbouring detail field) //merge it with the highest one if (detectMethod == EDetectionMethod.AddFactor2D) { if (treeToMerge.Equals(22)) { CDebug.WriteLine(); } if (treeToMerge.IsLocalMaximum()) { continue; } CTreeField f = CProjectData.Points.treeDetailArray.GetFieldContainingPoint(treeToMerge.peak.Center); List <CTree> treesInHood = f.GetDetectedTreesFromNeighbourhood(); foreach (CTree tree in treesInHood) { if (tree.Equals(treeToMerge)) { continue; } MergeTrees(tree, treeToMerge); break; } continue; } List <CTree> possibleTrees = GetPossibleTreesToMergeWith(treeToMerge, EPossibleTreesMethod.ClosestHigher); Vector3 pPoint = treeToMerge.peak.Center; float bestAddPointFactor = 0; CTree selectedTree = null; foreach (CTree possibleTree in possibleTrees) { bool isFar = false; bool isSimilarHeight = false; if (possibleTree.isValid && treeToMerge.isValid) { float mergeFactor = GetMergeValidFacor(treeToMerge, possibleTree); if (mergeFactor > 0.9f) { selectedTree = possibleTree; break; } } if (pOnlyInvalid && possibleTree.isValid && treeToMerge.isValid) { continue; } if (treeToMerge.isValid) { //treeToMerge is always lower float possibleTreeHeight = possibleTree.GetTreeHeight(); float treeToMergeHeight = treeToMerge.GetTreeHeight(); const float maxPeaksDistance = 1; float peaksDist = CUtils.Get2DDistance(treeToMerge.peak, possibleTree.peak); if (peaksDist > maxPeaksDistance) { isFar = true; } const float maxPeakHeightDiff = 1; if (possibleTreeHeight - treeToMergeHeight < maxPeakHeightDiff) { isSimilarHeight = true; } } float addPointFactor = possibleTree.GetAddPointFactor(pPoint, treeToMerge); float requiredFactor = 0.5f; if (isFar) { requiredFactor += 0.1f; } if (isSimilarHeight) { requiredFactor += 0.1f; } if (pOnlyInvalid) { requiredFactor -= 0.2f; } if (addPointFactor > requiredFactor && addPointFactor > bestAddPointFactor) { selectedTree = possibleTree; bestAddPointFactor = addPointFactor; if (bestAddPointFactor > 0.9f) { break; } } } if (selectedTree != null) { float dist = CUtils.Get2DDistance(treeToMerge.peak.Center, selectedTree.peak.Center); treeToMerge = MergeTrees(ref treeToMerge, ref selectedTree, pOnlyInvalid); } CDebug.Progress(iteration, maxIterations, 50, ref previousMergeStart, mergeStart, "merge"); iteration++; } }
public List <Vector3> GetMainPoints(bool pAddDebugLine) { List <Vector3> points = new List <Vector3>(); points.Add(ballTop); if (pAddDebugLine) { points.AddRange(CUtils.GetPointLine(ballTop, Vector3.UnitZ)); } if (pAddDebugLine) { points.AddRange(CUtils.GetPointLine(furthestPoint2D, -Vector3.UnitZ)); } if (ballBot != null) { points.Add((Vector3)ballBot); if (pAddDebugLine) { points.AddRange(CUtils.GetPointLine((Vector3)ballBot, -Vector3.UnitZ)); } } if (IsValidMainPoint(furthestPointPlusX)) { points.Add(furthestPointPlusX); if (pAddDebugLine) { points.AddRange(CUtils.GetPointLine(furthestPointPlusX, Vector3.UnitX)); } } if (IsValidMainPoint(furthestPointMinusX)) { points.Add(furthestPointMinusX); if (pAddDebugLine) { points.AddRange(CUtils.GetPointLine(furthestPointMinusX, -Vector3.UnitX)); } } if (IsValidMainPoint(furthestPointPlusY)) { points.Add(furthestPointPlusY); if (pAddDebugLine) { points.AddRange(CUtils.GetPointLine(furthestPointPlusY, Vector3.UnitY)); } } if (IsValidMainPoint(furthestPointMinusY)) { points.Add(furthestPointMinusY); if (pAddDebugLine) { points.AddRange(CUtils.GetPointLine(furthestPointMinusY, -Vector3.UnitY)); } } return(points); }
private float GetFurthestPointDist2D() { return(CUtils.Get2DDistance(ballTop, furthestPoint2D)); }
/// <summary> /// Mergind method for Detection2D /// </summary> private static void Merge2DTrees() { return; for (int i = Trees.Count - 1; i >= 0; i--) { CTree treeToMerge = Trees[i]; if (treeToMerge.Equals(173)) { CDebug.WriteLine(); } //if(treeToMerge.IsLocalMaximum()) //{ // continue; //} List <CTree> possibleTrees = GetPossibleTreesToMergeWith(treeToMerge, EPossibleTreesMethod.ClosestHigher); for (int t = 0; t < possibleTrees.Count; t++) { //merge trees if the target tree can be reached from the treeToMerge //and they are of different heights CTree tree = possibleTrees[t]; float maxRadius = CTreeRadiusCalculator.GetTreeRadius(tree); float treeAvgExtent = tree.GetAverageExtent(); float maxDist = Math.Max(maxRadius, treeAvgExtent); float treeDist = CUtils.Get2DDistance(treeToMerge, tree); if (treeDist > maxDist) { continue; } /*float heightDiff = tree.GetTreeHeight() - treeToMerge.GetTreeHeight(); * if(heightDiff < 2) * { * continue; * }*/ bool pathContainsTree = false; /* * CTreeField closeField = tree.GetClosestField(treeToMerge.peak.Center); * bool pathContainsTree = * CProjectData.Points.treeDetailArray.PathContainsTree( * treeToMerge.peak.Center, closeField.Center, tree, 1, 1); * * if(!pathContainsTree) * { * pathContainsTree = * CProjectData.Points.treeDetailArray.PathContainsTree( * treeToMerge.peak.Center, tree.peak.Center, tree, 1, 1); * }*/ bool canAdd = tree.CanAdd(treeToMerge.peak.Center, false); if (pathContainsTree || canAdd) { treeToMerge = MergeTrees(ref treeToMerge, ref tree, true); break; } } /*List<CField> fields = treeToMerge.peakDetailField.GetFieldsInDistance(5); * foreach(CField field in fields) * { * CTreeField treeField = (CTreeField)field; * CTree detectedTree = treeField.GetSingleDetectedTree(); * if(detectedTree != null && !detectedTree.Equals(treeToMerge)) * { * float heightDiff = detectedTree.GetTreeHeight() - treeToMerge.GetTreeHeight(); * if(heightDiff > 2) * { * treeToMerge = MergeTrees(ref treeToMerge, ref detectedTree, true); * break; * } * } * }*/ } }
private void UpdateFurthestPoints(Vector3 pPoint) { if (!IsAtMainPointZDistance(pPoint)) { return; } float minFurthestPointDist2D = GetMaxPointsDist(-6) / 2; Vector3 diff3D = ballTop - pPoint; float diffX = Math.Abs(diff3D.X); float diffY = Math.Abs(diff3D.Y); float diffZ = Math.Abs(diff3D.Z); if (diffZ < minFurthestPointDist2D) { return; } float dist2D = CUtils.Get2DDistance(pPoint, ballTop); if (dist2D > GetFurthestPointDist2D()) { furthestPoint2D = pPoint; } float diff; if (diffY < DIST_TOLLERANCE) { if (pPoint.X > ballTop.X) { diff = pPoint.X - ballTop.X; if (/*diff > minFurthestPointDist2D &&*/ diff >= furthestPointPlusX.X - ballTop.X) { furthestPointPlusX = pPoint; } } else { diff = ballTop.X - pPoint.X; if (/*diff > minFurthestPointDist2D && */ diff >= ballTop.X - furthestPointMinusX.X) { furthestPointMinusX = pPoint; } } } if (diffX < DIST_TOLLERANCE) { if (pPoint.Y > ballTop.Y) { diff = pPoint.Y - ballTop.Y; if (/*diff > minFurthestPointDist2D &&*/ diff >= furthestPointPlusY.Y - ballTop.Y) { furthestPointPlusY = pPoint; } } else { diff = ballTop.Y - pPoint.Y; if (/*diff > minFurthestPointDist2D && */ diff >= ballTop.Y - furthestPointMinusY.Y) { furthestPointMinusY = pPoint; } } } }
public CBall(List <Vector3> pPoints) { pPoints.Sort((a, b) => b.Z.CompareTo(a.Z)); //todo: make balltop a list of points and get avg ballTop = pPoints[0]; furthestPoint2D = ballTop; furthestPointPlusX = ballTop; furthestPointMinusX = ballTop; furthestPointPlusY = ballTop; furthestPointMinusY = ballTop; foreach (Vector3 point in pPoints) { float zDiff = ballTop.Z - point.Z; if (zDiff > GetMaxPointsDist(1)) { break; } bool isInBallExtent = Vector3.Distance(point, ballTop) > GetMaxPointsDist(1); float dist2D = CUtils.Get2DDistance(point, ballTop); UpdateFurthestPoints(point); if (ballBot == null && dist2D < 0.01 && zDiff > GetMaxPointsDist(0) / 2) { ballBot = point; } if (dist2D > GetMaxPointsDist(3) / 2) { isValid = false; return; } if (!isInBallExtent) { bool isUnderBallTop = dist2D < 0.1f; if (!isUnderBallTop) { isValid = false; return; } } } if (ballBot != null) { float topBotDiffZ = ballTop.Z - ((Vector3)ballBot).Z; //if bot is too close then it is probably not a ball if (topBotDiffZ < GetMaxPointsDist(2) / 2) { isValid = false; return; } } float furthestDist2D = GetFurthestPointDist2D(); float maxDist = GetMaxPointsDist(-3) / 2; if (furthestDist2D < maxDist) { isValid = false; return; } isValid = HasValidMainPoints(); if (isValid) { center = CalculateCenter(); } }
private float GetAddPointFactorInRefTo(Vector3 pPoint, Vector3 pReferencePoint, bool pSameBranch, bool pMerging, CTree pTreeToMerge = null) { //during merging it is expected, that added peak will be higher if (!pMerging && pPoint.Z > pReferencePoint.Z) { //points are added in descending order. if true => pPoint belongs to another tree return(0); } float pointDistToRef = Vector3.Distance(pPoint, pReferencePoint); if (pointDistToRef < 0.2) { return(1); } float refDistToPeak = CUtils.Get2DDistance(pReferencePoint, tree.peak); float pointDistToPeak = CUtils.Get2DDistance(pPoint, tree.peak); if (!pMerging && pSameBranch) { if (pointDistToPeak < refDistToPeak) { return(1); } } float distToPeakDiff = pointDistToPeak - refDistToPeak; if (!pMerging && distToPeakDiff < 0.3) { return(1); } if (!pMerging && distToPeakDiff > 0.5f && refDistToPeak > 0.5f) { float peakRefPointAngle = CUtils.AngleBetweenThreePoints(tree.peak.Center, pReferencePoint, pPoint); //new point is too far from furthest point and goes too much out of direction of peak->furthestPoint if (peakRefPointAngle < 180 - 45) { return(0); } } float refAngleToPoint = CUtils.AngleBetweenThreePoints(pReferencePoint - Vector3.UnitZ, pReferencePoint, pPoint); Vector3 suitablePeakPoint = tree.peak.Center; float peakAngleToPoint = CUtils.AngleBetweenThreePoints(suitablePeakPoint - Vector3.UnitZ, suitablePeakPoint, pPoint); float angle = Math.Min(refAngleToPoint, peakAngleToPoint); float maxBranchAngle = CTree.GetMaxBranchAngle(suitablePeakPoint, pPoint); float unacceptableAngle = maxBranchAngle; if (!pMerging && angle > unacceptableAngle) { return(0); } unacceptableAngle += 30; unacceptableAngle = Math.Min(unacceptableAngle, 100); float angleFactor = (unacceptableAngle - angle) / unacceptableAngle; float unacceptableDistance = tree.GetTreeExtentFor(pPoint, pMerging ? CParameterSetter.treeExtentMultiply : 1); unacceptableDistance += 0.5f; if (pointDistToPeak > unacceptableDistance) { return(0); } unacceptableDistance += 0.5f; float distFactor = (unacceptableDistance - pointDistToPeak) / unacceptableDistance; Vector3 closestPoint = GetClosestPointTo(pPoint); float distFromClosestPoint = Vector3.Distance(pPoint, closestPoint); float maxDistFromClosest = 0.5f; float distToClosestFactor = 2 * (maxDistFromClosest + 0.2f - distFromClosestPoint); distToClosestFactor = Math.Max(0, distToClosestFactor); float totalFactor; if (pMerging) { if (distFromClosestPoint < maxDistFromClosest && distToPeakDiff < maxDistFromClosest) { return(1); } } if (pTreeToMerge != null && pMerging && pTreeToMerge.isValid) { int factorCount = 3; if (distToClosestFactor < .1f) { factorCount -= 1; } totalFactor = (distToClosestFactor + angleFactor + distFactor) / factorCount; } else { //let dist factor have higher influence totalFactor = (angleFactor + 1.5f * distFactor) / 2.5f; } return(totalFactor); }
public string Serialize() { return(CUtils.SerializeVector3(minBB) + " " + CUtils.SerializeVector3(maxBB)); }
private void ProcessUnassignedPoints() { DateTime debugStart = DateTime.Now; DateTime previousDebugStart = DateTime.Now; for (int i = 0; i < unassigned.Count; i++) { if (CProjectData.backgroundWorker.CancellationPending) { return; } CDebug.Progress(i, unassigned.Count, 100000, ref previousDebugStart, debugStart, "ProcessUnassignedPoints"); if (CProjectData.backgroundWorker.CancellationPending) { return; } Vector3 point = unassigned[i]; unassignedArray.AddPointInField(point); if (CTreeManager.GetDetectMethod() == EDetectionMethod.Balls) { ballArray.AddPointInField(point); } } if (CTreeManager.GetDetectMethod() == EDetectionMethod.Balls) { //unassignedArray.FillArray(); //doesnt make sense const bool filterBasedOnheight = false; //balls are expected to be in this height above ground if (filterBasedOnheight) { ballArray.FilterPointsAtHeight(1.8f, 2.7f); } //add filtered points to detail array List <Vector3> filteredPoints = ballArray.GetPoints(); foreach (Vector3 point in filteredPoints) { if (CProjectData.backgroundWorker.CancellationPending) { return; } ballDetailArray.AddPointInField(point); } List <CBallField> ballFields = new List <CBallField>(); //vege.Sort((b, a) => a.Z.CompareTo(b.Z)); //sort descending by height List <CBallField> sortedFields = ballDetailArray.fields; //sortedFields.Sort((a, b) => a.indexInField.Item1.CompareTo(b.indexInField.Item1)); //sortedFields.Sort((a, b) => a.indexInField.Item2.CompareTo(b.indexInField.Item2)); sortedFields.OrderBy(a => a.indexInField.Item1).ThenBy(a => a.indexInField.Item2); //List<Vector3> mainPoints = new List<Vector3>(); debugStart = DateTime.Now; previousDebugStart = DateTime.Now; //process for (int i = 0; i < sortedFields.Count; i++) { if (CProjectData.backgroundWorker.CancellationPending) { return; } CDebug.Progress(i, sortedFields.Count, 100000, ref previousDebugStart, debugStart, "Detecting balls"); CBallField field = (CBallField)sortedFields[i]; field.Detect(); if (field.ball != null && field.ball.isValid) { ballFields.Add(field); ballsMainPoints.AddRange(field.ball.GetMainPoints(true)); ballsCenters.Add(field.ball.center); ballsCenters.AddRange(CUtils.GetPointCross(field.ball.center)); //return; ballsSurface.AddRange(field.ball.GetSurfacePoints()); } } foreach (CBallField field in ballFields) { ballPoints.AddRange(field.points); } } }
/// <summary> /// Returns closest trees to the point being in maximal distance of X steps. /// Trees are sorted based on distance to the point /// </summary> public List <CTree> GetTreesInMaxStepsFrom(Vector3 pPoint, int pSteps) { Tuple <int, int> index = GetIndexInArray(pPoint); //TODO: test newer approach using treeFields and sorting based on distance from point //to the field rather than to tree peak. //If ok => delete //List<CTree> trees = new List<CTree>(); Dictionary <CTree, CTreeField> treeFields = new Dictionary <CTree, CTreeField>(); for (int x = index.Item1 - pSteps; x <= index.Item1 + pSteps; x++) { for (int y = index.Item2 - pSteps; y <= index.Item2 + pSteps; y++) { CTreeField field = GetField(x, y); if (field != null) { List <CTree> detectedTrees = field.DetectedTrees; foreach (CTree tree in detectedTrees) { //if(!trees.Contains(tree)) // trees.Add(tree); CTreeField detectedField; if (treeFields.TryGetValue(tree, out detectedField)) { if (detectedField.GetDistanceTo(pPoint) < field.GetDistanceTo(pPoint)) { continue; } treeFields[tree] = field; continue; } treeFields.Add(tree, field); } } } } ////todo: sort based on a distance from point to the closest field where tree has been detected. ////in this implementation some tree in the result can be actually further from point than other //trees.Sort((a, b) => CUtils.Get2DDistance(pPoint, a.Center).CompareTo(CUtils.Get2DDistance(pPoint, b.Center))); List <KeyValuePair <CTree, CTreeField> > treeFieldList = treeFields.ToList(); //sort based on a distance from point to the closest field where tree has been detected treeFieldList.Sort((a, b) => CUtils.Get2DDistance(pPoint, a.Value.Center).CompareTo(CUtils.Get2DDistance(pPoint, b.Value.Center))); List <CTree> result = treeFieldList.ToDictionary(x => x.Key, x => x.Value).Keys.ToList(); return(result); //return trees; }
/// <summary> /// Calculates a rotation and translation that needs to be applied /// on pSetA to match pSetB. /// The process expects the matching points from sets having the same index. /// </summary> private static CRigidTransform CalculateRigidTransform(List <Vector3> pSetA, List <Vector3> pSetB) { //prevent modification of the input parameters List <Vector3> setA = CUtils.GetCopy(pSetA); List <Vector3> setB = CUtils.GetCopy(pSetB); //setA and setB will be modified List <Vector3> setAorig = CUtils.GetCopy(pSetA); List <Vector3> setBorig = CUtils.GetCopy(pSetB); Vector3 centroid_A = CUtils.GetAverage(setA); Vector3 centroid_B = CUtils.GetAverage(setB); CUtils.MovePointsBy(ref setA, -centroid_A); CUtils.MovePointsBy(ref setB, -centroid_B); Matrix mA = CreateMatrix(setA); Matrix mB = CreateMatrix(setB); Matrix A_multiply_B = mA.MultiplyTransposeBy(mB); double[,] H = ConvertToDouble(A_multiply_B); double[] w = new double[3]; double[,] u = new double[3, 3]; double[,] vt = new double[3, 3]; //U, S, Vt = linalg.svd(H) //viz https://radfiz.org.ua/files/temp/Lab1_16/alglib-3.4.0.csharp/csharp/manual.csharp.html const int u_needed = 2; const int vt_needed = 2; const int additional_memory = 2; alglib.svd.rmatrixsvd(H, 3, 3, u_needed, vt_needed, additional_memory, ref w, ref u, ref vt, null); Matrix Vt = new Matrix(vt); Matrix U = new Matrix(u); Matrix rotation = Vt.MultiplyTransposeBy(U.Transpose(true)); //R = Vt.T * U.T if (GetDeterminant(ConvertToDouble(rotation), 3) < 0) { //CDebug.Warning("Reflection detected"); //for(int i = 0; i < R.Rows; i++) //{ // R[i, 2] *= -1; // NOT THE SAME! //} vt[2, 0] *= -1; vt[2, 1] *= -1; vt[2, 2] *= -1; //Matrices need reinitialization - they are modified Vt = new Matrix(vt); U = new Matrix(u); rotation = Vt.MultiplyTransposeBy(U.Transpose(true)); } Matrix centerA = new Matrix(new double[, ] { { centroid_A.X, centroid_A.Y, centroid_A.Z } }); Matrix centerB = new Matrix(new double[, ] { { centroid_B.X, centroid_B.Y, centroid_B.Z } }); //create a copy - operations affects the original object Matrix rotationCopy = new Matrix(rotation); Matrix _R_mult_centerA = rotationCopy.Negate().MultiplyByTranspose(centerA); Matrix translation = _R_mult_centerA + centerB.Transpose(true); //CDebug.WriteLine("Rotation = "); //CDebug.WriteLine("" + R); //CDebug.WriteLine("Translation = "); //CDebug.WriteLine("" + T); float offset = GetOffset(setAorig, setBorig, rotation, translation); return(new CRigidTransform(rotation, translation, offset)); }