From acda8b9eacd6eccd1521665d24e945df8e480e5f Mon Sep 17 00:00:00 2001 From: Tim Wundenberg Date: Sun, 22 Aug 2021 18:58:41 +0200 Subject: [PATCH] implement registration try running simple wsl-command fails add Ply-File export change build-target to x64 (for wsl) --- .../PointCloudWeb.Server.ScanConverter.csproj | 8 ++ .../PointCloudWeb.Server.Tests.csproj | 8 ++ .../PointCloudWeb.Server/Globals.cs | 17 +++- .../PointCloudWeb.Server/Models/PointCloud.cs | 59 +++++++++++- .../PointCloudWeb.Server.csproj | 8 ++ .../IPointCloudRegistrationService.cs | 15 +++- .../PointCloudRegistrationServiceTeaerPp.cs | 46 ++++++++++ .../Services/PointCloudService.cs | 26 +++--- .../PointCloudWeb.Server/Startup.cs | 1 + .../PointCloudWeb.Server/Utils/EthTestData.cs | 4 +- .../Tools/TEASERpp/CMakeLists.txt | 2 +- PointCloudWeb.Server/Tools/TEASERpp/README.md | 3 + .../Tools/TEASERpp/teaser_cpp_ply.cc | 89 +++++++------------ 13 files changed, 204 insertions(+), 82 deletions(-) create mode 100644 PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs create mode 100644 PointCloudWeb.Server/Tools/TEASERpp/README.md diff --git a/PointCloudWeb.Server/PointCloudWeb.Server.ScanConverter/PointCloudWeb.Server.ScanConverter.csproj b/PointCloudWeb.Server/PointCloudWeb.Server.ScanConverter/PointCloudWeb.Server.ScanConverter.csproj index 8dc6818..6256707 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server.ScanConverter/PointCloudWeb.Server.ScanConverter.csproj +++ b/PointCloudWeb.Server/PointCloudWeb.Server.ScanConverter/PointCloudWeb.Server.ScanConverter.csproj @@ -5,6 +5,14 @@ netcoreapp3.1 + + x64 + + + + x64 + + diff --git a/PointCloudWeb.Server/PointCloudWeb.Server.Tests/PointCloudWeb.Server.Tests.csproj b/PointCloudWeb.Server/PointCloudWeb.Server.Tests/PointCloudWeb.Server.Tests.csproj index b8c75ff..7a495ef 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server.Tests/PointCloudWeb.Server.Tests.csproj +++ b/PointCloudWeb.Server/PointCloudWeb.Server.Tests/PointCloudWeb.Server.Tests.csproj @@ -5,6 +5,14 @@ false + + + x64 + + + + x64 + diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Globals.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Globals.cs index 381628e..0dbd6e0 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Globals.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Globals.cs @@ -1,4 +1,5 @@ -using System.IO; +using System; +using System.IO; namespace PointCloudWeb.Server { @@ -12,12 +13,24 @@ namespace PointCloudWeb.Server TempPath = basePath + "/temp"; CloudCompareExe = "C:/Program Files/CloudCompare/CloudCompare.exe"; LasToolsTxt2Las = basePath + "/PointCloudWeb.Server/Tools/LAStools/txt2las.exe"; + TeaserPp = basePath + "/PointCloudWeb.Server/Tools/TEASERpp/build/teaser_cpp_ply"; } - public static string LasToolsTxt2Las { get; } + + public static string LasToolsTxt2Las { get; } public static string PotreeDataPath { get; } public static string PotreeConverterExe { get; } public static string TempPath { get; } public static string CloudCompareExe { get; } + public static string TeaserPp { get; } + + public static string ToWslPath(string path) + { + path = path.Replace("\\", "/"); + if (path.StartsWith("C:")) + return path.Replace("C:", "/mnt/c"); + + throw new NotImplementedException(); + } } } \ No newline at end of file diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs index 6b680b0..b1c6a49 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Models/PointCloud.cs @@ -63,12 +63,20 @@ namespace PointCloudWeb.Server.Models public string ToStringXyz() { var stringBuilder = new StringBuilder(); - foreach (var point in Points) + + // var maxPoints = 5_000; + + var takeEvery = 1;//Points.Count / maxPoints; + + for (var i = 0; i < Points.Count; i++) { + if (i % takeEvery != 0) + continue; + var point = Points[i]; stringBuilder.AppendLine(string.Join(',', - (point.X).ToString(CultureInfo.InvariantCulture), - (point.Y).ToString(CultureInfo.InvariantCulture), - (point.Z).ToString(CultureInfo.InvariantCulture) + point.X.ToString(CultureInfo.InvariantCulture), + point.Y.ToString(CultureInfo.InvariantCulture), + point.Z.ToString(CultureInfo.InvariantCulture) ) ); } @@ -76,12 +84,55 @@ namespace PointCloudWeb.Server.Models return stringBuilder.ToString(); } + private string ToStringPly(int maxPoints) + { + var stringBuilder = new StringBuilder(); + stringBuilder.Append("ply\n"); + stringBuilder.Append("format ascii 1.0\n"); + + stringBuilder.Append($"element vertex COUNT_PLACEHOLDER {Points.Count}\n"); + stringBuilder.Append("property float x\n"); + stringBuilder.Append("property float y\n"); + stringBuilder.Append("property float z\n"); + stringBuilder.Append("end_header\n"); + + if (maxPoints == 0) + maxPoints = Points.Count; + + var takeEvery = Points.Count / maxPoints; + var count = 0; + + for (var i = 0; i < Points.Count; i++) + { + if (i % takeEvery != 0) + continue; + count += 1; + var point = Points[i]; + stringBuilder.Append(string.Join(' ', + point.X, + point.Y, + point.Z + ) + "\n" + ); + } + + stringBuilder.Replace("COUNT_PLACEHOLDER", count.ToString()); + + return stringBuilder.ToString(); + } + // ReSharper disable once MemberCanBePrivate.Global public void WriteToXyz(string fileName) { File.WriteAllText(fileName, ToStringXyz()); } + // ReSharper disable once MemberCanBePrivate.Global + public void WriteToPly(string fileName, int maxPoints) + { + File.WriteAllText(fileName, ToStringPly(maxPoints)); + } + public void WriteToLasCloudCompare(string fileName) { var fileNameXyz = Path.ChangeExtension(fileName, ".xyz"); diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/PointCloudWeb.Server.csproj b/PointCloudWeb.Server/PointCloudWeb.Server/PointCloudWeb.Server.csproj index e51f717..4f3f9f8 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/PointCloudWeb.Server.csproj +++ b/PointCloudWeb.Server/PointCloudWeb.Server/PointCloudWeb.Server.csproj @@ -4,6 +4,14 @@ netcoreapp3.1 + + x64 + + + + x64 + + diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Services/IPointCloudRegistrationService.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Services/IPointCloudRegistrationService.cs index 609e9d9..3f815c5 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Services/IPointCloudRegistrationService.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Services/IPointCloudRegistrationService.cs @@ -3,8 +3,19 @@ using System.Numerics; namespace PointCloudWeb.Server.Services { - public interface IPointCloudRegistationService + public class RegistrationResult { - public Matrix4x4 RegisterPointCloud(PointCloud source, PointCloud target); + public RegistrationResult() + { + Transformation = Vector3.Zero; + Rotation = Vector3.Zero; + } + + public Vector3 Transformation { get; set; } + public Vector3 Rotation { get; set; } + } + public interface IPointCloudRegistrationService + { + public RegistrationResult RegisterPointCloud(PointCloud source, PointCloud target); } } \ No newline at end of file diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs new file mode 100644 index 0000000..74555f3 --- /dev/null +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudRegistrationServiceTeaerPp.cs @@ -0,0 +1,46 @@ +using System; +using System.Diagnostics; +using System.Linq; +using PointCloudWeb.Server.Models; + +namespace PointCloudWeb.Server.Services +{ + public class PointCloudRegistrationServiceTeaerPp : IPointCloudRegistrationService + { + public RegistrationResult RegisterPointCloud(PointCloud source, PointCloud target) + { + var sourceFileName = Globals.TempPath + $"/{source.Id}.ply"; + var targetFileName = Globals.TempPath + $"/{target.Id}.ply"; + + var maxPoints = 5_000; + + 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 result = new RegistrationResult(); + + return result; + } + } +} \ No newline at end of file diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudService.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudService.cs index 7b7ecbd..3e041bb 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudService.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Services/PointCloudService.cs @@ -2,7 +2,6 @@ using System; using System.Collections.Generic; using System.Diagnostics; using System.IO; -using System.Numerics; using PointCloudWeb.Server.Models; using PointCloudWeb.Server.Utils; @@ -10,13 +9,13 @@ namespace PointCloudWeb.Server.Services { public class PointCloudService { - //private readonly IPointCloudRegistrationService pointCloudRegistration; + private readonly IPointCloudRegistrationService _pointCloudRegistration; private readonly PointCloudCollection _pointClouds; - public PointCloudService( /*IPointCloudRegistrationService pointCloudRegistration*/) + public PointCloudService( IPointCloudRegistrationService pointCloudRegistration) { _pointClouds = new PointCloudCollection(); - //this.pointCloudRegistration = pointCloudRegistration; + _pointCloudRegistration = pointCloudRegistration; InitSampleData(); } @@ -78,17 +77,18 @@ namespace PointCloudWeb.Server.Services { RaiseIfNotExists(id); - var pc = GetById(id); + var source = GetById(id); - pc.Transformation = Vector3.Zero; - pc.Rotation = Vector3.Zero; + //the first can't be registered + if (_pointClouds.IndexOf(source) == 0) + return; + + var target = _pointClouds[_pointClouds.IndexOf(source) - 1]; - // //the first can't be registered - // if (_pointClouds.IndexOf(pointCloud) == 0) - // return; - - //var transformation = pointCloudRegistration.RegisterPointCloud(pointCloud, pointClouds[0]); - //pointCloud.Transformation = transformation; + var result = _pointCloudRegistration.RegisterPointCloud(source, target); + + source.Rotation = result.Rotation; + source.Transformation = result.Transformation; } public void RegisterPointClouds() diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Startup.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Startup.cs index b71a987..0fd84ba 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Startup.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Startup.cs @@ -51,6 +51,7 @@ namespace PointCloudWeb.Server services.AddSingleton(); services.AddTransient(); services.AddTransient(); + services.AddTransient(); services.AddControllers(); } } diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Utils/EthTestData.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Utils/EthTestData.cs index eefb3d1..2823063 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Utils/EthTestData.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Utils/EthTestData.cs @@ -37,11 +37,11 @@ namespace PointCloudWeb.Server.Utils public static void CreateData(PointCloudService pointCloudService) { - var pc = pointCloudService.AddPointCloud(); + var pc = pointCloudService.AddPointCloud(new Guid("c4b9b7fc-0b97-4f52-ad1b-737aeca5ba97")); LoadPointCloudFromEthFile(pc, "ETH-Data/Hokuyo_0.csv"); pointCloudService.PointCloudCompleted(pc.Id); - pc = pointCloudService.AddPointCloud(); + pc = pointCloudService.AddPointCloud(new Guid("c620b175-ace8-42e5-bf29-55b6c99372bc")); LoadPointCloudFromEthFile(pc, "ETH-Data/Hokuyo_1.csv"); pointCloudService.PointCloudCompleted(pc.Id); } diff --git a/PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt b/PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt index 3677dbb..d938181 100644 --- a/PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt +++ b/PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10) project(teaser_cpp_ply) -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) find_package(Eigen3 REQUIRED) find_package(teaserpp REQUIRED) diff --git a/PointCloudWeb.Server/Tools/TEASERpp/README.md b/PointCloudWeb.Server/Tools/TEASERpp/README.md new file mode 100644 index 0000000..70165ff --- /dev/null +++ b/PointCloudWeb.Server/Tools/TEASERpp/README.md @@ -0,0 +1,3 @@ +`bun_zipper.ply` is taken from the [Stanford 3D Scanning Repository](https://graphics.stanford.edu/data/3Dscanrep/). + +In the `3dmatch_sample/` folder, a pair of scan from the `sun3d-home_at-home_at_scan1_2013_jan_1` scene in the 3DMatch dataset is provided. Descriptors calculated by using the `32_dim` pretrained network from [3DSmoothNet](https://github.com/zgojcic/3DSmoothNet) is also provided, together with the ground truth transformation. diff --git a/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc b/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc index a459825..c72e7a5 100644 --- a/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc +++ b/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc @@ -1,7 +1,9 @@ // An example showing TEASER++ registration with the Stanford bunny model #include #include +#include #include +#include #include @@ -10,70 +12,50 @@ // Macro constants for generating noise and outliers #define NOISE_BOUND 0.05 -#define N_OUTLIERS 1700 -#define OUTLIER_TRANSLATION_LB 5 -#define OUTLIER_TRANSLATION_UB 10 +// #define N_OUTLIERS 1700 +// #define OUTLIER_TRANSLATION_LB 5 +// #define OUTLIER_TRANSLATION_UB 10 -inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) { +inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) +{ return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0))); } -void addNoiseAndOutliers(Eigen::Matrix& tgt) { - // Add uniform noise - Eigen::Matrix noise = - Eigen::Matrix::Random(3, tgt.cols()) * NOISE_BOUND; - NOISE_BOUND / 2; - tgt = tgt + noise; +int main(int argc, char **argv) +{ + std::cout << "You have entered " << argc + << " arguments:" + << "\n"; - // Add outliers - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution<> dis2(0, tgt.cols() - 1); // pos of outliers - std::uniform_int_distribution<> dis3(OUTLIER_TRANSLATION_LB, - OUTLIER_TRANSLATION_UB); // random translation - std::vector expected_outlier_mask(tgt.cols(), false); - for (int i = 0; i < N_OUTLIERS; ++i) { - int c_outlier_idx = dis2(gen); - assert(c_outlier_idx < expected_outlier_mask.size()); - expected_outlier_mask[c_outlier_idx] = true; - tgt.col(c_outlier_idx).array() += dis3(gen); // random translation - } -} + for (int i = 0; i < argc; ++i) + std::cout << argv[i] << "\n"; + + auto src_fileName = argv[1]; + auto tgt_fileName = argv[2]; -int main() { // Load the .ply file teaser::PLYReader reader; teaser::PointCloud src_cloud; - auto status = reader.read("./example_data/bun_zipper_res3.ply", src_cloud); - int N = src_cloud.size(); + auto status = reader.read(src_fileName, src_cloud); + int src_cloud_size = src_cloud.size(); // Convert the point cloud to Eigen - Eigen::Matrix src(3, N); - for (size_t i = 0; i < N; ++i) { + Eigen::Matrix src(3, src_cloud_size); + for (size_t i = 0; i < src_cloud_size; ++i) + { src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z; } - // Homogeneous coordinates - Eigen::Matrix src_h; - src_h.resize(4, src.cols()); - src_h.topRows(3) = src; - src_h.bottomRows(1) = Eigen::Matrix::Ones(N); + teaser::PointCloud tgt_cloud; + status = reader.read(tgt_fileName, tgt_cloud); + int tgt_cloud_size = tgt_cloud.size(); - // Apply an arbitrary SE(3) transformation - Eigen::Matrix4d T; - // clang-format off - T << 9.96926560e-01, 6.68735757e-02, -4.06664421e-02, -1.15576939e-01, - -6.61289946e-02, 9.97617877e-01, 1.94008687e-02, -3.87705398e-02, - 4.18675510e-02, -1.66517807e-02, 9.98977765e-01, 1.14874890e-01, - 0, 0, 0, 1; - // clang-format on - - // Apply transformation - Eigen::Matrix tgt_h = T * src_h; - Eigen::Matrix tgt = tgt_h.topRows(3); - - // Add some noise & outliers - addNoiseAndOutliers(tgt); + // Convert the point cloud to Eigen + Eigen::Matrix tgt(3, tgt_cloud_size); + for (size_t i = 0; i < tgt_cloud_size; ++i) + { + tgt.col(i) << tgt_cloud[i].x, tgt_cloud[i].y, tgt_cloud[i].z; + } // Run TEASER++ registration // Prepare solver parameters @@ -99,21 +81,12 @@ int main() { std::cout << "=====================================" << std::endl; std::cout << " TEASER++ Results " << std::endl; std::cout << "=====================================" << std::endl; - std::cout << "Expected rotation: " << std::endl; - std::cout << T.topLeftCorner(3, 3) << std::endl; std::cout << "Estimated rotation: " << std::endl; std::cout << solution.rotation << std::endl; - std::cout << "Error (deg): " << getAngularError(T.topLeftCorner(3, 3), solution.rotation) - << std::endl; std::cout << std::endl; - std::cout << "Expected translation: " << std::endl; - std::cout << T.topRightCorner(3, 1) << std::endl; std::cout << "Estimated translation: " << std::endl; std::cout << solution.translation << std::endl; - std::cout << "Error (m): " << (T.topRightCorner(3, 1) - solution.translation).norm() << std::endl; std::cout << std::endl; - std::cout << "Number of correspondences: " << N << std::endl; - std::cout << "Number of outliers: " << N_OUTLIERS << std::endl; std::cout << "Time taken (s): " << std::chrono::duration_cast(end - begin).count() / 1000000.0