mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

change accessing order for column major (#112)

This commit is contained in:
koide3 2025-05-07 11:25:04 +09:00 committed by GitHub
parent df145cbb15
commit 3466ea263a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 32 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/small_gicp/ann/projective_search.hpp
View File

@ -110,15 +110,15 @@ private:
const int u = uv[0] * index_map.cols();
const int v = uv[1] * index_map.rows();
for (int dv = -search_window_v; dv <= search_window_v; dv++) {
const int v_clamped = border_v(v + dv, index_map.rows());
if (v_clamped < 0 || v_clamped >= index_map.rows()) {
for (int du = -search_window_h; du <= search_window_h; du++) {
const int u_clamped = border_h(u + du, index_map.cols());
if (u_clamped < 0 || u_clamped >= index_map.cols()) {
continue;
}
for (int du = -search_window_h; du <= search_window_h; du++) {
const int u_clamped = border_h(u + du, index_map.cols());
if (u_clamped < 0 || u_clamped >= index_map.cols()) {
for (int dv = -search_window_v; dv <= search_window_v; dv++) {
const int v_clamped = border_v(v + dv, index_map.rows());
if (v_clamped < 0 || v_clamped >= index_map.rows()) {
continue;
}

26
include/small_gicp/points/eigen.hpp
View File

@ -2,6 +2,7 @@
// SPDX-License-Identifier: MIT
#pragma once
#include <vector>
#include <Eigen/Core>
#include <small_gicp/points/traits.hpp>
@ -15,5 +16,30 @@ struct Traits<Eigen::MatrixXd> {
static Eigen::Vector4d point(const Eigen::MatrixXd& points, size_t i) { return Eigen::Vector4d(points(i, 0), points(i, 1), points(i, 2), 1.0); }
};
template <typename Scalar, int D>
struct Traits<std::vector<Eigen::Matrix<Scalar, D, 1>>> {
static_assert(D == 3 || D == 4, "Only 3D and 4D (homogeneous) points are supported");
static_assert(std::is_floating_point<Scalar>::value, "Only floating point types are supported");
using Point = Eigen::Matrix<Scalar, D, 1>;
static size_t size(const std::vector<Point>& points) { return points.size(); }
static bool has_points(const std::vector<Point>& points) { return points.size(); }
static Eigen::Vector4d point(const std::vector<Point>& points, size_t i) {
if constexpr (std::is_same_v<Scalar, double>) {
if constexpr (D == 3) {
return points[i].homogeneous();
} else {
return points[i];
}
} else {
if constexpr (D == 3) {
return points[i].homogeneous().template cast<double>();
} else {
return points[i].template cast<double>();
}
}
}
};
} // namespace traits
} // namespace small_gicp