From f4c340f9fb646ecc43c313c3a832ef43eb05ac58 Mon Sep 17 00:00:00 2001 From: SunDude Date: Sun, 7 Apr 2024 08:23:30 +1200 Subject: [PATCH] fixed translational matrix initialisation to eliminate unnecessary addition of [1,0,0] to output translation --- src/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index db922c7..2eee1a5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -236,7 +236,7 @@ void icp(MatrixXd mat1, MatrixXd mat2, int max_iteration_num) { my_kd_tree_t mat_index(3, std::cref(mat2), 10 /* max leaf */); mat_index.index->buildIndex(); Mat rotation_matrix = Mat::eye(3, 3, CV_64F); - Mat translation_matrix = Mat::eye(3, 1, CV_64F); + Mat translation_matrix = Mat::zeros(3, 1, CV_64F); for (int iters_ = 0; iters_ < max_iters_; iters_++) { findNearestNeighbours(cvSource, mat_index, cvSource.rows, 3); Mat cvNewTarget = Mat::zeros(cvSource.rows, 3, CV_64F); @@ -296,7 +296,7 @@ void tricp(MatrixXd mat1, MatrixXd mat2, int max_iteration_num) { int max_iters_ = max_iteration_num;//100; my_kd_tree_t mat_index(3, std::cref(mat2), 10 /* max leaf */); Mat rotation_matrix = Mat::eye(3, 3, CV_64F); - Mat translation_matrix = Mat::eye(3, 1, CV_64F); + Mat translation_matrix = Mat::zeros(3, 1, CV_64F); mat_index.index->buildIndex(); for (int iters_ = 0; iters_ < max_iters_; iters_++) {