25 #include <Image/RectificationBase.hh>
26 #include <Geometry/Projection.hh>
31 template <
class InputStorageType,
class OutputStorageType>
39 template <
class InputStorageType,
class OutputStorageType>
44 template <
class InputStorageType,
class OutputStorageType>
53 template <
class InputStorageType,
class OutputStorageType>
84 template <
class InputStorageType,
class OutputStorageType>
89 if (!IsInputCameraValid(img, proj)) {
90 BIASERR(
"input camera is invalid!");
95 bool setProjection =
false;
99 setProjection = (ppBA_->DoExtrinsicsDiffer(proj) || ppBA_->DoIntrinsicsDiffer(proj));
108 BIASERR(
"given projection is empty!")
112 ppBA_ = proj->
Clone();
127 template <
class InputStorageType,
class OutputStorageType>
162 template <
class InputStorageType,
class OutputStorageType>
167 if (!IsInputCameraValid(img, proj)) {
173 bool setProjection =
false;
175 setProjection =
true;
177 setProjection = (ppBB_->DoExtrinsicsDiffer(proj) || ppBB_->DoIntrinsicsDiffer(proj));
189 ppBB_ = proj->
Clone();
203 template <
class InputStorageType,
class OutputStorageType>
207 rectImg = rectImageA_;
210 template <
class InputStorageType,
class OutputStorageType>
214 rectImg = rectImageB_;
218 template <
class InputStorageType,
class OutputStorageType>
226 template <
class InputStorageType,
class OutputStorageType>
234 template <
class InputStorageType,
class OutputStorageType>
237 GetRectifiedImagePointerA()
243 template <
class InputStorageType,
class OutputStorageType>
246 GetRectifiedImagePointerB()
252 template <
class InputStorageType,
class OutputStorageType>
255 const Pose& poseCenter,
256 const Pose& poseRight,
262 if (
Equal((cL-cR).NormL2(), 0.0)) {
265 if (
Equal((cL-cC).NormL2(), 0.0)) {
268 if (
Equal((cR-cC).NormL2(), 0.0)) {
283 enum {left=0, center, right};
288 baselineParallel[left] = baseline.
ScalarProduct(z[left])*baseline;
289 baselineParallel[right] = baseline.
ScalarProduct(z[right])*baseline;
290 baselineParallel[center] = baseline.
ScalarProduct(z[center])*baseline;
293 baselineOrtho[left] = z[left]-baselineParallel[left];
294 baselineOrtho[right] = z[right]-baselineParallel[right];
295 baselineOrtho[center] = z[center]-baselineParallel[center];
297 meanOA = baselineOrtho[left]+baselineOrtho[right]+baselineOrtho[center];
303 for(
unsigned int i=0; i<3; i++) {
304 resOrientation[i][0] = baseline[i];
305 resOrientation[i][1] = newYAxis[i];
306 resOrientation[i][2] = meanOA[i];
309 cerr<<resOrientation<<endl;
315 template <
class InputStorageType,
class OutputStorageType>
321 bool failIfForwardMove)
324 BIASERR(
"poses for rectification have equal C" << endl)
336 A[0].
Set(R1[0][2], R1[1][2], R1[2][2]);
340 A[1].
Set(R2[0][2], R2[1][2], R2[2][2]);
345 double scalarProduct[2];
347 for (
unsigned int i=0;i<2;i++){
353 Ap[i] = A[i] - B * (scalarProduct[i]);
354 if(fabs(scalarProduct[i]) != 1.0)
357 if(failIfForwardMove){
358 BIASERR(
"no cylindrical rectification in case of forwar move" << endl)
365 bool first =
Equal(fabs(scalarProduct[0]), 1.0);
366 bool second =
Equal(fabs(scalarProduct[1]), 1.0);
368 if( first || second) {
377 Ap[0].
Set(R1[0][0], R1[1][0], R1[2][0]);
378 Ap[0]*= -scalarProduct[0];
381 Ap[1].
Set(R2[0][0], R2[1][0], R2[2][0]);
382 Ap[1]*= -scalarProduct[1];
404 for(
unsigned int i=0; i<3; i++) {
412 poseResA.
Set(Q, C[0]);
413 poseResB.
Set(Q, C[1]);
426 #if defined(BUILD_IMAGE_CHAR)
432 #if defined(BUILD_IMAGE_USHORT)
436 #if defined(BUILD_IMAGE_SHORT)
440 #if defined(BUILD_IMAGE_SHORT)&&defined(BUILD_IMAGE_USHORT)
444 #if defined(BUILD_IMAGE_INT)
448 #if defined(BUILD_IMAGE_USHORT)
452 #if defined(BUILD_IMAGE_USHORT) && defined(BUILD_IMAGE_INT)
456 #if defined(BUILD_IMAGE_DOUBLE)
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this->data_
int Set(const PoseParametrization &pose)
Set pose from pose parametrization.
bool IsEmpty() const
check if ImageData_ points to allocated image buffer or not
void GetRectifiedImageA(Image< OutputStorageType > &rectImg)
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
virtual ProjectionParametersBase * Clone() const =0
Covariant virtual copy constructor used in BIAS::Projection.
Vector3< double > GetZAxis() const
base class for rectification implementations and wrappers
virtual ~RectificationBase()
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
static int CalculateRectifiedBases(const Pose &poseA, const Pose &poseB, Pose &poseResA, Pose &poseResB, bool failIfForwardMove=false)
Method determines two orthonormal bases suitable for rectification.
Represents 3d pose transformations, parametrized as Euclidean translation and unit quaternion orienta...
bool IsInputCameraValid(BIAS::Camera< InputStorageType > &cam)
Method calls two parameteric pure virtual method.
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
const ProjectionParametersBase * GetParameters(unsigned int cam=0) const
const parameter access function
This class hides the underlying projection model, like projection matrix, spherical camera...
static int CalculateMeanOrientation(const Pose &poseLeft, const Pose &poseCenter, const Pose &poseRight, Quaternion< double > &orientation)
Method calculates an intermediate orientation for rectification from all three poses.
virtual int SetCameraB(BIAS::Camera< InputStorageType > &cam)
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_...
void GetRectifiedImageB(Image< OutputStorageType > &rectImg)
T GetDeterminant() const
returns the Determinant |A| of this
virtual int SetCameraA(BIAS::Camera< InputStorageType > &cam)
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
const BIAS::Projection & GetProj() const
int ParseMetaData(bool bUse2x64bitTS=true)
After ImageIO::Load() operated on AppData_, this method fills P_, Timestamp, DC_*, ...
Vector3< T > & Normalize()
normalize this vector to length 1
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)