From ae4f1b9080cfc236ed5d8a853e658084a1e423d7 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Thu, 5 Mar 2026 15:46:10 +0100 Subject: [PATCH 01/17] Add the IVUS project to Infinytoolkit. --- CMakeLists.txt | 14 ++++++++++++++ src/InfinyToolkit/initInfinyToolkit.cpp | 6 ++++++ 2 files changed, 20 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 15a7fbe..e82420a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,10 @@ find_package(Sofa.Component.Haptics REQUIRED) find_package(Sofa.GUI.Component REQUIRED) find_package(Sofa.Component.Engine.Select REQUIRED) +# Tell CMake where OpenCV is +set(OpenCV_DIR "C:/softwares/opencv-4.12.0/opencv/build") # Adjust if your path is different +find_package(OpenCV REQUIRED) + set(USE_INFINYTOOLKIT_PLUGIN true CACHE BOOL "Use Interaction Tools plugin") @@ -77,6 +81,9 @@ set(HEADER_FILES ## Replay motion controller ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.h + ## IVUS controller + ${INFINYTOOLKIT_SRC_DIR}/IVUSController/IVUSController.h + ) @@ -116,6 +123,9 @@ set(SOURCE_FILES ## Replay motion controller ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.cpp + ## IVUS controller + ${INFINYTOOLKIT_SRC_DIR}/IVUSController/IVUSController.cpp + ) @@ -156,6 +166,10 @@ target_link_libraries(${PROJECT_NAME} Sofa.Component.Haptics Sofa.GUI.Component Sofa.Component.Engine.Select + + # <-- Add OpenCV here + ${OpenCV_LIBS} + ) diff --git a/src/InfinyToolkit/initInfinyToolkit.cpp b/src/InfinyToolkit/initInfinyToolkit.cpp index abfdaaa..8012440 100644 --- a/src/InfinyToolkit/initInfinyToolkit.cpp +++ b/src/InfinyToolkit/initInfinyToolkit.cpp @@ -56,6 +56,9 @@ extern void registerRotationEngine(sofa::core::ObjectFactory* factory); // Heart Motion Replayer extern void registerMotionReplayController(sofa::core::ObjectFactory* factory); +// Intravascular Ultrasound +extern void registerIVUSController(sofa::core::ObjectFactory* factory); + } // namespace sofa::infinytoolkit @@ -145,6 +148,9 @@ void registerObjects(sofa::core::ObjectFactory* factory) // Heart Motion Replayer registerMotionReplayController(factory); + + // Intravascular Ultrasound + registerIVUSController(factory); } } // namespace sofa::component From 7dc83141ce4ae9ceef79a2254ddd96ff2e6ffcd6 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Thu, 5 Mar 2026 15:47:39 +0100 Subject: [PATCH 02/17] Add IVUS controller directory. --- .../IVUSController/IVUSController.cpp | 227 ++++++++++++++++++ .../IVUSController/IVUSController.h | 109 +++++++++ 2 files changed, 336 insertions(+) create mode 100644 src/InfinyToolkit/IVUSController/IVUSController.cpp create mode 100644 src/InfinyToolkit/IVUSController/IVUSController.h diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp new file mode 100644 index 0000000..52ba2e8 --- /dev/null +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -0,0 +1,227 @@ +#include +#include< sofa/component/collision/geometry/TriangleCollisionModel.h > +#include +#include +#include +#include +#include + +using sofa::defaulttype::Vec3dTypes; + + + +namespace sofa::infinytoolkit +{ + + using namespace sofa::defaulttype; + using namespace sofa::core::behavior; + using namespace sofa::defaulttype; + using namespace sofa::component::collision::geometry; + using namespace sofa::component::topology::container::dynamic; + + + void registerIVUSController(sofa::core::ObjectFactory* factory) + { + factory->registerObjects( + sofa::core::ObjectRegistrationData("") + .add< IVUSController >() + ); + } + void IVUSController::init() + { + // Resolve the MechanicalState of the catheter + if (l_CatheterState.get() == nullptr) + { + msg_error() << "Error no catheter is found!"; + this->d_componentState.setValue( + sofa::core::objectmodel::ComponentState::Invalid); + return; + + } + + if (l_triangleGeo.empty()) + { + msg_info() << "No TriangleSetGeometryAlgorithms link set. " + << "Trying to find first one in current context..."; + //l_triangleGeo.set(this->getContext()->getNodeObject>()); //will back to it + } + + currentFrame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + } + + void IVUSController::handleEvent(sofa::core::objectmodel::Event* event) + { + + + auto* mech = l_CatheterState.get(); + const VecCoord& positions = mech->readPositions(); + + // Extract last K points near tip + std::vector probePositions; + + unsigned int K = std::min( + static_cast(K_points), // should be passed by getValue + static_cast(positions.size()) + ); + + for (unsigned int k = 0; k < K; ++k) + { + const Coord& rigid = positions[positions.size() - 1 - k]; + probePositions.push_back(rigid.getCenter()); + } + + + // Accumulate frames for multi-point averaging + cv::Mat accumulated = cv::Mat::zeros(N_depth, N_rays, CV_64F); + + for (auto& probePos : probePositions) + { + cv::Mat tempFrame = computeSingleProbeFrame(probePos); + tempFrame.convertTo(tempFrame, CV_64F); + accumulated += tempFrame; + } + + // Average over K points + accumulated /= static_cast(K); + accumulated.convertTo(currentFrame, CV_8UC1); + + // Add Gaussian noise + cv::Mat noise(currentFrame.size(), CV_8UC1); + cv::randn(noise, 0, noiseSigma); + currentFrame += noise; + + // Store frame + ivusFrames.push_back(currentFrame.clone()); + if (ivusFrames.size() > maxStoredFrames) + ivusFrames.erase(ivusFrames.begin()); + + // Build longitudinal image (side view) + longitudinalImage = cv::Mat::zeros(N_depth, ivusFrames.size(), CV_8UC1); + unsigned int selectedAngle = N_rays / 2; // central radial slice + for (size_t t = 0; t < ivusFrames.size(); ++t) + { + for (unsigned int d = 0; d < N_depth; ++d) + { + longitudinalImage.at(d, t) = + ivusFrames[t].at(d, selectedAngle); + } + } + + // Display images + cv::imshow("IVUS Cross Section", currentFrame); + cv::imshow("IVUS Longitudinal", longitudinalImage); + cv::waitKey(1); + } + + cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) + { + cv::Mat frame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + + // build roi + std::vector roiTriangles; + IVUSController::buildROI(probepos, roiTriangles); + + auto* trimodel = vesselNode->getNodeObject>(); + + // Mechanical positions (Rigid3d coordinates) + const auto& positions = trimodel->getX(); // VecCoord + const auto& triangles = trimodel->getTriangles(); // SeqTriangles from topology + + // Temporary storage required by computeIntersectionsLineTriangle + sofa::type::vector indices; + sofa::type::vector vecBaryCoef; + sofa::type::vector vecCoordKmin; + + + for (unsigned int i = 0; i < N_rays; ++i) + { + double angle = 2.0 * std::numbers::pi * i / N_rays; + Vec3 dir(cos(angle), sin(angle), 0.0); + + Vec3 p1 = probepos; + Vec3 p2 = probepos + dir * maxDepth; + + double nearestHit = maxDepth; + + // Loop over triangles in ROI + for (auto triIndex : roiTriangles) + { + bool hit = l_triangleGeo->computeIntersectionsLineTriangle( + false, // is_entered + sofa::type::Vec<3, double>(p1[0], p1[1], p1[2]), + sofa::type::Vec<3, double>(p2[0], p2[1], p2[2]), + triIndex, + indices, + vecBaryCoef, + vecCoordKmin + ); + + if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) + { + nearestHit = vecCoordKmin[0]; + } + + // Clear vectors for next triangle + indices.clear(); + vecBaryCoef.clear(); + vecCoordKmin.clear(); + } + + // Fill ultrasound frame + for (unsigned int j = 0; j < N_depth; ++j) + { + double depth = maxDepth * j / N_depth; + double attenuation = exp(-alpha * depth); + double intensity = 40 * attenuation; + + if (std::abs(depth - nearestHit) < (maxDepth / N_depth)) + intensity = 255 * reflectionCoeff * attenuation; + + frame.at(j, i) = static_cast(std::min(255.0, intensity)); + } + } + + return frame; + + } + + void IVUSController::buildROI( + const Vec3& probepos, + std::vector& triangleIndices) + { + triangleIndices.clear(); + auto* triModel = vesselNode->getNodeObject>(); + if (!triModel) + { + msg_error() << "No TriangleCollisionModel found in vesselNode!"; + return; + } + + // Access mechanical object positions + const auto& positions = triModel->getX(); // This is a vector of Rigid3dTypes::Coord (or Vec3) + + // Access topology triangles + const auto& triangles = triModel->getTriangles(); // SeqTriangles from BaseMeshTopology + + for (sofa::Index i = 0; i < triangles.size(); ++i) + { + const auto& tri = triangles[i]; // tri is an array of 3 vertex indices + + // Get vertex positions from mechanical state + const auto& v0 = positions[tri[0]]; + const auto& v1 = positions[tri[1]]; + const auto& v2 = positions[tri[2]]; + + // Use the first vertex or the centroid to check distance + Vec3 center = (v0 + v1 + v2) / 3.0; + + if ((center - probepos).norm() < maxDepth) + triangleIndices.push_back(i); + } + + + } + + + +} diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h new file mode 100644 index 0000000..91ceb93 --- /dev/null +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -0,0 +1,109 @@ +/***************************************************************************** + * - Copyright (C) - 2020 - InfinyTech3D - * + * * + * This file is part of the InfinyToolkit plugin for the SOFA framework * + * * + * Commercial License Usage: * + * Licensees holding valid commercial license from InfinyTech3D may use this * + * file in accordance with the commercial license agreement provided with * + * the Software or, alternatively, in accordance with the terms contained in * + * a written agreement between you and InfinyTech3D. For further information * + * on the licensing terms and conditions, contact: contact@infinytech3d.com * + * * + * GNU General Public License Usage: * + * Alternatively, this file may be used under the terms of the GNU General * + * Public License version 3. The licenses are as published by the Free * + * Software Foundation and appearing in the file LICENSE.GPL3 included in * + * the packaging of this file. Please review the following information to * + * ensure the GNU General Public License requirements will be met: * + * https://www.gnu.org/licenses/gpl-3.0.html. * + * * + * Authors: see Authors.txt * + * Further information: https://infinytech3d.com * + ****************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include // Not sure I will use it +#include + +#include +#include +#include +#include +#include +#include +#include + + +namespace sofa::infinytoolkit +{ + + using namespace sofa::core::objectmodel; + using namespace sofa::component::topology::container::dynamic; + + class IVUSController : public sofa::component::controller::Controller + { + public: + SOFA_CLASS(IVUSController, sofa::component::controller::Controller); + + using DataTypes = sofa::defaulttype::Rigid3dTypes; + + using Coord = DataTypes::Coord; + using VecCoord = DataTypes::VecCoord; + using Vec3 = DataTypes::Vec3; + using Quat = DataTypes::Quat; + + + + // Scene references , nor sure if I use them + sofa::simulation::Node* catheterNode = nullptr; + sofa::simulation::Node* vesselNode = nullptr; + + + // IVUS parameters + unsigned int N_rays = 128; + unsigned int N_depth = 256; + double maxDepth = 10.0; + unsigned int K_points = 3; // points near tip to simulate finite transducer length + + double alpha = 0.15; // attenuation coeicient + double reflectionCoeff = 1.5; // reflection boost + double noiseSigma = 10.0; // Gaussian noise sigma + + unsigned int maxStoredFrames = 400; + + // Images + cv::Mat currentFrame; // cross-sectional frame + cv::Mat longitudinalImage; // longitudinal side view + std::vector ivusFrames; // history buffer + + void init() override; + void handleEvent(sofa::core::objectmodel::Event* event) override; + + private: + + SingleLink< IVUSController, + sofa::core::behavior::MechanicalState, + BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_CatheterState; + + + SingleLink, + BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_triangleGeo; + + + // void computeIVUSFrame(); + cv::Mat computeSingleProbeFrame(const Vec3& probePos); + void buildROI( + const Vec3& probePos, + std::vector& triangleIndices); + + }; +} + + + From 51fb515dbdba8bf7bd39bab917e2fcb70b735495 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 9 Mar 2026 09:54:54 +0100 Subject: [PATCH 03/17] [ci] Fix ci trigger option to activate tests on PR coming from external heads --- .github/workflows/ci.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7969670..e926638 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,6 +2,11 @@ name: CI on: push: + branches: + - main + pull_request: + branches: + - main jobs: build-and-test: From 8b824998a80cb920929f9c878cc7b123d38d13e9 Mon Sep 17 00:00:00 2001 From: erik pernod Date: Mon, 9 Mar 2026 11:00:12 +0100 Subject: [PATCH 04/17] [ci] Fix ci trigger option to activate tests on PR coming from external heads (#48) --- .github/workflows/ci.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7969670..e926638 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,6 +2,11 @@ name: CI on: push: + branches: + - main + pull_request: + branches: + - main jobs: build-and-test: From f36c17cf1b9da55875a9e77d8c17a70e0e522bbf Mon Sep 17 00:00:00 2001 From: rmolazem Date: Mon, 9 Mar 2026 11:18:25 +0100 Subject: [PATCH 05/17] Add constructor for passing the data. --- .../IVUSController/IVUSController.cpp | 53 ++++++++++++------- .../IVUSController/IVUSController.h | 32 +++++++---- 2 files changed, 57 insertions(+), 28 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 52ba2e8..56e52ae 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -27,6 +27,19 @@ namespace sofa::infinytoolkit .add< IVUSController >() ); } + IVUSController::IVUSController() + : d_N_rays(initData(&d_N_rays, (unsigned int)128, "N_rays", "Number of angular rays")) + , d_N_depth(initData(&d_N_depth, (unsigned int)256, "N_depth", "Number of radial depth samples")) + , d_maxDepth(initData(&d_maxDepth, 10.0, "maxDepth", "Maximum probe depth")) + , d_alpha(initData(&d_alpha, 0.15, "alpha", "Attenuation coefficient")) + , d_reflectionCoeff(initData(&d_reflectionCoeff, 1.5, "reflectionCoeff", "Reflection boost")) + , d_noiseSigma(initData(&d_noiseSigma, 10.0, "noiseSigma", "Gaussian noise sigma")) + , d_K_points(initData(&d_K_points, (unsigned int)3, "K_points", "Points near probe tip")) + , d_maxStoredFrames(initData(&d_maxStoredFrames, (unsigned int)400, "maxStoredFrames", "Number of stored IVUS frames")) + { + } + + void IVUSController::init() { // Resolve the MechanicalState of the catheter @@ -45,8 +58,10 @@ namespace sofa::infinytoolkit << "Trying to find first one in current context..."; //l_triangleGeo.set(this->getContext()->getNodeObject>()); //will back to it } + + this->f_listening.setValue(true); - currentFrame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + currentFrame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); } void IVUSController::handleEvent(sofa::core::objectmodel::Event* event) @@ -60,7 +75,7 @@ namespace sofa::infinytoolkit std::vector probePositions; unsigned int K = std::min( - static_cast(K_points), // should be passed by getValue + static_cast(d_K_points.getValue()), static_cast(positions.size()) ); @@ -72,7 +87,7 @@ namespace sofa::infinytoolkit // Accumulate frames for multi-point averaging - cv::Mat accumulated = cv::Mat::zeros(N_depth, N_rays, CV_64F); + cv::Mat accumulated = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_64F); for (auto& probePos : probePositions) { @@ -87,20 +102,20 @@ namespace sofa::infinytoolkit // Add Gaussian noise cv::Mat noise(currentFrame.size(), CV_8UC1); - cv::randn(noise, 0, noiseSigma); + cv::randn(noise, 0, d_noiseSigma.getValue()); currentFrame += noise; // Store frame ivusFrames.push_back(currentFrame.clone()); - if (ivusFrames.size() > maxStoredFrames) + if (ivusFrames.size() > d_maxStoredFrames.getValue()) ivusFrames.erase(ivusFrames.begin()); // Build longitudinal image (side view) - longitudinalImage = cv::Mat::zeros(N_depth, ivusFrames.size(), CV_8UC1); - unsigned int selectedAngle = N_rays / 2; // central radial slice + longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); + unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice for (size_t t = 0; t < ivusFrames.size(); ++t) { - for (unsigned int d = 0; d < N_depth; ++d) + for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) { longitudinalImage.at(d, t) = ivusFrames[t].at(d, selectedAngle); @@ -115,7 +130,7 @@ namespace sofa::infinytoolkit cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) { - cv::Mat frame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + cv::Mat frame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); // build roi std::vector roiTriangles; @@ -133,15 +148,15 @@ namespace sofa::infinytoolkit sofa::type::vector vecCoordKmin; - for (unsigned int i = 0; i < N_rays; ++i) + for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) { - double angle = 2.0 * std::numbers::pi * i / N_rays; + double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); Vec3 dir(cos(angle), sin(angle), 0.0); Vec3 p1 = probepos; - Vec3 p2 = probepos + dir * maxDepth; + Vec3 p2 = probepos + dir * d_maxDepth.getValue(); - double nearestHit = maxDepth; + double nearestHit = d_maxDepth.getValue(); // Loop over triangles in ROI for (auto triIndex : roiTriangles) @@ -168,14 +183,14 @@ namespace sofa::infinytoolkit } // Fill ultrasound frame - for (unsigned int j = 0; j < N_depth; ++j) + for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) { - double depth = maxDepth * j / N_depth; - double attenuation = exp(-alpha * depth); + double depth = d_maxDepth.getValue() * j / d_N_depth.getValue(); + double attenuation = exp(-d_alpha.getValue() * depth); double intensity = 40 * attenuation; - if (std::abs(depth - nearestHit) < (maxDepth / N_depth)) - intensity = 255 * reflectionCoeff * attenuation; + if (std::abs(depth - nearestHit) < (d_maxDepth.getValue() / d_N_depth.getValue())) + intensity = 255 * d_reflectionCoeff.getValue() * attenuation; frame.at(j, i) = static_cast(std::min(255.0, intensity)); } @@ -215,7 +230,7 @@ namespace sofa::infinytoolkit // Use the first vertex or the centroid to check distance Vec3 center = (v0 + v1 + v2) / 3.0; - if ((center - probepos).norm() < maxDepth) + if ((center - probepos).norm() < d_maxDepth.getValue()) triangleIndices.push_back(i); } diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index 91ceb93..dba6422 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -44,12 +44,16 @@ namespace sofa::infinytoolkit using namespace sofa::core::objectmodel; using namespace sofa::component::topology::container::dynamic; + using sofa::core::objectmodel::Data; class IVUSController : public sofa::component::controller::Controller { public: SOFA_CLASS(IVUSController, sofa::component::controller::Controller); + IVUSController(); + ~IVUSController() override = default; + using DataTypes = sofa::defaulttype::Rigid3dTypes; using Coord = DataTypes::Coord; @@ -65,16 +69,26 @@ namespace sofa::infinytoolkit // IVUS parameters - unsigned int N_rays = 128; - unsigned int N_depth = 256; - double maxDepth = 10.0; - unsigned int K_points = 3; // points near tip to simulate finite transducer length - - double alpha = 0.15; // attenuation coeicient - double reflectionCoeff = 1.5; // reflection boost - double noiseSigma = 10.0; // Gaussian noise sigma + //unsigned int N_rays = 128; + //unsigned int N_depth = 256; + //double maxDepth = 10.0; + //unsigned int K_points = 3; // points near tip to simulate finite transducer length + + //double alpha = 0.15; // attenuation coeicient + //double reflectionCoeff = 1.5; // reflection boost + //double noiseSigma = 10.0; // Gaussian noise sigma + + //unsigned int maxStoredFrames = 400; + + Data d_N_rays; + Data d_N_depth; + Data d_maxDepth; + Data d_alpha; + Data d_reflectionCoeff; + Data d_noiseSigma; + Data d_K_points; + Data d_maxStoredFrames; - unsigned int maxStoredFrames = 400; // Images cv::Mat currentFrame; // cross-sectional frame From cee1a6a8cff9bcbd1bfec73f8cb361e5dd1c1f79 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Thu, 5 Mar 2026 15:46:10 +0100 Subject: [PATCH 06/17] Add the IVUS project to Infinytoolkit. --- CMakeLists.txt | 14 ++++++++++++++ src/InfinyToolkit/initInfinyToolkit.cpp | 6 ++++++ 2 files changed, 20 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 15a7fbe..e82420a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,10 @@ find_package(Sofa.Component.Haptics REQUIRED) find_package(Sofa.GUI.Component REQUIRED) find_package(Sofa.Component.Engine.Select REQUIRED) +# Tell CMake where OpenCV is +set(OpenCV_DIR "C:/softwares/opencv-4.12.0/opencv/build") # Adjust if your path is different +find_package(OpenCV REQUIRED) + set(USE_INFINYTOOLKIT_PLUGIN true CACHE BOOL "Use Interaction Tools plugin") @@ -77,6 +81,9 @@ set(HEADER_FILES ## Replay motion controller ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.h + ## IVUS controller + ${INFINYTOOLKIT_SRC_DIR}/IVUSController/IVUSController.h + ) @@ -116,6 +123,9 @@ set(SOURCE_FILES ## Replay motion controller ${INFINYTOOLKIT_SRC_DIR}/MotionReplayController/MotionReplayController.cpp + ## IVUS controller + ${INFINYTOOLKIT_SRC_DIR}/IVUSController/IVUSController.cpp + ) @@ -156,6 +166,10 @@ target_link_libraries(${PROJECT_NAME} Sofa.Component.Haptics Sofa.GUI.Component Sofa.Component.Engine.Select + + # <-- Add OpenCV here + ${OpenCV_LIBS} + ) diff --git a/src/InfinyToolkit/initInfinyToolkit.cpp b/src/InfinyToolkit/initInfinyToolkit.cpp index abfdaaa..8012440 100644 --- a/src/InfinyToolkit/initInfinyToolkit.cpp +++ b/src/InfinyToolkit/initInfinyToolkit.cpp @@ -56,6 +56,9 @@ extern void registerRotationEngine(sofa::core::ObjectFactory* factory); // Heart Motion Replayer extern void registerMotionReplayController(sofa::core::ObjectFactory* factory); +// Intravascular Ultrasound +extern void registerIVUSController(sofa::core::ObjectFactory* factory); + } // namespace sofa::infinytoolkit @@ -145,6 +148,9 @@ void registerObjects(sofa::core::ObjectFactory* factory) // Heart Motion Replayer registerMotionReplayController(factory); + + // Intravascular Ultrasound + registerIVUSController(factory); } } // namespace sofa::component From 8a7d090cd5dc2e33b540f97a2673bc0ff0e65ac9 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Thu, 5 Mar 2026 15:47:39 +0100 Subject: [PATCH 07/17] Add IVUS controller directory. --- .../IVUSController/IVUSController.cpp | 227 ++++++++++++++++++ .../IVUSController/IVUSController.h | 109 +++++++++ 2 files changed, 336 insertions(+) create mode 100644 src/InfinyToolkit/IVUSController/IVUSController.cpp create mode 100644 src/InfinyToolkit/IVUSController/IVUSController.h diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp new file mode 100644 index 0000000..52ba2e8 --- /dev/null +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -0,0 +1,227 @@ +#include +#include< sofa/component/collision/geometry/TriangleCollisionModel.h > +#include +#include +#include +#include +#include + +using sofa::defaulttype::Vec3dTypes; + + + +namespace sofa::infinytoolkit +{ + + using namespace sofa::defaulttype; + using namespace sofa::core::behavior; + using namespace sofa::defaulttype; + using namespace sofa::component::collision::geometry; + using namespace sofa::component::topology::container::dynamic; + + + void registerIVUSController(sofa::core::ObjectFactory* factory) + { + factory->registerObjects( + sofa::core::ObjectRegistrationData("") + .add< IVUSController >() + ); + } + void IVUSController::init() + { + // Resolve the MechanicalState of the catheter + if (l_CatheterState.get() == nullptr) + { + msg_error() << "Error no catheter is found!"; + this->d_componentState.setValue( + sofa::core::objectmodel::ComponentState::Invalid); + return; + + } + + if (l_triangleGeo.empty()) + { + msg_info() << "No TriangleSetGeometryAlgorithms link set. " + << "Trying to find first one in current context..."; + //l_triangleGeo.set(this->getContext()->getNodeObject>()); //will back to it + } + + currentFrame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + } + + void IVUSController::handleEvent(sofa::core::objectmodel::Event* event) + { + + + auto* mech = l_CatheterState.get(); + const VecCoord& positions = mech->readPositions(); + + // Extract last K points near tip + std::vector probePositions; + + unsigned int K = std::min( + static_cast(K_points), // should be passed by getValue + static_cast(positions.size()) + ); + + for (unsigned int k = 0; k < K; ++k) + { + const Coord& rigid = positions[positions.size() - 1 - k]; + probePositions.push_back(rigid.getCenter()); + } + + + // Accumulate frames for multi-point averaging + cv::Mat accumulated = cv::Mat::zeros(N_depth, N_rays, CV_64F); + + for (auto& probePos : probePositions) + { + cv::Mat tempFrame = computeSingleProbeFrame(probePos); + tempFrame.convertTo(tempFrame, CV_64F); + accumulated += tempFrame; + } + + // Average over K points + accumulated /= static_cast(K); + accumulated.convertTo(currentFrame, CV_8UC1); + + // Add Gaussian noise + cv::Mat noise(currentFrame.size(), CV_8UC1); + cv::randn(noise, 0, noiseSigma); + currentFrame += noise; + + // Store frame + ivusFrames.push_back(currentFrame.clone()); + if (ivusFrames.size() > maxStoredFrames) + ivusFrames.erase(ivusFrames.begin()); + + // Build longitudinal image (side view) + longitudinalImage = cv::Mat::zeros(N_depth, ivusFrames.size(), CV_8UC1); + unsigned int selectedAngle = N_rays / 2; // central radial slice + for (size_t t = 0; t < ivusFrames.size(); ++t) + { + for (unsigned int d = 0; d < N_depth; ++d) + { + longitudinalImage.at(d, t) = + ivusFrames[t].at(d, selectedAngle); + } + } + + // Display images + cv::imshow("IVUS Cross Section", currentFrame); + cv::imshow("IVUS Longitudinal", longitudinalImage); + cv::waitKey(1); + } + + cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) + { + cv::Mat frame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + + // build roi + std::vector roiTriangles; + IVUSController::buildROI(probepos, roiTriangles); + + auto* trimodel = vesselNode->getNodeObject>(); + + // Mechanical positions (Rigid3d coordinates) + const auto& positions = trimodel->getX(); // VecCoord + const auto& triangles = trimodel->getTriangles(); // SeqTriangles from topology + + // Temporary storage required by computeIntersectionsLineTriangle + sofa::type::vector indices; + sofa::type::vector vecBaryCoef; + sofa::type::vector vecCoordKmin; + + + for (unsigned int i = 0; i < N_rays; ++i) + { + double angle = 2.0 * std::numbers::pi * i / N_rays; + Vec3 dir(cos(angle), sin(angle), 0.0); + + Vec3 p1 = probepos; + Vec3 p2 = probepos + dir * maxDepth; + + double nearestHit = maxDepth; + + // Loop over triangles in ROI + for (auto triIndex : roiTriangles) + { + bool hit = l_triangleGeo->computeIntersectionsLineTriangle( + false, // is_entered + sofa::type::Vec<3, double>(p1[0], p1[1], p1[2]), + sofa::type::Vec<3, double>(p2[0], p2[1], p2[2]), + triIndex, + indices, + vecBaryCoef, + vecCoordKmin + ); + + if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) + { + nearestHit = vecCoordKmin[0]; + } + + // Clear vectors for next triangle + indices.clear(); + vecBaryCoef.clear(); + vecCoordKmin.clear(); + } + + // Fill ultrasound frame + for (unsigned int j = 0; j < N_depth; ++j) + { + double depth = maxDepth * j / N_depth; + double attenuation = exp(-alpha * depth); + double intensity = 40 * attenuation; + + if (std::abs(depth - nearestHit) < (maxDepth / N_depth)) + intensity = 255 * reflectionCoeff * attenuation; + + frame.at(j, i) = static_cast(std::min(255.0, intensity)); + } + } + + return frame; + + } + + void IVUSController::buildROI( + const Vec3& probepos, + std::vector& triangleIndices) + { + triangleIndices.clear(); + auto* triModel = vesselNode->getNodeObject>(); + if (!triModel) + { + msg_error() << "No TriangleCollisionModel found in vesselNode!"; + return; + } + + // Access mechanical object positions + const auto& positions = triModel->getX(); // This is a vector of Rigid3dTypes::Coord (or Vec3) + + // Access topology triangles + const auto& triangles = triModel->getTriangles(); // SeqTriangles from BaseMeshTopology + + for (sofa::Index i = 0; i < triangles.size(); ++i) + { + const auto& tri = triangles[i]; // tri is an array of 3 vertex indices + + // Get vertex positions from mechanical state + const auto& v0 = positions[tri[0]]; + const auto& v1 = positions[tri[1]]; + const auto& v2 = positions[tri[2]]; + + // Use the first vertex or the centroid to check distance + Vec3 center = (v0 + v1 + v2) / 3.0; + + if ((center - probepos).norm() < maxDepth) + triangleIndices.push_back(i); + } + + + } + + + +} diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h new file mode 100644 index 0000000..91ceb93 --- /dev/null +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -0,0 +1,109 @@ +/***************************************************************************** + * - Copyright (C) - 2020 - InfinyTech3D - * + * * + * This file is part of the InfinyToolkit plugin for the SOFA framework * + * * + * Commercial License Usage: * + * Licensees holding valid commercial license from InfinyTech3D may use this * + * file in accordance with the commercial license agreement provided with * + * the Software or, alternatively, in accordance with the terms contained in * + * a written agreement between you and InfinyTech3D. For further information * + * on the licensing terms and conditions, contact: contact@infinytech3d.com * + * * + * GNU General Public License Usage: * + * Alternatively, this file may be used under the terms of the GNU General * + * Public License version 3. The licenses are as published by the Free * + * Software Foundation and appearing in the file LICENSE.GPL3 included in * + * the packaging of this file. Please review the following information to * + * ensure the GNU General Public License requirements will be met: * + * https://www.gnu.org/licenses/gpl-3.0.html. * + * * + * Authors: see Authors.txt * + * Further information: https://infinytech3d.com * + ****************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include // Not sure I will use it +#include + +#include +#include +#include +#include +#include +#include +#include + + +namespace sofa::infinytoolkit +{ + + using namespace sofa::core::objectmodel; + using namespace sofa::component::topology::container::dynamic; + + class IVUSController : public sofa::component::controller::Controller + { + public: + SOFA_CLASS(IVUSController, sofa::component::controller::Controller); + + using DataTypes = sofa::defaulttype::Rigid3dTypes; + + using Coord = DataTypes::Coord; + using VecCoord = DataTypes::VecCoord; + using Vec3 = DataTypes::Vec3; + using Quat = DataTypes::Quat; + + + + // Scene references , nor sure if I use them + sofa::simulation::Node* catheterNode = nullptr; + sofa::simulation::Node* vesselNode = nullptr; + + + // IVUS parameters + unsigned int N_rays = 128; + unsigned int N_depth = 256; + double maxDepth = 10.0; + unsigned int K_points = 3; // points near tip to simulate finite transducer length + + double alpha = 0.15; // attenuation coeicient + double reflectionCoeff = 1.5; // reflection boost + double noiseSigma = 10.0; // Gaussian noise sigma + + unsigned int maxStoredFrames = 400; + + // Images + cv::Mat currentFrame; // cross-sectional frame + cv::Mat longitudinalImage; // longitudinal side view + std::vector ivusFrames; // history buffer + + void init() override; + void handleEvent(sofa::core::objectmodel::Event* event) override; + + private: + + SingleLink< IVUSController, + sofa::core::behavior::MechanicalState, + BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_CatheterState; + + + SingleLink, + BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_triangleGeo; + + + // void computeIVUSFrame(); + cv::Mat computeSingleProbeFrame(const Vec3& probePos); + void buildROI( + const Vec3& probePos, + std::vector& triangleIndices); + + }; +} + + + From 5f986a49237d1bb7cdc0bb4f21837b7d89b6b436 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Mon, 9 Mar 2026 11:18:25 +0100 Subject: [PATCH 08/17] Add constructor for passing the data. --- .../IVUSController/IVUSController.cpp | 53 ++++++++++++------- .../IVUSController/IVUSController.h | 32 +++++++---- 2 files changed, 57 insertions(+), 28 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 52ba2e8..56e52ae 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -27,6 +27,19 @@ namespace sofa::infinytoolkit .add< IVUSController >() ); } + IVUSController::IVUSController() + : d_N_rays(initData(&d_N_rays, (unsigned int)128, "N_rays", "Number of angular rays")) + , d_N_depth(initData(&d_N_depth, (unsigned int)256, "N_depth", "Number of radial depth samples")) + , d_maxDepth(initData(&d_maxDepth, 10.0, "maxDepth", "Maximum probe depth")) + , d_alpha(initData(&d_alpha, 0.15, "alpha", "Attenuation coefficient")) + , d_reflectionCoeff(initData(&d_reflectionCoeff, 1.5, "reflectionCoeff", "Reflection boost")) + , d_noiseSigma(initData(&d_noiseSigma, 10.0, "noiseSigma", "Gaussian noise sigma")) + , d_K_points(initData(&d_K_points, (unsigned int)3, "K_points", "Points near probe tip")) + , d_maxStoredFrames(initData(&d_maxStoredFrames, (unsigned int)400, "maxStoredFrames", "Number of stored IVUS frames")) + { + } + + void IVUSController::init() { // Resolve the MechanicalState of the catheter @@ -45,8 +58,10 @@ namespace sofa::infinytoolkit << "Trying to find first one in current context..."; //l_triangleGeo.set(this->getContext()->getNodeObject>()); //will back to it } + + this->f_listening.setValue(true); - currentFrame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + currentFrame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); } void IVUSController::handleEvent(sofa::core::objectmodel::Event* event) @@ -60,7 +75,7 @@ namespace sofa::infinytoolkit std::vector probePositions; unsigned int K = std::min( - static_cast(K_points), // should be passed by getValue + static_cast(d_K_points.getValue()), static_cast(positions.size()) ); @@ -72,7 +87,7 @@ namespace sofa::infinytoolkit // Accumulate frames for multi-point averaging - cv::Mat accumulated = cv::Mat::zeros(N_depth, N_rays, CV_64F); + cv::Mat accumulated = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_64F); for (auto& probePos : probePositions) { @@ -87,20 +102,20 @@ namespace sofa::infinytoolkit // Add Gaussian noise cv::Mat noise(currentFrame.size(), CV_8UC1); - cv::randn(noise, 0, noiseSigma); + cv::randn(noise, 0, d_noiseSigma.getValue()); currentFrame += noise; // Store frame ivusFrames.push_back(currentFrame.clone()); - if (ivusFrames.size() > maxStoredFrames) + if (ivusFrames.size() > d_maxStoredFrames.getValue()) ivusFrames.erase(ivusFrames.begin()); // Build longitudinal image (side view) - longitudinalImage = cv::Mat::zeros(N_depth, ivusFrames.size(), CV_8UC1); - unsigned int selectedAngle = N_rays / 2; // central radial slice + longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); + unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice for (size_t t = 0; t < ivusFrames.size(); ++t) { - for (unsigned int d = 0; d < N_depth; ++d) + for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) { longitudinalImage.at(d, t) = ivusFrames[t].at(d, selectedAngle); @@ -115,7 +130,7 @@ namespace sofa::infinytoolkit cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) { - cv::Mat frame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); + cv::Mat frame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); // build roi std::vector roiTriangles; @@ -133,15 +148,15 @@ namespace sofa::infinytoolkit sofa::type::vector vecCoordKmin; - for (unsigned int i = 0; i < N_rays; ++i) + for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) { - double angle = 2.0 * std::numbers::pi * i / N_rays; + double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); Vec3 dir(cos(angle), sin(angle), 0.0); Vec3 p1 = probepos; - Vec3 p2 = probepos + dir * maxDepth; + Vec3 p2 = probepos + dir * d_maxDepth.getValue(); - double nearestHit = maxDepth; + double nearestHit = d_maxDepth.getValue(); // Loop over triangles in ROI for (auto triIndex : roiTriangles) @@ -168,14 +183,14 @@ namespace sofa::infinytoolkit } // Fill ultrasound frame - for (unsigned int j = 0; j < N_depth; ++j) + for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) { - double depth = maxDepth * j / N_depth; - double attenuation = exp(-alpha * depth); + double depth = d_maxDepth.getValue() * j / d_N_depth.getValue(); + double attenuation = exp(-d_alpha.getValue() * depth); double intensity = 40 * attenuation; - if (std::abs(depth - nearestHit) < (maxDepth / N_depth)) - intensity = 255 * reflectionCoeff * attenuation; + if (std::abs(depth - nearestHit) < (d_maxDepth.getValue() / d_N_depth.getValue())) + intensity = 255 * d_reflectionCoeff.getValue() * attenuation; frame.at(j, i) = static_cast(std::min(255.0, intensity)); } @@ -215,7 +230,7 @@ namespace sofa::infinytoolkit // Use the first vertex or the centroid to check distance Vec3 center = (v0 + v1 + v2) / 3.0; - if ((center - probepos).norm() < maxDepth) + if ((center - probepos).norm() < d_maxDepth.getValue()) triangleIndices.push_back(i); } diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index 91ceb93..dba6422 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -44,12 +44,16 @@ namespace sofa::infinytoolkit using namespace sofa::core::objectmodel; using namespace sofa::component::topology::container::dynamic; + using sofa::core::objectmodel::Data; class IVUSController : public sofa::component::controller::Controller { public: SOFA_CLASS(IVUSController, sofa::component::controller::Controller); + IVUSController(); + ~IVUSController() override = default; + using DataTypes = sofa::defaulttype::Rigid3dTypes; using Coord = DataTypes::Coord; @@ -65,16 +69,26 @@ namespace sofa::infinytoolkit // IVUS parameters - unsigned int N_rays = 128; - unsigned int N_depth = 256; - double maxDepth = 10.0; - unsigned int K_points = 3; // points near tip to simulate finite transducer length - - double alpha = 0.15; // attenuation coeicient - double reflectionCoeff = 1.5; // reflection boost - double noiseSigma = 10.0; // Gaussian noise sigma + //unsigned int N_rays = 128; + //unsigned int N_depth = 256; + //double maxDepth = 10.0; + //unsigned int K_points = 3; // points near tip to simulate finite transducer length + + //double alpha = 0.15; // attenuation coeicient + //double reflectionCoeff = 1.5; // reflection boost + //double noiseSigma = 10.0; // Gaussian noise sigma + + //unsigned int maxStoredFrames = 400; + + Data d_N_rays; + Data d_N_depth; + Data d_maxDepth; + Data d_alpha; + Data d_reflectionCoeff; + Data d_noiseSigma; + Data d_K_points; + Data d_maxStoredFrames; - unsigned int maxStoredFrames = 400; // Images cv::Mat currentFrame; // cross-sectional frame From 3bbd400eaf9ef9a3095dbd024f2ea3e980377c71 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Mon, 9 Mar 2026 12:15:26 +0100 Subject: [PATCH 09/17] Small modifs. --- .../IVUSController/IVUSController.cpp | 31 ++++++++++++++++--- .../IVUSController/IVUSController.h | 1 + 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 56e52ae..8af8ea2 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -1,3 +1,28 @@ +/***************************************************************************** + * - Copyright (C) - 2020 - InfinyTech3D - * + * * + * This file is part of the InfinyToolkit plugin for the SOFA framework * + * * + * Commercial License Usage: * + * Licensees holding valid commercial license from InfinyTech3D may use this * + * file in accordance with the commercial license agreement provided with * + * the Software or, alternatively, in accordance with the terms contained in * + * a written agreement between you and InfinyTech3D. For further information * + * on the licensing terms and conditions, contact: contact@infinytech3d.com * + * * + * GNU General Public License Usage: * + * Alternatively, this file may be used under the terms of the GNU General * + * Public License version 3. The licenses are as published by the Free * + * Software Foundation and appearing in the file LICENSE.GPL3 included in * + * the packaging of this file. Please review the following information to * + * ensure the GNU General Public License requirements will be met: * + * https://www.gnu.org/licenses/gpl-3.0.html. * + * * + * Authors: see Authors.txt * + * Further information: https://infinytech3d.com * + ****************************************************************************/ +#pragma once + #include #include< sofa/component/collision/geometry/TriangleCollisionModel.h > #include @@ -6,9 +31,6 @@ #include #include -using sofa::defaulttype::Vec3dTypes; - - namespace sofa::infinytoolkit { @@ -23,10 +45,11 @@ namespace sofa::infinytoolkit void registerIVUSController(sofa::core::ObjectFactory* factory) { factory->registerObjects( - sofa::core::ObjectRegistrationData("") + sofa::core::ObjectRegistrationData("IVUSController") .add< IVUSController >() ); } + IVUSController::IVUSController() : d_N_rays(initData(&d_N_rays, (unsigned int)128, "N_rays", "Number of angular rays")) , d_N_depth(initData(&d_N_depth, (unsigned int)256, "N_depth", "Number of radial depth samples")) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index dba6422..34ad887 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -22,6 +22,7 @@ * Further information: https://infinytech3d.com * ****************************************************************************/ #pragma once + #include #include From 445627228869c13cfa8ef6e7d5ba8cf251a315ea Mon Sep 17 00:00:00 2001 From: rmolazem Date: Mon, 9 Mar 2026 13:21:24 +0100 Subject: [PATCH 10/17] Remove the hard-coded line. --- CMakeLists.txt | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e82420a..d73d967 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,8 +35,23 @@ find_package(Sofa.GUI.Component REQUIRED) find_package(Sofa.Component.Engine.Select REQUIRED) # Tell CMake where OpenCV is -set(OpenCV_DIR "C:/softwares/opencv-4.12.0/opencv/build") # Adjust if your path is different +#set(OpenCV_DIR "C:/softwares/opencv-4.12.0/opencv/build") # Adjust if your path is different +#find_package(OpenCV REQUIRED) +# ------------------------------- +# Find OpenCV (branch-independent) +# ------------------------------- +# Prefer environment variable OPENCV_DIR if set; fallback to default path +if(NOT DEFINED OpenCV_DIR) + set(OpenCV_DIR "$ENV{OPENCV_DIR}" CACHE PATH "Path to OpenCVConfig.cmake") +endif() + +if(NOT OpenCV_DIR) + message(FATAL_ERROR "OpenCV_DIR is not set. Please set the environment variable OPENCV_DIR or edit CMakeLists.") +endif() + find_package(OpenCV REQUIRED) +message(STATUS "Found OpenCV version ${OpenCV_VERSION} at ${OpenCV_DIR}") + set(USE_INFINYTOOLKIT_PLUGIN true CACHE BOOL "Use Interaction Tools plugin") From 0e75bc76e3370df26cb1b6a25ab05bfe0119af34 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Mon, 9 Mar 2026 17:57:04 +0100 Subject: [PATCH 11/17] Clean the file. --- CMakeLists.txt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d73d967..9905772 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,11 +34,8 @@ find_package(Sofa.Component.Haptics REQUIRED) find_package(Sofa.GUI.Component REQUIRED) find_package(Sofa.Component.Engine.Select REQUIRED) -# Tell CMake where OpenCV is -#set(OpenCV_DIR "C:/softwares/opencv-4.12.0/opencv/build") # Adjust if your path is different -#find_package(OpenCV REQUIRED) # ------------------------------- -# Find OpenCV (branch-independent) +# Find OpenCV # ------------------------------- # Prefer environment variable OPENCV_DIR if set; fallback to default path if(NOT DEFINED OpenCV_DIR) From 0bd220238122a29cce1def980b746c5fd393e374 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Mon, 9 Mar 2026 17:57:59 +0100 Subject: [PATCH 12/17] Fix run-time errors. --- .../IVUSController/IVUSController.cpp | 225 +++++++++++------- .../IVUSController/IVUSController.h | 7 +- 2 files changed, 143 insertions(+), 89 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 8af8ea2..8e08c1d 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -51,7 +51,10 @@ namespace sofa::infinytoolkit } IVUSController::IVUSController() - : d_N_rays(initData(&d_N_rays, (unsigned int)128, "N_rays", "Number of angular rays")) + :l_CatheterState(initLink("catheterState", "Link to the mechanical object of the catheter.")) + , l_triangleGeo(initLink("triangleGeo", "Link to the collision model of the vessel.")) + , l_vesselNode(initLink("vesselNode", "Link to the vessel node.")) + , d_N_rays(initData(&d_N_rays, (unsigned int)128, "N_rays", "Number of angular rays")) , d_N_depth(initData(&d_N_depth, (unsigned int)256, "N_depth", "Number of radial depth samples")) , d_maxDepth(initData(&d_maxDepth, 10.0, "maxDepth", "Maximum probe depth")) , d_alpha(initData(&d_alpha, 0.15, "alpha", "Attenuation coefficient")) @@ -75,13 +78,27 @@ namespace sofa::infinytoolkit } - if (l_triangleGeo.empty()) + if (l_triangleGeo.get() == nullptr) { msg_info() << "No TriangleSetGeometryAlgorithms link set. " << "Trying to find first one in current context..."; - //l_triangleGeo.set(this->getContext()->getNodeObject>()); //will back to it + d_componentState.setValue( + sofa::core::objectmodel::ComponentState::Invalid); + return; } + //vesselNode = this->getRoot()->getChild("VesselNode"); + + ///* msg_info() << "Catheter DOFs: " << cath->getSize(); + // msg_info() << "Triangles: " << geo->getTopology()->getNbTriangles();*/ + + if (l_vesselNode.get() == nullptr) + { + msg_error() << "Vessel node link not set!"; + return; + } + + this->f_listening.setValue(true); currentFrame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); @@ -105,6 +122,12 @@ namespace sofa::infinytoolkit for (unsigned int k = 0; k < K; ++k) { const Coord& rigid = positions[positions.size() - 1 - k]; + auto center = rigid.getCenter(); + std::cout << "Probe " << k << " center: " + << center[0] << ", " + << center[1] << ", " + << center[2] << std::endl; + probePositions.push_back(rigid.getCenter()); } @@ -119,36 +142,36 @@ namespace sofa::infinytoolkit accumulated += tempFrame; } - // Average over K points - accumulated /= static_cast(K); - accumulated.convertTo(currentFrame, CV_8UC1); - - // Add Gaussian noise - cv::Mat noise(currentFrame.size(), CV_8UC1); - cv::randn(noise, 0, d_noiseSigma.getValue()); - currentFrame += noise; - - // Store frame - ivusFrames.push_back(currentFrame.clone()); - if (ivusFrames.size() > d_maxStoredFrames.getValue()) - ivusFrames.erase(ivusFrames.begin()); - - // Build longitudinal image (side view) - longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); - unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice - for (size_t t = 0; t < ivusFrames.size(); ++t) - { - for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) - { - longitudinalImage.at(d, t) = - ivusFrames[t].at(d, selectedAngle); - } - } - - // Display images - cv::imshow("IVUS Cross Section", currentFrame); - cv::imshow("IVUS Longitudinal", longitudinalImage); - cv::waitKey(1); + //// Average over K points + //accumulated /= static_cast(K); + //accumulated.convertTo(currentFrame, CV_8UC1); + + //// Add Gaussian noise + //cv::Mat noise(currentFrame.size(), CV_8UC1); + //cv::randn(noise, 0, d_noiseSigma.getValue()); + //currentFrame += noise; + + //// Store frame + //ivusFrames.push_back(currentFrame.clone()); + //if (ivusFrames.size() > d_maxStoredFrames.getValue()) + // ivusFrames.erase(ivusFrames.begin()); + + //// Build longitudinal image (side view) + //longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); + //unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice + //for (size_t t = 0; t < ivusFrames.size(); ++t) + //{ + // for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) + // { + // longitudinalImage.at(d, t) = + // ivusFrames[t].at(d, selectedAngle); + // } + //} + + //// Display images + //cv::imshow("IVUS Cross Section", currentFrame); + //cv::imshow("IVUS Longitudinal", longitudinalImage); + //cv::waitKey(1); } cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) @@ -159,65 +182,65 @@ namespace sofa::infinytoolkit std::vector roiTriangles; IVUSController::buildROI(probepos, roiTriangles); - auto* trimodel = vesselNode->getNodeObject>(); + auto* trimodel = l_vesselNode->getNodeObject>(); // Mechanical positions (Rigid3d coordinates) const auto& positions = trimodel->getX(); // VecCoord const auto& triangles = trimodel->getTriangles(); // SeqTriangles from topology - // Temporary storage required by computeIntersectionsLineTriangle - sofa::type::vector indices; - sofa::type::vector vecBaryCoef; - sofa::type::vector vecCoordKmin; - - - for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) - { - double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); - Vec3 dir(cos(angle), sin(angle), 0.0); - - Vec3 p1 = probepos; - Vec3 p2 = probepos + dir * d_maxDepth.getValue(); - - double nearestHit = d_maxDepth.getValue(); - - // Loop over triangles in ROI - for (auto triIndex : roiTriangles) - { - bool hit = l_triangleGeo->computeIntersectionsLineTriangle( - false, // is_entered - sofa::type::Vec<3, double>(p1[0], p1[1], p1[2]), - sofa::type::Vec<3, double>(p2[0], p2[1], p2[2]), - triIndex, - indices, - vecBaryCoef, - vecCoordKmin - ); - - if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) - { - nearestHit = vecCoordKmin[0]; - } - - // Clear vectors for next triangle - indices.clear(); - vecBaryCoef.clear(); - vecCoordKmin.clear(); - } - - // Fill ultrasound frame - for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) - { - double depth = d_maxDepth.getValue() * j / d_N_depth.getValue(); - double attenuation = exp(-d_alpha.getValue() * depth); - double intensity = 40 * attenuation; - - if (std::abs(depth - nearestHit) < (d_maxDepth.getValue() / d_N_depth.getValue())) - intensity = 255 * d_reflectionCoeff.getValue() * attenuation; - - frame.at(j, i) = static_cast(std::min(255.0, intensity)); - } - } + //// Temporary storage required by computeIntersectionsLineTriangle + //sofa::type::vector indices; + //sofa::type::vector vecBaryCoef; + //sofa::type::vector vecCoordKmin; + + + //for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) + //{ + // double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); + // Vec3 dir(cos(angle), sin(angle), 0.0); + + // Vec3 p1 = probepos; + // Vec3 p2 = probepos + dir * d_maxDepth.getValue(); + + // double nearestHit = d_maxDepth.getValue(); + + // // Loop over triangles in ROI + // for (auto triIndex : roiTriangles) + // { + // bool hit = l_triangleGeo->computeIntersectionsLineTriangle( + // false, // is_entered + // sofa::type::Vec<3, double>(p1[0], p1[1], p1[2]), + // sofa::type::Vec<3, double>(p2[0], p2[1], p2[2]), + // triIndex, + // indices, + // vecBaryCoef, + // vecCoordKmin + // ); + + // if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) + // { + // nearestHit = vecCoordKmin[0]; + // } + + // // Clear vectors for next triangle + // indices.clear(); + // vecBaryCoef.clear(); + // vecCoordKmin.clear(); + // } + + // // Fill ultrasound frame + // for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) + // { + // double depth = d_maxDepth.getValue() * j / d_N_depth.getValue(); + // double attenuation = exp(-d_alpha.getValue() * depth); + // double intensity = 40 * attenuation; + + // if (std::abs(depth - nearestHit) < (d_maxDepth.getValue() / d_N_depth.getValue())) + // intensity = 255 * d_reflectionCoeff.getValue() * attenuation; + + // frame.at(j, i) = static_cast(std::min(255.0, intensity)); + // } + //} return frame; @@ -228,7 +251,7 @@ namespace sofa::infinytoolkit std::vector& triangleIndices) { triangleIndices.clear(); - auto* triModel = vesselNode->getNodeObject>(); + auto* triModel = l_vesselNode->getNodeObject>(); if (!triModel) { msg_error() << "No TriangleCollisionModel found in vesselNode!"; @@ -259,6 +282,32 @@ namespace sofa::infinytoolkit } + + /* void IVUSController::buildROI( + const Vec3& probepos, + std::vector& triangleIndices) + { + triangleIndices.clear(); + + if (!l_triangleGeo) + { + msg_error() << "TriangleSetGeometryAlgorithms not initialized!"; + return; + } + + const sofa::Size nbTriangles = l_triangleGeo->getTopology()->getNbTriangles(); + const auto maxDepth = d_maxDepth.getValue(); + + for (sofa::Index i = 0; i < nbTriangles; ++i) + { + Vec3 center = l_triangleGeo->computeTriangleCenter(i); + + if ((center - probepos).norm() < maxDepth) + { + triangleIndices.push_back(i); + } + } + }*/ diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index dba6422..293f590 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -65,7 +65,8 @@ namespace sofa::infinytoolkit // Scene references , nor sure if I use them sofa::simulation::Node* catheterNode = nullptr; - sofa::simulation::Node* vesselNode = nullptr; + //sofa::simulation::Node* vesselNode = nullptr; + // IVUS parameters @@ -109,6 +110,10 @@ namespace sofa::infinytoolkit TriangleSetGeometryAlgorithms, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_triangleGeo; + SingleLink l_vesselNode; + // void computeIVUSFrame(); cv::Mat computeSingleProbeFrame(const Vec3& probePos); From 0885fcb259b4b32c0570b1e23bc72fee40f3deb4 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Wed, 11 Mar 2026 14:23:21 +0100 Subject: [PATCH 13/17] Save the work. --- .../IVUSController/IVUSController.cpp | 186 ++++++++++-------- .../IVUSController/IVUSController.h | 2 +- 2 files changed, 104 insertions(+), 84 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 8e08c1d..8fb5b87 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -138,40 +138,45 @@ namespace sofa::infinytoolkit for (auto& probePos : probePositions) { cv::Mat tempFrame = computeSingleProbeFrame(probePos); + + //cv::imshow("Probe Frame", tempFrame); + //cv::waitKey(1); // needed for OpenCV window refresh + + tempFrame.convertTo(tempFrame, CV_64F); accumulated += tempFrame; } - //// Average over K points - //accumulated /= static_cast(K); - //accumulated.convertTo(currentFrame, CV_8UC1); - - //// Add Gaussian noise - //cv::Mat noise(currentFrame.size(), CV_8UC1); - //cv::randn(noise, 0, d_noiseSigma.getValue()); - //currentFrame += noise; - - //// Store frame - //ivusFrames.push_back(currentFrame.clone()); - //if (ivusFrames.size() > d_maxStoredFrames.getValue()) - // ivusFrames.erase(ivusFrames.begin()); - - //// Build longitudinal image (side view) - //longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); - //unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice - //for (size_t t = 0; t < ivusFrames.size(); ++t) - //{ - // for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) - // { - // longitudinalImage.at(d, t) = - // ivusFrames[t].at(d, selectedAngle); - // } - //} - - //// Display images - //cv::imshow("IVUS Cross Section", currentFrame); - //cv::imshow("IVUS Longitudinal", longitudinalImage); - //cv::waitKey(1); + // Average over K points + accumulated /= static_cast(K); + accumulated.convertTo(currentFrame, CV_8UC1); + + // Add Gaussian noise + cv::Mat noise(currentFrame.size(), CV_8UC1); + cv::randn(noise, 0, d_noiseSigma.getValue()); + currentFrame += noise; + + // Store frame + ivusFrames.push_back(currentFrame.clone()); + if (ivusFrames.size() > d_maxStoredFrames.getValue()) + ivusFrames.erase(ivusFrames.begin()); + + // Build longitudinal image (side view) + longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); + unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice + for (size_t t = 0; t < ivusFrames.size(); ++t) + { + for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) + { + longitudinalImage.at(d, t) = + ivusFrames[t].at(d, selectedAngle); + } + } + + // Display images + cv::imshow("IVUS Cross Section", currentFrame); + cv::imshow("IVUS Longitudinal", longitudinalImage); + cv::waitKey(1); } cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) @@ -188,59 +193,59 @@ namespace sofa::infinytoolkit const auto& positions = trimodel->getX(); // VecCoord const auto& triangles = trimodel->getTriangles(); // SeqTriangles from topology - //// Temporary storage required by computeIntersectionsLineTriangle - //sofa::type::vector indices; - //sofa::type::vector vecBaryCoef; - //sofa::type::vector vecCoordKmin; - - - //for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) - //{ - // double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); - // Vec3 dir(cos(angle), sin(angle), 0.0); - - // Vec3 p1 = probepos; - // Vec3 p2 = probepos + dir * d_maxDepth.getValue(); - - // double nearestHit = d_maxDepth.getValue(); - - // // Loop over triangles in ROI - // for (auto triIndex : roiTriangles) - // { - // bool hit = l_triangleGeo->computeIntersectionsLineTriangle( - // false, // is_entered - // sofa::type::Vec<3, double>(p1[0], p1[1], p1[2]), - // sofa::type::Vec<3, double>(p2[0], p2[1], p2[2]), - // triIndex, - // indices, - // vecBaryCoef, - // vecCoordKmin - // ); - - // if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) - // { - // nearestHit = vecCoordKmin[0]; - // } - - // // Clear vectors for next triangle - // indices.clear(); - // vecBaryCoef.clear(); - // vecCoordKmin.clear(); - // } - - // // Fill ultrasound frame - // for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) - // { - // double depth = d_maxDepth.getValue() * j / d_N_depth.getValue(); - // double attenuation = exp(-d_alpha.getValue() * depth); - // double intensity = 40 * attenuation; - - // if (std::abs(depth - nearestHit) < (d_maxDepth.getValue() / d_N_depth.getValue())) - // intensity = 255 * d_reflectionCoeff.getValue() * attenuation; - - // frame.at(j, i) = static_cast(std::min(255.0, intensity)); - // } - //} + // Temporary storage required by computeIntersectionsLineTriangle + sofa::type::vector indices; + sofa::type::vector vecBaryCoef; + sofa::type::vector vecCoordKmin; + + + for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) + { + double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); + Vec3 dir(cos(angle), sin(angle), 0.0); + + Vec3 p1 = probepos; + Vec3 p2 = probepos + dir * d_maxDepth.getValue(); + + double nearestHit = d_maxDepth.getValue(); + + // Loop over triangles in ROI + for (auto triIndex : roiTriangles) + { + bool hit = l_triangleGeo->computeIntersectionsLineTriangle( + false, // is_entered + sofa::type::Vec<3, double>(p1[0], p1[1], p1[2]), + sofa::type::Vec<3, double>(p2[0], p2[1], p2[2]), + triIndex, + indices, + vecBaryCoef, + vecCoordKmin + ); + + if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) + { + nearestHit = vecCoordKmin[0]; + } + + // Clear vectors for next triangle + indices.clear(); + vecBaryCoef.clear(); + vecCoordKmin.clear(); + } + + // Fill ultrasound frame + for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) + { + double depth = d_maxDepth.getValue() * j / d_N_depth.getValue(); + double attenuation = exp(-d_alpha.getValue() * depth); + double intensity = 40 * attenuation; + + if (std::abs(depth - nearestHit) < (d_maxDepth.getValue() / d_N_depth.getValue())) + intensity = 255 * d_reflectionCoeff.getValue() * attenuation; + + frame.at(j, i) = static_cast(std::min(255.0, intensity)); + } + } return frame; @@ -280,6 +285,21 @@ namespace sofa::infinytoolkit triangleIndices.push_back(i); } + // For Debug + /* if (!triangleIndices.empty()) + { + std::cout << "Triangles found: " << triangleIndices.size() << std::endl; + + for (const auto& idx : triangleIndices) + { + std::cout << "Triangle index: " << idx << std::endl; + } + } + else + { + std::cout << "triangleIndices vector is empty." << std::endl; + }*/ + } diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index 293f590..3fade0e 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -108,7 +108,7 @@ namespace sofa::infinytoolkit SingleLink, - BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_triangleGeo; + BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_triangleGeo; // To be removed when the whole node is passed SingleLink Date: Tue, 17 Mar 2026 13:11:05 +0100 Subject: [PATCH 14/17] Debug functions for ray intersection, using rayIntersection from triangle. --- .../IVUSController/IVUSController.cpp | 408 +++++++++++++----- .../IVUSController/IVUSController.h | 31 +- 2 files changed, 324 insertions(+), 115 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 8fb5b87..6dcdffa 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -26,6 +26,8 @@ #include #include< sofa/component/collision/geometry/TriangleCollisionModel.h > #include +#include +#include #include #include #include @@ -54,6 +56,7 @@ namespace sofa::infinytoolkit :l_CatheterState(initLink("catheterState", "Link to the mechanical object of the catheter.")) , l_triangleGeo(initLink("triangleGeo", "Link to the collision model of the vessel.")) , l_vesselNode(initLink("vesselNode", "Link to the vessel node.")) + , r_catheter(initData(&r_catheter,1.0,"device_radius","Device raduis where the prob located.")) , d_N_rays(initData(&d_N_rays, (unsigned int)128, "N_rays", "Number of angular rays")) , d_N_depth(initData(&d_N_depth, (unsigned int)256, "N_depth", "Number of radial depth samples")) , d_maxDepth(initData(&d_maxDepth, 10.0, "maxDepth", "Maximum probe depth")) @@ -101,138 +104,172 @@ namespace sofa::infinytoolkit this->f_listening.setValue(true); - currentFrame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); + //currentFrame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); } void IVUSController::handleEvent(sofa::core::objectmodel::Event* event) { - - - auto* mech = l_CatheterState.get(); - const VecCoord& positions = mech->readPositions(); - - // Extract last K points near tip - std::vector probePositions; - unsigned int K = std::min( - static_cast(d_K_points.getValue()), - static_cast(positions.size()) - ); - - for (unsigned int k = 0; k < K; ++k) + // End of the animation step + if (dynamic_cast(event)) { - const Coord& rigid = positions[positions.size() - 1 - k]; - auto center = rigid.getCenter(); - std::cout << "Probe " << k << " center: " - << center[0] << ", " - << center[1] << ", " - << center[2] << std::endl; - - probePositions.push_back(rigid.getCenter()); - } + auto* mech = l_CatheterState.get(); + const VecCoord& positions = mech->readPositions(); + // Extract last K points near tip + std::vector probePositions; - // Accumulate frames for multi-point averaging - cv::Mat accumulated = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_64F); + unsigned int K = std::min( + static_cast(d_K_points.getValue()), + static_cast(positions.size()) + ); - for (auto& probePos : probePositions) - { - cv::Mat tempFrame = computeSingleProbeFrame(probePos); + for (unsigned int k = 0; k < K; ++k) + { + const Coord& rigid = positions[positions.size() - 1 - k]; + auto center = rigid.getCenter(); - //cv::imshow("Probe Frame", tempFrame); - //cv::waitKey(1); // needed for OpenCV window refresh + probePositions.push_back(rigid.getCenter()); + } - tempFrame.convertTo(tempFrame, CV_64F); - accumulated += tempFrame; - } + // Accumulate frames for multi-point averaging + currentFrames.clear(); - // Average over K points - accumulated /= static_cast(K); - accumulated.convertTo(currentFrame, CV_8UC1); + cv::Mat accumulated = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_64F); - // Add Gaussian noise - cv::Mat noise(currentFrame.size(), CV_8UC1); - cv::randn(noise, 0, d_noiseSigma.getValue()); - currentFrame += noise; + debug_RayIntersections_hitPoints.clear(); - // Store frame - ivusFrames.push_back(currentFrame.clone()); - if (ivusFrames.size() > d_maxStoredFrames.getValue()) - ivusFrames.erase(ivusFrames.begin()); - - // Build longitudinal image (side view) - longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); - unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice - for (size_t t = 0; t < ivusFrames.size(); ++t) - { - for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) + for (unsigned int k = 0; k < probePositions.size(); ++k) { - longitudinalImage.at(d, t) = - ivusFrames[t].at(d, selectedAngle); + unsigned int catheterIndex = positions.size() - 1 - k; + + /* std::cout << "Processing probe k=" << k + << " (catheter index = " << catheterIndex << ")" + << std::endl;*/ + + + // For debguing intersection: + //debugrayIntersection(probePositions[k]); + + + cv::Mat tempFrame = computeSingleProbeFrame(probePositions[k]); + + //// Add Gaussian noise + //cv::Mat noise(tempFrame.size(), CV_8UC1); + //cv::randn(noise, 0, d_noiseSigma.getValue()); + //tempFrame += noise; + + //// Convert for accumulation + //cv::Mat tempFrame64; + //tempFrame.convertTo(tempFrame64, CV_64F); + + //accumulated += tempFrame64; + + //// Store displayable version + //currentFrames.push_back(tempFrame.clone()); } - } - // Display images - cv::imshow("IVUS Cross Section", currentFrame); - cv::imshow("IVUS Longitudinal", longitudinalImage); - cv::waitKey(1); + // Average over K points + /* accumulated /= static_cast(K); + cv::Mat averagedFrame; + accumulated.convertTo(averagedFrame, CV_8UC1);*/ + + + // Store frame + /* ivusFrames.push_back(currentFrame.clone()); + if (ivusFrames.size() > d_maxStoredFrames.getValue()) + ivusFrames.erase(ivusFrames.begin());*/ + + // Build longitudinal image (side view) + //longitudinalImage = cv::Mat::zeros(d_N_depth.getValue(), ivusFrames.size(), CV_8UC1); + //unsigned int selectedAngle = d_N_rays.getValue() / 2; // central radial slice + //for (size_t t = 0; t < ivusFrames.size(); ++t) + //{ + // for (unsigned int d = 0; d < d_N_depth.getValue(); ++d) + // { + // longitudinalImage.at(d, t) = + // ivusFrames[t].at(d, selectedAngle); + // } + //} + + // Display images + //cv::namedWindow("currentFrame", cv::WINDOW_AUTOSIZE); + //cv::imshow("IVUS Longitudinal", longitudinalImage); + /* for (size_t k = 0; k < currentFrames.size(); ++k) + { + std::string name = "Probe " + std::to_string(k); + cv::imshow(name, currentFrames[k]); + }*/ + + //cv::imshow("Averaged IVUS", averagedFrame); + + //cv::waitKey(1); + } + } cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) { + cv::Mat frame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); // build roi std::vector roiTriangles; + IVUSController::buildROI(probepos, roiTriangles); + - auto* trimodel = l_vesselNode->getNodeObject>(); - - // Mechanical positions (Rigid3d coordinates) - const auto& positions = trimodel->getX(); // VecCoord - const auto& triangles = trimodel->getTriangles(); // SeqTriangles from topology + auto* triModel = l_vesselNode->getNodeObject>(); - // Temporary storage required by computeIntersectionsLineTriangle - sofa::type::vector indices; - sofa::type::vector vecBaryCoef; - sofa::type::vector vecCoordKmin; + // Access mechanical object positions + const auto& positions = triModel->getX(); + // Access topology triangles + const auto& triangles = triModel->getTriangles(); + debug_hitPoints.clear(); + for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) { double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); Vec3 dir(cos(angle), sin(angle), 0.0); - Vec3 p1 = probepos; - Vec3 p2 = probepos + dir * d_maxDepth.getValue(); - + Vec3 origin = probepos; + Vec3 direction = dir; + double nearestHit = d_maxDepth.getValue(); + Vec3 nearstHitPoint; // Loop over triangles in ROI for (auto triIndex : roiTriangles) { - bool hit = l_triangleGeo->computeIntersectionsLineTriangle( - false, // is_entered - sofa::type::Vec<3, double>(p1[0], p1[1], p1[2]), - sofa::type::Vec<3, double>(p2[0], p2[1], p2[2]), - triIndex, - indices, - vecBaryCoef, - vecCoordKmin - ); - if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) + const auto& tri = triangles[triIndex]; + Vec3 n0 = positions[tri[0]]; + Vec3 n1 = positions[tri[1]]; + Vec3 n2 = positions[tri[2]]; + + double t, u, v; + + bool hit = sofa::geometry::Triangle::rayIntersection(n0, n1, n2, origin, direction, t, u, v); + + if (hit && t < nearestHit) { - nearestHit = vecCoordKmin[0]; + nearestHit = t; } - // Clear vectors for next triangle - indices.clear(); - vecBaryCoef.clear(); - vecCoordKmin.clear(); } + //---------------------------------------------------- + if (nearestHit < d_maxDepth.getValue()) + { + Vec3 hitPoint = origin + direction * (nearestHit); + + debug_hitPoints.push_back(hitPoint); + } + //---------------------------------------------------- + // Fill ultrasound frame for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) { @@ -269,6 +306,9 @@ namespace sofa::infinytoolkit // Access topology triangles const auto& triangles = triModel->getTriangles(); // SeqTriangles from BaseMeshTopology + debug_roiTrianglesPerProbe.clear(); + + for (sofa::Index i = 0; i < triangles.size(); ++i) { const auto& tri = triangles[i]; // tri is an array of 3 vertex indices @@ -281,8 +321,22 @@ namespace sofa::infinytoolkit // Use the first vertex or the centroid to check distance Vec3 center = (v0 + v1 + v2) / 3.0; + if ((center - probepos).norm() < d_maxDepth.getValue()) + { + std::vector roiVertices; + + roiVertices.push_back(v0); + roiVertices.push_back(v1); + roiVertices.push_back(v2); + + // store triangles for this probe + debug_roiTrianglesPerProbe.push_back(roiVertices); + triangleIndices.push_back(i); + + } + } // For Debug @@ -303,31 +357,189 @@ namespace sofa::infinytoolkit } - /* void IVUSController::buildROI( - const Vec3& probepos, - std::vector& triangleIndices) + void IVUSController::debugcomputeIntersectionsLineTriangle(const Vec3& probePos) { - triangleIndices.clear(); + + std::vector roiTriangles; + IVUSController::buildROI(probePos, roiTriangles); + + sofa::type::vector indices; + sofa::type::vector vecBaryCoef; + sofa::type::vector vecCoordKmin; + + // ---- Define a specific angle ---- + double angle = M_PI / 2.0; // 90 degrees, along +Y direction + Vec3 dir(cos(angle), sin(angle), 0.0); // ray direction in XY plane + Vec3 p1 = probePos ; // offset outside catheter + Vec3 p2 = probePos + dir * d_maxDepth.getValue(); + + //Vec3 rayDir = dir.normalized(); + + double nearestHit = d_maxDepth.getValue(); + Vec3 nearestHitPoint; + + std::cout << "Debugging ray at angle pi/2 from probe position (" + << probePos[0] << "," << probePos[1] << "," << probePos[2] << ")\n"; + + + for (auto triIndex : roiTriangles) + { + bool hit = l_triangleGeo->computeIntersectionsLineTriangle( + false, // is_entered + p1, + p2, + triIndex, + indices, + vecBaryCoef, + vecCoordKmin + ); + + if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) + { + nearestHit = vecCoordKmin[0]; + } + + // Clear vectors for next triangle + indices.clear(); + vecBaryCoef.clear(); + vecCoordKmin.clear(); + } + + //---------------------------------------------------- + debug_RayIntersections_hitPoints.clear(); - if (!l_triangleGeo) + if (nearestHit < d_maxDepth.getValue()) + { + Vec3 hitPoint = p1 + (p2 - p1) * (1.0-nearestHit); + + std::cout << "p1: " << p1 << std::endl; + std::cout << "p2: " << p2 << std::endl; + + std::cout << "coord_k: " << vecCoordKmin[0] << std::endl; + // ---- Print hit point ---- + std::cout << " HitPoint = (" + << hitPoint[0] << ", " + << hitPoint[1] << ", " + << hitPoint[2] << ")" + << std::endl; + + debug_RayIntersections_hitPoints.push_back(hitPoint); + + } + + } + + void IVUSController::debugrayIntersection(const Vec3& probePos) + { + + std::vector roiTriangles; + IVUSController::buildROI(probePos, roiTriangles); + + auto* triModel = l_vesselNode->getNodeObject>(); + if (!triModel) { - msg_error() << "TriangleSetGeometryAlgorithms not initialized!"; + msg_error() << "No TriangleCollisionModel found in vesselNode!"; return; } - const sofa::Size nbTriangles = l_triangleGeo->getTopology()->getNbTriangles(); - const auto maxDepth = d_maxDepth.getValue(); + // Access mechanical object positions + const auto& positions = triModel->getX(); + // Access topology triangles + const auto& triangles = triModel->getTriangles(); + + + + // ---- Define a specific angle ---- + double angle = M_PI / 2.0; // 90 degrees, along +Y direction + Vec3 dir(cos(angle), sin(angle), 0.0); // ray direction in XY plane + Vec3 origin = probePos; + Vec3 direction = dir; - for (sofa::Index i = 0; i < nbTriangles; ++i) + + double nearestHit = d_maxDepth.getValue(); + Vec3 nearestHitPoint; + + std::cout << "Debugging ray at angle pi/2 from probe position (" + << probePos[0] << "," << probePos[1] << "," << probePos[2] << ")\n"; + + debug_RayIntersections_hitPoints.clear(); + + for (auto triIndex : roiTriangles) { - Vec3 center = l_triangleGeo->computeTriangleCenter(i); + const auto& tri = triangles[triIndex]; + Vec3 n0 = positions[tri[0]]; + Vec3 n1 = positions[tri[1]]; + Vec3 n2 = positions[tri[2]]; + + double t, u, v; - if ((center - probepos).norm() < maxDepth) + bool hit = sofa::geometry::Triangle::rayIntersection(n0, n1, n2, origin, direction, t, u, v); + + if (hit && t < nearestHit) { - triangleIndices.push_back(i); + nearestHit = t; } + + + + //---------------------------------------------------- + + + if (nearestHit < d_maxDepth.getValue()) + { + Vec3 hitPoint = origin + direction * nearestHit; + //debug_hitPoints.push_back(hitPoint); + + + // ---- Print hit point ---- + std::cout << " HitPoint = (" + << hitPoint[0] << ", " + << hitPoint[1] << ", " + << hitPoint[2] << ")" + << std::endl; + + debug_RayIntersections_hitPoints.push_back(hitPoint); + + } + } - }*/ + + } + + + + + + void IVUSController::draw(const sofa::core::visual::VisualParams * vparams) + { + sofa::type::RGBAColor color_1(0.f, 1.f, 0.f, 1.f); + + /* for (const auto& probeTriangles : debug_roiTrianglesPerProbe) + { + if (!probeTriangles.empty()) + vparams->drawTool()->drawTriangles(probeTriangles, color_1); + }*/ + + if (!debug_hitPoints.empty()) + { + sofa::type::RGBAColor color(0.f, 0.f, 1.f, 1.f); // blue + vparams->drawTool()->drawPoints(debug_hitPoints, 5.0f, color); + } + + //if (!debug_RayIntersections_hitPoints.empty()) + //{ + // sofa::type::RGBAColor color(0.f, 0.f, 1.f, 1.f); // blue + // vparams->drawTool()->drawPoints(debug_RayIntersections_hitPoints, 10.0f, color); + //} + + + + + + } + + + diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index 3fade0e..e7ce6a1 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -36,6 +36,7 @@ #include #include #include +#include #include @@ -63,24 +64,10 @@ namespace sofa::infinytoolkit - // Scene references , nor sure if I use them + // Scene references , nor sure if I use it sofa::simulation::Node* catheterNode = nullptr; - //sofa::simulation::Node* vesselNode = nullptr; - - - - // IVUS parameters - //unsigned int N_rays = 128; - //unsigned int N_depth = 256; - //double maxDepth = 10.0; - //unsigned int K_points = 3; // points near tip to simulate finite transducer length - - //double alpha = 0.15; // attenuation coeicient - //double reflectionCoeff = 1.5; // reflection boost - //double noiseSigma = 10.0; // Gaussian noise sigma - - //unsigned int maxStoredFrames = 400; + Data r_catheter; Data d_N_rays; Data d_N_depth; Data d_maxDepth; @@ -92,12 +79,13 @@ namespace sofa::infinytoolkit // Images - cv::Mat currentFrame; // cross-sectional frame + std::vector currentFrames; // cross-sectional frame for each prob cv::Mat longitudinalImage; // longitudinal side view std::vector ivusFrames; // history buffer void init() override; void handleEvent(sofa::core::objectmodel::Event* event) override; + void draw(const sofa::core::visual::VisualParams* vparams) override; private: @@ -121,6 +109,15 @@ namespace sofa::infinytoolkit const Vec3& probePos, std::vector& triangleIndices); + // For debug + std::vector> debug_roiTrianglesPerProbe; + std::vector debug_hitPoints; + + std::vector debug_RayIntersections_hitPoints; + + void debugcomputeIntersectionsLineTriangle(const Vec3& probePos); + void debugrayIntersection(const Vec3& probePos); + }; } From ddcf9add0349335fd827b13c6b306ff95e1151af Mon Sep 17 00:00:00 2001 From: rmolazem Date: Tue, 17 Mar 2026 16:25:40 +0100 Subject: [PATCH 15/17] Shoot rays in a plane prependicular to the catheter. --- .../IVUSController/IVUSController.cpp | 57 +++++++++++++++++-- .../IVUSController/IVUSController.h | 2 +- 2 files changed, 52 insertions(+), 7 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 6dcdffa..90a3347 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -118,6 +118,7 @@ namespace sofa::infinytoolkit // Extract last K points near tip std::vector probePositions; + std::vector probeTangents; unsigned int K = std::min( static_cast(d_K_points.getValue()), @@ -126,10 +127,47 @@ namespace sofa::infinytoolkit for (unsigned int k = 0; k < K; ++k) { - const Coord& rigid = positions[positions.size() - 1 - k]; + // Index of this probe + unsigned int idx = positions.size() - 1 - k; + const Coord& rigid = positions[idx]; auto center = rigid.getCenter(); - probePositions.push_back(rigid.getCenter()); + probePositions.push_back(center); + + // Compute tangent using neighbor points + Vec3 p_prev, p_next; + + if (idx == 0) + { + p_prev = positions[idx].getCenter(); + p_next = positions[idx + 1].getCenter(); + } + else if (idx == positions.size() - 1) + { + p_prev = positions[idx - 1].getCenter(); + p_next = positions[idx].getCenter(); + } + else + { + p_prev = positions[idx - 1].getCenter(); + p_next = positions[idx + 1].getCenter(); + } + + // Tangent with fallback for undeployed catheter + Vec3 diff = p_next - p_prev; + Vec3 tangent; + + if (norm(diff) < 1e-8) + { + tangent = Vec3(0, 0, 1); // fallback axis (default deployment) + } + else + { + tangent = diff.normalized(); + } + + probeTangents.push_back(tangent); + } @@ -153,7 +191,7 @@ namespace sofa::infinytoolkit //debugrayIntersection(probePositions[k]); - cv::Mat tempFrame = computeSingleProbeFrame(probePositions[k]); + cv::Mat tempFrame = computeSingleProbeFrame(probePositions[k], probeTangents[k]); //// Add Gaussian noise //cv::Mat noise(tempFrame.size(), CV_8UC1); @@ -209,7 +247,7 @@ namespace sofa::infinytoolkit } - cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos) + cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos, const Vec3& probeTangent) { cv::Mat frame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); @@ -229,14 +267,21 @@ namespace sofa::infinytoolkit debug_hitPoints.clear(); + + // Build local plane + Vec3 tangent = probeTangent; + Vec3 arbitrary = (fabs(tangent[0]) < 0.9) ? Vec3(1, 0, 0) : Vec3(0, 1, 0); + Vec3 u = (cross(tangent, arbitrary)).normalized(); + Vec3 v = cross(tangent, u); + //Shoot rays in the (u,v) plane for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) { double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); - Vec3 dir(cos(angle), sin(angle), 0.0); + + Vec3 direction = cos(angle) * u + sin(angle) * v; Vec3 origin = probepos; - Vec3 direction = dir; double nearestHit = d_maxDepth.getValue(); Vec3 nearstHitPoint; diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index e7ce6a1..b865717 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -104,7 +104,7 @@ namespace sofa::infinytoolkit // void computeIVUSFrame(); - cv::Mat computeSingleProbeFrame(const Vec3& probePos); + cv::Mat computeSingleProbeFrame(const Vec3& probePos,const Vec3& probrTangent); void buildROI( const Vec3& probePos, std::vector& triangleIndices); From c62175002b2cf139aed2461fc7fe0c9b191f1308 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Wed, 18 Mar 2026 16:14:45 +0100 Subject: [PATCH 16/17] Refactoring debug tools and computeSingleProbeFrame function. --- .../IVUSController/IVUSController.cpp | 396 ++++++++++-------- .../IVUSController/IVUSController.h | 21 +- 2 files changed, 228 insertions(+), 189 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index 90a3347..c6f4706 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -56,7 +56,7 @@ namespace sofa::infinytoolkit :l_CatheterState(initLink("catheterState", "Link to the mechanical object of the catheter.")) , l_triangleGeo(initLink("triangleGeo", "Link to the collision model of the vessel.")) , l_vesselNode(initLink("vesselNode", "Link to the vessel node.")) - , r_catheter(initData(&r_catheter,1.0,"device_radius","Device raduis where the prob located.")) + , r_catheter(initData(&r_catheter, 1.0, "device_radius", "Device raduis where the prob located.")) , d_N_rays(initData(&d_N_rays, (unsigned int)128, "N_rays", "Number of angular rays")) , d_N_depth(initData(&d_N_depth, (unsigned int)256, "N_depth", "Number of radial depth samples")) , d_maxDepth(initData(&d_maxDepth, 10.0, "maxDepth", "Maximum probe depth")) @@ -65,6 +65,9 @@ namespace sofa::infinytoolkit , d_noiseSigma(initData(&d_noiseSigma, 10.0, "noiseSigma", "Gaussian noise sigma")) , d_K_points(initData(&d_K_points, (unsigned int)3, "K_points", "Points near probe tip")) , d_maxStoredFrames(initData(&d_maxStoredFrames, (unsigned int)400, "maxStoredFrames", "Number of stored IVUS frames")) + , d_useDebugRay(initData(&d_useDebugRay, false, "useDebugRay", "Enable debug ray intersection.")) + , d_drawROI(initData(&d_drawROI, false, "drawROI", "Draw ROI triangles.")) + , d_drawHitPoints(initData(&d_drawHitPoints, true, "drawHitPoints", "Draw debug hit points.")) { } @@ -90,11 +93,7 @@ namespace sofa::infinytoolkit return; } - //vesselNode = this->getRoot()->getChild("VesselNode"); - - ///* msg_info() << "Catheter DOFs: " << cath->getSize(); - // msg_info() << "Triangles: " << geo->getTopology()->getNbTriangles();*/ - + if (l_vesselNode.get() == nullptr) { msg_error() << "Vessel node link not set!"; @@ -170,13 +169,31 @@ namespace sofa::infinytoolkit } + if (d_useDebugRay.getValue()) + { + // DEBUG MODE + debug_RayIntersections_hitPoints.clear(); + + Vec3 tangent = probeTangents[0]; // tip tangent + Vec3 probePos = probePositions[0]; + + debugSingleRayIntersection( + probePos, + tangent, + M_PI / 2.0, + IntersectionMethod::RayTriangle, + "HandleEventDebug" + ); + return; + } + // Accumulate frames for multi-point averaging currentFrames.clear(); cv::Mat accumulated = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_64F); - debug_RayIntersections_hitPoints.clear(); + for (unsigned int k = 0; k < probePositions.size(); ++k) { @@ -186,10 +203,6 @@ namespace sofa::infinytoolkit << " (catheter index = " << catheterIndex << ")" << std::endl;*/ - - // For debguing intersection: - //debugrayIntersection(probePositions[k]); - cv::Mat tempFrame = computeSingleProbeFrame(probePositions[k], probeTangents[k]); @@ -249,8 +262,14 @@ namespace sofa::infinytoolkit cv::Mat IVUSController::computeSingleProbeFrame(const Vec3& probepos, const Vec3& probeTangent) { - - cv::Mat frame = cv::Mat::zeros(d_N_depth.getValue(), d_N_rays.getValue(), CV_8UC1); + //Precompute constants + const double maxDepth = d_maxDepth.getValue(); + const unsigned int N_rays = d_N_rays.getValue(); + const unsigned int N_depth = d_N_depth.getValue(); + const double alpha = d_alpha.getValue(); + const double reflectionCoeff = d_reflectionCoeff.getValue(); + + cv::Mat frame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); // build roi std::vector roiTriangles; @@ -270,59 +289,40 @@ namespace sofa::infinytoolkit // Build local plane Vec3 tangent = probeTangent; - Vec3 arbitrary = (fabs(tangent[0]) < 0.9) ? Vec3(1, 0, 0) : Vec3(0, 1, 0); - Vec3 u = (cross(tangent, arbitrary)).normalized(); - Vec3 v = cross(tangent, u); + + // Compute orthonormal plane perpendicular to tangent + Vec3 u, v; + computePerpendicularPlane(tangent, u, v); + + //Shoot rays in the (u,v) plane - for (unsigned int i = 0; i < d_N_rays.getValue(); ++i) + for (unsigned int i = 0; i < N_rays; ++i) { - double angle = 2.0 * std::numbers::pi * i / d_N_rays.getValue(); + double angle = 2.0 * std::numbers::pi * i / N_rays; Vec3 direction = cos(angle) * u + sin(angle) * v; Vec3 origin = probepos; - - double nearestHit = d_maxDepth.getValue(); - Vec3 nearstHitPoint; - // Loop over triangles in ROI - for (auto triIndex : roiTriangles) - { + Vec3 hitPoint = computeRayHitPoint(origin, direction, roiTriangles); - const auto& tri = triangles[triIndex]; - Vec3 n0 = positions[tri[0]]; - Vec3 n1 = positions[tri[1]]; - Vec3 n2 = positions[tri[2]]; - - double t, u, v; - - bool hit = sofa::geometry::Triangle::rayIntersection(n0, n1, n2, origin, direction, t, u, v); - - if (hit && t < nearestHit) - { - nearestHit = t; - } - - } - - //---------------------------------------------------- - if (nearestHit < d_maxDepth.getValue()) + double nearestHit = (hitPoint - origin).norm(); + + + if (nearestHit < maxDepth) { - Vec3 hitPoint = origin + direction * (nearestHit); - debug_hitPoints.push_back(hitPoint); } - //---------------------------------------------------- // Fill ultrasound frame - for (unsigned int j = 0; j < d_N_depth.getValue(); ++j) + for (unsigned int j = 0; j < N_depth; ++j) { - double depth = d_maxDepth.getValue() * j / d_N_depth.getValue(); - double attenuation = exp(-d_alpha.getValue() * depth); + double depth = maxDepth * j / N_depth; + double attenuation = exp(-alpha * depth); double intensity = 40 * attenuation; - if (std::abs(depth - nearestHit) < (d_maxDepth.getValue() / d_N_depth.getValue())) + if (std::abs(depth - nearestHit) < (maxDepth / N_depth)) intensity = 255 * d_reflectionCoeff.getValue() * attenuation; frame.at(j, i) = static_cast(std::min(255.0, intensity)); @@ -384,54 +384,90 @@ namespace sofa::infinytoolkit } - // For Debug - /* if (!triangleIndices.empty()) + + } + + + IVUSController::Vec3 IVUSController::computeRayHitPoint(const Vec3& origin, const Vec3& rayDir, const std::vector& roiTriangles) + { + auto* triModel = l_vesselNode->getNodeObject>(); + + const auto& positions = triModel->getX(); + const auto& triangles = triModel->getTriangles(); + + double nearestT = d_maxDepth.getValue(); + Vec3 nearestHit = origin; + + for (auto triIndex : roiTriangles) { - std::cout << "Triangles found: " << triangleIndices.size() << std::endl; + const auto& tri = triangles[triIndex]; + Vec3 n0 = positions[tri[0]]; + Vec3 n1 = positions[tri[1]]; + Vec3 n2 = positions[tri[2]]; + + double t, u_coef, v_coef; + bool hit = sofa::geometry::Triangle::rayIntersection(n0, n1, n2, origin, rayDir, t, u_coef, v_coef); - for (const auto& idx : triangleIndices) + if (hit && t < nearestT) { - std::cout << "Triangle index: " << idx << std::endl; + nearestT = t; + nearestHit = origin + rayDir * t; } } - else - { - std::cout << "triangleIndices vector is empty." << std::endl; - }*/ - + return nearestHit; } - void IVUSController::debugcomputeIntersectionsLineTriangle(const Vec3& probePos) + void IVUSController::debugSingleRayIntersection( + const Vec3& probePos, + const Vec3& tangent, + double angle, + IntersectionMethod method, + const std::string& debugName) { - - std::vector roiTriangles; - IVUSController::buildROI(probePos, roiTriangles); - - sofa::type::vector indices; - sofa::type::vector vecBaryCoef; - sofa::type::vector vecCoordKmin; - - // ---- Define a specific angle ---- - double angle = M_PI / 2.0; // 90 degrees, along +Y direction - Vec3 dir(cos(angle), sin(angle), 0.0); // ray direction in XY plane - Vec3 p1 = probePos ; // offset outside catheter - Vec3 p2 = probePos + dir * d_maxDepth.getValue(); - - //Vec3 rayDir = dir.normalized(); + // ROI + std::vector roiTriangles; + IVUSController::buildROI(probePos, roiTriangles); - double nearestHit = d_maxDepth.getValue(); - Vec3 nearestHitPoint; + // Triangle model (needed for RayTriangle) + auto* triModel = l_vesselNode->getNodeObject>(); + + const auto& positions = triModel->getX(); + const auto& triangles = triModel->getTriangles(); + + // Buffers (for LineTriangle) + sofa::type::vector indices; + sofa::type::vector vecBaryCoef; + sofa::type::vector vecCoordKmin; + + // Ray definition + // Compute orthonormal plane perpendicular to tangent + Vec3 u, v; + computePerpendicularPlane(tangent, u, v); + + // Ray direction in the plane + Vec3 dir = std::cos(angle) * u + std::sin(angle) * v; + + Vec3 origin = probePos; + Vec3 p2 = origin + dir * d_maxDepth.getValue(); - std::cout << "Debugging ray at angle pi/2 from probe position (" - << probePos[0] << "," << probePos[1] << "," << probePos[2] << ")\n"; + double nearestHit = d_maxDepth.getValue(); + bool foundHit = false; + std::cout << "\n[DEBUG: " << debugName << "]\n"; + std::cout << "Angle: " << angle << "\n"; - for (auto triIndex : roiTriangles) + //-------------------------------------------------- + for (auto triIndex : roiTriangles) + { + bool hit = false; + double t = 0.0; + + if (method == IntersectionMethod::LineTriangle) { - bool hit = l_triangleGeo->computeIntersectionsLineTriangle( - false, // is_entered - p1, + hit = l_triangleGeo->computeIntersectionsLineTriangle( + false, + origin, p2, triIndex, indices, @@ -439,149 +475,137 @@ namespace sofa::infinytoolkit vecCoordKmin ); - if (hit && !vecCoordKmin.empty() && vecCoordKmin[0] < nearestHit) + if (hit && !vecCoordKmin.empty()) { - nearestHit = vecCoordKmin[0]; + t = vecCoordKmin[0]; } - // Clear vectors for next triangle indices.clear(); vecBaryCoef.clear(); vecCoordKmin.clear(); } - - //---------------------------------------------------- - debug_RayIntersections_hitPoints.clear(); - - if (nearestHit < d_maxDepth.getValue()) + else // RayTriangle { - Vec3 hitPoint = p1 + (p2 - p1) * (1.0-nearestHit); - - std::cout << "p1: " << p1 << std::endl; - std::cout << "p2: " << p2 << std::endl; + const auto& tri = triangles[triIndex]; - std::cout << "coord_k: " << vecCoordKmin[0] << std::endl; - // ---- Print hit point ---- - std::cout << " HitPoint = (" - << hitPoint[0] << ", " - << hitPoint[1] << ", " - << hitPoint[2] << ")" - << std::endl; + Vec3 n0 = positions[tri[0]]; + Vec3 n1 = positions[tri[1]]; + Vec3 n2 = positions[tri[2]]; - debug_RayIntersections_hitPoints.push_back(hitPoint); - + double u, v; + hit = sofa::geometry::Triangle::rayIntersection( + n0, n1, n2, + origin, dir, + t, u, v + ); } - - } - - void IVUSController::debugrayIntersection(const Vec3& probePos) - { - std::vector roiTriangles; - IVUSController::buildROI(probePos, roiTriangles); + //-------------------------------------------------- - auto* triModel = l_vesselNode->getNodeObject>(); - if (!triModel) - { - msg_error() << "No TriangleCollisionModel found in vesselNode!"; - return; + if (hit && t < nearestHit) + { + nearestHit = t; + foundHit = true; + } } - // Access mechanical object positions - const auto& positions = triModel->getX(); - // Access topology triangles - const auto& triangles = triModel->getTriangles(); - - - - // ---- Define a specific angle ---- - double angle = M_PI / 2.0; // 90 degrees, along +Y direction - Vec3 dir(cos(angle), sin(angle), 0.0); // ray direction in XY plane - Vec3 origin = probePos; - Vec3 direction = dir; - - - double nearestHit = d_maxDepth.getValue(); - Vec3 nearestHitPoint; + //-------------------------------------------------- - std::cout << "Debugging ray at angle pi/2 from probe position (" - << probePos[0] << "," << probePos[1] << "," << probePos[2] << ")\n"; - - debug_RayIntersections_hitPoints.clear(); - - for (auto triIndex : roiTriangles) + if (foundHit) { - const auto& tri = triangles[triIndex]; - Vec3 n0 = positions[tri[0]]; - Vec3 n1 = positions[tri[1]]; - Vec3 n2 = positions[tri[2]]; - - double t, u, v; - - bool hit = sofa::geometry::Triangle::rayIntersection(n0, n1, n2, origin, direction, t, u, v); + Vec3 hitPoint; - if (hit && t < nearestHit) + if (method == IntersectionMethod::LineTriangle) { - nearestHit = t; + hitPoint = origin + (p2 - origin) * (1.0 - nearestHit); + } + else + { + hitPoint = origin + dir * nearestHit; } + std::cout << "Nearest t: " << nearestHit << "\n"; + std::cout << "HitPoint = (" + << hitPoint[0] << ", " + << hitPoint[1] << ", " + << hitPoint[2] << ")\n"; + debug_RayIntersections_hitPoints.push_back(hitPoint); + } + else + { + std::cout << "No intersection found.\n"; + } + } - //---------------------------------------------------- + - if (nearestHit < d_maxDepth.getValue()) - { - Vec3 hitPoint = origin + direction * nearestHit; - //debug_hitPoints.push_back(hitPoint); + void IVUSController::computePerpendicularPlane( + const Vec3& tangent, // catheter tangent + Vec3& u, // first perpendicular vector (output) + Vec3& v // second perpendicular vector (output) + ) + { + // Step 1: normalize tangent + Vec3 t = tangent.normalized(); - // ---- Print hit point ---- - std::cout << " HitPoint = (" - << hitPoint[0] << ", " - << hitPoint[1] << ", " - << hitPoint[2] << ")" - << std::endl; + // Step 2: pick a vector not parallel to tangent + Vec3 arbitrary; + if (std::abs(t[2]) < 0.9) // not mostly along Z + arbitrary = Vec3(0, 0, 1); + else + arbitrary = Vec3(1, 0, 0); - debug_RayIntersections_hitPoints.push_back(hitPoint); + // Step 3: compute first perpendicular vector + u = cross(t, arbitrary).normalized(); - } + // Step 4: second perpendicular vector + v = cross(t, u).normalized(); + } - } - - } + void IVUSController::draw(const sofa::core::visual::VisualParams* vparams) + { + if (!vparams || !vparams->displayFlags().getShowVisualModels()) + return; + auto* drawTool = vparams->drawTool(); + if (!drawTool) + return; + //---------------------------------- + // Colors (define once) + static const sofa::type::RGBAColor roiColor(0.f, 1.f, 0.f, 1.f); // green + static const sofa::type::RGBAColor hitColor(0.f, 0.f, 1.f, 1.f); // blue + static const sofa::type::RGBAColor rayColor(1.f, 0.f, 0.f, 1.f); // red - - void IVUSController::draw(const sofa::core::visual::VisualParams * vparams) + //---------------------------------- + // ROI TRIANGLES + if (d_drawROI.getValue()) { - sofa::type::RGBAColor color_1(0.f, 1.f, 0.f, 1.f); - - /* for (const auto& probeTriangles : debug_roiTrianglesPerProbe) + for (const auto& probeTriangles : debug_roiTrianglesPerProbe) { - if (!probeTriangles.empty()) - vparams->drawTool()->drawTriangles(probeTriangles, color_1); - }*/ - - if (!debug_hitPoints.empty()) - { - sofa::type::RGBAColor color(0.f, 0.f, 1.f, 1.f); // blue - vparams->drawTool()->drawPoints(debug_hitPoints, 5.0f, color); + if (!probeTriangles.empty()) + drawTool->drawTriangles(probeTriangles, roiColor); } + } - //if (!debug_RayIntersections_hitPoints.empty()) - //{ - // sofa::type::RGBAColor color(0.f, 0.f, 1.f, 1.f); // blue - // vparams->drawTool()->drawPoints(debug_RayIntersections_hitPoints, 10.0f, color); - //} - - - - + //---------------------------------- + // GENERAL HIT POINTS + if (d_drawHitPoints.getValue() && !debug_hitPoints.empty()) + { + drawTool->drawPoints(debug_hitPoints, 5.0f, hitColor); + } + //---------------------------------- + // RAY INTERSECTION POINTS + if (d_useDebugRay.getValue() && !debug_RayIntersections_hitPoints.empty()) + { + drawTool->drawPoints(debug_RayIntersections_hitPoints, 8.0f, rayColor); } + } diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index b865717..eb5e405 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -76,6 +76,10 @@ namespace sofa::infinytoolkit Data d_noiseSigma; Data d_K_points; Data d_maxStoredFrames; + Data d_useDebugRay; + Data d_drawROI; + Data d_drawHitPoints; + // Images @@ -89,6 +93,13 @@ namespace sofa::infinytoolkit private: + // + enum class IntersectionMethod + { + LineTriangle, + RayTriangle + }; + SingleLink< IVUSController, sofa::core::behavior::MechanicalState, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_CatheterState; @@ -115,9 +126,13 @@ namespace sofa::infinytoolkit std::vector debug_RayIntersections_hitPoints; - void debugcomputeIntersectionsLineTriangle(const Vec3& probePos); - void debugrayIntersection(const Vec3& probePos); - + Vec3 computeRayHitPoint( + const Vec3& origin, + const Vec3& raydir, + const std::vector& roitriangles); + + void debugSingleRayIntersection(const Vec3& probePos, const Vec3& tangent, double angle, IntersectionMethod method, const std::string& debugName); + void computePerpendicularPlane(const Vec3& tangent, Vec3& u, Vec3& v); }; } From 7e4c1d5b269f87b6802e991bc65ea46d8accfc42 Mon Sep 17 00:00:00 2001 From: rmolazem Date: Thu, 19 Mar 2026 16:47:05 +0100 Subject: [PATCH 17/17] Refactoring the polar grid image and adding Cartesian image generator. --- .../IVUSController/IVUSController.cpp | 137 ++++++++++++++++-- .../IVUSController/IVUSController.h | 20 +++ 2 files changed, 141 insertions(+), 16 deletions(-) diff --git a/src/InfinyToolkit/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp index c6f4706..dcb9e6f 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.cpp +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -68,6 +68,7 @@ namespace sofa::infinytoolkit , d_useDebugRay(initData(&d_useDebugRay, false, "useDebugRay", "Enable debug ray intersection.")) , d_drawROI(initData(&d_drawROI, false, "drawROI", "Draw ROI triangles.")) , d_drawHitPoints(initData(&d_drawHitPoints, true, "drawHitPoints", "Draw debug hit points.")) + , d_imPolar(initData(&d_imPolar,true,"polarGrid", "Generate the image in the polar grid.")) { } @@ -217,8 +218,8 @@ namespace sofa::infinytoolkit //accumulated += tempFrame64; - //// Store displayable version - //currentFrames.push_back(tempFrame.clone()); + // Store displayable version + currentFrames.push_back(tempFrame.clone()); } // Average over K points @@ -247,11 +248,18 @@ namespace sofa::infinytoolkit // Display images //cv::namedWindow("currentFrame", cv::WINDOW_AUTOSIZE); //cv::imshow("IVUS Longitudinal", longitudinalImage); - /* for (size_t k = 0; k < currentFrames.size(); ++k) + for (size_t k = 0; k < currentFrames.size(); ++k) { std::string name = "Probe " + std::to_string(k); + + int width = currentFrames[k].cols; + int height = currentFrames[k].rows; + + // allow resizing + cv::namedWindow(name, cv::WINDOW_NORMAL); + cv::resizeWindow(name, width, height); cv::imshow(name, currentFrames[k]); - }*/ + } //cv::imshow("Averaged IVUS", averagedFrame); @@ -268,6 +276,8 @@ namespace sofa::infinytoolkit const unsigned int N_depth = d_N_depth.getValue(); const double alpha = d_alpha.getValue(); const double reflectionCoeff = d_reflectionCoeff.getValue(); + bool imPolar = d_imPolar.getValue(); + cv::Mat frame = cv::Mat::zeros(N_depth, N_rays, CV_8UC1); @@ -294,7 +304,8 @@ namespace sofa::infinytoolkit Vec3 u, v; computePerpendicularPlane(tangent, u, v); - + // Compute nearestHit for all rays first + std::vector nearestHits(N_rays, maxDepth); //Shoot rays in the (u,v) plane for (unsigned int i = 0; i < N_rays; ++i) @@ -309,24 +320,43 @@ namespace sofa::infinytoolkit double nearestHit = (hitPoint - origin).norm(); + nearestHits[i] = std::min(nearestHit, maxDepth); // clamp to maxDepth + if (nearestHit < maxDepth) { debug_hitPoints.push_back(hitPoint); } - // Fill ultrasound frame - for (unsigned int j = 0; j < N_depth; ++j) - { - double depth = maxDepth * j / N_depth; - double attenuation = exp(-alpha * depth); - double intensity = 40 * attenuation; - - if (std::abs(depth - nearestHit) < (maxDepth / N_depth)) - intensity = 255 * d_reflectionCoeff.getValue() * attenuation; + + + + } - frame.at(j, i) = static_cast(std::min(255.0, intensity)); - } + // Fill ultrasound frame + if (imPolar) + { + frame = generatePolarImage( + N_depth, + N_rays, + maxDepth, + nearestHits, + alpha, + reflectionCoeff + ); + } + else + { + frame = generateCartesianImage( + 512, // width + 512, // height + N_depth, + N_rays, + maxDepth, + nearestHits, + alpha, + reflectionCoeff + ); } return frame; @@ -418,6 +448,81 @@ namespace sofa::infinytoolkit return nearestHit; } + uint8_t IVUSController::computeIntensity(const double depth, double nearestHit, + const double alpha,const double reflectionCoeff,const double maxDepth, const unsigned int N_depth) + { + + double attenuation = exp(-alpha * depth); + double intensity = 40 * attenuation; + + if (std::abs(depth - nearestHit) < (maxDepth / N_depth)) + intensity = 255 * reflectionCoeff * attenuation; + + return static_cast(std::min(255.0, intensity)); + } + + cv::Mat IVUSController::generatePolarImage(const unsigned int N_depth, const unsigned int N_rays + ,const double maxDepth, const std::vector& nearestHits,const double alpha,const double reflectionCoeff) + { + cv::Mat frame(N_depth, N_rays, CV_8UC1, cv::Scalar(0)); + + for (int i = 0; i < N_rays; ++i) + { + double nearestHitForRay = nearestHits[i]; // nearst hit per ray + for (int j = 0; j < N_depth; ++j) + { + const double depth = maxDepth * j / N_depth; + + frame.at(j, i) = + computeIntensity(depth, nearestHitForRay, alpha, + reflectionCoeff, maxDepth, N_depth); + } + } + + return frame; + } + + cv::Mat IVUSController::generateCartesianImage(const unsigned int width, const unsigned int height, const unsigned int N_depth, const unsigned int N_rays, + const double maxDepth, const std::vector& nearestHits + , const double alpha, const double reflectionCoeff) + { + cv::Mat img(height, width, CV_8UC1, cv::Scalar(0)); + + double cx = width / 2.0; + double cy = height / 2.0; + + for (int y = 0; y < height; ++y) + { + for (int x = 0; x < width; ++x) + { + double dx = x - cx; + double dy = y - cy; + + double depth = sqrt(dx * dx + dy * dy); + if (depth > maxDepth) + continue; + + double theta = atan2(dy, dx); + if (theta < 0) theta += 2 * M_PI; + + int j = static_cast(depth / maxDepth * N_depth); + int i = static_cast(theta / (2 * M_PI) * N_rays); + + if (j >= N_depth || i >= N_rays) + continue; + + double nearestHitForRay = nearestHits[i]; // per-angle nearestHit + + img.at(y, x) = + computeIntensity(depth, nearestHitForRay, alpha, + reflectionCoeff, maxDepth, N_depth); + } + } + + return img; + } + + void IVUSController::debugSingleRayIntersection( const Vec3& probePos, const Vec3& tangent, diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h index eb5e405..75cd484 100644 --- a/src/InfinyToolkit/IVUSController/IVUSController.h +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -79,6 +79,7 @@ namespace sofa::infinytoolkit Data d_useDebugRay; Data d_drawROI; Data d_drawHitPoints; + Data d_imPolar; @@ -130,9 +131,28 @@ namespace sofa::infinytoolkit const Vec3& origin, const Vec3& raydir, const std::vector& roitriangles); + + uint8_t computeIntensity(const double depth, double nearestHit, + const double alpha, const double reflectionCoeff, const double maxDepth, + const unsigned int N_depth); + + + + cv::Mat generatePolarImage(const unsigned int N_depth, + const unsigned int N_rays, const double maxDepth, + const std::vector& nearstHits, const double alpha, const double reflectionCoeff); + + cv::Mat generateCartesianImage(const unsigned int + width, const unsigned int height, + const unsigned int N_depth, const unsigned int N_rays , + const double maxDepth, + const std::vector& nearstHits, + const double alpha, + const double reflectionCoeff); void debugSingleRayIntersection(const Vec3& probePos, const Vec3& tangent, double angle, IntersectionMethod method, const std::string& debugName); void computePerpendicularPlane(const Vec3& tangent, Vec3& u, Vec3& v); + }; }