diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs index b1c6a49..b2c8bad 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs @@ -90,7 +90,7 @@ namespace PointCloudWeb.Server.Models stringBuilder.Append("ply\n"); stringBuilder.Append("format ascii 1.0\n"); - stringBuilder.Append($"element vertex COUNT_PLACEHOLDER {Points.Count}\n"); + stringBuilder.Append("element vertex COUNT_PLACEHOLDER\n"); stringBuilder.Append("property float x\n"); stringBuilder.Append("property float y\n"); stringBuilder.Append("property float z\n"); diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs index 100595c..78e3428 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs @@ -1,8 +1,13 @@ using System; using System.Diagnostics; +using System.Globalization; using System.IO; using System.Linq; +using System.Numerics; using PointCloudWeb.Server.Models; +using PointCloudWeb.Server.Utils; + +// ReSharper disable PossibleMultipleEnumeration namespace PointCloudWeb.Server.Services { @@ -13,92 +18,66 @@ namespace PointCloudWeb.Server.Services var sourceFileName = Globals.TempPath + $"/{source.Id}.ply"; var targetFileName = Globals.TempPath + $"/{target.Id}.ply"; + var outputFileName = Globals.TempPath + $"/{target.Id}.txt"; + var maxPoints = 5_000; - - // var p = new Process(); - // var info = new ProcessStartInfo - // { - // FileName = "cmd.exe", - // RedirectStandardInput = true, - // RedirectStandardOutput = true, - // UseShellExecute = false, - // Verb = "OPEN", - // CreateNoWindow = false, - // WindowStyle = ProcessWindowStyle.Normal - // }; - // p.StartInfo = info; - // p.Start(); - // - // using var sw = p.StandardInput; - // if (sw.BaseStream.CanWrite) - // { - // // sw.Write("wsl \n"); - // // sw.Write("echo \"Test\" \n"); - // // sw.Write("echo curl https://www.google.de \n"); - // // sw.Write("exit \n"); - // // sw.Write("exit \n"); - // - // //sw.Write("wsl curl https://www.google.de & exit \n"); - // - // sw.Write("wsl echo \"Test\" >> /mnt/c/Users/timwu/test.txt & exit \n"); - // - // } - // - // - // var outputN = p.StandardOutput.ReadToEnd(); - // Console.WriteLine(outputN[..Math.Min(outputN.Length, 100)]); - // - // p.WaitForExit(); - // - // Console.WriteLine($"RegistrationExitCode: {p.ExitCode}"); - source.WriteToPly(sourceFileName, maxPoints); target.WriteToPly(targetFileName, maxPoints); - // var teaserPp = new Process(); - // teaserPp.StartInfo.FileName = "C:\\Windows\\System32\\wsl.exe"; - // // teaserPp.StartInfo.FileName = "wsl"; - // teaserPp.StartInfo.Arguments = "curl https://www.google.de"; - // // teaserPp.StartInfo.Arguments = $"\"{sourceFileName}\" \"{targetFileName}\""; - // //teaserPp.StartInfo.Arguments = $" ls "; //{Globals.ToWslPath(Globals.TeaserPp)} ";// + - // //"\"/mnt/c/Users/timwu/source/repos/PointCloudWeb/PointCloudWeb.Server/Tools/TEASERpp/c4b9b7fc-0b97-4f52-ad1b-737aeca5ba97.ply\" " + - // //"\"/mnt/c/Users/timwu/source/repos/PointCloudWeb/PointCloudWeb.Server/Tools/TEASERpp/c620b175-ace8-42e5-bf29-55b6c99372bc.ply\" "; - // - // teaserPp.StartInfo.RedirectStandardOutput = true; - // teaserPp.Start(); - // - // - // var output = teaserPp.StandardOutput.ReadToEnd(); - // Console.WriteLine(output[..Math.Min(output.Length, 100)]); - // - // - // teaserPp.WaitForExit(); - // - // Console.WriteLine($"RegistrationExitCode: {teaserPp.ExitCode}"); - - var teaserPp = new Process(); teaserPp.StartInfo.FileName = "wsl"; teaserPp.StartInfo.Arguments = $"{Globals.ToWslPath(Globals.TeaserPp)} " + - "\"/mnt/c/Users/timwu/source/repos/PointCloudWeb/PointCloudWeb.Server/Tools/TEASERpp/c4b9b7fc-0b97-4f52-ad1b-737aeca5ba97.ply\" " + - "\"/mnt/c/Users/timwu/source/repos/PointCloudWeb/PointCloudWeb.Server/Tools/TEASERpp/c620b175-ace8-42e5-bf29-55b6c99372bc.ply\" "; + $"\"{Globals.ToWslPath(outputFileName)}\" " + + $"\"{Globals.ToWslPath(sourceFileName)}\" " + + $"\"{Globals.ToWslPath(targetFileName)}\" "; - // teaserPp.StartInfo.RedirectStandardOutput = true; teaserPp.Start(); - - - // var output = teaserPp.StandardOutput.ReadToEnd(); - // Console.WriteLine(output[..Math.Min(output.Length, 100)]); - - teaserPp.WaitForExit(); - Console.WriteLine($"RegistrationExitCode: {teaserPp.ExitCode}"); + //Console.WriteLine($"RegistrationExitCode: {teaserPp.ExitCode}"); var result = new RegistrationResult(); + // if (teaserPp.ExitCode != 0) + // return result; + + var lines = File.ReadAllLines(outputFileName).ToList(); + while (lines.Any(s => s.Contains(" "))) + lines = lines.Select(s => + { + var newString = s.Replace(" ", " "); + if (newString.Length > 0 && newString[0] == ' ') + newString = newString.Remove(0, 1); + return newString; + }).ToList(); + + var rotationIndex = lines.FindIndex(s => s.Contains("Estimated rotation:")); + var translationIndex = lines.FindIndex(s => s.Contains("Estimated translation:")); + + var ci = CultureInfo.InvariantCulture; + + var rotationMatrix = Matrix4x4.Identity; + rotationMatrix.M11 = float.Parse(lines[rotationIndex + 1].Split(' ')[0], ci); + rotationMatrix.M12 = float.Parse(lines[rotationIndex + 1].Split(' ')[1], ci); + rotationMatrix.M13 = float.Parse(lines[rotationIndex + 1].Split(' ')[2], ci); + rotationMatrix.M21 = float.Parse(lines[rotationIndex + 2].Split(' ')[0], ci); + rotationMatrix.M22 = float.Parse(lines[rotationIndex + 2].Split(' ')[1], ci); + rotationMatrix.M23 = float.Parse(lines[rotationIndex + 2].Split(' ')[2], ci); + rotationMatrix.M31 = float.Parse(lines[rotationIndex + 3].Split(' ')[0], ci); + rotationMatrix.M32 = float.Parse(lines[rotationIndex + 3].Split(' ')[1], ci); + rotationMatrix.M33 = float.Parse(lines[rotationIndex + 3].Split(' ')[2], ci); + + result.Rotation = MatrixUtils.RotationMatrixToAngles(rotationMatrix); + + var transformation = Vector3.Zero; + transformation.X = float.Parse(lines[translationIndex + 1], ci); + transformation.Y = float.Parse(lines[translationIndex + 2], ci); + transformation.Z = float.Parse(lines[translationIndex + 3], ci); + + result.Transformation = transformation; + return result; } } diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Utils/MatrixUtils.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Utils/MatrixUtils.cs new file mode 100644 index 0000000..e4b3a45 --- /dev/null +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Utils/MatrixUtils.cs @@ -0,0 +1,19 @@ +using System; +using System.Numerics; + +namespace PointCloudWeb.Server.Utils +{ + public class MatrixUtils + { + public static Vector3 RotationMatrixToAngles(Matrix4x4 m) + { + var result = Vector3.Zero; + + result.X = (float)Math.Atan2(m.M32, m.M33); + result.Y = (float)Math.Atan2(-m.M31, Math.Sqrt(m.M32 * m.M32 + m.M33 * m.M33)); + result.Z = (float)Math.Atan2(m.M21, m.M11); + + return result; + } + } +} \ No newline at end of file diff --git a/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc b/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc index b48b036..d382f35 100644 --- a/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc +++ b/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc @@ -23,7 +23,7 @@ inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) int main(int argc, char **argv) { - std::ofstream out("/mnt/c/Users/timwu/source/repos/PointCloudWeb/PointCloudWeb.Server/Tools/TEASERpp/build/out.txt"); + std::ofstream out(argv[1]); std::streambuf *coutbuf = std::cout.rdbuf(); //save old buf std::cout.rdbuf(out.rdbuf()); //redirect std::cout to out.txt! @@ -34,8 +34,8 @@ int main(int argc, char **argv) for (int i = 0; i < argc; ++i) std::cout << argv[i] << "\n"; - auto src_fileName = argv[1]; - auto tgt_fileName = argv[2]; + auto src_fileName = argv[2]; + auto tgt_fileName = argv[3]; // Load the .ply file teaser::PLYReader reader;