25 #include <Image/PMDImageProc.hh>
26 #include <Geometry/RMatrix.hh>
27 #include <Geometry/Projection.hh>
28 #include <Image/ProjectionMapping.hh>
29 #include <Image/HomographyMapping.hh>
30 #include <Base/Image/ImageIO.hh>
31 #include <Filter/Dilation.hh>
32 #include <Filter/Median.hh>
33 #include <bias_config.h>
35 #include <Base/Math/Random.hh>
36 #include <MathAlgo/SVD.hh>
37 #include <MathAlgo/Histogram1D.hh>
48 unsigned int depthWidth=0,depthHeight=0;
52 unsigned int twoDWidth=0, twoDHeight=0;
54 ImgDepthTo2D.
Init(twoDWidth,twoDHeight, 1);
63 for (
unsigned int x=0; x< depthWidth; x++) {
64 for (
unsigned int y=0; y< depthHeight; y++) {
67 float depth = idaDepth[y][x];
75 unsigned int xx=(
unsigned int)rintf(pos[0]);
76 unsigned int yy=(
unsigned int)rintf(pos[1]);
78 if (xx>=0 && xx < twoDWidth && yy >= 0 && yy < twoDHeight) {
79 idaDepthTo2D[yy][xx] = depth;
81 if (y>0 && posOld.NormL2()>0 && fabs((depth-depthold)/depth)<0.001) {
82 if (maxPosDiff.
NormL2()<(posOld-pos).NormL2()) {
83 maxPosDiff = posOld-pos;
108 kernelsize=int(ceil(maxPosDiff.
NormL2()));
109 if (kernelsize==0 || kernelsize>60) {
110 BIASERR(
"Calculation of Kernelsize did not work, calculated "
114 kernelsize = kernelsize/2 +1;
117 cout<<
"Kernelsize:"<<kernelsize<<endl;
121 dilation.
Filter(ImgDepthTo2D,res);
142 ret = FitDepthTo2DImageFree_(ImgMod, ImgDepth, MetaData2D, MetaDataDepth);
151 ret = FitDepthTo2DImageHomogr_(ImgMod, MetaData2D, MetaDataDepth);
153 ret = FitDepthTo2DImageKOnly_(ImgMod, MetaData2D, MetaDataDepth);
171 ret = FitDepthTo2DImageFree_(ImgModCoeff, ImgDepth,
172 MetaData2D, MetaDataDepth);
181 ret = FitDepthTo2DImageHomogr_(ImgModCoeff, MetaData2D, MetaDataDepth);
183 ret = FitDepthTo2DImageKOnly_(ImgModCoeff, MetaData2D, MetaDataDepth);
196 K2D = KFromMeta(MetaData2D);
197 KDepth = KFromMeta(MetaDataDepth);
204 ret = hm.
Map(ImgToModify, mapto);
206 MetaDataDepth = MetaData2D;
216 BIASWARN(
"This is only a rough approximation better performance in derived" <<
217 "class 'GLFilter/PMDImageProcGL' (class unimplemented ischiller 04/07)");
219 vector<double> undistDepth(4,0.0);
225 vector<double> undist2D(4,0.0);
235 radDistType, undistDepth);
239 radDistType,undist2D);
246 for (
unsigned int x=0; x<ImgDepth.
GetWidth(); x++) {
247 for (
unsigned int y=0; y<ImgDepth.
GetHeight(); y++) {
257 int xx=(int)rintf(pos[0]);
258 int yy=(int)rintf(pos[1]);
259 if (xx>=0 && xx<(
int)ImgDepthTo2D.
GetWidth() &&
260 yy>=0 && yy<(int)ImgDepthTo2D.
GetHeight()) {
263 if (y>0 && posOld.NormL2()>0 && fabs((depth-depthold)/depth)<0.001) {
264 if (maxPosDiff.
NormL2()<(posOld-pos).NormL2()) {
265 maxPosDiff = posOld-pos;
287 int kernelsize=int(ceil(maxPosDiff.
NormL2()));
288 if (kernelsize==0 || kernelsize>25) {
289 BIASERR(
"Calculation of Kernelsize did not work, calculated " << kernelsize);
292 kernelsize = kernelsize/2*2+1;
296 dilation.
Filter(ImgDepthTo2D,res);
308 HMatrix H = HFromMeta_(MetaData2D, MetaDataDepth);
313 ret = hm.
Map(ImgToModify, mapto);
316 MetaDataDepth = MetaData2D;
331 if(newColorImage.
IsEmpty()) newColorImage.
Init(w,h,c );
335 unsigned char color[3]={0,0,0};
341 for(
unsigned y=0;y<h;y++){
342 for(
unsigned x=0;x<w;x++){
343 if(idaD[y][x] != 0.0){
344 double depth = idaD[y][x];
348 for(
unsigned i=0;i<c;i++){
349 idaC[y][x*c+i] = idaOldC[unsigned(p2D[1])][(unsigned)(p2D[0])*c+i];
365 BIASERR(
"Can not Fit Depth Image to 2D Image, centers are not equal!!");
369 K2D = KFromMeta(MetaData2D);
370 KDepth = KFromMeta(MetaDataDepth);
412 int height,
int channels)
438 MetaData.
width = width;
452 float mean=0.0;
int counter=0;
454 float min=100000.0f,max=0.0f;
455 vector<float> histogramData;
457 for(
unsigned y=0;y<h;y++){
458 for(
unsigned x=0;x<w;x++){
459 if (ida[y][x] == 0) {
463 if(ida[y][x] < min) min = ida[y][x];
464 if(ida[y][x] > max) max = ida[y][x];
467 histogramData.push_back(ida[y][x]);
484 for(
unsigned y=0;y<h;y++){
485 for(
unsigned x=0;x<w;x++){
486 if(ida[y][x] != 0.0){
487 if(ida[y][x] > depthAccumulationValue+maxDeviation) {
488 ida[y][x] = depthAccumulationValue+maxDeviation;
489 }
else if (ida[y][x] < depthAccumulationValue-maxDeviation) {
490 ida[y][x] = depthAccumulationValue-maxDeviation;
500 double sigmaVarianz) {
505 median.
Filter(ImgDepth, res);
506 DeleteHighVarianceValues(res, sigmaVarianz,size);
516 for (
unsigned int x=0; x<ImgDepth.
GetWidth(); x++) {
517 for (
unsigned int y=0; y<ImgDepth.
GetHeight(); y++) {
533 double scaleFactor) {
535 for (
unsigned int x=0; x<ImgDepth.
GetWidth(); x++) {
536 for (
unsigned int y=0; y<ImgDepth.
GetHeight(); y++) {
545 const unsigned int x,
546 const unsigned int y,
547 const unsigned int HalfWinSize)
549 if (x<HalfWinSize || x>ImgDepth.
GetWidth()-HalfWinSize-1 ||
550 y<HalfWinSize || y>ImgDepth.
GetHeight()-HalfWinSize-1) {
555 double minDepth = 1000.0;
556 double maxDepth = 0.0;
558 for (
int dx=-1*(
int)HalfWinSize; dx<=int(HalfWinSize); dx++) {
559 for (
int dy=-1*(
int)HalfWinSize; dy<=int(HalfWinSize); dy++) {
560 if (ImgDepth.
PixelValue(x+dx, y+dy)<minDepth &&
569 double minDev= 0.002734*minDepth*minDepth +
570 0.00286723*minDepth - 0.0004229692;
571 double maxDev= 0.002734*maxDepth*maxDepth +
572 0.00286723*maxDepth - 0.0004229692;
577 return (maxDepth-minDepth)/2;
582 const double sigmaThresh,
583 const int halfwinSize)
591 for (
int row=0; row<rows; row++)
592 for (
int col=0; col<cols; col++) {
595 for (
int i=-halfwinSize; i<=halfwinSize; i++){
596 for (
int j=-halfwinSize; j<=halfwinSize; j++){
597 if ( ((row+i)>=0) && ((col+j)>=0) &&
598 ((row+i)<rows) && ((col+j)<cols) )
600 diff= data[row+i][col+j]-data[row][col];
624 for (
int row=0; row<rows; row++)
625 for (
int col=0; col<cols; col++) {
628 for (
int i=-halfwinSize; i<halfwinSize; i++)
629 for (
int j=-halfwinSize; j<halfwinSize; j++)
630 if ( ((row+i)>=0) && ((col+j)>=0) &&
631 ((row+i)<rows) && ((col+j)<cols) )
633 diff= data[row+i][col+j]-data[row][col];
638 if (sqrt(var)>sigmaThresh) mdata[row][col]=float(var);
639 else mdata[row][col]=0.0f;
647 std::vector<const Projection*>& cPCheck,
659 for(
unsigned y=0;y<h;y++){
660 for(
unsigned x=0;x<w;x++){
666 for(
unsigned i=0;i<cPCheck.size();i++){
667 isIn.push_back(
false);
669 bool projects =
false;
670 projects = cPCheck[i]->DoesPointProjectIntoImage(point3D,point );
672 if(projects && point[2]!=0){
673 unsigned w2,h2,width,height;
674 cPCheck[i]->GetParameters()->GetImageSize(w2,h2);
675 width = cImCheck[i]->GetWidth();
676 height = cImCheck[i]->GetHeight();
677 if(w2 != width || h2 != height) {
678 BIASERR(
"Unmatching image dimensions between projection and image");
682 int cX = (int) rint(point[0]);
683 int cY = (int) rint(point[1]);
686 const float** idaCheck = cImCheck[i]->GetImageDataArray();
687 if (idaCheck[cY][cX] != 0.0) {
693 for (
int runX = cX - searchSize; runX < (cX + searchSize); runX++) {
694 for (
int runY = cY - searchSize; runY < (cY + searchSize); runY++) {
695 if (runX >= 0 && runY >= 0 && runX < (
int)w2 && runY< (int)h2) {
696 if (idaCheck[runY][runX] != 0) {
707 else isIn[i] =
false;
709 bool bMustBeValidInAll=
true;
710 if(bMustBeValidInAll){
711 for(
unsigned i=0;i<isIn.size();i++) {
721 for(
unsigned i=0;i<isIn.size();i++) {
void AddData(const std::vector< T > &data)
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
a 3x3 Matrix describing projective transformations between planes
Maps image src to image sink with homography H (software implementation)
bool IsEmpty() const
check if ImageData_ points to allocated image buffer or not
Dilation operator for binary images (black and white)
static void LimitDepthSpread(BIAS::Image< float > &depthImage, float maxDeviation)
use to limit the depth spread to a certain range around the
virtual HomgPoint3D UnProjectToPoint(const HomgPoint2D &pos, double depth, bool IgnoreDistortion=false) const
calculates a 3D point in the global (not the rig) coordinate system, which belongs to the image posit...
void SetC(const Vector3< POSE_TYPE > &C, const Matrix3x3< POSE_TYPE > &cov)
Set translation (resp.
static int CrossCheck(Image< float > &cIm, const Projection &cP, std::vector< const Image< float > * > &cImCheck, std::vector< const Projection * > &cPCheck, int searchSize=3)
Projects all points != 0 in image cIm to both other images cIm1 and cIm2 and checks whether the 3D po...
static HMatrix HFromMeta_(const PMDImageMetaData &MetaData2D, const PMDImageMetaData &MetaDataDepth)
Create a H-Matrix that converts the depth image to the 2D view.
static int FitDepthTo2DImage(Image< float > &ImgDepth, const PMDImageMetaData &MetaData2D, PMDImageMetaData &MetaDataDepth)
convert a depth image such that the depth and the grey value at the same pixel position belong to the...
T GetBinCenterWithMaxEntries()
virtual parent class for API definition of all (future) filters
bool IsIdentity(const T eps=std::numeric_limits< T >::epsilon()) const
static int FitDepthTo2DImageHomogr_(Image< float > &ImgToModify, const PMDImageMetaData &MetaData2D, PMDImageMetaData &MetaDataDepth)
void SetQ(const Quaternion< POSE_TYPE > &Q, const Matrix4x4< POSE_TYPE > &cov)
Set rotation (resp.
void SetHistogramBoundaries(const T &min_val, const T &max_val)
unsigned int GetWidth() const
int GetAxisAngle(Vector3< QUAT_TYPE > &axis, QUAT_TYPE &angle) const
Returns rotation in axis and angle notation (angle in radians).
StorageType PixelValue(const unsigned int x, const unsigned int y, const unsigned short int channel=0) const
Returns value of pixel at specific position, using specific channel as offset.
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
Represents 3d pose transformations, parametrized as Euclidean translation and unit quaternion orienta...
static void MarkHighVarianceValues(const BIAS::Image< float > &img, BIAS::Image< float > &mark, double sigmaThresh, int halfwinSize=1)
Markes pixels in PMD-Image with a local variance > sigmaThresh.
const ProjectionParametersBase * GetParameters(unsigned int cam=0) const
const parameter access function
virtual int Filter(const Image< InputStorageType > &src, Image< OutputStorageType > &dst)
Does the dilation.
static void DeleteHighVarianceValues(BIAS::Image< float > &img, const double sigmaThresh, const int halfwinSize=1)
Deletes (sets to 0) pixels in PMD-Image with a local variance > sigmaThresh.
void ReInit(const unsigned int &width, const unsigned int &height, const unsigned int nChannels=1, const enum EStorageType storageType=ST_unsignedchar, const bool interleaved=true, const EColorModel colormodel=CM_Grey)
(Re-)Initialize Image data if required.
static int AddNoiseToDepthImage(Image< float > &ImgDepth)
This function adds noise to a depth image.
This class hides the underlying projection model, like projection matrix, spherical camera...
unsigned int GetChannelCount() const
returns the number of Color channels, e.g.
Simple one dimensional histogram computation.
unsigned int GetHeight() const
int Map(const Image< InputStorageType > &src, Image< OutputStorageType > &sink, InterpolationMethod=MapTrilinear, bool newSink=false, double SuperSampling=1.0)
backward mapping with various interpolations
void SetBorderHandling(const int bh)
void SetKernelSize(int size)
static void ScaleDepthImage(Image< float > &ImgDepth, double scaleFactor)
This function scales the depth image with the given scale factor.
static double CalcSigmaDepth(const Image< float > &ImgDepth, const unsigned int x, const unsigned int y, const unsigned int HalfWinSize)
Give the standard deviation for a pixel in the depth image.
void FillImageWithConstValue(StorageType Value)
fill grey images
static PMDImageMetaData MetaFromPoseAndK(const Pose &Po, const KMatrix &K, int width, int height, int channels)
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
virtual bool DoesPointProjectIntoImage(const BIAS::HomgPoint3D &X, BIAS::HomgPoint2D &x, unsigned int cam=0, bool IgnoreDistortion=false) const
Checks if 3D point projects into specified image and returns belonging 2D image point.
void SetPixel(const StorageType &value, const unsigned int &x, const unsigned int &y, const unsigned short int channel=0)
Set the value of a given pixel (x,y) in channel to value.
void SetFromAxisAngle(Vector3< ROTATION_MATRIX_TYPE > w)
Set from rotation axis * angle (modified Rodrigues vector)
virtual int GetImageSize(unsigned int &Width, unsigned int &Height) const
Obtain image dimensions.
void Init(unsigned int Width, unsigned int Height, unsigned int channels=1, enum EStorageType storageType=ST_unsignedchar, const bool interleaved=true)
calls Init from ImageBase storageType is ignored, just dummy argument
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
static int FitDepthTo2DImageKOnly_(Image< float > &ImgToModify, const PMDImageMetaData &MetaData2D, PMDImageMetaData &MetaDataDepth)
These methods transform the depth image to fit the 2D-image.
static void Fit2DToDepthImage(Image< float > &depthImage, Image< unsigned char > &textureImage, Projection &depthProj, Projection &textureProj, Image< unsigned char > &newColorImage)
calculates a color image of the size of the depth image by projecting every pixel of the depth image ...
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
static int FitModCoeffTo2DImage(Image< float > &ImgModCoeff, const Image< float > &ImgDepth, const PMDImageMetaData &MetaData2D, PMDImageMetaData &MetaDataDepth)
Vector3< double > UnProjectToPoint(const HomgPoint2D &pos, double depth, unsigned int cam=0, bool IgnoreDistortion=false) const
calculates a 3D point in the global (not the rig) coordinate system, which belongs to the image posit...
BIAS_ProjParaPersp_DISTORTION_TYPE
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
double GetNormalDistributed(const double mean, const double sigma)
on succesive calls return normal distributed random variable with mean and standard deviation sigma ...
void CreatePerspective(const BIAS::Pose &pose, const BIAS::KMatrix &K, int width, int height, BIAS_ProjParaPersp_DISTORTION_TYPE radDistType=DISTYPE_DEF, const std::vector< double > &UndistortionCoefficients=std::vector< double >(4, 0.0))
Create a perspective camera and add to projection.
static Pose PoseFromMeta(const PMDImageMetaData &MetaData)
KMatrix Invert() const
returns analyticaly inverted matrix
virtual HomgPoint2D Project(const HomgPoint3D &X, unsigned int cam=0, bool IgnoreDistortion=false) const
returns the 2d projection of X in camera cam, where X is given in the global coordinate frame (not th...
static KMatrix KFromMeta(const PMDImageMetaData &MetaData)
void SetHomography(const HMatrix &H)
set your homography H (source = H * sink) before calling Map()
Vector3< T > & Normalize()
normalize this vector to length 1
class for producing random numbers from different distributions
void SetZero()
zeroes the image
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
static int DenoiseDepthImage(Image< float > &ImgDepth, int size=1, double sigmaVarianz=0.5)
This function denoises a PMD-Depth image by Median and variance filtering.
class BIASGeometryBase_EXPORT HomgPoint2D
static int FitDepthTo2DImageFree_(Image< float > &ImgToModify, const Image< float > &ImgDepth, const PMDImageMetaData &MetaData2D, PMDImageMetaData &MetaDataDepth)
void SetNumBins(const unsigned val)
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase