public void Cuboid_New() { ResetPointCloudForOpenGL(); List <Vector3> listVectors = ExamplePointClouds.Cuboid_Corners_CenteredAt0(1, 1, 1); pointCloudSource = PointCloud.FromListVector3(listVectors); this.pointCloudSource = PointCloud.Clone(pointCloudSource); PointCloud.RotateDegrees(pointCloudSource, 45, 45, 45); pointCloudSource = pointCloudSource; pca.PCA_OfPointCloud(pointCloudSource); //-----------Show in Window if (UIMode) { this.ShowResultsInWindow_CubeNew(true, true); //ShowResultsInWindow_Cube(true); } }
protected void CreateCuboid(float sizeX, float sizeY, float sizeZ) { List <Vector3> listVectors = ExamplePointClouds.Cuboid_Corners_CenteredAt0(sizeX, sizeY, sizeZ); this.pointCloudTarget = PointCloud.FromListVector3(listVectors); PointCloud.ResetToOriginAxis(pointCloudTarget); PointCloud.SetIndicesForCubeCorners(this.pointCloudTarget); this.pointCloudSource = pointCloudTarget.Clone(); PointCloud.SetIndicesForCubeCorners(pointCloudSource); }
internal PointCloud Process(PointCloud cloud, PointCloud prevCloud, Matrix bestGuessTransformation) { if (cloud.Points.Count < 4) { return(cloud); } var T_0 = (DenseMatrix)bestGuessTransformation; // sample point clouds PointCloud sampleScene; PointCloud model = prevCloud; if (this.UseSampling) { var samplingParams = new DistributedSamplingParameters( (int)(cloud.Points.Count * Registration.Settings.Default.SamplingProportion), Registration.Settings.Default.MinimumSamplingDistance); sampleScene = DistributedSampler.GetRandomSample(cloud, samplingParams); } else { sampleScene = cloud; } // run ICP on sampled data var T_opt = CalculateTransformation(sampleScene, model, T_0); // create deep clone of unoptimised cloud var optCloud = (PointCloud)cloud.Clone(); // apply T_opt to entire cloud and return for (int i = 0; i < optCloud.Points.Count; i++) { optCloud.Points[i].ApplyAffineTransformation(T_opt); } // return optimised cloud return(optCloud); }
private DenseMatrix CalculateTransformation(PointCloud scene_in, PointCloud model_in, DenseMatrix T_0) { var T = new DenseMatrix[maxIter]; T[0] = T_0; var startTime = DateTime.Now; // perform deep copy of input scene because we modify it directly in transform calculation var scene = (PointCloud)scene_in.Clone(); // no need to clone the model since we don't modify it var model = model_in; // precompute KdTree of model (this doesn't change for each model-scene pair) var kdModel = KdTree.Construct(3, (List <Vector>)model); int k = 1; float err = 0f; for (; k < maxIter; k++) { // Step 1 - Apply previous transform T_k-1 for (int i = 0; i < scene.Points.Count; i++) { scene.Points[i].ApplyAffineTransformation(T[k - 1]); } // Step 2 - Find correspondances var pairs = new PointPair <Vector> [scene.Points.Count]; for (int i = 0; i < scene.Points.Count; i++) { var nn = kdModel.FindNearestNeighbour(scene.Points[i]); pairs[i] = new PointPair <Vector>(scene.Points[i], nn); } // Step 3 - Check convergence criterion err = 0; foreach (var pair in pairs) { var v = pair.Scene - pair.Model; // measure 'distance' or error between colour of these points err += (float)Math.Sqrt(v.DotProduct(v)); } err /= scene.Points.Count; // average error if (err <= this.minError) { break; // transform T_k-1 was best! terminate. } if (k + 1 == maxIter) { break; // pointless compute ahead? else, must compute better transform!... } // Step 4 - Calculate composite matrix H var H = new DenseMatrix(3); for (int i = 0; i < pairs.Length; i++) { H += (DenseMatrix)DenseVector.OuterProduct(pairs[i].Scene, pairs[i].Model); } // Step 5 - Decompose H-matrix using SVD; calculate 3x3 rotation matrix R var svd = H.Svd(true); // Check if special reflection case (det(V) < 0) var VT = svd.VT(); if (VT.Transpose().Determinant() < 0) { VT.SetRow(2, -VT.Row(2)); // negate row 3 } DenseMatrix R = (DenseMatrix)(svd.U() * VT); // Step 6 - Calculate complete affine transformation matrix, T // --> R_ext = 4x4 transformation matrix using R rotation var R_ext = DenseMatrix.Identity(4); R_ext.SetSubMatrix(0, 3, 0, 3, R.Transpose()); // --> T = C_model R_ext C_scene T[k] = R_ext; // new transform ready (from T[k-1] * Scene -> Model) } // reached optimal transformation sequence, return compound matrix T_opt = T_n * ... * T_1 * T_0 var T_opt = DenseMatrix.Identity(4); foreach (var Tk in T) { if (Tk == null) { break; } T_opt = Tk * T_opt; } if (DebugSettings.Default.DebugMode) { this.log.WriteLine("{0},{1},{2}", this.logN++, k, err); } return(T_opt); }