private static double CalculateScale_Zinsser(PointCloud pointsSourceShift, PointCloud pointsTargetShift, ref Matrix3d R) { double sum1 = 0; double sum2 = 0; Matrix3d RT = Matrix3d.Transpose(R); Matrix3d checkT = Matrix3d.Mult(RT, R); Matrix4d R4D = new Matrix4d(); R4D = R4D.PutTheMatrix4dtogether(Vector3d.Zero, R); for (int i = 0; i < pointsSourceShift.Count; i++) { Vector3d vSource = new Vector3d(pointsSourceShift.Vectors[i].X, pointsSourceShift.Vectors[i].Y, pointsSourceShift.Vectors[i].Z); Vector3d vTarget = new Vector3d(pointsTargetShift.Vectors[i].X, pointsTargetShift.Vectors[i].Y, pointsTargetShift.Vectors[i].Z); Vector3d vd = Vector3d.TransformNormalInverse(new Vector3d(pointsSourceShift.Vectors[i].X, pointsSourceShift.Vectors[i].Y, pointsSourceShift.Vectors[i].Z), R4D); vd = R.MultiplyVector(vSource); //Vector3 v = new Vector3(vd.X, vd.Y, vd.Z); //sum1 += Vector3.Dot(v, pointsTargetShift.Vectors[i]); //sum2 += Vector3.Dot(pointsSourceShift.Vectors[i], pointsSourceShift.Vectors[i]); sum1 += Vector3d.Dot(vd, vTarget); sum2 += Vector3d.Dot(vSource, vSource); } double c = sum1 / sum2; R = R.MultiplyScalar(c); return(c); }
private static double CalculateScale_Zinsser(List <Vector3d> pointsSourceShift, List <Vector3d> pointsTargetShift, ref Matrix3d R) { double sum1 = 0; double sum2 = 0; Matrix3d RT = Matrix3d.Transpose(R); Matrix3d checkT = Matrix3d.Mult(RT, R); Matrix4d R4D = new Matrix4d(); R4D = R4D.PutTheMatrix4dtogether(Vector3d.Zero, R); for (int i = 0; i < pointsSourceShift.Count; i++) { Vector3d v = Vector3d.TransformNormalInverse(pointsSourceShift[i], R4D); v = R.MultiplyVector(pointsSourceShift[i]); sum1 += Vector3d.Dot(v, pointsTargetShift[i]); sum2 += Vector3d.Dot(pointsSourceShift[i], pointsSourceShift[i]); } double c = sum1 / sum2; R = R.MultiplyScalar(c); return(c); }
public static Matrix4d FindTransformationMatrix(List <Vector3d> pointsSource, List <Vector3d> pointsTarget, ICP_VersionUsed icpVersionUsed) { //shift points to the center of mass (centroid) Vector3d centroidTarget = pointsTarget.CalculateCentroid(); List <Vector3d> pointsTargetTranslated = pointsTarget.Clone(); pointsTargetTranslated.SubtractVector(centroidTarget); Vector3d centroidSource = pointsSource.CalculateCentroid(); List <Vector3d> pointsSourceTranslated = pointsSource.Clone(); pointsSourceTranslated.SubtractVector(centroidSource); Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed); Vector3d T = SVD.CalculateTranslation(centroidSource, centroidTarget, R); Matrix4d myMatrix = new Matrix4d(); myMatrix = myMatrix.PutTheMatrix4dtogether(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); }