stereoRecon.C - Hosted on DriveHQ Cloud IT Platform

Up
Upload
Download
Share
Publish
New Folder
New File
Copy
Cut
Delete
Paste
Rate
Upgrade
- Rotate
- Effect
- Edit
- Slide
- History
//=============================================================================
#include "stereoRecon.h"

#include "numericalMath.h"
#include <math.h>

stereoRecon::stereoRecon( const stereoRecon &src, bool invert )
  : origImgSizeX_(src.origImgSizeX_), origImgSizeY_(src.origImgSizeY_),
    e1_(0,0,0), e2_(0,0,0)
{
  if ( !invert )
  {
    F_ = src.F_;
    Fp_ = src.Fp_;
    leftP_ = src.leftP_;
    rightP_ = src.rightP_;
    e1_ = src.e1_;
    e2_ = src.e2_;
    idealReconPnts_ = src.idealReconPnts_;
    measuredPnts_ = src.idealReconPnts_;
    avgPntReconError_ = src.avgPntReconError_;
    avgFReconError_ = src.avgFReconError_;
    valid_ = src.valid_;
  }
  else
  {
    F_ = src.F_.transpose();
    calcFpAndEpipoles( F_, Fp_, e1_, e2_ );
    if ( !Fp_.isValid() )
    {
      valid_=false;
      return;
    }
  
    computeP( Fp_, e1_, e2_, leftP_, rightP_ );

    for ( int i=0; i<src.idealReconPnts_.size(); i++ )
    {
      idealReconPnts_.addMatch(
	src.idealReconPnts_.right(i),
	src.idealReconPnts_.left(i) );
    }
  
    for ( i=0; i<src.measuredPnts_.size(); i++ )
    {
      measuredPnts_.addMatch(
	src.measuredPnts_.right(i),
	src.measuredPnts_.left(i) );
    }
  
    avgPntReconError_ = src.avgPntReconError_;
    avgFReconError_ = src.avgFReconError_;
    valid_ = src.valid_;
  }
}

bool
stereoRecon::epipolarRecon( pointMatches2D pntList,
			    int imgSizeX, int imgSizeY )
{
  origImgSizeX_=imgSizeX;
  origImgSizeY_=imgSizeY;
  pntList.scaleAllXs( 2.0f/origImgSizeX_ );
  pntList.offsetAllXs( -1.0f );
  pntList.scaleAllYs( 2.0f/origImgSizeY_ );
  pntList.offsetAllYs( -1.0f );
  
  F_ = estimateF( pntList );
  if ( !F_.isValid() ) return false;
  
  calcFpAndEpipoles( F_, Fp_, e1_, e2_ );
  if ( !Fp_.isValid() ) return false;

  measuredPnts_ = pntList;
  
  computeP( Fp_, e1_, e2_, leftP_, rightP_ );
  
  avgPntReconError_ = 0.0f;
  int numReconPnts=0;
  avgFReconError_ = 0.0f;
  point3D reconPnt(0,0,0);
  point3D reconPntL(0,0,0);
  point3D reconPntR(0,0,0);
  
  for ( int i=0; i<pntList.size(); i++ )
  {
    avgFReconError_ +=
      fabs( ( pntList.left(i).augment(1.0f) * Fp_ ).dot(
	        pntList.right(i).augment(1.0f) ) );
    
    if ( triangulate( pntList.left(i), pntList.right(i), reconPnt ) )
    {
      reconPntL = leftP_*reconPnt;
      reconPntR = rightP_*reconPnt;
      idealReconPnts_.addMatch(
	point2D(reconPntL[X]/reconPntL[Z], reconPntL[Y]/reconPntL[Z]),
	point2D(reconPntR[X]/reconPntR[Z], reconPntR[Y]/reconPntR[Z]) );
      
      avgPntReconError_ +=
	sqrtf( (pntList.left(i)[X]-reconPntL[X]/reconPntL[Z]) *
	       (pntList.left(i)[X]-reconPntL[X]/reconPntL[Z]) +
	       (pntList.left(i)[Y]-reconPntL[Y]/reconPntL[Z]) *
	       (pntList.left(i)[Y]-reconPntL[Y]/reconPntL[Z]) ) +
	sqrtf( (pntList.right(i)[X]-reconPntR[X]/reconPntR[Z]) *
	       (pntList.right(i)[X]-reconPntR[X]/reconPntR[Z]) +
	       (pntList.right(i)[Y]-reconPntR[Y]/reconPntR[Z]) *
	       (pntList.right(i)[Y]-reconPntR[Y]/reconPntR[Z]) );

      numReconPnts++;
    
//     printf( "(%3i %3i, %3i %3i) => (%3i %3i, %3i %3i), (%+6.4f)\n",
// 	    (int)((pntList.left(i)[X]+1.0f)*origImgSizeX_/2.0f),
// 	    (int)((pntList.left(i)[Y]+1.0f)*origImgSizeY_/2.0f),
// 	    (int)((pntList.right(i)[X]+1.0f)*origImgSizeX_/2.0f),
// 	    (int)((pntList.right(i)[Y]+1.0f)*origImgSizeY_/2.0f),
// 	    (int)((idealReconPnts_.left(i)[X]+1.0f)*origImgSizeX_/2.0f),
// 	    (int)((idealReconPnts_.left(i)[Y]+1.0f)*origImgSizeY_/2.0f),
// 	    (int)((idealReconPnts_.right(i)[X]+1.0f)*origImgSizeX_/2.0f),
// 	    (int)((idealReconPnts_.right(i)[Y]+1.0f)*origImgSizeY_/2.0f),
// 	    (reconPntR * Fp_).dot(reconPntL) );
    }
  }
  avgPntReconError_ /= numReconPnts;
  avgFReconError_ /= pntList.size();
  return true;
}

