public static PointCloud ShiftByCenterOfMass(PointCloud pointsSource) { Vector3 Centroid = pointsSource.CentroidVectorRecalc; PointCloud pcNew = PointCloud.SubtractVector(pointsSource, Centroid); return(pcNew); }
private PointCloud ShiftByCenterOfMass(PointCloud pointsSource) { Centroid = pointsSource.CentroidVectorRecalc; PointCloud pcNew = PointCloud.SubtractVector(pointsSource, this.Centroid); return(pcNew); }
public static Matrix4 FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed) { //shift points to the center of mass (centroid) Vector3 centroidTarget = pointsTarget.CentroidVector; PointCloud pointsTargetTranslated = pointsTarget.Clone(); pointsTargetTranslated.SubtractVector(centroidTarget); Vector3 centroidSource = pointsSource.CentroidVector; PointCloud pointsSourceTranslated = pointsSource.Clone(); pointsSourceTranslated.SubtractVector(centroidSource); Matrix3 R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed); Vector3 T = SVD_Float.CalculateTranslation(centroidSource, centroidTarget, R); Matrix4 myMatrix = new Matrix4(); myMatrix = myMatrix.PutTheMatrix4together(T, R); return(myMatrix); }
public static Matrix4d FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed) { //shift points to the center of mass (centroid) Vector3 centroidTarget = pointsTarget.CentroidVector; //Vector3d centroidTarget_Double = new Vector3d(pointsTarget.CentroidVector.X, pointsTarget.CentroidVector.Y, pointsTarget.CentroidVector.Z); PointCloud pointsTargetTranslated = pointsTarget.Clone(); pointsTargetTranslated.SubtractVector(centroidTarget); Vector3 centroidSource = pointsSource.CentroidVector; PointCloud pointsSourceTranslated = pointsSource.Clone(); pointsSourceTranslated.SubtractVector(centroidSource); Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed); Vector3d T = CalculateTranslation(centroidSource, centroidTarget, R); Matrix4d myMatrix = new Matrix4d(); myMatrix = myMatrix.PutTheMatrix4dtogether(T, R); return(myMatrix); }