diff --git a/src/aliceVision/mesh/MeshIntersection.cpp b/src/aliceVision/mesh/MeshIntersection.cpp index b173cec24b..c2dce3fef2 100644 --- a/src/aliceVision/mesh/MeshIntersection.cpp +++ b/src/aliceVision/mesh/MeshIntersection.cpp @@ -88,5 +88,41 @@ bool MeshIntersection::peekNormal(Vec3 & output, const camera::IntrinsicBase & i return true; } +bool MeshIntersection::peekPointAndNormal(Vec3 & point, Vec3 & normal, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords) +{ + const Vec3 posCamera = _pose.center(); + const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0); + const Vec3 dir = (wdir - posCamera).normalized(); + + //Create geogram ray from alicevision ray + GEO::Ray ray; + ray.origin.x = posCamera.x(); + ray.origin.y = -posCamera.y(); + ray.origin.z = -posCamera.z(); + ray.direction.x = dir.x(); + ray.direction.y = -dir.y(); + ray.direction.z = -dir.z(); + + GEO::MeshFacetsAABB::Intersection intersection; + if (!_aabb.ray_nearest_intersection(ray, intersection)) + { + return false; + } + + const GEO::vec3 p = ray.origin + intersection.t * ray.direction; + + point.x() = p.x; + point.y() = -p.y; + point.z() = -p.z; + + const GEO::vec3 n = GEO::normalize(intersection.N); + + normal.x() = n.x; + normal.y() = -n.y; + normal.z() = -n.z; + + return true; +} + } } \ No newline at end of file diff --git a/src/aliceVision/mesh/MeshIntersection.hpp b/src/aliceVision/mesh/MeshIntersection.hpp index a0936cc526..bd1df86408 100644 --- a/src/aliceVision/mesh/MeshIntersection.hpp +++ b/src/aliceVision/mesh/MeshIntersection.hpp @@ -48,6 +48,16 @@ class MeshIntersection */ bool peekNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords); + /** + * @brief peek a point and get its normal on the mesh given a input camera observation + * @param point the output measured point + * @param normal the output measured normal + * @param intrinsic the camera intrinsics to use for ray computation + * @param imageCoords the camera observation we want to use to estimate its 'depth' + * @return true if the ray intersect the mesh. + */ + bool peekPointAndNormal(Vec3 & point, Vec3 & normal, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords); + private: GEO::Mesh _mesh; GEO::MeshFacetsAABB _aabb;