bool 
stereoRecon::epipolarRecon( const vector<subWindowListAnim> &features,
			    int frame1, int frame2,
			    int imgSizeX, int imgSizeY )
{
  vector<subWindowListAnim>::const_iterator iter;
  pointMatches2D pntList;
  
  for ( iter = features.begin(); iter!=features.end(); iter++ )
  {
    if ( (iter->includesFrame(frame1)) &&
	 (iter->includesFrame(frame2)) )
    { // feature is present in both images
      pntList.addMatch( point2D( iter->getSubWindow(frame1).centerX(),
				 iter->getSubWindow(frame1).centerY() ),
			point2D( iter->getSubWindow(frame2).centerX(),
				 iter->getSubWindow(frame2).centerY() ) );
    }
  }

  if (pntList.size()<9)
    return false;
  else
    return epipolarRecon( pntList, imgSizeX, imgSizeY );
}

matrix3x3 
stereoRecon::estimateF( pointMatches2D pntList )
{
  int i,j;
  
  matrixMxN A( pntList.size(), 9 );
  vectorN W(9);
  matrixMxN V(9,9);
  matrix3x3 F;
  
  A.makeZero();
  for ( i=0; i<pntList.size(); i++ )
  {
    float x1 = pntList.left(i)[X];
    float y1 = pntList.left(i)[Y];
    float x2 = pntList.right(i)[X];
    float y2 = pntList.right(i)[Y];
    
    A[i][0] = x2*x1;
    A[i][1] = x2*y1;
    A[i][2] = x2;
    A[i][3] = y2*x1;
    A[i][4] = y2*y1;
    A[i][5] = y2;
    A[i][6] = x1;
    A[i][7] = y1;
    A[i][8] = 1.0f;
  }

  if ( !numericalMath::singularValueDecomp(A,W,V) )
  {
    printf("SVD failed (stereoRecon::estimateF)\n");
    F.setValidity(false);
    return F;
  }
  
  int smallestSV=W.smallestComponent();
  for ( i=0; i<3; i++ )
    for ( j=0; j<3; j++ )
      F[i][j] = V[smallestSV][i*3+j];
  
  return F;
}

