diff --git a/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp b/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp index 3d117f0483..e060f29831 100644 --- a/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp +++ b/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp @@ -1,83 +1,91 @@ /*=================================================================== 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 #include #include /**Documentation * test for the class "ToFProcessingCommon". */ int mitkToFProcessingCommonTest(int /* argc */, char* /*argv*/[]) { MITK_TEST_BEGIN("ToFProcessingCommon"); unsigned int i = 10; unsigned int j = 50; mitk::ScalarType distance = 1000; mitk::ScalarType focalLength = 10; mitk::Point2D focalLength_XY; focalLength_XY[0] = 200; focalLength_XY[1] = 200; mitk::Point2D interPixelDistance; interPixelDistance[0] = 0.05; interPixelDistance[1] = 0.05; mitk::Point2D principalPoint; principalPoint[0] = 100; principalPoint[1] = 100; // expected coordinate mitk::ToFProcessingCommon::ToFPoint3D expectedCoordinate; expectedCoordinate[0] = -400.0988; expectedCoordinate[1] = -222.2771; expectedCoordinate[2] = 889.1084; // resulting coordinate without using the interpixeldistance mitk::ToFProcessingCommon::ToFPoint3D resultingCoordinate = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLength_XY,principalPoint); MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedCoordinate,resultingCoordinate,1e-3),"Testing IndexToCartesianCoordinates()"); // resulting coordinate with using the interpixeldistance mitk::ToFProcessingCommon::ToFPoint3D resultingCoordinateInterpix = mitk::ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(i,j,distance,focalLength,interPixelDistance,principalPoint); MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedCoordinate,resultingCoordinateInterpix,1e-3),"Testing IndexToCartesianCoordinatesWithInterpixdist()"); // expected index mitk::ToFProcessingCommon::ToFPoint3D expectedIndex; expectedIndex[0] = i; expectedIndex[1] = j; expectedIndex[2] = 1000; mitk::ToFProcessingCommon::ToFPoint3D resultingIndex = mitk::ToFProcessingCommon::CartesianToIndexCoordinates(expectedCoordinate,focalLength_XY,principalPoint); MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedIndex,resultingIndex,1e-3),"Testing CartesianToIndexCoordinates()"); mitk::ToFProcessingCommon::ToFPoint3D resultingIndexInterpix = mitk::ToFProcessingCommon::CartesianToIndexCoordinatesWithInterpixdist(expectedCoordinate,focalLength,interPixelDistance,principalPoint); MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedIndex,resultingIndexInterpix,1e-3),"Testing CartesianToIndexCoordinatesWithInterpixdist()"); //########## Kinect Reconstruction ############# mitk::ToFProcessingCommon::ToFPoint3D expectedKinectCoordinate; expectedKinectCoordinate[0] = -450.0; expectedKinectCoordinate[1] = -250.0; expectedKinectCoordinate[2] = 1000.0; mitk::Index3D index; index[0] = i; index[1] = j; index[2] = 0; mitk::ToFProcessingCommon::ToFPoint3D kinectReconstructionResult = mitk::ToFProcessingCommon::KinectIndexToCartesianCoordinates(index,distance, focalLength_XY,principalPoint); - MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedKinectCoordinate,kinectReconstructionResult),"Testing KinectIndexToCartesianCoordinates()"); + MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedKinectCoordinate,kinectReconstructionResult),"Compare the expected result with the result of reconstruction from KinectIndexToCartesianCoordinates()"); mitk::ToFProcessingCommon::ToFPoint3D kinectReconstructionResultBackward = mitk::ToFProcessingCommon::CartesianToKinectIndexCoordinates(kinectReconstructionResult, focalLength_XY, principalPoint); - MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedIndex,kinectReconstructionResultBackward),"Testing CartesianToKinectIndexCoordinates()"); + MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedIndex,kinectReconstructionResultBackward),"Transform everything back to distance image and compare it to the original input"); + + mitk::Point2D continuousIndex; + continuousIndex[0] = i; + continuousIndex[1] = j; + mitk::ToFProcessingCommon::ToFPoint3D continuousKinectReconstructionResult = mitk::ToFProcessingCommon::ContinuousKinectIndexToCartesianCoordinates(continuousIndex,distance, focalLength_XY,principalPoint); + + MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedKinectCoordinate,continuousKinectReconstructionResult),"Compare the expected result with the result of reconstruction from ContinuousKinectIndexToCartesianCoordinates(). Since the index is not continuous, the result has to be the same like for KinectIndexToCartesianCoordinates()."); + //########## End Kinect Reconstruction ############# MITK_TEST_END(); } diff --git a/Modules/ToFProcessing/mitkToFProcessingCommon.cpp b/Modules/ToFProcessing/mitkToFProcessingCommon.cpp index 6390d2a92c..94a907b27a 100644 --- a/Modules/ToFProcessing/mitkToFProcessingCommon.cpp +++ b/Modules/ToFProcessing/mitkToFProcessingCommon.cpp @@ -1,153 +1,163 @@ /*=================================================================== 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 "mitkToFProcessingCommon.h" namespace mitk { ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY) { ToFPoint3D cartesianCoordinates; // calculate image coordinates in pixel units; // Note: pixel unit (pX) in x direction does normally not equal pixel unit (pY) in y direction. // Therefore, a transformation in one of the pixel units is necessary // Here, pX as image coordinate unit is chosen // pY = (focalLengthX / focalLengthY) * pX ToFScalarType imageX = i - principalPointX; ToFScalarType imageY = j - principalPointY; ToFScalarType imageY_in_pX = imageY * (focalLengthX / focalLengthY); //distance from pinhole to pixel (i,j) in pX units (pixel unit in x direction) ToFScalarType d_in_pX = sqrt(imageX*imageX + imageY_in_pX*imageY_in_pX + focalLengthX*focalLengthX); cartesianCoordinates[0] = distance * imageX / d_in_pX; //Strahlensatz: x / imageX = distance / d cartesianCoordinates[1] = distance * imageY_in_pX / d_in_pX; //Strahlensatz: y / imageY = distances / d cartesianCoordinates[2] = distance * focalLengthX / d_in_pX; //Strahlensatz: z / f = distance / d. return cartesianCoordinates; } //See also "Hacking the Kinect" - Jeff Kramer, Matt Parker, Daniel Herrera C., Nicolas Burrus, Florian Echtler, Chapter 7, Part 1 "Moving from Depth Map to Point Cloud. ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::KinectIndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY) { ToFPoint3D cartesianCoordinates; cartesianCoordinates[0] = distance * (i - principalPointX) / focalLengthX; cartesianCoordinates[1] = distance * (j - principalPointY) / focalLengthY; cartesianCoordinates[2] = distance; return cartesianCoordinates; } ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::CartesianToKinectIndexCoordinates(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY, ToFScalarType cartesianPointZ, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance) { ToFPoint3D indexCoordinatesAndDistanceValue; indexCoordinatesAndDistanceValue[0] = ((cartesianPointX*focalLengthX)/cartesianPointZ) + principalPointX; indexCoordinatesAndDistanceValue[1] = ((cartesianPointY*focalLengthY)/cartesianPointZ) + principalPointY; if (calculateDistance) { //There is no computation for kinect. See KinectIndexToCartesianCoordinates. indexCoordinatesAndDistanceValue[2] = cartesianPointZ; } else { indexCoordinatesAndDistanceValue[2] = 0.0; } return indexCoordinatesAndDistanceValue; } ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength, ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY, ToFScalarType principalPointX, ToFScalarType principalPointY) { ToFPoint3D cartesianCoordinates; // calculate image coordinates in mm; ToFScalarType imageX = (( i - principalPointX ) * interPixelDistanceX); ToFScalarType imageY = (( j - principalPointY ) * interPixelDistanceY); //distance from pinhole to pixel ToFScalarType d = sqrt(imageX*imageX + imageY*imageY + focalLength*focalLength); cartesianCoordinates[0] = (distance)*imageX / d; //Strahlensatz: x / imageX = (distance) / d cartesianCoordinates[1] = (distance)*imageY / d; //Strahlensatz: y / imageY = (distance) / d cartesianCoordinates[2] = ((distance*focalLength) / d); //Strahlensatz: z / f = distance / d. return cartesianCoordinates; } ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::CartesianToIndexCoordinates(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance) { ToFPoint3D indexCoordinatesAndDistanceValue; ToFScalarType imageX = cartesianPointX*focalLengthX/cartesianPointZ; //Strahlensatz: cartesianPointX / imageX = cartesianPointZ / focalLengthX ToFScalarType imageY = cartesianPointY*focalLengthY/cartesianPointZ; //Strahlensatz: cartesianPointY / imageY = cartesianPointZ / focalLengthY indexCoordinatesAndDistanceValue[0] = imageX + principalPointX; indexCoordinatesAndDistanceValue[1] = imageY + principalPointY; // Note: pixel unit (pX) in x direction does normally not equal pixel unit (pY) in y direction. // Therefore, a transformation in one of the pixel units is necessary (for calculation of the distance value only) // Here, pX as image coordinate unit is chosen // pY = (focalLengthX / focalLengthY) * pX ToFScalarType imageY_in_pX = imageY * focalLengthX/focalLengthY; //distance from pinhole to pixel ToFScalarType d_in_pX = sqrt(imageX*imageX + imageY_in_pX*imageY_in_pX + focalLengthX*focalLengthX); if (calculateDistance) { indexCoordinatesAndDistanceValue[2] = d_in_pX*(cartesianPointZ) / focalLengthX; } else { indexCoordinatesAndDistanceValue[2] = 0.0; } return indexCoordinatesAndDistanceValue; } - ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::CartesianToIndexCoordinatesWithInterpixdist(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ, ToFScalarType focalLength, ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY, ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance) { ToFPoint3D indexCoordinatesAndDistanceValue; ToFScalarType imageX = cartesianPointX*focalLength/cartesianPointZ; ToFScalarType imageY = cartesianPointY*focalLength/cartesianPointZ; indexCoordinatesAndDistanceValue[0] = imageX/interPixelDistanceX + principalPointX; indexCoordinatesAndDistanceValue[1] = imageY/interPixelDistanceY + principalPointY; ToFScalarType d = sqrt(imageX*imageX + imageY*imageY + focalLength*focalLength); if (calculateDistance) { indexCoordinatesAndDistanceValue[2] = d*(cartesianPointZ) / focalLength; } else { indexCoordinatesAndDistanceValue[2] = 0.0; } return indexCoordinatesAndDistanceValue; } + + ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::ContinuousKinectIndexToCartesianCoordinates(mitk::Point2D continuousIndex, ToFScalarType distance, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY) + { + ToFPoint3D cartesianCoordinates; + + cartesianCoordinates[0] = distance * (continuousIndex[0] - principalPointX) / focalLengthX; + cartesianCoordinates[1] = distance * (continuousIndex[1] - principalPointY) / focalLengthY; + cartesianCoordinates[2] = distance; + + return cartesianCoordinates; + } } diff --git a/Modules/ToFProcessing/mitkToFProcessingCommon.h b/Modules/ToFProcessing/mitkToFProcessingCommon.h index 8da5b9d91a..3f45304a2b 100644 --- a/Modules/ToFProcessing/mitkToFProcessingCommon.h +++ b/Modules/ToFProcessing/mitkToFProcessingCommon.h @@ -1,321 +1,335 @@ /*=================================================================== 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. ===================================================================*/ #ifndef MITKTOFPROCESSINGCOMMON_H #define MITKTOFPROCESSINGCOMMON_H #include "mitkToFProcessingExports.h" #include "mitkVector.h" #include namespace mitk { /** * @brief Helper class providing functions which are useful for multiple usage * * Currently the following methods are provided: *
    *
  • Conversion from 2D image coordinates to 3D world coordinates (IndexToCartesianCoordinates()) *
  • Conversion from 3D world coordinates to 2D image coordinates (CartesianToIndexCoordinates()) *
* The coordinate conversion follows the model of a common pinhole camera where the origin of the camera * coordinate system (world coordinates) is at the pinhole * \image html ../Modules/ToFProcessing/Documentation/PinholeCameraModel.png * The definition of the image plane and its coordinate systems (pixel and mm) is depicted in the following image * \image html ../Modules/ToFProcessing/Documentation/ImagePlane.png * @ingroup ToFProcessing */ class mitkToFProcessing_EXPORT ToFProcessingCommon { public: typedef double ToFScalarType; typedef itk::Point ToFPoint2D; typedef itk::Point ToFPoint3D; typedef itk::Vector ToFVector2D; typedef itk::Vector ToFVector3D; /*! \brief Convert index based distances to cartesian coordinates \param i index in x direction of image plane \param j index in y direction of image plane \param distance distance value at given index in mm \param focalLengthX focal length of optical system in pixel units in x-direction (mostly obtained from camera calibration) \param focalLengthY focal length of optical system in pixel units in y-direction (mostly obtained from camera calibration) \param principalPointX x coordinate of principal point on image plane in pixel \param principalPointY y coordinate of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ static ToFPoint3D IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY); /*! \brief Convert index based distances to cartesian coordinates \param i index in x direction of image plane \param j index in y direction of image plane \param distance distance value at given index in mm \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration) \param principalPoint coordinates of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ inline static ToFPoint3D IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFPoint2D focalLength, ToFPoint2D principalPoint) { return IndexToCartesianCoordinates(i,j,distance,focalLength[0],focalLength[1],principalPoint[0],principalPoint[1]); } /*! \brief Convert index based distances to cartesian coordinates \param index index coordinates \param distance distance value at given index in mm \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration) \param principalPoint coordinates of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ inline static ToFPoint3D IndexToCartesianCoordinates(mitk::Index3D index, ToFScalarType distance, ToFPoint2D focalLength, ToFPoint2D principalPoint) { return IndexToCartesianCoordinates(index[0],index[1],distance,focalLength[0],focalLength[1],principalPoint[0], principalPoint[1]); } /*! \brief Convenience method to convert index based distances to cartesian coordinates using array as input \param i index in x direction of image plane \param j index in y direction of image plane \param distance distance value at given index in mm \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration) \param principalPoint coordinates of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ inline static ToFPoint3D IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength[2], ToFScalarType principalPoint[2]) { return IndexToCartesianCoordinates(i,j,distance,focalLength[0],focalLength[1],principalPoint[0],principalPoint[1]); } /*! \brief Convert index based distances to cartesian coordinates \param i index in x direction of image plane \param j index in y direction of image plane \param distance distance value at given index in mm \param focalLength focal length of optical system in mm (mostly obtained from camera calibration) \param interPixelDistanceX distance in x direction between adjacent pixels in mm \param interPixelDistanceY distance in y direction between adjacent pixels in mm \param principalPointX x coordinate of principal point on image plane in pixel \param principalPointY y coordinate of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength, ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY, ToFScalarType principalPointX, ToFScalarType principalPointY); /*! \brief Convert index based distances to cartesian coordinates \param i index in x direction of image plane \param j index in y direction of image plane \param distance distance value at given index in mm \param focalLength focal length of optical system in mm (mostly obtained from camera calibration) \param interPixelDistance distance between adjacent pixels in mm \param principalPoint coordinates of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ inline static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength, ToFPoint2D interPixelDistance, ToFPoint2D principalPoint) { return IndexToCartesianCoordinatesWithInterpixdist(i,j,distance,focalLength,interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1]); } /*! \brief Convert index based distances to cartesian coordinates \param index index coordinates \param distance distance value at given index in mm \param focalLength focal length of optical system (mostly obtained from camera calibration) \param interPixelDistance distance between adjacent pixels in mm for x and y direction \param principalPoint coordinates of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ inline static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(mitk::Index3D index, ToFScalarType distance, ToFScalarType focalLength, ToFPoint2D interPixelDistance, ToFPoint2D principalPoint) { return IndexToCartesianCoordinatesWithInterpixdist(index[0],index[1],distance,focalLength,interPixelDistance[0], interPixelDistance[1],principalPoint[0], principalPoint[1]); } /*! \brief Convenience method to convert index based distances to cartesian coordinates using array as input \param i index in x direction of image plane \param j index in y direction of image plane \param distance distance value at given index in mm \param focalLength focal length of optical system in mm (mostly obtained from camera calibration) \param interPixelDistance distance between adjacent pixels in mm \param principalPoint coordinates of principal point on image plane in pixel \return cartesian coordinates for current index will be written here */ inline static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength, ToFScalarType interPixelDistance[2], ToFScalarType principalPoint[2]) { return IndexToCartesianCoordinatesWithInterpixdist(i,j,distance,focalLength,interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1]); } /*! \brief Convert cartesian coordinates to index based distances \param cartesianPointX x coordinate of point (of a surface or point set) to convert in 3D coordinates \param cartesianPointY y coordinate of point (of a surface or point set) to convert in 3D coordinates \param cartesianPointZ z coordinate of point (of a surface or point set) to convert in 3D coordinates \param focalLengthX focal length of optical system in pixel units in x-direction (mostly obtained from camera calibration) \param focalLengthY focal length of optical system in pixel units in y-direction (mostly obtained from camera calibration) \param principalPointX x coordinate of principal point on image plane in pixel \param principalPointY y coordinate of principal point on image plane in pixel \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0 \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value */ static ToFPoint3D CartesianToIndexCoordinates(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance=true); /*! \brief Convenience method to convert cartesian coordinates to index based distances using arrays \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration) \param principalPoint coordinates of principal point on image plane in pixel \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0 \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value */ inline static ToFPoint3D CartesianToIndexCoordinates(ToFScalarType cartesianPoint[3], ToFScalarType focalLength[2], ToFScalarType principalPoint[2], bool calculateDistance=true) { return CartesianToIndexCoordinates(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength[0], focalLength[1], principalPoint[0],principalPoint[1],calculateDistance); } /*! \brief Convert cartesian coordinates to index based distances \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration) \param principalPoint coordinates of principal point on image plane in pixel \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0 \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value */ inline static ToFPoint3D CartesianToIndexCoordinates(ToFPoint3D cartesianPoint, ToFPoint2D focalLength, ToFPoint2D principalPoint, bool calculateDistance=true) { return CartesianToIndexCoordinates(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength[0], focalLength[1], principalPoint[0],principalPoint[1],calculateDistance); } /*! \brief Convert cartesian coordinates to index based distances \param cartesianPointX x coordinate of point (of a surface or point set) to convert in 3D coordinates \param cartesianPointY y coordinate of point (of a surface or point set) to convert in 3D coordinates \param cartesianPointZ z coordinate of point (of a surface or point set) to convert in 3D coordinates \param focalLength focal length of optical system in mm (mostly obtained from camera calibration) \param interPixelDistanceX distance in x direction between adjacent pixels in mm \param interPixelDistanceY distance in y direction between adjacent pixels in mm \param principalPointX x coordinate of principal point on image plane in pixel \param principalPointY y coordinate of principal point on image plane in pixel \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0 \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value */ static ToFPoint3D CartesianToIndexCoordinatesWithInterpixdist(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ, ToFScalarType focalLength, ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY, ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance=true); /*! \brief Convenience method to convert cartesian coordinates to index based distances using arrays \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates \param focalLength focal length of optical system in mm (mostly obtained from camera calibration) \param interPixelDistance distance between adjacent pixels in mm for x and y direction \param principalPoint coordinates of principal point on image plane in pixel \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0 \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value */ inline static ToFPoint3D CartesianToIndexCoordinatesWithInterpixdist(ToFScalarType cartesianPoint[3], ToFScalarType focalLength, ToFScalarType interPixelDistance[2], ToFScalarType principalPoint[2], bool calculateDistance=true) { return CartesianToIndexCoordinatesWithInterpixdist(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength, interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1],calculateDistance); } /*! \brief Convert cartesian coordinates to index based distances \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates \param focalLength focal length of optical system in mm (mostly obtained from camera calibration) \param interPixelDistance distance between adjacent pixels in mm for x and y direction \param principalPoint coordinates of principal point on image plane in pixel \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0 \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value */ inline static ToFPoint3D CartesianToIndexCoordinatesWithInterpixdist(ToFPoint3D cartesianPoint, ToFScalarType focalLength, ToFPoint2D interPixelDistance, ToFPoint2D principalPoint, bool calculateDistance=true) { return CartesianToIndexCoordinatesWithInterpixdist(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength, interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1],calculateDistance); } /** \addtogroup * @{ * * @brief KinectIndexToCartesianCoordinates Convert a pixel (i,j) with value d to a 3D world point. This conversion is meant for Kinect and slightly different then ToF reconstruction. See also "Hacking the Kinect" - Jeff Kramer, Matt Parker, Daniel Herrera C., Nicolas Burrus, Florian Echtler, Chapter 7, Part 1 "Moving from Depth Map to Point Cloud. * @param i Pixel index i. * @param j Pixel index j. * @param distance Distance value d in mm as obtained from OpenNI. * @param focalLengthX Focallength from calibration. * @param focalLengthY Focallength from calibration. * @param principalPointX Principal point from calibration. * @param principalPointY Principal point from calibration. * @return a ToFPoint3D. The point in world coordinates (mm). */ static ToFProcessingCommon::ToFPoint3D KinectIndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY); inline static ToFPoint3D KinectIndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength[2], ToFScalarType principalPoint[2]) { return KinectIndexToCartesianCoordinates(i,j,distance,focalLength[0],focalLength[1],principalPoint[0],principalPoint[1]); } inline static ToFPoint3D KinectIndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance, ToFPoint2D focalLength, ToFPoint2D principalPoint) { return KinectIndexToCartesianCoordinates(i,j,distance,focalLength[0],focalLength[1],principalPoint[0],principalPoint[1]); } inline static ToFPoint3D KinectIndexToCartesianCoordinates(mitk::Index3D index, ToFScalarType distance, ToFPoint2D focalLength, ToFPoint2D principalPoint) { return KinectIndexToCartesianCoordinates(index[0],index[1],distance,focalLength[0],focalLength[1],principalPoint[0], principalPoint[1]); } /** @}*/ /** \addtogroup * @{ * @brief CartesianCoordinatesToKinectIndexCoordinates Transform a 3D world point back to distance image pixel coordinates. * @param cartesianPointX x value of the cartesian point. * @param cartesianPointY y value of the cartesian point. * @param cartesianPointZ z value of the cartesian point. * @param focalLengthX x value of the focal length (from calibration). * @param focalLengthY y value of the focal length (from calibration). * @param principalPointX x value of the principal point (from calibration). * @param principalPointY y value of the principal point (from calibration). * @param calculateDistance Do you want to compute also the distance of the distance image? For Kinect, this value is always the same in cartesian and index coordinates. * @return A ToFPoint3D containing the pixel indices (i,j) in [0] and [1] and (optionally) the distance value in [2] (or just 0.0). */ static ToFPoint3D CartesianToKinectIndexCoordinates(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY, ToFScalarType cartesianPointZ, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance=true); inline static ToFProcessingCommon::ToFPoint3D CartesianToKinectIndexCoordinates(ToFPoint3D cartesianPoint, ToFPoint2D focalLength, ToFPoint2D principalPoint, bool calculateDistance=true) { - return CartesianToKinectIndexCoordinates( cartesianPoint[0], cartesianPoint[1], cartesianPoint[2], focalLength[0], focalLength[1], principalPoint[0], principalPoint[1], calculateDistance); + return CartesianToKinectIndexCoordinates( cartesianPoint[0], cartesianPoint[1], cartesianPoint[2], focalLength[0], focalLength[1], principalPoint[0], principalPoint[1], calculateDistance); } /** @}*/ + /** + * @brief ContinuousKinectIndexToCartesianCoordinates This method is escpially meant for reconstructing a Kinect point + * with continuous index coordinates (i.e. not exactly a pixel position, but a point interpolated between two pixels). + * The only difference to KinectIndexToCartesianCoordinates() is that ContinuousKinectIndexToCartesianCoordinates does not + * cast to unsigned int for the index. + * @param continuousIndex The continuous coordinates (e.g. 0.5; 0.5). + * @param distance Distance value d in mm as obtained from OpenNI. + * @param focalLengthX x value of the focal length (from calibration). + * @param focalLengthY y value of the focal length (from calibration) + * @param principalPointX x value of the principal point (from calibration). + * @param principalPointY y value of the principal point (from calibration). + * @return a ToFPoint3D. The point in world coordinates (mm). + */ + static ToFProcessingCommon::ToFPoint3D ContinuousKinectIndexToCartesianCoordinates(mitk::Point2D continuousIndex, ToFScalarType distance, ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY); }; } #endif