diff --git a/src/driver/spinnakerdriver.cpp b/src/driver/spinnakerdriver.cpp index 5d17dc0..c58f413 100644 --- a/src/driver/spinnakerdriver.cpp +++ b/src/driver/spinnakerdriver.cpp @@ -16,9 +16,20 @@ #ifdef SPINNAKER #include "spinnakerdriver.h" +#include +#include +#include +#include #define CATCH_SPINNAKER(f) try { f; } catch (Spinnaker::Exception &e) { std::cerr << "[Spinnaker] Could not set parameter: " << e.GetFullErrorMessage() << std::endl; } +constexpr int kMinBufferCount = 3; // Minimum buffer count required for NewestOnly mode +constexpr size_t kUsb3PacketSize = 1024; // USB3 packet size alignment requirement +// constexpr size_t kPageSize = 4096; // Page size alignment requirement for Meteor Lake + +static size_t alignUp(size_t size, size_t alignment) { + return ((size + alignment - 1) / alignment) * alignment; +} class SpinnakerImage : public RawImage { public: @@ -114,23 +125,40 @@ SpinnakerDriver::SpinnakerDriver(const CameraConfig& config) { } pCam->TLStream.StreamBufferHandlingMode.SetValue(Spinnaker::StreamBufferHandlingMode_NewestOnly); - pCam->TLStream.StreamBufferCountManual.SetValue(pCam->TLStream.StreamBufferCountManual.GetMin()); + + // NewestOnly mode requires at least kMinBufferCount buffers + int requiredBuffers = std::max(kMinBufferCount, (int)pCam->TLStream.StreamBufferCountManual.GetMin()); + pCam->TLStream.StreamBufferCountManual.SetValue(requiredBuffers); // Provide image buffers to achieve faster mapping with OpenCL - int width = pCam->WidthMax.GetValue(); - int height = pCam->HeightMax.GetValue(); - for(int i = 0; i < pCam->TLStream.StreamBufferCountManual.GetMin(); i++) { - std::shared_ptr buffer = std::make_shared(&PixelFormat::RGGB8, width/2, height/2, "spinnaker"); - buffers[buffer] = std::make_unique>(buffer->write()); - } + int width = (int)pCam->Width.GetValue(); + int height = (int)pCam->Height.GetValue(); + size_t rawSize = (size_t)width * height; + + // Align to both USB3 packet and Page size for Meteor Lake stability/decoding + finalBufferSize_ = alignUp(rawSize, kUsb3PacketSize); std::vector bufferPtrs; - for (auto& item: buffers) - bufferPtrs.push_back(**item.second); + for(int i = 0; i < requiredBuffers; i++) { + void* alignedPtr = nullptr; + if (posix_memalign(&alignedPtr, kUsb3PacketSize, finalBufferSize_) != 0) { + throw std::runtime_error("[Spinnaker] posix_memalign failed"); + } + + // Track pointer for explicit free() in destructor + m_allocatedPtrs.push_back(alignedPtr); + + auto buffer = std::make_shared(&PixelFormat::RGGB8, width/2, height/2, (unsigned char*)alignedPtr); + + // Create OpenCL mapping + buffers[buffer] = std::make_unique>(buffer->write()); + + bufferPtrs.push_back(alignedPtr); + } + // Register user-owned buffers with the camera pCam->SetBufferOwnership(Spinnaker::SPINNAKER_BUFFER_OWNERSHIP_USER); - pCam->SetUserBuffers(bufferPtrs.data(), buffers.size(), width*height); - //pCam->Timestamp.SetValue(); + pCam->SetUserBuffers(bufferPtrs.data(), bufferPtrs.size(), finalBufferSize_); if (IsWritable(pCam->GevSCPSPacketSize)) { CATCH_SPINNAKER(pCam->GevSCPSPacketSize.SetValue(9000)); @@ -152,22 +180,27 @@ double SpinnakerDriver::expectedFrametime() { } SpinnakerDriver::~SpinnakerDriver() { - pCam->EndAcquisition(); -} + if (pCam) { + pCam->EndAcquisition(); + pCam->DeInit(); + } + // Explicitly free memory to prevent leaks + for (void* ptr : m_allocatedPtrs) { + if (ptr) free(ptr); + } + m_allocatedPtrs.clear(); +} std::shared_ptr SpinnakerDriver::borrow(const Spinnaker::ImagePtr& pImage) { void* data = pImage->GetData(); - for (auto& item : buffers) { - if(item.second != nullptr && **item.second == data) { - item.second = nullptr; - return item.first; - } + + auto it = m_fastBufferPool.find(data); + if (it != m_fastBufferPool.end() && it->second.clMap != nullptr) { + it->second.clMap = nullptr; // mark as in use + return it->second.image; } - std::cerr << "[Spinnaker] Did not get image with given buffer, creating new buffer; expect OpenCL performance degradation" << std::endl; - std::shared_ptr image = std::make_shared(&PixelFormat::RGGB8, (int)pImage->GetWidth() / 2, (int)pImage->GetHeight() / 2, (unsigned char*)pImage->GetData()); - buffers[image] = nullptr; - return image; + return std::make_shared(&PixelFormat::RGGB8, (int)pImage->GetWidth() / 2, (int)pImage->GetHeight() / 2, (unsigned char*)data); } void SpinnakerDriver::restore(const RawImage& image) { diff --git a/src/driver/spinnakerdriver.h b/src/driver/spinnakerdriver.h index af0d52b..bdaa22a 100644 --- a/src/driver/spinnakerdriver.h +++ b/src/driver/spinnakerdriver.h @@ -19,25 +19,33 @@ #include "cameradriver.h" #include "Spinnaker.h" +// Map raw pointer addresses to corresponding {RawImage, CLMap} context +struct BufferContext { + std::shared_ptr image; + std::unique_ptr> clMap; +}; class SpinnakerDriver : public CameraDriver { -public: - explicit SpinnakerDriver(const CameraConfig& config); - ~SpinnakerDriver() override; + public: + explicit SpinnakerDriver(const CameraConfig& config); + ~SpinnakerDriver() override; - std::shared_ptr readImage() override; + std::shared_ptr readImage() override; - const PixelFormat format() override; + const PixelFormat format() override; - double expectedFrametime() override; + double expectedFrametime() override; - std::shared_ptr borrow(const Spinnaker::ImagePtr& pImage); - void restore(const RawImage& image); + std::shared_ptr borrow(const Spinnaker::ImagePtr& pImage); + void restore(const RawImage& image); -private: - Spinnaker::SystemPtr pSystem; - Spinnaker::CameraPtr pCam; + private: + Spinnaker::SystemPtr pSystem; + Spinnaker::CameraPtr pCam; - std::map, std::unique_ptr>> buffers; // Use own image buffers for page size alignment (OpenCL pinned memory and zero copy) + std::map, std::unique_ptr>> buffers; // Use own image buffers for page size alignment (OpenCL pinned memory and zero copy) + size_t finalBufferSize_ = 0; // Allocated buffer size per frame, used for buffer matching in borrow() + std::vector m_allocatedPtrs; + std::map m_fastBufferPool; }; -#endif \ No newline at end of file +#endif diff --git a/src/main.cpp b/src/main.cpp index 5523b9c..e4de0a8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -421,6 +421,7 @@ int main(int argc, char* argv[]) { #else detection->set_t_sent(r.camera->getTime()); #endif + std::cout << wrapper.DebugString() << std::endl; r.socket->send(wrapper); r.socket->updateTime(); r.openCl->clearEvents();