void
stereoRecon::calcFpAndEpipoles( const matrix3x3 &F,
				matrix3x3 &Fp, point3D &e1, point3D &e2 )
{
  int i;

  matrixMxN A(F);
  vectorN W(3);
  matrixMxN V(3,3);

  if ( !numericalMath::singularValueDecomp(A,W,V) )
  {
    Fp.setValidity(false);
    return;
  }
  
  // smallest singular value is always the last... I think.
  int smallestSV = W.smallestComponent();
  W[smallestSV] = 0.0f;

  matrixMxN Wmat(3,3);
  Wmat.makeZero();
  Wmat.setDiagonal(W);

  Fp = (A * Wmat * V.transpose()).extractUpperLeft3x3();

  
  for ( i=0; i<3; i++ )
    e1[i] = V[i][smallestSV];
  
  for ( i=0; i<3; i++ )
    e2[i] = A[i][smallestSV];
}

point3D 
stereoRecon::getLeftE() const
{
  point3D retPnt( e1_ );
  if (retPnt[Z]<0) retPnt[X] *= -1.0f;
  if (retPnt[Z]<0) retPnt[Y] *= -1.0f;
//   retPnt[X] /= retPnt[Z];
//   retPnt[Y] /= retPnt[Z];
  retPnt[X] += 1.0f;
  retPnt[Y] += 1.0f;
  retPnt[X] *= origImgSizeX_/2.0f;
  retPnt[Y] *= origImgSizeY_/2.0f;
  return retPnt;
}

point3D 
stereoRecon::getRightE() const
{
  point3D retPnt( e2_ );
  if (retPnt[Z]<0) retPnt[X] *= -1.0f;
  if (retPnt[Z]<0) retPnt[Y] *= -1.0f;
//   retPnt[X] /= retPnt[Z];
//   retPnt[Y] /= retPnt[Z];
  retPnt[X] += 1.0f;
  retPnt[Y] += 1.0f;
  retPnt[X] *= origImgSizeX_/2.0f;
  retPnt[Y] *= origImgSizeY_/2.0f;
  return retPnt;
}

point2D
stereoRecon::getReconPntRight(int i) const
{
  point2D retPnt(idealReconPnts_.right(i));
  retPnt[X] += 1.0f;
  retPnt[Y] += 1.0f;
  retPnt[X] *= origImgSizeX_/2.0f;
  retPnt[Y] *= origImgSizeY_/2.0f;
  return retPnt;
}

point2D
stereoRecon::getReconPntLeft(int i) const
{
  point2D retPnt(idealReconPnts_.left(i));
  retPnt[X] += 1.0f;
  retPnt[Y] += 1.0f;
  retPnt[X] *= origImgSizeX_/2.0f;
  retPnt[Y] *= origImgSizeY_/2.0f;
  return retPnt;
}

point2D
stereoRecon::getMeasuredPntRight(int i) const
{
  point2D retPnt(measuredPnts_.right(i));
  retPnt[X] += 1.0f;
  retPnt[Y] += 1.0f;
  retPnt[X] *= origImgSizeX_/2.0f;
  retPnt[Y] *= origImgSizeY_/2.0f;
  return retPnt;
}

point2D
stereoRecon::getMeasuredPntLeft(int i) const
{
  point2D retPnt(measuredPnts_.left(i));
  retPnt[X] += 1.0f;
  retPnt[Y] += 1.0f;
  retPnt[X] *= origImgSizeX_/2.0f;
  retPnt[Y] *= origImgSizeY_/2.0f;
  return retPnt;
}

point3D 
stereoRecon::getEpipolarLineRight( int i ) const
{
  point3D line2( Fp_ * measuredPnts_.left(i).augment(1.0f) );
//   line2[0] *= origImgSizeX_/2.0f;
//   line2[1] *= origImgSizeY_/2.0f;
//   line2[2] += line2[0] + line2[1];
  line2[2] += line2[0] + line2[1];
  line2[0] *= 2.0f/origImgSizeX_;
  line2[1] *= 2.0f/origImgSizeY_;
  return line2;
}

point3D 
stereoRecon::getEpipolarLineLeft( int i ) const
{
  point3D line1( measuredPnts_.right(i).augment(1.0f) * Fp_ );
//   line1[0] *= origImgSizeX_/2.0f;
//   line1[1] *= origImgSizeY_/2.0f;
//   line1[2] += line1[0] + line1[1];
  line1[2] += line1[0] + line1[1];
  line1[0] *= 2.0f/origImgSizeX_;
  line1[1] *= 2.0f/origImgSizeY_;
  return line1;
}

