From e8557230923ac9b3e76771a08d10fc78b6a7d450 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Fri, 6 Sep 2019 10:31:40 +0200 Subject: [PATCH] Added function for getting normals at a set of points on the mesh --- src/bathy_maps/include/bathy_maps/mesh_map.h | 1 + src/bathy_maps/src/mesh_map.cpp | 20 ++++++++++++++++++++ src/pybathy_maps/src/pymesh_map.cpp | 1 + 3 files changed, 22 insertions(+) diff --git a/src/bathy_maps/include/bathy_maps/mesh_map.h b/src/bathy_maps/include/bathy_maps/mesh_map.h index a9fa9faf..283b0d3f 100644 --- a/src/bathy_maps/include/bathy_maps/mesh_map.h +++ b/src/bathy_maps/include/bathy_maps/mesh_map.h @@ -48,6 +48,7 @@ namespace mesh_map { std::tuple mesh_and_normals_from_pings(const std_data::mbes_ping::PingsT& pings, double res); Eigen::MatrixXd shade_image_from_normals(const Eigen::MatrixXd& N, const BoundsT& bounds, double res, const Eigen::Vector3d& light_dir); Eigen::MatrixXd compute_normals(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F); + Eigen::MatrixXd normals_at_points(const Eigen::MatrixXd& points, const Eigen::MatrixXd& N, const BoundsT& bounds, double res); void write_dae_mesh(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, const boost::filesystem::path& filename); void write_dae_mesh_from_str(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, const std::string& filename); diff --git a/src/bathy_maps/src/mesh_map.cpp b/src/bathy_maps/src/mesh_map.cpp index cd57ff5d..05f4cbfa 100644 --- a/src/bathy_maps/src/mesh_map.cpp +++ b/src/bathy_maps/src/mesh_map.cpp @@ -526,4 +526,24 @@ Eigen::MatrixXd shade_image_from_normals(const Eigen::MatrixXd& N, const BoundsT return shade_image; } +Eigen::MatrixXd normals_at_points(const Eigen::MatrixXd& points, const Eigen::MatrixXd& N, const BoundsT& bounds, double res) +{ + int cols = std::ceil((bounds(1, 0) - bounds(0, 0)) / res); // min/max should be in the center + int rows = std::ceil((bounds(1, 1) - bounds(0, 1)) / res); + + cout << "Rows: " << rows << ", Cols: " << cols << endl; + + Eigen::MatrixXd N_points = Eigen::MatrixXd::Zero(points.rows(), points.cols()); + + for (int i = 0; i < points.rows(); ++i) { + //int x = int((points(i, 0)-bounds(0, 0))/res); + //int y = int((points(i, 1)-bounds(0, 1))/res); + int x = int(points(i, 0)/res); + int y = int(points(i, 1)/res); + N_points.row(i) = N.row(y*cols+x); + } + + return N_points; +} + } // namespace mesh_map diff --git a/src/pybathy_maps/src/pymesh_map.cpp b/src/pybathy_maps/src/pymesh_map.cpp index 887c1f54..eb39e80a 100644 --- a/src/pybathy_maps/src/pymesh_map.cpp +++ b/src/pybathy_maps/src/pymesh_map.cpp @@ -40,5 +40,6 @@ PYBIND11_MODULE(mesh_map, m) { m.def("mesh_and_normals_from_pings", &mesh_map::mesh_and_normals_from_pings, "Get vertices, faces, normals and bounds form mbes_ping::PingsT"); m.def("shade_image_from_normals", &mesh_map::shade_image_from_normals, "Compute [0, 1] shade image from normals and lighting direction"); m.def("compute_normals", &mesh_map::compute_normals, "Compute normals from the mesh, per vertex"); + m.def("normals_at_points", &mesh_map::normals_at_points, "Get the normals at a set of points on the mesh"); }