/*
* Point cloud color:
* AntiqueWhite is ref
* YellowGreen is pending
* blue is established
*/
private void TransformPC(Point3DCollection source, Point3DCollection reference)
{
// As far as i know, source[i] maps to reference[i]
// compute initial transformation
//compute transformation from reference
_icpData = _icp.ComputeICP(
Parser3DPoint.FromPoint3DToDataPoints(source),
Parser3DPoint.FromPoint3DToDataPoints(reference),
_initialTransformation);
_icpData.transform = _initialTransformation;
var p = ICP.ApplyTransformation(_icpData.transform, Parser3DPoint.FromPoint3DToDataPoints(_displayPointCloud));
_displayPointCloud = Parser3DPoint.FromDataPointsToPoint3DCollection(p);
_renderer.CreatePointCloud(_displayPointCloud, Brushes.BlueViolet);
}
private static void ConstructTestCase(out EuclideanTransform t, out ErrorElements errorElements)
{
// pick some random points
var points = new List <DataPoint>();
for (int i = 0; i < 10000; i++)
{
var n = RandomVector();
points.Add(new DataPoint
{
point = 100.0f * RandomVector() - new Vector3(50.0f),
normal = Vector3.Normalize(n),
});
}
var dataPoints = new DataPoints
{
points = points.ToArray(),
contiansNormals = true,
};
t = new EuclideanTransform();
t.translation = RandomVector() * 50.0f;
//t.translation = new Vector3(0f);
var axis = Vector3.Normalize(RandomVector());
t.rotation = Quaternion.CreateFromAxisAngle(axis, (float)(r.NextDouble() * Math.PI * 2));
t.rotation = Quaternion.Normalize(t.rotation);
//t.rotation = Quaternion.Identity;
var transformedPoints = ICP.ApplyTransformation(dataPoints, t.Inverse());
errorElements = new ErrorElements
{
reference = dataPoints,
reading = transformedPoints,
weights = Enumerable.Repeat(1.0f, points.Count).ToArray()
};
}
private void ScanReading_Click(object sender, RoutedEventArgs e)
{
// for intersection testing
//_displayPointCloud = _renderer.ReadData();
//_rgbv._bvh = BVH.InitBVH(_displayPointCloud);
var point1 = new Point3D(-0.8f, 0, 0);
var point2 = new Point3D(-1f, 0, 0);
var point3 = new Point3D(-0.6f, 0.5f, 0);
var point4 = new Point3D(0.8f, -0.5f, 0);
var pcReference = new Point3DCollection
{
point1,
point2,
point3
};
Log.WriteLog("--------------------");
/*
* var rotationAngle = 0.707106781187f;
*
* Matrix3D m = new Matrix3D(
* rotationAngle, 0, rotationAngle, 0,
* 0, 1, 0, 0,
* -rotationAngle, 0, rotationAngle, 0,
* 1, 0, 0, 1);
*
* // Transform the thumb according to m
* _thumbReading = m.Transform(_readingFeatures[0]);
* point1 = m.Transform(point1);
* point2 = m.Transform(point2);
* point3 = m.Transform(point3);
* point4 = m.Transform(point4);
*
* var pcReading = new Point3DCollection
* {
* point1,
* point2,
* point3
* };
*/
var depthData = _pcv.GetDepthDataFromLatestFrame();
_reading = depthData.Item1; // all points
_readingFeatures = depthData.Item2; // only feature points [4]
_initialTransformation = Util.ComputeInitialTransformation(_readingFeatures, _referenceFeatures);
_reading = Parser3DPoint.FromDataPointsToPoint3DCollection(ICP.ApplyTransformation(_initialTransformation, Parser3DPoint.FromPoint3DToDataPoints(_reading)));
ComputeRMSE(_referenceFeatures, _readingFeatures, _initialTransformation);
_renderer.CreatePointCloud(_referenceFeatures, Brushes.Pink, false, 0.0125f);
// Transform readingFeatures
_renderer.CreatePointCloud(_readingFeatures, Brushes.YellowGreen, false, 0.0125f);
_readingFeatures = Parser3DPoint.FromDataPointsToPoint3DCollection(ICP.ApplyTransformation(_initialTransformation, Parser3DPoint.FromPoint3DToDataPoints(_readingFeatures)));
_renderer.CreatePointCloud(_readingFeatures, Brushes.Violet, false, 0.0125f);
_renderer.CreatePointCloud(_reading, Brushes.BlueViolet, false, 0.0025f);
// _renderer.CreatePointCloud(_reading, Brushes.Violet,false);
_cycleRuns++;
label_Cycle.Content = "cycle: " + _cycleRuns;
}