bool 
stereoRecon::triangulate( const point2D &leftPnt, const point2D &rightPnt,
			  point3D &scenePnt )
{
  point3D l1( Fp_*rightPnt.augment(1.0f) );
  // algorithm fails for horizontal OR vertical lines
  if ( (l1[0]==0.0f) || (l1[1]==0.0f) ) return false;
  
  // slope of perpendicular is (l1[1]/l1[0])
  double d = leftPnt[Y] - (l1[1]/l1[0])*leftPnt[X];

  double a=l1[0];
  double b=l1[1];
  double c=l1[2];

  double x1 = -(1/(a*a+b*b)) * (a*c+b*d);
  double y1 = -(1/(a*a+b*b)) * (b*c-a*d);

//   x1 = (leftPnt[X]+x1) / 2.0f;
//   y1 = (leftPnt[Y]+y1) / 2.0f;

//   point3D l2( Fp_*point3D(x1,y1,1.0f) );
//   // algorithm fails for horizontal OR vertical lines
//   if ( (l2[0]==0.0f) || (l2[1]==0.0f) ) return false;
  
//   // slope of perpendicular is (l2[1]/l2[0])
//   d = rightPnt[Y] - (l2[1]/l2[0])*rightPnt[X];

//   float x2 = (-d - l2[2]/d) / (l2[1]/l2[0] + l2[0]/l2[1]);
//   float y2 = (l2[1]/l2[0]*x2) + d;
  float x2 = rightPnt[X];
  float y2 = rightPnt[Y];

  //float check = (point3D(x1,y1,1.0f)*Fp_).dot(point3D(x2,y2,1.0f));
  float check = -l1[0]/l1[1]*x1 - l1[2]/l1[1] - y1;
  printf("%6.4f\n",check);
  check = l1[1]/l1[0]*x1 + d - y1;
  printf("%6.4f\n",check);
  

  // There! Now we should have a corrected (x1,y1), (x2,y2) that can
  // be used to triangulate the 3D point
  
  matrixMxN A(6,4);
  vectorN B(6);
  
  for ( int i=0; i<3; i++ )
  {
    for ( int j=0; j<4; j++ )
    {
      A[i  ][j] = leftP_ [i][j];
      A[i+3][j] = rightP_[i][j];
    }
  }
  
  B[0] = x2;
  B[1] = y2;
  B[2] = 1.0f;
  
  B[3] = x1;
  B[4] = y1;
  B[5] = 1.0f;

  vectorN W(4);
  matrixMxN V(4,4);
  
  if ( !numericalMath::singularValueDecomp(A,W,V) )
  {
    //printf("SVD failed (stereoRecon::triangulate)\n");
    return false;
  }

  vectorN triPnt(4);
  
  numericalMath::singularValueBacksub(A,W,V,B,triPnt);

  scenePnt = point3D( triPnt[0]/triPnt[3],
		      triPnt[1]/triPnt[3],
		      triPnt[2]/triPnt[3] );
  
  matrixMxN lP(leftP_);
  matrixMxN rP(rightP_);
  matrixMxN fp(Fp_);

  vectorN x( triPnt*rP.transpose() );
  vectorN y( lP*triPnt );
  x[0]/=x[2];
  x[1]/=x[2];
  y[0]/=y[2];
  y[1]/=y[2];

  float res1 = (triPnt * rP.transpose() * Fp_).dot( lP * triPnt );
  float res2 = (x * Fp_).dot( y );
  printf("%+6.4f %+6.4f, %+6.4f %+6.4f\n",
	 (x[0]+1.0f)*origImgSizeX_/2.0f, (x[1]+1.0f)*origImgSizeY_/2.0f,
	 (y[0]+1.0f)*origImgSizeX_/2.0f, (y[1]+1.0f)*origImgSizeY_/2.0f );
  printf("%+6.4f %+6.4f, %+6.4f %+6.4f\n",
	 (x1+1.0f)*origImgSizeX_/2.0f, (y1+1.0f)*origImgSizeY_/2.0f,
	 (x2+1.0f)*origImgSizeX_/2.0f, (y2+1.0f)*origImgSizeY_/2.0f );
  printf("%+6.4f %+6.4f, %+6.4f %+6.4f\n",
	 (leftPnt[0]+1.0f)*origImgSizeX_/2.0f, (leftPnt[1]+1.0f)*origImgSizeY_/2.0f,
	 (rightPnt[0]+1.0f)*origImgSizeX_/2.0f, (rightPnt[1]+1.0f)*origImgSizeY_/2.0f );
  printf("%+6.4f %+6.4f\n", res1, res2 );

  return true;
  
//   matrixMxN A(6,4);
//   vectorN B(6);
  
//   for ( int i=0; i<3; i++ )
//   {
//     for ( int j=0; j<4; j++ )
//     {
//       A[i  ][j] = leftP_ [i][j];
//       A[i+3][j] = rightP_[i][j];
//     }
//   }
  
//   B[0] = leftPnt[0]/(leftPnt[0]+leftPnt[1]);
//   B[1] = leftPnt[1]/(leftPnt[0]+leftPnt[1]);
//   B[2] = 1.0f/(leftPnt[0]+leftPnt[1]);
  
//   B[3] = rightPnt[0]/(rightPnt[0]+rightPnt[1]);
//   B[4] = rightPnt[1]/(rightPnt[0]+rightPnt[1]);
//   B[5] = 1.0f/(rightPnt[0]+rightPnt[1]);

//   vectorN W(4);
//   matrixMxN V(4,4);
  
//   if ( !numericalMath::singularValueDecomp(A,W,V) )
//   {
//     //printf("SVD failed (stereoRecon::triangulate)\n");
//     return false;
//   }

//   vectorN triPnt(4);
  
//   numericalMath::singularValueBacksub(A,W,V,B,triPnt);

//   scenePnt = point3D( triPnt[0]/triPnt[3],
// 		      triPnt[1]/triPnt[3],
// 		      triPnt[2]/triPnt[3] );

//   return true;
  
//   matrixMxN lP(leftP_);
//   matrixMxN rP(rightP_);
//   matrixMxN fp(Fp_);

//   vectorN x( triPnt*rP.transpose() );
//   x[0]/=x[2];
//   x[1]/=x[2];
//   vectorN y( lP*triPnt );
//   y[0]/=y[2];
//   y[1]/=y[2];

//   float res1 = (triPnt * rP.transpose() * Fp_).dot( lP * triPnt );
//   float res2 = (x * Fp_).dot( y );
//   printf("%+6.4f %+6.4f %+6.4f, %+6.4f %+6.4f %+6.4f\n", x[0], x[1], x[2], y[0], y[1], y[2] );
//   printf("%+6.4f %+6.4f, %+6.4f %+6.4f\n", leftPnt[0], leftPnt[1], rightPnt[0], rightPnt[1] );
//   printf("%+6.4f %+6.4f\n", res1, res2 );

  
//   // Triangulation as given in "Multiple View Goem"
//   // I don't like it... not very good
//   matrixMxN A(4,4);
//   A[0][0] = leftPnt[X]*leftP_[2][0] - leftP_[0][0];
//   A[0][1] = leftPnt[X]*leftP_[2][1] - leftP_[0][1];
//   A[0][2] = leftPnt[X]*leftP_[2][2] - leftP_[0][2];
//   A[0][3] = leftPnt[X]*leftP_[2][3] - leftP_[0][3];
  
//   A[1][0] = leftPnt[Y]*leftP_[2][0] - leftP_[1][0];
//   A[1][1] = leftPnt[Y]*leftP_[2][1] - leftP_[1][1];
//   A[1][2] = leftPnt[Y]*leftP_[2][2] - leftP_[1][2];
//   A[1][3] = leftPnt[Y]*leftP_[2][3] - leftP_[1][3];
  
//   A[2][0] = rightPnt[X]*rightP_[2][0] - rightP_[0][0];
//   A[2][1] = rightPnt[X]*rightP_[2][1] - rightP_[0][1];
//   A[2][2] = rightPnt[X]*rightP_[2][2] - rightP_[0][2];
//   A[2][3] = rightPnt[X]*rightP_[2][3] - rightP_[0][3];
  
//   A[3][0] = rightPnt[Y]*rightP_[2][0] - rightP_[1][0];
//   A[3][1] = rightPnt[Y]*rightP_[2][1] - rightP_[1][1];
//   A[3][2] = rightPnt[Y]*rightP_[2][2] - rightP_[1][2];
//   A[3][3] = rightPnt[Y]*rightP_[2][3] - rightP_[1][3];

//   vectorN W(4);
//   matrixMxN V(4,4);
  
//   if ( !numericalMath::singularValueDecomp(A,W,V) )
//   {
//     //printf("SVD failed (stereoRecon::triangulate)\n");
//     return false;
//   }

//   int smallestSV=W.smallestComponent();
// //   matrixMxN lP(leftP_);
// //   matrixMxN rP(rightP_);
// //   vectorN dub(4);
// //   dub[0]=V[smallestSV][0];
// //   dub[1]=V[smallestSV][1];
// //   dub[2]=V[smallestSV][2];
// //   dub[3]=V[smallestSV][3];
// //   matrixMxN fp(Fp_);
// //   float res = (dub*rP.transpose()*fp).dot((lP*dub));
// //   printf("%+6.4f %+6.4f %+6.4f %+6.4f => %i\n", W[0], W[1], W[2], W[3], W.smallestComponent());
// //   printf("%+6.4f\n", res );

//   scenePnt = point3D( V[smallestSV][0]/V[smallestSV][3],
// 		      V[smallestSV][1]/V[smallestSV][3],
// 		      V[smallestSV][2]/V[smallestSV][3] );
//   return true;
}

