/* * 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; }