read registration output

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");

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\" " +
+                $"\"{Globals.ToWslPath(outputFileName)}\" " +
-                "\"/mnt/c/Users/timwu/source/repos/PointCloudWeb/PointCloudWeb.Server/Tools/TEASERpp/c620b175-ace8-42e5-bf29-55b6c99372bc.ply\" ";
+                $"\"{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;
         }
     }

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;
+        }
+    }
+}

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 src_fileName = argv[2];
-  auto tgt_fileName = argv[2];
+  auto tgt_fileName = argv[3];
 
   // Load the .ply file
   teaser::PLYReader reader;