void 
stereoRecon::computeP( const matrix3x3 F, const point3D &e1, const point3D &e2,
		       matrix3x4 &P1, matrix3x4 &P2 )
{
  // P1 is set to be I
  P1.makeZero();
  P1[0][0] = 1.0f;
  P1[1][1] = 1.0f;
  P1[2][2] = 1.0f;

  // P2 = [ [e2]x F | e2 ]
  matrix3x3 e2XMat(  0.0f,  -e2[2],  e2[1],
		     e2[2],  0.0f,  -e2[0],
		    -e2[1],  e2[0],  0.0f   );

  P2.setLeft3x3( e2XMat*F );
  P2[0][3] = e2[0];
  P2[1][3] = e2[1];
  P2[2][3] = e2[2];
}

void
stereoMovieRecon::calcDirectFs( const vector<subWindowListAnim> &features )
{
  int i,j;
  
  stereoRecon *Ftemp;
  for ( i=0; i<length_-2; i++ )
  {
    for ( j=i+1; j<length_-1; j++ )
    {
      Ftemp = new stereoRecon( features, i, j, width_, height_ );
      if ( Ftemp->isValid() )
      {
	F_[i][j] = Ftemp;
	F_[j][i] = new stereoRecon( *Ftemp, true );
	if ( !F_[j][i]->isValid() )
	{
	  delete F_[j][i];
	  printf("ERROR: stereoMovieRecon::calcDirectFs\n");
	}
      }
      else
      {
	delete Ftemp;
      }
    }
  }
}

