private void Clouds_AlignPCAAxes()
{
if (!GetFirstTwoCloudsFromOpenGLControl())
{
return;
}
PointCloudVertices pointCloudTarget = PointCloudVertices.FromPointCloud(this.pSource);
pointCloudTarget = PCA.RotateToOriginAxes(pointCloudTarget);
PointCloudVertices pointCloudSource = PointCloudVertices.FromPointCloud(this.pTarget);
PointCloudVertices pointCloudResult = PCA.RotateToOriginAxes(pointCloudSource);
pResult = pointCloudResult.ToPointCloud();
//pResult.SetColor(new OpenTK.Vector3(1, 0, 0));
pointCloudResult.AddPointCloud(this.pTarget.ToPointCloudVertices());
SaveResultCloudAndShow(pointCloudResult);
//DisplayResultPointCloud();
//DisplayObjects();
}
public void AddVertexListAsModel(string name, PointCloudVertices myVertexList)
{
Model myModel = new Model();
myModel.PointCloud = myVertexList.ToPointCloud();
this.glControl1.GLrender.AddModel(myModel);
}
/// <summary>Generates a 3D Model for a cone.</summary>
/// <summary>
/// Generates a 3D Model for a cuboid
/// </summary>
/// <param name="Name">Model name</param>
/// <param name="u">Length of the lower part</param>
/// <param name="v">Length of the high part</param>
/// <param name="numberOfPoints">Number of points to use in circumference</param>
/// <param name="Color">Color vector</param>
/// <param name="Texture">Texture bitmap. Null uses no texture</param>
/// <returns></returns>
public static Model Cuboid(string Name, double u, double v, int numberOfPoints, System.Drawing.Color color, System.Drawing.Bitmap Texture)
{
PointCloudVertices points = PointCloudVertices.CreateCuboid(u, v, numberOfPoints);
PointCloudVertices.SetColorOfListTo(points, color);
Model myModel = new Model("Cuboid");
myModel.PointCloud = points.ToPointCloud();
return(myModel);
}
private void SaveResultCloudAndShow(PointCloudVertices pointCloudResult)
{
//-------------------------------------
//save
if (pointCloudResult != null)
{
string path = string.Empty;
string fileNameShort = string.Empty;
IOUtils.ExtractDirectoryAndNameFromFileName(pSource.FileNameLong, ref fileNameShort, ref path);
pointCloudResult.Save(this.pSource.Path, "Result.obj");
this.pResult = pointCloudResult.ToPointCloud();
//pResult.SetColor(new OpenTK.Vector3(1, 0, 0));
DisplayResultPointCloud();
}
}
public PointCloudVertices PerformICP()
{
double convergenceThreshold = PTarget.BoundingBoxMaxFloat * ICPSettings.ConvergenceThreshold;
PointCloudVertices PT = PointCloudVertices.CopyVertices(PTarget);
PointCloudVertices PS = PointCloudVertices.CopyVertices(PSource);
Vertex pSOrigin = new Vertex(0, new Vector3d(0, 0, 0));
Vertex pTOrigin = new Vertex(0, new Vector3d(0, 0, 0));
PointCloudVertices myPointsTransformed = null;
ICPSettings.ResetVertexToOrigin = false;
if (ICPSettings.ResetVertexToOrigin)
{
pTOrigin = PointCloudVertices.ResetCentroid(PT, true);
pSOrigin = PointCloudVertices.ResetCentroid(PS, true);
}
KDTreeVertex kdTreee = Helper_CreateTree(PT);
kdTreee.DistanceOptimization = this.ICPSettings.DistanceOptimization;
CalculateNormals(PS.ToPointCloud(), PT.ToPointCloud(), kdTreee);
try
{
if (!CheckSourceTarget(PT, PS))
{
return(null);
}
PointCloudVertices pointsTarget = PointCloudVertices.CopyVertices(PT);
PointCloudVertices pointsSource = PointCloudVertices.CopyVertices(PS);
this.Matrix = Matrix4d.Identity;
double oldMeanDistance = 0;
for (NumberOfIterations = 0; NumberOfIterations < ICPSettings.MaximumNumberOfIterations; NumberOfIterations++)
{
kdTreee.NormalsSource = this.normalsSource;
float angleThreshold = Convert.ToSingle(this.startAngleForNormalsCheck - 5) * (1.0f - this.NumberOfIterations * 1.0f / this.ICPSettings.MaximumNumberOfIterations) + 5;
if (ICPSettings.SimulatedAnnealing)
{
if (Helper_ICP_Iteration_SA(PS, PT, kdTreee, angleThreshold))
{
break;
}
}
else
{
myPointsTransformed = Helper_ICP_Iteration(ref pointsSource, ref pointsTarget, PT, PS, kdTreee, angleThreshold);
if (myPointsTransformed != null)
{
break;
}
}
Debug.WriteLine("--------------Iteration: " + NumberOfIterations.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000") + ": duration: " + GlobalVariables.TimeSpanString());
if (Math.Abs(oldMeanDistance - MeanDistance) < convergenceThreshold)
{
Debug.WriteLine("Convergence reached - changes under: " + convergenceThreshold.ToString());
break;
}
oldMeanDistance = MeanDistance;
}
Debug.WriteLine("--------****** Solution of ICP after : " + NumberOfIterations.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000"));
//if number of Iteration
if (myPointsTransformed == null)
{
myPointsTransformed = pointsTransformed;
}
if (this.ICPSettings.ShuffleEffect)
{
PT = pointsTarget;
PTransformed = myPointsTransformed;
}
else
{
PTransformed = MathUtilsVTK.TransformPoints(PS, Matrix);
}
//re-reset vector
if (ICPSettings.ResetVertexToOrigin)
{
PointCloudVertices.AddVertex(PTransformed, pTOrigin);
}
//DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, this.PSource, PTransformed, PTarget);
if (myPointsTransformed != null)
{
DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, pointsSource, myPointsTransformed, pointsTarget);
}
else
{
//no convergence - write matrix
this.Matrix.Print("Cumulated Matrix ");
}
PMerged = CalculateMergedPoints(PTransformed, PT);
return(PTransformed);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + NumberOfIterations.ToString() + " : " + err.Message);
return(null);
}
}
