diff --git a/Modules/ToFHardware/Kinect/mitkKinectController.cpp b/Modules/ToFHardware/Kinect/mitkKinectController.cpp index 7eca0d9b92..2d6189ec0e 100644 --- a/Modules/ToFHardware/Kinect/mitkKinectController.cpp +++ b/Modules/ToFHardware/Kinect/mitkKinectController.cpp @@ -1,281 +1,293 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkKinectController.h" #include namespace mitk { class KinectController::KinectControllerPrivate { public: KinectControllerPrivate(); ~KinectControllerPrivate(); bool ErrorText(unsigned int error); //OpenNI related stuff xn::Context m_Context; ///< OpenNI context xn::DepthGenerator m_DepthGenerator; ///< Depth generator to access depth image of kinect xn::ImageGenerator m_ImageGenerator; ///< Image generator to access RGB image of kinect xn::IRGenerator m_IRGenerator; ///< IR generator to access IR image of kinect bool m_ConnectionCheck; ///< check if camera is connected or not bool m_UseIR; ///< flag indicating whether IR image is used or not unsigned int m_CaptureWidth; ///< image width unsigned int m_CaptureHeight; ///< image height }; KinectController::KinectControllerPrivate::KinectControllerPrivate(): m_Context(NULL), m_DepthGenerator(NULL), m_ImageGenerator(NULL), m_IRGenerator(NULL), m_ConnectionCheck(false), m_UseIR(false), m_CaptureWidth(640), m_CaptureHeight(480) { } KinectController::KinectControllerPrivate::~KinectControllerPrivate() { } bool KinectController::KinectControllerPrivate::ErrorText(unsigned int error) { if(error != XN_STATUS_OK) { MITK_ERROR << "Camera Error " << xnGetStatusString(error); return false; } else return true; } KinectController::KinectController(): d(new KinectControllerPrivate) { } KinectController::~KinectController() { delete d; } bool KinectController::OpenCameraConnection() { if (!d->m_ConnectionCheck) { // Initialize the OpenNI status - d->m_ConnectionCheck = !d->ErrorText(d->m_Context.Init()); + d->m_ConnectionCheck = d->ErrorText(d->m_Context.Init()); + if (!d->m_ConnectionCheck) return false; // Create a depth generator and set its resolution XnMapOutputMode DepthMode; - d->m_ConnectionCheck = !d->ErrorText(d->m_DepthGenerator.Create(d->m_Context)); + d->m_ConnectionCheck = d->ErrorText(d->m_DepthGenerator.Create(d->m_Context)); + if (!d->m_ConnectionCheck) return false; d->m_DepthGenerator.GetMapOutputMode(DepthMode); DepthMode.nXRes = xn::Resolution((XnResolution)XN_RES_VGA).GetXResolution(); DepthMode.nYRes = xn::Resolution((XnResolution)XN_RES_VGA).GetYResolution(); - d->m_ConnectionCheck = !d->ErrorText(d->m_DepthGenerator.SetMapOutputMode(DepthMode)); + d->m_ConnectionCheck = d->ErrorText(d->m_DepthGenerator.SetMapOutputMode(DepthMode)); + if (!d->m_ConnectionCheck) return false; if (d->m_UseIR) { // Create the IR generator and set its resolution - d->m_ConnectionCheck = !d->ErrorText(d->m_IRGenerator.Create(d->m_Context)); + d->m_ConnectionCheck = d->ErrorText(d->m_IRGenerator.Create(d->m_Context)); + if (!d->m_ConnectionCheck) return false; XnMapOutputMode IRMode; d->m_IRGenerator.GetMapOutputMode(IRMode); IRMode.nXRes = XN_VGA_X_RES; IRMode.nYRes = XN_VGA_Y_RES; IRMode.nFPS = 30; - d->m_ConnectionCheck = !d->ErrorText(d->m_IRGenerator.SetMapOutputMode(IRMode)); + d->m_ConnectionCheck = d->ErrorText(d->m_IRGenerator.SetMapOutputMode(IRMode)); + if (!d->m_ConnectionCheck) return false; } else { // Create an image generator and set its resolution XnMapOutputMode ImageMode; - d->m_ConnectionCheck = !d->ErrorText(d->m_ImageGenerator.Create(d->m_Context)); + d->m_ConnectionCheck = d->ErrorText(d->m_ImageGenerator.Create(d->m_Context)); + if (!d->m_ConnectionCheck) return false; d->m_ImageGenerator.GetMapOutputMode(ImageMode); ImageMode.nXRes = xn::Resolution((XnResolution)XN_RES_VGA).GetXResolution(); ImageMode.nYRes = xn::Resolution((XnResolution)XN_RES_VGA).GetYResolution(); - d->m_ConnectionCheck = !d->ErrorText(d->m_ImageGenerator.SetMapOutputMode(ImageMode)); + d->m_ConnectionCheck = d->ErrorText(d->m_ImageGenerator.SetMapOutputMode(ImageMode)); + if (!d->m_ConnectionCheck) return false; } // Camera registration if ( d->m_DepthGenerator.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT) ) { if (d->m_UseIR) { - d->m_ConnectionCheck = !d->ErrorText(d->m_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(d->m_IRGenerator)); + d->m_ConnectionCheck = d->ErrorText(d->m_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(d->m_IRGenerator)); + if (!d->m_ConnectionCheck) return false; } else { - d->m_ConnectionCheck = !d->ErrorText(d->m_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(d->m_ImageGenerator)); + d->m_ConnectionCheck = d->ErrorText(d->m_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(d->m_ImageGenerator)); + if (!d->m_ConnectionCheck) return false; } } else { std::cout << "Alternative view point not supported by the depth generator..." << std::endl; } if (d->m_UseIR) { if ( d->m_IRGenerator.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT) ) { - d->m_ConnectionCheck = !d->ErrorText(d->m_IRGenerator.GetAlternativeViewPointCap().SetViewPoint(d->m_DepthGenerator)); + d->m_ConnectionCheck = d->ErrorText(d->m_IRGenerator.GetAlternativeViewPointCap().SetViewPoint(d->m_DepthGenerator)); + if (!d->m_ConnectionCheck) return false; } else { std::cout << "Alternative view point not supported by the depth generator..." << std::endl; } } // Mirror data d->m_ConnectionCheck = d->ErrorText(d->m_Context.SetGlobalMirror(!d->m_Context.GetGlobalMirror())); + if (!d->m_ConnectionCheck) return false; // Start data generation d->m_ConnectionCheck = d->ErrorText(d->m_Context.StartGeneratingAll()); + if (!d->m_ConnectionCheck) return false; // // Update the connected flag // d->m_ConnectionCheck = true; } return d->m_ConnectionCheck; } bool KinectController::CloseCameraConnection() { d->m_ConnectionCheck = !d->ErrorText(d->m_Context.StopGeneratingAll()); return !d->m_ConnectionCheck; } bool KinectController::UpdateCamera() { bool updateSuccessful = d->ErrorText(d->m_Context.WaitAndUpdateAll()); xn::DepthMetaData DepthMD; d->m_DepthGenerator.GetMetaData(DepthMD); d->m_CaptureWidth = DepthMD.XRes(); d->m_CaptureHeight = DepthMD.YRes(); return updateSuccessful; } // TODO flag image void KinectController::GetDistances(float* distances) { xn::DepthMetaData DepthMD; d->m_DepthGenerator.GetMetaData(DepthMD); const XnDepthPixel* DepthData = DepthMD.Data(); for (unsigned int i=0; im_CaptureWidth*d->m_CaptureHeight; i++) { distances[i] = DepthData[i]; } } void KinectController::GetRgb(unsigned char* rgb) { if (!d->m_UseIR) { xn::ImageMetaData ImageMD; d->m_ImageGenerator.GetMetaData(ImageMD); const XnRGB24Pixel* rgbPixelArray = ImageMD.RGB24Data(); for (int i=0; im_CaptureWidth*d->m_CaptureHeight; i++) { rgb[i*3] = rgbPixelArray[i].nRed; rgb[i*3+1] = rgbPixelArray[i].nGreen; rgb[i*3+2] = rgbPixelArray[i].nBlue; } } } void KinectController::GetAllData(float* distances, float* amplitudes, unsigned char* rgb) { // get current distance data xn::DepthMetaData DepthMD; d->m_DepthGenerator.GetMetaData(DepthMD); const XnDepthPixel* DepthData = DepthMD.Data(); // IR data xn::IRMetaData IRData; const XnIRPixel* IRPixelData; // Image data xn::ImageMetaData ImageMD; const XnRGB24Pixel* rgbPixelArray; if (d->m_UseIR) { d->m_IRGenerator.GetMetaData(IRData); IRPixelData = IRData.Data(); } else { // get current rgb data d->m_ImageGenerator.GetMetaData(ImageMD); rgbPixelArray = ImageMD.RGB24Data(); } for (unsigned int i=0; im_CaptureWidth*d->m_CaptureHeight; i++) { distances[i] = DepthData[i]; if (d->m_UseIR) { amplitudes[i] = IRPixelData[i]; } else { rgb[i*3] = rgbPixelArray[i].nRed; rgb[i*3+1] = rgbPixelArray[i].nGreen; rgb[i*3+2] = rgbPixelArray[i].nBlue; } } } void KinectController::GetAmplitudes( float* amplitudes ) { if (d->m_UseIR) { xn::IRMetaData IRData; d->m_IRGenerator.GetMetaData(IRData); const XnIRPixel* IRPixelData = IRData.Data(); for (unsigned int i=0; im_CaptureWidth*d->m_CaptureHeight; i++) { amplitudes[i] = IRPixelData[i]; } } } void KinectController::GetIntensities( float* intensities ) { } unsigned int KinectController::GetCaptureWidth() const { return d->m_CaptureWidth; } unsigned int KinectController::GetCaptureHeight() const { return d->m_CaptureHeight; } bool KinectController::GetUseIR() const { return d->m_UseIR; } void KinectController::SetUseIR(bool useIR) { if (d->m_UseIR!=useIR) { d->m_UseIR = useIR; this->Modified(); } } } diff --git a/Modules/ToFHardware/mitkToFImageGrabber.cpp b/Modules/ToFHardware/mitkToFImageGrabber.cpp index 5b2b0a24b2..4f0820f507 100644 --- a/Modules/ToFHardware/mitkToFImageGrabber.cpp +++ b/Modules/ToFHardware/mitkToFImageGrabber.cpp @@ -1,340 +1,340 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkToFImageGrabber.h" //#include "mitkToFCameraPMDCamCubeDevice.h" #include "itkCommand.h" namespace mitk { ToFImageGrabber::ToFImageGrabber():m_CaptureWidth(204),m_CaptureHeight(204),m_PixelNumber(41616), m_ImageSequence(0), m_RGBImageWidth(0), m_RGBImageHeight(0), m_RGBPixelNumber(0), m_IntensityArray(NULL), m_DistanceArray(NULL), m_AmplitudeArray(NULL), m_SourceDataArray(NULL), m_RgbDataArray(NULL) { // Create the output. We use static_cast<> here because we know the default // output must be of type TOutputImage OutputImageType::Pointer output0 = static_cast(this->MakeOutput(0).GetPointer()); OutputImageType::Pointer output1 = static_cast(this->MakeOutput(1).GetPointer()); OutputImageType::Pointer output2 = static_cast(this->MakeOutput(2).GetPointer()); OutputImageType::Pointer output3 = static_cast(this->MakeOutput(3).GetPointer()); mitk::ImageSource::SetNumberOfRequiredOutputs(3); mitk::ImageSource::SetNthOutput(0, output0.GetPointer()); mitk::ImageSource::SetNthOutput(1, output1.GetPointer()); mitk::ImageSource::SetNthOutput(2, output2.GetPointer()); mitk::ImageSource::SetNthOutput(3, output3.GetPointer()); } ToFImageGrabber::~ToFImageGrabber() { if (m_IntensityArray||m_AmplitudeArray||m_DistanceArray||m_RgbDataArray) { if (m_ToFCameraDevice) { m_ToFCameraDevice->RemoveObserver(m_DeviceObserverTag); } this->DisconnectCamera(); this->CleanUpImageArrays(); } } mitk::ImageSource::DataObjectPointer mitk::ImageSource::MakeOutput(unsigned int) { return static_cast(OutputImageType::New().GetPointer()); } void ToFImageGrabber::GenerateData() { int requiredImageSequence = 0; int capturedImageSequence = 0; unsigned int dimensions[3]; dimensions[0] = this->m_ToFCameraDevice->GetCaptureWidth(); dimensions[1] = this->m_ToFCameraDevice->GetCaptureHeight(); dimensions[2] = 1; mitk::PixelType FloatType = MakeScalarPixelType(); // acquire new image data this->m_ToFCameraDevice->GetAllImages(this->m_DistanceArray, this->m_AmplitudeArray, this->m_IntensityArray, this->m_SourceDataArray, requiredImageSequence, this->m_ImageSequence, this->m_RgbDataArray ); mitk::Image::Pointer distanceImage = this->GetOutput(0); if (!distanceImage->IsInitialized()) { distanceImage->ReleaseData(); distanceImage->Initialize(FloatType, 3, dimensions, 1); } mitk::Image::Pointer amplitudeImage = this->GetOutput(1); if (!amplitudeImage->IsInitialized()) { amplitudeImage->ReleaseData(); amplitudeImage->Initialize(FloatType, 3, dimensions, 1); } mitk::Image::Pointer intensityImage = this->GetOutput(2); if (!intensityImage->IsInitialized()) { intensityImage->ReleaseData(); intensityImage->Initialize(FloatType, 3, dimensions, 1); } unsigned int rgbDimension[3]; rgbDimension[0] = this->GetRGBImageWidth(); rgbDimension[1] = this->GetRGBImageHeight(); rgbDimension[2] = 1 ; mitk::Image::Pointer rgbImage = this->GetOutput(3); if (!rgbImage->IsInitialized()) { rgbImage->ReleaseData(); rgbImage->Initialize(mitk::PixelType(MakePixelType, 3>()), 3, rgbDimension,1); } capturedImageSequence = this->m_ImageSequence; if (m_DistanceArray) { distanceImage->SetSlice(this->m_DistanceArray, 0, 0, 0); } if (m_AmplitudeArray) { amplitudeImage->SetSlice(this->m_AmplitudeArray, 0, 0, 0); } if (m_IntensityArray) { intensityImage->SetSlice(this->m_IntensityArray, 0, 0, 0); } if (m_RgbDataArray) { rgbImage->SetSlice(this->m_RgbDataArray, 0, 0, 0); } } bool ToFImageGrabber::ConnectCamera() { bool ok = m_ToFCameraDevice->ConnectCamera(); if (ok) { this->m_CaptureWidth = this->m_ToFCameraDevice->GetCaptureWidth(); this->m_CaptureHeight = this->m_ToFCameraDevice->GetCaptureHeight(); this->m_PixelNumber = this->m_CaptureWidth * this->m_CaptureHeight; this->m_RGBImageWidth = this->m_ToFCameraDevice->GetRGBCaptureWidth(); this->m_RGBImageHeight = this->m_ToFCameraDevice->GetRGBCaptureHeight(); this->m_RGBPixelNumber = this->m_RGBImageWidth * this->m_RGBImageHeight; this->m_SourceDataSize = m_ToFCameraDevice->GetSourceDataSize(); this->AllocateImageArrays(); } - return true; + return ok; } bool ToFImageGrabber::DisconnectCamera() { bool success = m_ToFCameraDevice->DisconnectCamera(); return success; } void ToFImageGrabber::StartCamera() { m_ToFCameraDevice->StartCamera(); } void ToFImageGrabber::StopCamera() { m_ToFCameraDevice->StopCamera(); } bool ToFImageGrabber::IsCameraActive() { return m_ToFCameraDevice->IsCameraActive(); } bool ToFImageGrabber::IsCameraConnected() { return m_ToFCameraDevice->IsCameraConnected(); } void ToFImageGrabber::SetCameraDevice(ToFCameraDevice* aToFCameraDevice) { m_ToFCameraDevice = aToFCameraDevice; itk::SimpleMemberCommand::Pointer modifiedCommand = itk::SimpleMemberCommand::New(); modifiedCommand->SetCallbackFunction(this, &ToFImageGrabber::OnToFCameraDeviceModified); m_DeviceObserverTag = m_ToFCameraDevice->AddObserver(itk::ModifiedEvent(), modifiedCommand); this->Modified(); } ToFCameraDevice* ToFImageGrabber::GetCameraDevice() { return m_ToFCameraDevice; } int ToFImageGrabber::GetCaptureWidth() { return m_CaptureWidth; } int ToFImageGrabber::GetCaptureHeight() { return m_CaptureHeight; } int ToFImageGrabber::GetPixelNumber() { return m_PixelNumber; } int ToFImageGrabber::GetRGBImageWidth() { return m_RGBImageWidth; } int ToFImageGrabber::GetRGBImageHeight() { return m_RGBImageHeight; } int ToFImageGrabber::GetRGBPixelNumber() { return m_RGBPixelNumber; } int ToFImageGrabber::SetModulationFrequency(int modulationFrequency) { this->m_ToFCameraDevice->SetProperty("ModulationFrequency",mitk::IntProperty::New(modulationFrequency)); this->Modified(); modulationFrequency = this->GetModulationFrequency(); // return the new valid modulation frequency from the camera return modulationFrequency; } int ToFImageGrabber::SetIntegrationTime(int integrationTime) { this->m_ToFCameraDevice->SetProperty("IntegrationTime",mitk::IntProperty::New(integrationTime)); this->Modified(); integrationTime = this->GetIntegrationTime(); // return the new valid integration time from the camera return integrationTime; } int ToFImageGrabber::GetIntegrationTime() { int integrationTime = 0; this->m_ToFCameraDevice->GetIntProperty("IntegrationTime",integrationTime); return integrationTime; } int ToFImageGrabber::GetModulationFrequency() { int modulationFrequency = 0; this->m_ToFCameraDevice->GetIntProperty("ModulationFrequency",modulationFrequency); return modulationFrequency; } void ToFImageGrabber::SetBoolProperty( const char* propertyKey, bool boolValue ) { SetProperty(propertyKey, mitk::BoolProperty::New(boolValue)); } void ToFImageGrabber::SetIntProperty( const char* propertyKey, int intValue ) { SetProperty(propertyKey, mitk::IntProperty::New(intValue)); } void ToFImageGrabber::SetFloatProperty( const char* propertyKey, float floatValue ) { SetProperty(propertyKey, mitk::FloatProperty::New(floatValue)); } void ToFImageGrabber::SetStringProperty( const char* propertyKey, const char* stringValue ) { SetProperty(propertyKey, mitk::StringProperty::New(stringValue)); } void ToFImageGrabber::SetProperty( const char *propertyKey, BaseProperty* propertyValue ) { this->m_ToFCameraDevice->SetProperty(propertyKey, propertyValue); } bool ToFImageGrabber::GetBoolProperty( const char* propertyKey) { mitk::BoolProperty::Pointer boolProp = dynamic_cast(GetProperty(propertyKey)); if(!boolProp) return false; return boolProp->GetValue(); } int ToFImageGrabber::GetIntProperty( const char* propertyKey) { mitk::IntProperty::Pointer intProp = dynamic_cast(GetProperty(propertyKey)); return intProp->GetValue(); } float ToFImageGrabber::GetFloatProperty( const char* propertyKey) { mitk::FloatProperty::Pointer floatProp = dynamic_cast(GetProperty(propertyKey)); return floatProp->GetValue(); } const char* ToFImageGrabber::GetStringProperty( const char* propertyKey) { mitk::StringProperty::Pointer stringProp = dynamic_cast(GetProperty(propertyKey)); return stringProp->GetValue(); } BaseProperty* ToFImageGrabber::GetProperty( const char *propertyKey) { return this->m_ToFCameraDevice->GetProperty(propertyKey); } void ToFImageGrabber::OnToFCameraDeviceModified() { this->Modified(); } void ToFImageGrabber::CleanUpImageArrays() { // free buffer if (m_IntensityArray) { delete [] m_IntensityArray; m_IntensityArray = NULL; } if (m_DistanceArray) { delete [] m_DistanceArray; m_DistanceArray = NULL; } if (m_AmplitudeArray) { delete [] m_AmplitudeArray; m_AmplitudeArray = NULL; } if (m_SourceDataArray) { delete [] m_SourceDataArray; m_SourceDataArray = NULL; } if (m_RgbDataArray) { delete [] m_RgbDataArray; m_RgbDataArray = NULL; } } void ToFImageGrabber::AllocateImageArrays() { // cleanup memory if necessary this->CleanUpImageArrays(); // allocate buffer m_IntensityArray = new float[m_PixelNumber]; m_DistanceArray = new float[m_PixelNumber]; m_AmplitudeArray = new float[m_PixelNumber]; m_SourceDataArray = new char[m_SourceDataSize]; m_RgbDataArray = new unsigned char[m_RGBPixelNumber*3]; } }