void
stereoMovieRecon::extrapolateFs()
{
  int i,j,k,l;
  stereoRecon *Ftemp;

  setvbuf(stdout,0,_IONBF,0);
  printf("           ");
  int numRefined=INT_MAX;
  while ( numRefined>100 )
  {
    numRefined=0;
    for ( i=0; i<length_-4; i++ )
    {
      for ( j=i+1; j<length_-3; j++ )
      {
	if (j%8==0) printf(".");
	for ( k=j+1; k<length_-2; k++ )
	{
	  for ( l=k+1; l<length_-1; l++ )
	  {
	    if ( (F_[i][j]!=NULL) && (F_[j][k]!=NULL) && (F_[k][l]!=NULL) &&
		 (F_[i][k]!=NULL) && (F_[j][l]!=NULL) )
	    {
	      Ftemp = calcSingleF( i, j, k, l );
	      if ( !Ftemp->isValid() )
	      {
		delete Ftemp;
		printf("ERROR: stereoMovieRecon::interpolateFs\n");
		printf("while interpolating Fp from %2i %2i %2i %2i\n",i,j,k,l);
	      }
	      else if ( F_[i][l]==NULL )
	      {
		F_[i][l] = Ftemp;
		F_[l][i] = new stereoRecon( *Ftemp, true );
	      }
	      else if ( F_[i][l]->avgPntReconError()>Ftemp->avgPntReconError() )
	      {
		delete F_[i][l];
		delete F_[l][i];
		F_[i][l] = Ftemp;
		F_[l][i] = new stereoRecon( *Ftemp, true );
		numRefined++;
	      }
	      else
	      {
		delete Ftemp;
	      }
	    }
	  }
	}
      }
    }
    printf("\nRefined %i",numRefined);
  }
  printf("\n");
}

 
stereoRecon *
stereoMovieRecon::calcSingleF( int i, int j, int k, int l )
{
  int idx;
  pointMatches2D firstToLastMatches;
  for ( idx=0; idx<F_[j][k]->getIdealPntMatches().size(); idx++ )
  {
    point3D pointIn1( 
      pointTransfer3View( *F_[i][j], *F_[i][k],
			  F_[j][k]->getIdealPntMatches().left(idx),
			  F_[j][k]->getIdealPntMatches().right(idx) ) );
    point3D pointIn4( 
      pointTransfer3View( *F_[l][j], *F_[l][k],
			  F_[j][k]->getIdealPntMatches().left(idx),
			  F_[j][k]->getIdealPntMatches().right(idx) ) );
    firstToLastMatches.addMatch(
      point2D( pointIn1[0]/pointIn1[2], pointIn1[1]/pointIn1[2] ),
      point2D( pointIn4[0]/pointIn4[2], pointIn4[1]/pointIn4[2] ) );
  }

  stereoRecon *newRecon =
    new stereoRecon( firstToLastMatches, width_, height_ );

  newRecon->measuredPnts_.clear();
  
  newRecon->avgPntReconError_ =
    F_[i][j]->avgPntReconError_ +
    F_[j][k]->avgPntReconError_ +
    F_[k][l]->avgPntReconError_ +
    F_[i][k]->avgPntReconError_ +
    F_[j][l]->avgPntReconError_;
  
  newRecon->avgFReconError_ =
    F_[i][j]->avgFReconError_ +
    F_[j][k]->avgFReconError_ +
    F_[k][l]->avgFReconError_ +
    F_[i][k]->avgFReconError_ +
    F_[j][l]->avgFReconError_;
  
  return newRecon;
}

