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: 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/IVUSController/IVUSController.cpp b/src/InfinyToolkit/IVUSController/IVUSController.cpp new file mode 100644 index 0000000..8af8ea2 --- /dev/null +++ b/src/InfinyToolkit/IVUSController/IVUSController.cpp @@ -0,0 +1,265 @@ +/***************************************************************************** + * - 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 +#include +#include +#include +#include + + +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("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")) + , 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 + 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 + } + + this->f_listening.setValue(true); + + 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) + { + 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(d_N_depth.getValue(), d_N_rays.getValue(), 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, 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) + { + 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 = 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)); + } + } + + 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() < d_maxDepth.getValue()) + triangleIndices.push_back(i); + } + + + } + + + +} diff --git a/src/InfinyToolkit/IVUSController/IVUSController.h b/src/InfinyToolkit/IVUSController/IVUSController.h new file mode 100644 index 0000000..dba6422 --- /dev/null +++ b/src/InfinyToolkit/IVUSController/IVUSController.h @@ -0,0 +1,123 @@ +/***************************************************************************** + * - 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; + 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; + 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; + + 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; + + + // 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); + + }; +} + + + 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