From b7a1ee83859efce19a27f2390e1161f2687b41cb Mon Sep 17 00:00:00 2001 From: Anthony Lombardi Date: Mon, 12 Aug 2024 14:56:53 -0400 Subject: [PATCH] WIP: Support collision detection --- libautoscoper/CMakeLists.txt | 1 + libautoscoper/src/Mesh.cpp | 67 +++++++++++++- libautoscoper/src/Mesh.hpp | 12 ++- libautoscoper/src/PSO.cpp | 94 ++++++++++++++++++++ libautoscoper/src/PSO.hpp | 16 ++++ libautoscoper/src/Tracker.cpp | 162 +++++++++++++++++++++++++++++++++- libautoscoper/src/Tracker.hpp | 21 ++++- libautoscoper/src/Trial.cpp | 12 +-- 8 files changed, 372 insertions(+), 13 deletions(-) diff --git a/libautoscoper/CMakeLists.txt b/libautoscoper/CMakeLists.txt index 00e6e26e..6ede4436 100644 --- a/libautoscoper/CMakeLists.txt +++ b/libautoscoper/CMakeLists.txt @@ -142,6 +142,7 @@ if (Autoscoper_COLLISION_DETECTION) # Add definitions for collision detection CommonCore CommonSystem FiltersSources + FiltersModeling IOGeometry ) if(NOT VTK_FOUND) diff --git a/libautoscoper/src/Mesh.cpp b/libautoscoper/src/Mesh.cpp index f8a42760..6ef92bd1 100644 --- a/libautoscoper/src/Mesh.cpp +++ b/libautoscoper/src/Mesh.cpp @@ -1,20 +1,81 @@ #include "Mesh.hpp" #include #include +#include +#include +#include Mesh::Mesh(const std::string& filename) { + this->filename = filename; + vtkSTLReader* reader = vtkSTLReader::New(); + this->polyData = vtkPolyData::New(); + reader->SetFileName(filename.c_str()); reader->Update(); this->polyData = reader->GetOutput(); + + // Create OBB Tree + this->meshOBB = vtkOBBTree::New(); + // this->meshOBB->SetDataSet(this->polyData); + this->meshOBB->SetMaxLevel(1); + + double corner[3]; + double maxAxis[3]; + double midAxis[3]; + double minAxis[3]; + double size[3]; + + this->meshOBB->ComputeOBB(this->polyData, corner, maxAxis, midAxis, minAxis, size); + + /*std::cout << "corner = " << corner[0] << ", " << corner[1] << ", " << corner[2] << std::endl; + std::cout << "maxAxis = " << maxAxis[0] << ", " << maxAxis[1] << ", " << maxAxis[2] << std::endl; + std::cout << "midAxis = " << midAxis[0] << ", " << midAxis[1] << ", " << midAxis[2] << std::endl; + std::cout << "minAxis = " << minAxis[0] << ", " << minAxis[1] << ", " << minAxis[2] << std::endl; + std::cout << "size = " << size[0] << ", " << size[1] << ", " << size[2] << std::endl;*/ + + /*this->meshOBB->Update(); + this->meshOBB->PrintSelf(std::cout, vtkIndent(2));*/ + + // Register so it exists after deleting the reader + this->polyData->Register(nullptr); + + // Compute bounding radius + double centerA[3]; + this->polyData->GetCenter(centerA); + + double bounds[6]; + this->polyData->GetBounds(bounds); + + boundingRadius = (bounds[1] - centerA[0]) * (bounds[1] - centerA[0]) + + (bounds[3] - centerA[1]) * (bounds[3] - centerA[1]) + + (bounds[5] - centerA[2]) * (bounds[5] - centerA[2]); + + boundingRadius = sqrt(boundingRadius); + + std::cout << "Mesh " << filename << ", has a bounding radius of: " << boundingRadius << std::endl; + reader->Delete(); } -Mesh::Mesh(const Mesh& other) +// Helper function to transform mesh after reading it in. Physically moves the mesh to the new transformed position. +void Mesh::Transform(double xAngle, double yAngle, double zAngle, double shiftX, double shiftY, double shiftZ) { - this->polyData = vtkPolyData::New(); - this->polyData->DeepCopy(other.polyData); + vtkNew transform; + // Shift in Y to overlay tiff (double bounding box center + transform->Translate(shiftX, shiftY, shiftZ); + transform->RotateX(xAngle); + transform->RotateY(yAngle); + transform->RotateZ(zAngle); + + vtkNew transformFilter; + transformFilter->SetInputData(this->polyData); + transformFilter->SetTransform(transform); + transformFilter->Update(); + + this->polyData = transformFilter->GetOutput(); + this->polyData->Register(nullptr); } void Mesh::Write(const std::string& filename) const diff --git a/libautoscoper/src/Mesh.hpp b/libautoscoper/src/Mesh.hpp index 5d066feb..03283e83 100644 --- a/libautoscoper/src/Mesh.hpp +++ b/libautoscoper/src/Mesh.hpp @@ -1,17 +1,25 @@ #pragma once #include #include +#include class Mesh { public: Mesh(const std::string& filename); - Mesh(const Mesh&); vtkPolyData* GetPolyData() const { return this->polyData; } void Write(const std::string& filename) const; + void Transform(double xAngle, double yAngle, double zAngle, double shiftX, double shiftY, double shiftZ); + + double getBoundingRadius() const { return boundingRadius; } + private: + double boundingRadius = 0.0; + std::string filename; + vtkPolyData* polyData; -}; \ No newline at end of file + vtkOBBTree* meshOBB; +}; diff --git a/libautoscoper/src/PSO.cpp b/libautoscoper/src/PSO.cpp index 0ce71c84..bb2b9620 100644 --- a/libautoscoper/src/PSO.cpp +++ b/libautoscoper/src/PSO.cpp @@ -3,12 +3,18 @@ #include // For FLT_MAX #include +#define VELOCITY_FILTER 0 +#define COLLISION_RESPONSE 0 + // Particle Struct Function Definitions Particle::Particle(const Particle& p) { this->NCC = p.NCC; this->Position = p.Position; this->Velocity = p.Velocity; +#ifdef Autoscoper_COLLISION_DETECTION + this->collided = p.collided; +#endif // Autoscoper_COLLISION_DETECTION } Particle::Particle() @@ -16,6 +22,10 @@ Particle::Particle() this->NCC = FLT_MAX; this->Position = std::vector(NUM_OF_DIMENSIONS, 0.f); this->Velocity = std::vector(NUM_OF_DIMENSIONS, 0.f); + +#ifdef Autoscoper_COLLISION_DETECTION + this->collided = false; +#endif // Autoscoper_COLLISION_DETECTION } Particle::Particle(const std::vector& pos) @@ -23,6 +33,10 @@ Particle::Particle(const std::vector& pos) this->NCC = FLT_MAX; this->Position = pos; this->Velocity = std::vector(NUM_OF_DIMENSIONS, 0.f); + +#ifdef Autoscoper_COLLISION_DETECTION + this->collided = false; +#endif // Autoscoper_COLLISION_DETECTION } Particle::Particle(float start_range_min, float start_range_max) @@ -31,6 +45,10 @@ Particle::Particle(float start_range_min, float start_range_max) this->Position = std::vector(NUM_OF_DIMENSIONS, 0.f); this->Velocity = std::vector(NUM_OF_DIMENSIONS, 0.f); this->initializePosition(start_range_min, start_range_max); + +#ifdef Autoscoper_COLLISION_DETECTION + this->collided = false; +#endif // Autoscoper_COLLISION_DETECTION } Particle& Particle::operator=(const Particle& p) @@ -38,6 +56,11 @@ Particle& Particle::operator=(const Particle& p) this->NCC = p.NCC; this->Position = p.Position; this->Velocity = p.Velocity; + +#ifdef Autoscoper_COLLISION_DETECTION + this->collided = p.collided; +#endif // Autoscoper_COLLISION_DETECTION + return *this; } @@ -69,6 +92,21 @@ void Particle::updateVelocityAndPosition(const Particle& pBest, const Particle& this->Velocity[dim] = omega * this->Velocity[dim] + c1 * rp * (pBest.Position[dim] - this->Position[dim]) + c2 * rg * (gBest.Position[dim] - this->Position[dim]); +#ifdef VELOCITY_FILTER + float velClamp = 0.2; + float speed = 0.0; + for (int i = 0; i < NUM_OF_DIMENSIONS; i++) { + speed += this->Velocity[i] * this->Velocity[i]; + } + speed = sqrt(speed); + if (speed > velClamp) { + for (int i = 0; i < NUM_OF_DIMENSIONS; i++) { + this->Velocity[i] /= speed; + this->Velocity[i] *= velClamp; + } + } +#endif // VELOCITY_FILTER + this->Position[dim] += this->Velocity[dim]; } } @@ -123,6 +161,40 @@ float getRandomClamped() return (float)rand() / (float)RAND_MAX; } +#ifdef Autoscoper_COLLISION_DETECTION +void checkCollision(Particle& p, + std::vector& avgNonCollidedPosition, + std::vector& avgCollidedPosition, + unsigned int& collidedCount) +{ + if (p.NCC > 1.0E4) { + collidedCount++; + p.collided = true; + + for (int i = 0; i < NUM_OF_DIMENSIONS; ++i) { + avgCollidedPosition[i] += p.Position[i]; + } + } else { + for (int i = 0; i < NUM_OF_DIMENSIONS; ++i) { + avgNonCollidedPosition[i] += p.Position[i]; + } + } +} + +void computeCorrectionVector(std::vector& avgNonCollidedPosition, + std::vector& avgCollidedPosition, + std::vector& correctionVector, + const unsigned int& collidedCount) +{ + if (collidedCount != 0 && collidedCount != NUM_OF_PARTICLES) { + for (int i = 0; i < NUM_OF_DIMENSIONS; ++i) { + avgNonCollidedPosition[i] /= ((float)NUM_OF_PARTICLES - (float)collidedCount); + avgCollidedPosition[i] /= (float)collidedCount; + } + } +} +#endif // Autoscoper_COLLISION_DETECTION + Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOCHS, unsigned int MAX_STALL) { int stall_iter = 0; @@ -154,6 +226,12 @@ Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOC } currentBest = gBest; +#ifdef Autoscoper_COLLISION_DETECTION + unsigned int collidedCount = 0; + std::vector avgNonCollidedPosition = std::vector(NUM_OF_DIMENSIONS, 0.f); + std::vector avgCollidedPosition = std::vector(NUM_OF_DIMENSIONS, 0.f); + std::vector correctionVector = std::vector(NUM_OF_DIMENSIONS, 0.f); +#endif // Autoscoper_COLLISION_DETECTION for (int idx = 0; idx < NUM_OF_PARTICLES; idx++) { @@ -172,7 +250,23 @@ Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOC if (particles[idx].NCC < gBest.NCC) { gBest = particles[idx]; } +#ifdef Autoscoper_COLLISION_DETECTION + checkCollision(particles[idx], avgNonCollidedPosition, avgCollidedPosition, collidedCount); +#endif // Autoscoper_COLLISION_DETECTION + } + +#if defined(Autoscoper_COLLISION_DETECTION) and defined(COLLISION_RESPONSE) + computeCorrectionVector(avgNonCollidedPosition, avgCollidedPosition, correctionVector, collidedCount); + for (int idx = 0; idx < NUM_OF_PARTICLES; ++idx) { + if (particles[idx].collided) { + for (int j = 0; j < NUM_OF_DIMENSIONS; ++j) { + particles[idx].Position[j] += ((float)collidedCount / (float)NUM_OF_PARTICLES) * correctionVector[j]; + pBest[idx] = particles[idx]; + // Why always update the pBest here? + } + } } +#endif // Autoscoper_COLLISION_DETECTION and COLLISION_RESPONSE OMEGA = OMEGA * 0.9f; diff --git a/libautoscoper/src/PSO.hpp b/libautoscoper/src/PSO.hpp index 52432a75..0a54ec2b 100644 --- a/libautoscoper/src/PSO.hpp +++ b/libautoscoper/src/PSO.hpp @@ -34,6 +34,10 @@ struct Particle std::vector Position; std::vector Velocity; +#ifdef Autoscoper_COLLISION_DETECTION + bool collided; +#endif // Autoscoper_COLLISION_DETECTION + // Copy constructor Particle(const Particle& p); // Default constructor @@ -52,3 +56,15 @@ extern std::ostream& operator<<(std::ostream& os, const std::vector& valu extern std::ostream& operator<<(std::ostream& os, const Particle& p); Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOCHS, unsigned int MAX_STALL); + +#ifdef Autoscoper_COLLISION_DETECTION +void checkCollision(Particle& p, + std::vector& avgNonCollidedPosition, + std::vector& avgCollidedPosition, + unsigned int& collidedCount); + +void computeCorrectionVector(std::vector& avgNonCollidedPosition, + std::vector& avgCollidedPosition, + std::vector& correctionVector, + const unsigned int& collidedCount); +#endif // Autoscoper_COLLISION_DETECTION diff --git a/libautoscoper/src/Tracker.cpp b/libautoscoper/src/Tracker.cpp index aea2d893..55320d9c 100644 --- a/libautoscoper/src/Tracker.cpp +++ b/libautoscoper/src/Tracker.cpp @@ -75,7 +75,7 @@ #include "PSO.hpp" static bool firstRun = true; - +static int m_numBoxTest = (int)0; #define DEBUG 0 // XXX @@ -210,6 +210,11 @@ Tracker::Tracker() , drr_mask_(NULL) , background_mask_(NULL) { +#ifdef Autoscoper_COLLISION_DETECTION + transformA = vtkTransform::New(); + transformB = vtkTransform::New(); + collide = vtkCollisionDetectionFilter::New(); +#endif g_markerless = this; optimization_method = 0; // initialize cost function cf_model_select = 0; // cost function selector @@ -240,6 +245,32 @@ void Tracker::load(const Trial& trial) { trial_ = trial; + // If meshes are present and collision detection is enabled, create colliders +#ifdef Autoscoper_COLLISION_DETECTION + if (trial_.meshes.size() > 0) { + for (int i = 0; i < trial_.meshes.size(); i++) { + for (int j = i + 1; j < trial_.meshes.size(); j++) { + + vtkCollisionDetectionFilter* collide = vtkCollisionDetectionFilter::New(); + collide->SetInputData(0, trial_.meshes[i].GetPolyData()); + collide->SetInputData(1, trial_.meshes[j].GetPolyData()); + collide->SetCollisionModeToFirstContact(); + collide->SetNumberOfCellsPerNode(2); + collide->Register(nullptr); + + std::pair, vtkCollisionDetectionFilter*> collider; + collider.first.first = i; + collider.first.second = j; + collider.second = collide; + + std::cout << "Creating collider between mesh " << i << " and mesh " << j << std::endl; + + colliders.push_back(collider); + } + } + } +#endif + std::vector::iterator viewIter; for (viewIter = views_.begin(); viewIter != views_.end(); ++viewIter) { delete *viewIter; @@ -741,7 +772,7 @@ std::vector Tracker::trackFrame(unsigned int volumeID, double* xyzypr) c return correlations; } // Minimizing Function for Bone Matching -double Tracker::minimizationFunc(const double* values) const +double Tracker::minimizationFunc(double* values) const { // Construct a coordinate frame from the given values float x_val = (*(const_cast(trial_)).getXCurve(-1))(trial_.frame); @@ -759,6 +790,27 @@ double Tracker::minimizationFunc(const double* values) const unsigned int idx = trial_.current_volume; xcframe.to_xyzypr(xyzypr); + +#ifdef Autoscoper_COLLISION_DETECTION + std::vector> poses; + if (trial_.meshes.size() > 0) { + for (unsigned int i = 0; i < trial_.meshes.size(); ++i) { + poses.push_back(std::vector(6)); + poses[i][0] = (*(const_cast(trial_)).getXCurve(i))(trial_.frame); + poses[i][1] = (*(const_cast(trial_)).getYCurve(i))(trial_.frame); + poses[i][2] = (*(const_cast(trial_)).getZCurve(i))(trial_.frame); + quat_val = (*(const_cast(trial_)).getQuatCurve(i))(trial_.frame); + eulers = quat_val.toEuler(); + poses[i][3] = eulers.z; + poses[i][4] = eulers.y; + poses[i][5] = eulers.x; + } + + if (computeCollisions(trial_.meshes, trial_.current_volume, xyzypr, poses)) + return 1.0E6; + } +#endif // Autoscoper_COLLISION_DETECTION + std::vector correlations = trackFrame(idx, &xyzypr[0]); double correlation = correlations[0]; @@ -1033,4 +1085,110 @@ void Tracker::getFullDRR(unsigned int volumeID) const } } +#ifdef Autoscoper_COLLISION_DETECTION +bool Tracker::computeCollisions(std::vector meshes, + unsigned int current_volume, + double* xyzypr, + std::vector> poses) const +{ + // meshes is a vector of meshes, one for each volume + // current_volume is the index of the volume we are currently tracking + // xyzypr is the possible new pose of the current volume + // poses is a vector of poses for all volumes + + // BEGIN COLLIDER BASED IMPLEMENTATION + int meshA = current_volume; + int meshB = -1; + + // Set up transformA + transformA->Identity(); + + // Apply Translation + transformA->Translate(xyzypr[0], xyzypr[1], xyzypr[2]); + + // Apply Rotation + transformA->RotateZ(xyzypr[3]); + transformA->RotateY(xyzypr[4]); + transformA->RotateX(xyzypr[5]); + + // Get bounding box to generate spherical sweep + double centerA[3]; + meshes[meshA].GetPolyData()->GetCenter(centerA); + + double radiusA = meshes[meshA].getBoundingRadius(); + + transformA->TransformVector(centerA, centerA); + + centerA[0] += xyzypr[0]; + centerA[1] += xyzypr[1]; + centerA[2] += xyzypr[2]; + + for (int i = 0; i < colliders.size(); i++) { + + if (colliders[i].first.first != current_volume && colliders[i].first.second != current_volume) { + continue; + } + + // Set meshB to be the non current volume mesh + meshB = (colliders[i].first.first == current_volume) ? meshB = colliders[i].first.second + : meshB = colliders[i].first.first; + + // Set up transformB + transformB->Identity(); + + // Translate to pose position + transformB->Translate(poses[meshB][0], poses[meshB][1], poses[meshB][2]); + // Apply Rotation + transformB->RotateZ(poses[meshB][3]); + transformB->RotateY(poses[meshB][4]); + transformB->RotateX(poses[meshB][5]); + + double centerB[3]; + meshes[meshB].GetPolyData()->GetCenter(centerB); + double radiusB = meshes[meshB].getBoundingRadius(); + + transformB->TransformVector(centerB, centerB); + centerB[0] += poses[meshB][0]; + centerB[1] += poses[meshB][1]; + centerB[2] += poses[meshB][2]; + + // Check for intersection of spherical representation + double distance = + sqrt(pow(centerA[0] - centerB[0], 2) + pow(centerA[1] - centerB[1], 2) + pow(centerA[2] - centerB[2], 2)); + + if (distance > radiusA + radiusB) { +# if DEBUG + std::cout << "Skipped in CD in spherical sweep" << std::endl; + std::cout << "Distance = " << distance << std::endl; + std::cout << "Radius sum = " << radiusA + radiusB << std::endl; +# endif + return false; + } + + if (colliders[i].first.first == current_volume) { + colliders[i].second->SetTransform(0, transformA); + colliders[i].second->SetTransform(1, transformB); + } + + if (colliders[i].first.second == current_volume) { + colliders[i].second->SetTransform(0, transformB); + colliders[i].second->SetTransform(1, transformA); + } + + colliders[i].second->Update(); + + m_numBoxTest += colliders[i].second->GetNumberOfBoxTests(); + + if (colliders[i].second->GetNumberOfContacts() != 0) { +# if DEBUG + std::cout << "Collision using " << colliders[i].second->GetNumberOfBoxTests() << " box test**" << std::endl; +# endif + return true; + } + } // END COLLIDER IMPLEMENTATION + + return false; +} +#endif // Autoscoper_COLLISION_DETECTION + } // namespace xromm diff --git a/libautoscoper/src/Tracker.hpp b/libautoscoper/src/Tracker.hpp index f8348d04..0b982a2e 100644 --- a/libautoscoper/src/Tracker.hpp +++ b/libautoscoper/src/Tracker.hpp @@ -58,6 +58,13 @@ # include "gpu/opencl/OpenCL.hpp" #endif +#ifdef Autoscoper_COLLISION_DETECTION +# include +# include +# include +# include "Mesh.hpp" +#endif + #include "Trial.hpp" namespace xromm { @@ -134,7 +141,7 @@ class Tracker double max_limit, int cf_model, unsigned int max_stall_iter); - double minimizationFunc(const double* values) const; + double minimizationFunc(double* values) const; std::vector trackFrame(unsigned int volumeID, double* xyzpr) const; std::vector& views() { return views_; } const std::vector& views() const { return views_; } @@ -158,6 +165,18 @@ class Tracker private: bool calculate_viewport(const CoordFrame& modelview, const Camera& camera, double* viewport) const; +#ifdef Autoscoper_COLLISION_DETECTION + bool computeCollisions(std::vector meshes, + unsigned int current_volume, + double* xyzypr, + std::vector> poses) const; + + vtkTransform* transformA; + vtkTransform* transformB; + + vtkCollisionDetectionFilter* collide; + std::vector, vtkCollisionDetectionFilter*>> colliders; +#endif int optimization_method = (int)0; int cf_model_select = (int)0; diff --git a/libautoscoper/src/Trial.cpp b/libautoscoper/src/Trial.cpp index e22ebbbf..1b0b4f44 100644 --- a/libautoscoper/src/Trial.cpp +++ b/libautoscoper/src/Trial.cpp @@ -297,11 +297,13 @@ void Trial::validate(const std::vector& mayaCams, } if (meshFiles.size() != 0 && volumeFiles.size() != meshFiles.size()) { - throw std::runtime_error(filename, std::string("You must specify a mesh file for each volume or none at all.\n") + "Found" - + std::to_string(volumeFiles.size()) - + " volumes " - "and " - + std::to_string(meshFiles.size()) + " mesh files.")); + throw std::runtime_error( + trialReadingError(filename, + std::string("You must specify a mesh file for each volume or none at all.\n") + "Found" + + std::to_string(volumeFiles.size()) + + " volumes " + "and " + + std::to_string(meshFiles.size()) + " mesh files.")); } }