point3D 
stereoMovieRecon::pointTransfer3View( const stereoRecon &F31,
				      const stereoRecon &F32,
				      const stereoRecon &F12,
				      const point3D pntInRef12 )
{
  point3D pnt1( F12.getLeftP() * pntInRef12 );
  point3D pnt2( F12.getRightP() * pntInRef12 );
  return pointTransfer3View( F31, F32, pnt1, pnt2 );
}

point3D 
stereoMovieRecon::pointTransfer3View( const stereoRecon &F31,
				      const stereoRecon &F32,
				      const point3D &pnt1,
				      const point3D &pnt2 )
{
  return (F31.getFp()*pnt1).cross(F32.getFp()*pnt2);
}

point3D 
stereoMovieRecon::pointTransfer3View( const stereoRecon &F31,
				      const stereoRecon &F32,
				      const point2D &pnt1,
				      const point2D &pnt2 )
{
  return pointTransfer3View(F31,F32,pnt1.augment(1.0f),pnt2.augment(1.0f));
}
stereoRecon.C
Page URL
File URL
Prev 39/50 Next
Download ( 18 KB )
Leading Cloud Surveillance, recording and storage service. IP camera remote viewing, live streaming, broadcasting
Leading Enterprise Cloud IT Service. Cloud File Server, Drive Mapping, FTP Hosting, Online Storage
Note: The DriveHQ service banners will NOT be displayed if the file owner is a paid member.
Comments
Total ratings: 0 Average rating:
Not Rated
Would you like to comment?

Join DriveHQ for a free account, or Logon if you are already a member.