25 #include <Image/RectificationViaProjectionMappingBase.hh>
27 #include <Image/RectificationViaProjectionMappingBase.hh>
28 #include <Image/ProjectionMapping.hh>
29 #include <Geometry/Triangulation.hh>
34 #include <Base/Image/ImageIO.hh>
39 #ifdef SUBPIXEL_REFINEMENT
40 # include <Matcher2D/TrackerBaseWeighted1D.hh>
41 # include <Base/Image/ImageConvert.hh>
42 # include <Filter/GradientSimple.hh>
43 # include <Filter/Gauss.hh>
44 # include <Base/Image/ImageIO.hh>
47 #define PARENT RectificationBase<InputStorageType, OutputStorageType>
52 template<
class InputStorageType,
class OutputStorageType>
54 OutputStorageType>::~RectificationViaProjectionMappingBase()
60 template<
class InputStorageType,
class OutputStorageType>
63 const std::string& stringID,
const double& scale) :
64 stringID_(stringID), rectPPA_(NULL), rectPPB_(NULL), scale_(scale),
65 targetWidth_(0), rectificationParamsAreValid_(false),
66 interpolationType_(
MapTrilinear), mappingInitialized_(false),
67 thetaMinOverride_(0), thetaMaxOverride_(M_PI), phiMinOverride_(-M_PI),
68 phiMaxOverride_(M_PI), useLookUpTables_(true)
70 secondFill_ = std::numeric_limits<OutputStorageType>::max();
73 template<
class InputStorageType,
class OutputStorageType>
84 template<
class InputStorageType,
class OutputStorageType>
90 int res = PARENT::SetCameraA(img, proj);
93 rectificationParamsAreValid_ =
false;
98 template<
class InputStorageType,
class OutputStorageType>
109 template<
class InputStorageType,
class OutputStorageType>
115 int res = PARENT::SetCameraB(img, proj);
118 rectificationParamsAreValid_ =
false;
123 template<
class InputStorageType,
class OutputStorageType>
126 OutputStorageType>::SetInterpolationMethod(
129 interpolationType_ = interpolationType;
130 rectificationParamsAreValid_ =
false;
133 template<
class InputStorageType,
class OutputStorageType>
136 OutputStorageType>::AreRectificationParametersValid_()
138 if (!rectificationParamsAreValid_)
140 if (DetermineScaledRectificationParameters_() == 0)
142 rectificationParamsAreValid_ =
true;
143 mappingInitialized_ =
false;
146 return rectificationParamsAreValid_;
149 template<
class InputStorageType,
class OutputStorageType>
152 OutputStorageType>::DetermineScaledRectificationParameters_()
154 int res = DetermineRectificationParameters_();
157 BIASASSERT(rectPPA_!=NULL);
158 BIASASSERT(rectPPB_!=NULL);
159 if (targetWidth_ > 0)
161 unsigned int width, height;
162 rectPPA_->GetImageSize(width, height);
163 float scale = float(width) / float(targetWidth_);
164 rectPPA_->Rescale(
float(1.0 / scale));
166 rectPPB_->GetImageSize(width, height);
167 scale = float(width) / float(targetWidth_);
168 rectPPB_->Rescale(
float(1.0 / scale));
172 rectPPA_->Rescale(
float(1.0 / scale_));
173 rectPPB_->Rescale(
float(1.0 / scale_));
180 template<
class InputStorageType,
class OutputStorageType>
183 OutputStorageType>::InitializeSinkImages_()
186 unsigned int width, height;
188 rectPPA_->GetImageSize(width, height);
189 if (!PARENT::rectImageA_.IsEmpty())
190 PARENT::rectImageA_.Release();
191 PARENT::rectImageA_.Init(width, height, PARENT::imageA_.GetChannelCount());
192 PARENT::rectImageA_.Clear();
194 rectPPB_->GetImageSize(width, height);
195 if (!PARENT::rectImageB_.IsEmpty())
196 PARENT::rectImageB_.Release();
197 PARENT::rectImageB_.Init(width, height, PARENT::imageB_.GetChannelCount());
201 template<
class InputStorageType,
class OutputStorageType>
204 OutputStorageType>::InitializeProjectionMappingObjects_()
206 rectMapperA_.SetSourceCam(
Projection(*PARENT::ppBA_));
207 rectMapperA_.SetSinkCam(
Projection(*rectPPA_));
209 rectMapperB_.SetSourceCam(
Projection(*PARENT::ppBB_));
210 rectMapperB_.SetSinkCam(
Projection(*rectPPB_));
213 template<
class InputStorageType,
class OutputStorageType>
216 OutputStorageType>::InitializeMapping_()
218 if (mappingInitialized_)
221 InitializeSinkImages_();
222 InitializeProjectionMappingObjects_();
224 if (useLookUpTables_)
226 rectMapperA_.PrepareLookupTableMapping(PARENT::imageA_,
227 PARENT::rectImageA_, interpolationType_);
229 rectMapperB_.PrepareLookupTableMapping(PARENT::imageB_,
230 PARENT::rectImageB_, interpolationType_);
232 mappingInitialized_ =
true;
235 template<
class InputStorageType,
class OutputStorageType>
238 OutputStorageType>::GetClonesOfRectificationParameters(
244 if (!AreRectificationParametersValid_())
249 if (rectPPA_ == NULL || rectPPB_ == NULL)
254 rectPPA = rectPPA_->
Clone();
255 rectPPB = rectPPB_->
Clone();
259 template<
class InputStorageType,
class OutputStorageType>
262 OutputStorageType>::GetRectifyingDisplacementMaps(
265 if (!AreRectificationParametersValid_())
269 InitializeProjectionMappingObjects_();
271 unsigned int width, height;
272 rectPPA_->GetImageSize(width, height);
275 displacementA.
Init(width, height, 3);
276 if (rectMapperA_.GetDisplacementMap(displacementA, PARENT::imageA_) != 0)
281 rectPPB_->GetImageSize(width, height);
284 displacementB.
Init(width, height, 3);
285 if (rectMapperB_.GetDisplacementMap(displacementB, PARENT::imageB_) != 0)
293 template<
class InputStorageType,
class OutputStorageType>
299 if (!AreRectificationParametersValid_())
304 InitializeMapping_();
306 unsigned int width, channels;
307 unsigned int widthB, channelsB;
308 if (rectMapperA_.GetLookupTableSize(width, channels, interpolationType_)
311 if (rectMapperB_.GetLookupTableSize(widthB, channelsB, interpolationType_)
315 BIASASSERT(width == widthB);
316 BIASASSERT(channels == channelsB);
317 rectSetup.
Init(width, 2, channels);
336 if (rectMapperA_.GetLookupTable(rows[0], interpolationType_) < 0)
338 if (rectMapperB_.GetLookupTable(rows[1], interpolationType_) < 0)
344 template<
class InputStorageType,
class OutputStorageType>
353 BIASERR(
"loading rectification setup with wrong class");
365 rectMapperA_.SetLookupTable(rows[0], rectSetup.
GetWidth(), method);
366 rectMapperB_.SetLookupTable(rows[1], rectSetup.
GetWidth(),
368 BIASASSERT(method == interpolationType_);
370 InitializeSinkImages_();
372 rectificationParamsAreValid_ =
true;
373 mappingInitialized_ =
true;
374 useLookUpTables_ =
true;
379 template<
class InputStorageType,
class OutputStorageType>
382 OutputStorageType>::SetOverrideAngles(
double thetaMinOverride,
383 double thetaMaxOverride,
double phiMinOverride,
double phiMaxOverride)
385 BIASASSERT(thetaMinOverride>=0 && thetaMinOverride<thetaMaxOverride);
386 BIASASSERT(thetaMaxOverride>0 && thetaMaxOverride<=M_PI);
387 BIASASSERT(phiMinOverride>=-M_PI && phiMinOverride<phiMaxOverride);
388 BIASASSERT(phiMaxOverride>0 && phiMaxOverride<=M_PI);
390 thetaMinOverride_ = thetaMinOverride;
391 thetaMaxOverride_ = thetaMaxOverride;
392 phiMinOverride_ = phiMinOverride;
393 phiMaxOverride_ = phiMaxOverride;
394 rectificationParamsAreValid_ =
false;
397 template<
class InputStorageType,
class OutputStorageType>
400 OutputStorageType>::Rectify()
402 if (!AreRectificationParametersValid_())
407 InitializeMapping_();
410 PARENT::rectImageA_.SetZero();
411 OutputStorageType* fill =
412 new OutputStorageType[PARENT::imageB_.GetChannelCount()];
413 for (
unsigned int i = 0; i < PARENT::imageB_.GetChannelCount(); i++)
415 fill[i] = secondFill_;
417 PARENT::rectImageB_.FillImageWithConstValue(*fill);
420 if (useLookUpTables_)
422 if (rectMapperA_.MapWithLookupTable(PARENT::imageA_,
423 PARENT::rectImageA_, interpolationType_) < 0)
425 BIASERR(
"LUT rectification of image A failed");
429 if (rectMapperB_.MapWithLookupTable(PARENT::imageB_,
430 PARENT::rectImageB_, interpolationType_) < 0)
432 BIASERR(
"LUT rectification of image B failed");
438 if (rectMapperA_.Map(PARENT::imageA_, PARENT::rectImageA_,
439 interpolationType_) < 0)
441 BIASERR(
"rectification of image A failed");
445 if (rectMapperB_.Map(PARENT::imageB_, PARENT::rectImageB_,
446 interpolationType_) < 0)
448 BIASERR(
"rectification of image B failed");
456 template<
class InputStorageType,
class OutputStorageType>
464 unsigned int width, height;
467 if (!DepthMap.IsEmpty())
469 DepthMap.Init(width, height);
474 ValidityMap.
Init(width, height);
476 unsigned char** ValidityMapPel =
480 float** DepthMapPel = DepthMap.GetImageDataArray();
485 float disparityThreshold = 0.0001f
498 #ifdef SUBPIXEL_REFINEMENT
505 Blurrer.
Filter(this->rectImageA_, rectAf);
507 Blurrer.
Filter(this->rectImageB_, rectBf);
516 MyGradient.
Filter(rectAf, gradax, graday);
517 MyGradient.
Filter(rectBf, gradbx, gradby);
534 SubPixelRefiner.
Init(rectAf, rectBf, gradax, graday, gradbx, gradby);
537 KLT_TYPE kltsteperror = 0;
538 int kltiterations = 0;
539 KLT_TYPE residuumMAD = 0;
540 KLT_TYPE residuumMSD = 0;
545 HomgPoint2D correspondenceInRectB, correspondenceInOrigB;
549 double disparity = 0;
551 depthMapPoint[2] = 1.0;
552 dispMapPoint[2] = 1.0;
553 for (
unsigned int y = border; y < height - border; y++)
555 for (
unsigned int x = border; x < width - border; x++)
558 depthMapPoint[0] = x;
559 depthMapPoint[1] = y;
567 dispMapPoint[0], dispMapPoint[1], disparityThreshold);
568 if (disparity > disparityThreshold)
570 correspondenceInRectB.
Set(dispMapPoint[0] - disparity,
571 dispMapPoint[1], 1.0);
573 #define MAX_OFFSET_REFINEMENT 0.6
574 #ifdef SUBPIXEL_REFINEMENT
589 int refinementresult =
590 SubPixelRefiner.
Track(dispMapPoint, correspondenceInRectB,
592 kltsteperror, kltiterations,residuumMAD,
596 if (refinementresult==0)
600 double diff = refinedPoint[0]-correspondenceInRectB[0];
602 BIASASSERT(
Equal(refinedPoint[1], correspondenceInRectB[1]));
603 if (fabs(diff)<MAX_OFFSET_REFINEMENT)
606 correspondenceInRectB = refinedPoint;
622 if (relativeRectBToOrigB. GetSourceCoordinates(
623 correspondenceInRectB, correspondenceInOrigB) == 0)
627 if (triangulator.
Triangulate(ppBA, ppBB, depthMapPoint,
628 correspondenceInOrigB, point3D) == 0)
635 BIASASSERT(diff.
NormL2()>1e-5);
636 DepthMapPel[y][x] = (float) diff.
NormL2();
652 DepthMapPel[y][x] = UNDEFINED_DEPTH;
654 ValidityMapPel[y][x] = 0;
659 ValidityMapPel[y][x] = 255;
663 #ifdef SUBPIXEL_REFINEMENT
676 template<
class InputStorageType,
class OutputStorageType>
680 float>& DepthMap,
unsigned int border)
682 if (!AreRectificationParametersValid_())
688 DisparityMap, DepthMap, PARENT::ppBA_, PARENT::ppBB_, rectPPA_,
905 template<
class InputStorageType,
class OutputStorageType>
908 OutputStorageType>::SetTargetWidth(
unsigned int targetWidth)
910 targetWidth_ = targetWidth;
913 template<
class InputStorageType,
class OutputStorageType>
916 OutputStorageType>::UseLookUpTables(
bool useLUT)
922 if (useLUT && !useLookUpTables_)
923 mappingInitialized_ =
false;
924 useLookUpTables_ = useLUT;
934 #if defined(BUILD_IMAGE_CHAR)
939 #if defined(BUILD_IMAGE_USHORT)
944 #if defined(BUILD_IMAGE_SHORT)
948 #if defined(BUILD_IMAGE_SHORT)&&defined(BUILD_IMAGE_USHORT)
952 #if defined(BUILD_IMAGE_INT)
956 #if defined(BUILD_IMAGE_USHORT)
960 #if defined(BUILD_IMAGE_USHORT) && defined(BUILD_IMAGE_INT)
964 #if defined(BUILD_IMAGE_DOUBLE)
virtual BIAS::Vector3< double > GetC() const
Get projection center.
void Release()
reimplemented from ImageBase
void AddRelativeCamera(ProjectionParametersBase *ppb)
Add a camera to the projection,the coordinates of the pose are given in relative Coordinates, first cam is origin.
virtual int Disp2Depth(const Image< float > &DisparityMap, Image< float > &DepthMap, unsigned int border)
Uses the passed disparity map for camera A to calculate the corresponding depth map.
void GetEuclidean(Vector3< HOMGPOINT3D_TYPE > &dest) const
calculate affine coordinates of this and write them to dest affine coordinates are projective coordin...
InterpolationMethod
accuracy for resampling
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Base class for rectification implementations that make use of projections to represent rectified stat...
void SetASCIIString(const std::string &ASCIIString)
void Init(Image< StorageType > &im1, Image< StorageType > &im2, Image< StorageType > &gradx1, Image< StorageType > &grady1, Image< StorageType > &gradx2, Image< StorageType > &grady2)
Prepare for tracking with prefiltered images.
bool IsEmpty() const
check if ImageData_ points to allocated image buffer or not
void SetSinkCam(const Projection &P, const Image< float > *sinkdepth=NULL)
Set your sink projection before calling Map(),.
void SetSourceCam(const Projection &P)
Set your source projection before calling Map()
virtual ProjectionParametersBase * Clone() const =0
Covariant virtual copy constructor used in BIAS::Projection.
Maps source pixel to sink pixel of given projections.
void SetMaxResiduumMAD(const KLT_TYPE maxres)
!!! Also used for X84M as maximal residuum.
void SetMaxError(const KLT_TYPE maxerr)
unsigned int GetWidth() const
void SetHalfWinSize(const int hws)
void SetMaxIterations(const int maxiter)
simple gradient calculation gx(x,y) = I(x+1,y) - I(x-1,y) gy(x,y) = I(x,y+1) - I(x,y-1)
Class for triangulation of 3Dpoints from 2D matches. Covariance matrix (refering to an uncertainty el...
smoothing with gaussian kernel
const ProjectionParametersBase * GetParameters(unsigned int cam=0) const
const parameter access function
This class hides the underlying projection model, like projection matrix, spherical camera...
int Track(HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint2D &p2tracked, KLT_TYPE &error, int &iter, KLT_TYPE &residuumMAD, KLT_TYPE &residuumMSD, Matrix< KLT_TYPE > &cov, const Matrix2x2< KLT_TYPE > &AffinePred=Matrix2x2< KLT_TYPE >(MatrixIdentity), Matrix2x2< KLT_TYPE > *AffineResult=NULL)
Calculates correspondence from image1 to image2.
int SetProj(const Projection &Proj)
int Triangulate(PMatrix &P1, PMatrix &P2, const HomgPoint2D &p1, const HomgPoint2D &p2, BIAS::Vector3< double > &point3d)
Triangulation for metric PMatrices (using C and Hinf)
void SetSigma(const double si)
std::string GetASCIIString()
OutputStorageType secondFill_
static int Save(const std::string &filename, const ImageBase &img, const enum TFileFormat FileFormat=FF_auto, const bool sync=BIAS_DEFAULT_SYNC, const int c_jpeg_quality=BIAS_DEFAULT_IMAGE_QUALITY, const bool forceNewID=BIAS_DEFAULT_FORCENEWID, const bool &writeMetaData=true)
Export image as file using extrnal libs.
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
virtual int Filter(const Image< InputStorageType > &src, Image< OutputStorageType > &dst)
sets gauss kernel if params changed and calls convolution or fast grey implementation if possible ...
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
int GetSourceCoordinates(const HomgPoint2D &sink, HomgPoint2D &source) const
wrapper function to allow the lookup implementation to be shared with other algorithms.
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_...
int UpdateMetaData()
copy P_ and co.
virtual void SetAffineBrightnessInvariance(bool bi)
enable brightness variance and offset invariant computation
void SetRejectionType(const int rejection_type)
double BilinearInterpolationGreyMinVal(const double x, const double y, const StorageType &minVal) const
Bilinear interpolation for grey-value images with pixel value check.
StorageType GetMinPixelValue(unsigned short int channel=0, unsigned int *coo=NULL) const
Get the minimal pixel value if coo!=NULL the coo[0]=x of min and coo[1]=y of min. ...
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void Set(const HOMGPOINT2D_TYPE &x, const HOMGPOINT2D_TYPE &y)
set elementwise with given 2 euclidean scalar values.
const BIAS::Projection & GetProj() const
int ParseMetaData(bool bUse2x64bitTS=true)
After ImageIO::Load() operated on AppData_, this method fills P_, Timestamp, DC_*, ...
void SetZero()
zeroes the image
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
virtual int Filter(const Image< InputStorageType > &src, Image< OutputStorageType > &grad)
returns a 2 channel image containing gx and gy
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase