PMatrixEstimation.cpp

/* 
 This file is part of the BIAS library (Basic ImageAlgorithmS).

 Copyright (C) 2003-2009    (see file CONTACT for details)
 Multimediale Systeme der Informationsverarbeitung
 Institut fuer Informatik
 Christian-Albrechts-Universitaet Kiel


 BIAS is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 BIAS is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with BIAS; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "PMatrixEstimation.hh"
#include "MathAlgo/Minpack.hh"
#include <Base/Math/Operators.hh>
#include <Geometry/EMatrix.hh>
#include <Base/Common/BIASpragma.hh>

#include <fstream>

using namespace BIAS;

/// error function, which punishes large differences of submatrix from 
/// a rotation matrix 
int DistanceFromRotation4Var(void *p,int n,const double *x, double *fvec, int iflag) {
  if (n!=4) {
    BIASERR("Caller supplied invalid n="<< (n));
    return -1;
  }
  PMatrixEstimation *pCurrentObject = (PMatrixEstimation *)p;
  BIAS::Matrix<double> E2(3, 1), BMat(1, 3);
  for (int k=0; k< 3; k++) {
    E2[k][0] = pCurrentObject->globalEpipole2_[k];
    BMat[0][k] = x[3]*x[k];
  }
  BIAS::Matrix<double> Outer = E2*BMat;
  BIAS::Matrix<double> SubMatrix = pCurrentObject->globalM2_*x[3] + Outer;

  // enforce all rows to be orthogonal
  fvec[0] = SubMatrix.GetRow(0)*SubMatrix.GetRow(1);
  fvec[1] = SubMatrix.GetRow(1)*SubMatrix.GetRow(2);
  fvec[2] = SubMatrix.GetRow(2)*SubMatrix.GetRow(0);
  // enforce all rows to be of unit length
  fvec[3] = fabs(1.0-(SubMatrix.GetRow(0) * SubMatrix.GetRow(0)));
  fvec[3] += fabs(1.0-(SubMatrix.GetRow(1) * SubMatrix.GetRow(1)));
  fvec[3] += fabs(1.0-(SubMatrix.GetRow(2) * SubMatrix.GetRow(2)));
  return 0;
}


PMatrixEstimation::PMatrixEstimation()
{
  NewDebugLevel("BIAS_FMATRIXDEBUG");
  NewDebugLevel("BIAS_PMATRIXDEBUG");
  NewDebugLevel("BIAS_FQuasiEuklid");
}

int PMatrixEstimation::ComputeFromFDirect(BIAS::FMatrix &F, const double& BaselineMagnitude,
    BIAS::PMatrix &P1, BIAS::PMatrix &P2) {

  HomgPoint2D Epipole1, Epipole2;
  if (F.GetEpipoles(Epipole1, Epipole2)<0)
    return -1;

  Epipole2.Normalize();
  if (BaselineMagnitude<0.0)
    Epipole2 *= -1.0;

  // initially set p2 to be the identity in its left 3x3 i.e. no rotation
  // translation of P2 is set to Epipole2
  InitP1P2Trans(P1, P2, Epipole2);

  // 
  RMatrix R;
  ComputeRotation(F, Epipole1, Epipole2, R);
  R = R.Transpose();

  // copy R into P2
  for (unsigned int i = 0; i < 3; i++)
    for (unsigned int j = 0; j < 3; j++)
      P2[i][j] = R[i][j];

  // sanity check: has R rank 3 ?
  SVD RSVD((Matrix<ROTATION_MATRIX_TYPE>)R);
  Vector< double> S = RSVD.GetS();

  // catch division by zero problem in next check
  if (S[0]<1e-10)
    return -1;
  // sanity check on rank 3 of R
  if (S[2]/S[0]<1e-5)
    return -1;

  // if (P2_base.NormFrobenius()<1e-10) return -1;

  P1.InvalidateDecomposition();
  P2.InvalidateDecomposition();

  // assume p1.c==0:
  double myscale = fabs(P2.GetC().NormL2() / BaselineMagnitude);
  P2[0][3] /= myscale;
  P2[1][3] /= myscale;
  P2[2][3] /= myscale;
  P2.InvalidateDecomposition();
  return 0;
}

void PMatrixEstimation::ComputeRotation(BIAS::FMatrix &F, BIAS::HomgPoint2D &Epipole1,
    BIAS::HomgPoint2D &Epipole2, BIAS::RMatrix &R) {

  // set rotation part as described in Foerstner
  Matrix3x3<double> ee = Epipole2.OuterProduct(Epipole1);
  Matrix3x3<double> e, eF;
  e.SetAsCrossProductMatrix(Epipole2);
  eF=e*F;

  eF *= ee.NormFrobenius();
  ee *= eF.NormFrobenius()/sqrt(2.0);

  R = eF+ee;

  // enforce right-handed rotation !
  R /= (R.GetDeterminant() < 0.0) ? -sqrt(R[2][0]*R[2][0]+R[2][1]*R[2][1] + R[2][2]*R[2][2])
      : sqrt(R[2][0]*R[2][0]+R[2][1]*R[2][1] + R[2][2] *R[2][2]);

  // check for positive optical axis of the second cemara because the first
  // cameras axis is (0 , 0 , 1) otherwise rotate around baseline (epipole) 
  if (R[2][2] < 0.0) {
    RMatrix rot(Epipole2, M_PI);
    R = rot*R;
  }
  R = R.Transpose();
}

void PMatrixEstimation::ComputeRotationCenter(BIAS::PMatrix P1, BIAS::PMatrix P2, BIAS::RMatrix &R,
    BIAS::Vector3<double> &C) {

  Vector3<double> Epipole2 = P2*P1.GetC();
  Epipole2.MultiplyIP(Epipole2.NormL2());

  // compute homography for F
  Matrix3x3<double> H, H1, H2;
  for (unsigned int i = 0; i < 3; i++)
    for (unsigned int j = 0; j < 3; j++) {
      H1[i][j] = P1[i][j];
      H2[i][j] = P2[i][j];
    }
  Matrix3x3<double> H1i;
  int ires = H1.GetInverse(H1i);
  if (ires!=0)
    BIASERR("error: Unable to invert matrix "<<H1<<std::endl);
  H = H2*H1i;
  H2.SetAsCrossProductMatrix(Epipole2);
  H1 = H2*H;
  FMatrix F(H1);

  HomgPoint2D e1, e2;
  F.GetEpipoles(e1, e2);
  ComputeRotation(F, e1, e2, R);

  // compute center
  for (unsigned int j = 0; j < 3; j++)
    Epipole2[j] = P2[j][3];

  C = R.Transpose()*Epipole2;
  C *= -1.0;

}

void PMatrixEstimation::ComputeFromFQuasiEuklid(BIAS::FMatrix &F, double BaselineMagnitude,
    BIAS::PMatrix &P1, BIAS::PMatrix &P2) {

  Matrix<double> SubMatrix;
  Vector<double> C(4);
  Matrix3x3<double> M2Matrix;

  BCDOUT(BIAS_FMATRIXDEBUG, "F = " << F);

  HomgPoint2D Epipole1, Epipole2;
  F.GetEpipoles(Epipole1, Epipole2);

  Epipole2.Normalize();
  Epipole2 *= BaselineMagnitude;

  BCDOUT(BIAS_FQuasiEuklid, "Epipole 1 = " << Epipole1 << " Epipole2 = " << Epipole2);

  // initially set p2 to be the identity in its left 3x3 i.e. no rotation
  InitP1P2Trans(P1, P2, Epipole2);
  BCDOUT(BIAS_FQuasiEuklid, "After InitP1P2Trans P2= "<<P2);

  // find the decomposition of p2, and the closest p2 to the current setting.
  //FindClosestP2(F, P1, P2, M2Matrix, C, Epipole2);
  FindClosestP2(F, P2, M2Matrix, C, Epipole2);
  BCDOUT(BIAS_FQuasiEuklid, "After FindClosestP2 P2 = "<< P2);

  // now want to adjust the three DOF in b_trivec to make (m2+e2 b^T) as 
  // close to orthogonal as possible.

  // memcpy (pm_m2_matrix, m2_matrix, sizeof (pm_m2_matrix));
  // fm_compute_epipoles (f_matrix, &epipole1_trivec, 
  // &pm_epipole2_trivec);


  // set stop criterion for powell:
  double dMaxError = 1e-10;

  //these variables are now threadsave as members and accessible via
  // pointer to class in Powell error function
  globalEpipole2_ = Epipole2;
  globalM2_ = (Matrix<double>)M2Matrix;

  // set current value as start value
  Vector<double> CStart = C;

  int iError=0;
  // minimize error funtion DistanceFromRotation4Var on P using Powell
  if ((iError=Powell(&DistanceFromRotation4Var, this, CStart, C, dMaxError))<0) {
    BIASERR("Minimization did not succeed, error code is "<<iError);
  }
  BCDOUT(BIAS_FQuasiEuklid, "C =" << C);

  //ho_trivec_outer_product (&pm_epipole2_trivec, &b_trivec, outer_matrix);
  Matrix<double> E2(3, 1), BMat(1, 3);
  for (int k=0; k< 3; k++) {
    E2[k][0] = Epipole2[k];
    BMat[0][k] = C[k];
  }
  Matrix<double> Outer = E2*BMat;
  BCDOUT(BIAS_FQuasiEuklid, "M2Matrix = " << M2Matrix);
  BCDOUT(BIAS_FQuasiEuklid, "Outer = " << Outer);

  SubMatrix = (Matrix<double>)(M2Matrix + (Matrix3x3<double>)Outer);
  BCDOUT(BIAS_FQuasiEuklid, "SubMatrix = " << SubMatrix);

  // copy left part of P from submatrix
  for (int row = 0; row < 3; row++)
    for (int col = 0; col < 3; col++) {
      P2[row][col] = SubMatrix [row][col];
    }
  BCDOUT(BIAS_FQuasiEuklid, "P2 = " << P2);

}

int PErrorFunction(void *p, int m, int n,const double *x, double *fvec, int iflag) {

    PMatrixEstimation *pCurrentPEstimation= (PMatrixEstimation *)p;

  // rebuild Kmatrix
  KMatrix K(MatrixIdentity);
  K.SetFx(x[0]);
  K.SetFy(x[0]);
  
  K.SetHx(((double)pCurrentPEstimation->width_-1.0)/2.0);
  K.SetHy(((double)pCurrentPEstimation->height_-1.0)/2.0);
  K[2][2] = 1.0;

  // rebuild epipole'
  Vector3<double> t;
  t[0] = 1;
  t[1] = x[1];
  t[2] = x[2];

  t = t.CoordSphereToEuclidean();
  
  // rebuild Rmatrix
  RMatrix R(MatrixIdentity);
  R.SetXYZ(x[3], x[4], x[5]);

  // get correspondences
  const unsigned int uiDataSize = pCurrentPEstimation->p1_->size();
  const std::vector<BIAS::HomgPoint2D> &p1 = *pCurrentPEstimation->p1_;
  const std::vector<BIAS::HomgPoint2D> &p2 = *pCurrentPEstimation->p2_;

  // rebuild F matrix
  FMatrix F;
  K = K.Invert();
  F = K.Transpose() * (Matrix3x3<double>)t.GetSkewSymmetricMatrix() * R.Transpose() * K;
  
  pCurrentPEstimation->lastError_ = 0;
  for (unsigned int j=0; j<uiDataSize; j++) {
    fvec[j] = F.GetEpipolarErrorHomogenized(p1[j], p2[j]);
    pCurrentPEstimation->lastError_ += fvec[j];
 }
  (pCurrentPEstimation->errorSum_).push_back(pCurrentPEstimation->lastError_);
  pCurrentPEstimation->lastError_ /= uiDataSize;
  return 0;
}

int PMatrixEstimation::AutoCalib(const BIAS::FMatrix &F, const int width, const int height,
    const std::vector<BIAS::HomgPoint2D> &p1, const std::vector<BIAS::HomgPoint2D> &p2,
    BIAS::PMatrix &P1, BIAS::PMatrix &P2) {

  // prepare private vars for Levenberg-Marquardt algorithm (are equal for all trials)
  width_ = width;
  height_ = height;

  // we need to call an external function from levenberg-marquardt,
  // prepare data structs
  // warning: this section is not thread safe !
  p1_ = &p1;
  p2_ = &p2;

  // prepare initial K matrix with width + height as initial guess
  KMatrix K(MatrixIdentity);
  // camera matrix for both cameras
  K.SetFx(((double)width + (double)height));
  K.SetFy(((double)width + (double)height));
  std::vector<double> focalLengths;

  focalLengths.push_back(((double)width + (double)height)*2.0);
   //focalLengths.push_back((double)width + (double)height);
//  for (int foc = 2; foc < BIAS_MAXNUMAUTOCALIBITERS; foc++) {
  
  for (int foc = 1; foc < BIAS_MAXNUMAUTOCALIBITERS; foc++) {
    if(focalLengths[foc-1]-50 < 100) break;
    focalLengths.push_back(focalLengths[foc-1]-50);
  }
  // in image coordinates
  double mx = ((double)width - 1.0) / 2.0;
  double my = ((double)height - 1.0) / 2.0;
  K.SetHx(mx);
  K.SetHy(my);
  K.SetSkew(0.0);

  K[2][2] = 1;

  std::vector<PMatrix> P1Results;
  std::vector<PMatrix> P2Results;
  std::vector<double> smallestError;

  // Call to private AutoCalib
  int res = 0;
  for (unsigned int calibIters = 0; calibIters < focalLengths.size(); calibIters++) {
    K.SetFx(focalLengths[calibIters]);
    K.SetFy(focalLengths[calibIters]);
    res = AutoCalib_(F, p1.size(), K, P1, P2);
    std::cout << "result of AutoCalib_ " << res << " focal length guess " << focalLengths[calibIters] << " est focal length " << K.GetFx() << " error " << lastError_ << std::endl;
    if (res == 0) {
      P1Results.push_back(P1);
      P2Results.push_back(P2);
      smallestError.push_back(lastError_);
    }
  }

  // find best results and fix baseline direction
  if (P1Results.size() >= 1) {

    P1 = P1Results[0];
    P2 = P2Results[0];
    // find two closest focal lengths
    double errorS = DBL_MAX;

    for (unsigned int i = 0; i < P1Results.size(); i++) {
      if (errorS > smallestError[i]) {
        errorS = smallestError[i];
        P1 = P1Results[i];
        P2 = P2Results[i];
      } // end if shortestDist
    } // end for i
 
    P2.InvalidateDecomposition();
    RMatrix R = P2.GetR();
    Vector3<double> C = P2.GetC();
    K = P2.GetK();
    
    // check if baseline points into the right direction, check if "other" rotation yields more
    // points in front of the camera
    RMatrix RTmp = R;
    Vector3<double> CTmp = C;
    EMatrix E;
    E.InitFromF(F, K, K);
    E.GetRotationTranslation(RTmp, CTmp, p1, p2);
    
    P2.SetIdentity();
    P2.Compose(K, RTmp, CTmp);
    
  } else { // else not even one valid result, indicate error
    P1.SetIdentity();
    P2.SetIdentity();
    return -1;
  }

// write errors into a file  
//  std::fstream filestr;
//  filestr.open ("errors.txt", std::fstream::in | std::fstream::out | std::fstream::app);
//  double lowest = DBL_MAX;
//  int lowestIndex;
//  for(unsigned int i = 0; i < errorAll_.size(); i++){
//    filestr << focalLengths[i] << "   " << (errorAll_[i]).back();
//    lowest = DBL_MAX;
//    for(unsigned int j = 0; j < (errorAll_[i]).size(); j++){
//      if((errorAll_[i])[j] < lowest){
//        lowest = (errorAll_[i])[j];
//        lowestIndex = j;
//      } 
//    }
//    filestr << " " << lowest << std::endl;
//   }
//
//  filestr.close();
  
  return 0;
}

int PMatrixEstimation::AutoCalib_(const BIAS::FMatrix &F, const long int numCorrs, BIAS::KMatrix &K,
    BIAS::PMatrix &P1, BIAS::PMatrix &P2) {

  // save original focal length
  double focalLength_saved = K.GetFx();
  double princPointX_saved = K.GetHx();
  double princPointY_saved = K.GetHy();

//  int numParams = 7;
// to do: remove when done testing
  long int numParams = 6;

  EMatrix E;
  E.InitFromF(F, K, K);

//  std::cout << "AutoCalib_ K " << K << std::endl;
//  std::cout << "AutoCalib_ F " << 1/F[2][2] * F << std::endl;
    
  // use foerstner method for rotation estimation
  double baselineMagnitude = 1.0;
  ComputeFromFDirect(E, baselineMagnitude, P1, P2);

//  std::cout << "AutoCalib_ P1 and P2 " << P1 << " " << P2 << std::endl;
  
  // decompose P2 in order to gain parameters for Levenberg-Marquardt algorithm
  P2.InvalidateDecomposition();

  RMatrix R;
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R[i][j] = P2[i][j];
    }
  }
  R = R.Transpose();
  Vector3<double> t = P2.GetCol(3);

  
  // save initial R and t for later
  RMatrix R_saved = R;
  Vector3<double> t_saved = t;
 
//  std::cout << "epipole " << t << std::endl;
  
  t = t.CoordEuclideanToSphere();
  
  double phiX, phiY, phiZ;
  R.GetRotationAnglesXYZ(phiX, phiY, phiZ);
  
//  std::cout << "AutoCalib_ Rotation angles " << phiX*180/M_PI << " " << phiY*180/M_PI << " " << phiZ*180/M_PI << std::endl;
//
//  std::cout << "autoCalib_ rotation matrix " << R << std::endl;
  
 
  
  // assuming focal length is equal for both directions
  double focalLength = K.GetFx();

  // build vector containing parameters to be optimized
  Vector<double> FParamsStart(numParams), FParamsResult(numParams);

  // TODO remove when done testing
//  FParamsStart[0] = t[1];
//  FParamsStart[1] = t[2];
//  FParamsStart[2] = phiX;
//  FParamsStart[3] = phiY;
//  FParamsStart[4] = phiZ;

  FParamsStart[0] = focalLength;
  FParamsStart[1] = t[1];
  FParamsStart[2] = t[2];
  FParamsStart[3] = phiX;
  FParamsStart[4] = phiY;
  FParamsStart[5] = phiZ;
 
  
  long int res;
  errorSum_.clear();
  if ((res = LevenbergMarquardt(&PErrorFunction, this,
                                numCorrs, numParams, FParamsStart, FParamsResult,
                                MINPACK_DEF_TOLERANCE))!=0) {
    BIASERR("Error in Levenberg-Marquardt-Optimization: "<<res);
  }

  // rebuild, K, e, and R
  K.SetIdentity();
  K.SetHx(princPointX_saved);
  K.SetHy(princPointY_saved);
  // set only in case res == 5
  K.SetFx(focalLength_saved);
  K.SetFy(focalLength_saved);
  
  if(res == 5) return -1;
  
  errorAll_.push_back(errorSum_);
    
  K.SetFx(FParamsResult[0]);
  K.SetFy(FParamsResult[0]);
//  std::cout << "K still whole ? " << K << std::endl;

  // Check if estimated focal length is valid

  if(K.GetFx() < 100) return -2;
  if(K.GetFx() > 10000) return -2;
//  double testFocalLength = FParamsResult[0]/focalLength_saved;
//  if ((testFocalLength >= 10) || res == 5) {
//    return -1;
//  }
//  if (FParamsResult[0]<=0.0) {
//    return -2;
//  }

  // reconstruct t
//  t[0] = 1;
//  t[1] = FParamsResult[0];
//  t[2] = FParamsResult[1];
  // TODO remove when done testing
  t[0] = 1;
  t[1] = FParamsResult[1];
  t[2] = FParamsResult[2];

  t = t.CoordSphereToEuclidean();
  
  t = t.Normalize();

  // reconstruct R
  R.SetIdentity();
//  R.SetXYZ(FParamsResult[2], FParamsResult[3], FParamsResult[4]);
  R.SetXYZ(FParamsResult[3], FParamsResult[4], FParamsResult[5]);

  //reconstruct P1 and P2 with new Kmatrix
  P1.SetIdentity();
  P1 = K * P1;
  P1.InvalidateDecomposition();
  // rebuild P2
  P2.SetIdentity();
  P2.Compose(K, R, (-1.0)*R*t);

  //  std::cout << "P2 after successful autocalib_ " << P2 << std::endl;
  return 0;
}



void PMatrixEstimation::InitP1P2Trans(BIAS::PMatrix &P1, BIAS::PMatrix &P2, HomgPoint2D &Epipole2) {
  //  BCDOUT(BIAS_FMATRIXDEBUG,"In InitP1P2Trans E2 = " << Epipole2);
  P1.set_identity();
  P2.set_identity();

  P2[0][3] = Epipole2[0];
  P2[1][3] = Epipole2[1];
  P2[2][3] = Epipole2[2];
}

/*---------------------------------------------------------------------------
 given approximate P2, compute the nearest value of P2 which is exactly
 compatible with P1 and F, using the method in the IJCV paper.  variable names
 are from the paper too.
 ----------------------------------------------------------------------------*/
void PMatrixEstimation::FindClosestP2(BIAS::FMatrix &F,
//BIAS::PMatrix &P1,
    BIAS::PMatrix &P2, Matrix3x3<double> &M2Matrix, Vector<double> &C, HomgPoint2D& Epipole2) {

  BCDOUT(BIAS_FMATRIXDEBUG, "F = " << F);
  Matrix3x3<double> SkewMatrix, SubMatrix;

  double DotProduct=0.0, Magnitude;

  Matrix<double> A(9, 4);
  Vector<double> B(9);

  F.DecomposetoSR(SkewMatrix, M2Matrix);

  BCDOUT(BIAS_PMATRIXDEBUG, "M2Matrix = " << M2Matrix);

  A[0][0] = M2Matrix [0][0];
  A[0][1] = Epipole2[0];
  A[0][2] = 0;
  A[0][3] = 0;
  B[0] = P2[0][0];

  A[1][0] = M2Matrix[0][1];
  A[1][1] = 0;
  A[1][2] = Epipole2[0];
  A[1][3] = 0;
  B[1] = P2[0][1];

  A[2][0] = M2Matrix[0][2];
  A[2][1] = 0;
  A[2][2] = 0;
  A[2][3] = Epipole2[0];
  B[2] = P2[0][2];

  A[3][0] = M2Matrix[1][0];
  A[3][1] = Epipole2[1];
  A[3][2] = 0;
  A[3][3] = 0;
  B[3] = P2[1][0];

  A[4][0] = M2Matrix[1][1];
  A[4][1] = 0;
  A[4][2] = Epipole2[1];
  A[4][3] = 0;
  B[4] = P2[1][1];

  A[5][0] = M2Matrix[1][2];
  A[5][1] = 0;
  A[5][2] = 0;
  A[5][3] = Epipole2[1];
  B[5] = P2[1][2];

  A[6][0] = M2Matrix[2][0];
  A[6][1] = Epipole2[2];
  A[6][2] = 0;
  A[6][3] = 0;
  B[6] = P2[2][0];

  A[7][0] = M2Matrix[2][1];
  A[7][1] = 0;
  A[7][2] = Epipole2[2];
  A[7][3] = 0;
  B[7] = P2[2][1];

  A[8][0] = M2Matrix[2][2];
  A[8][1] = 0;
  A[8][2] = 0;
  A[8][3] = Epipole2[2];
  B[8] = P2[2][2];

  BCDOUT(BIAS_PMATRIXDEBUG, "A = " << A);
  BCDOUT(BIAS_PMATRIXDEBUG, "B = " << B);

  //ma2_solve_equation_svd (a_matrix, x_vector, b_vector);
  Matrix<double> AT = A.Transpose();
  BCDOUT(BIAS_PMATRIXDEBUG, "AT = " << AT);
  Vector<double> ATB = AT*B;
  BCDOUT(BIAS_PMATRIXDEBUG, "ATB = " << ATB);
  Matrix<double> ATA = AT*A;
  BCDOUT(BIAS_PMATRIXDEBUG, "ATA = " << ATA);

  Vector<double> X=Lapack_LU_linear_solve(ATA, ATB);
  BCDOUT(BIAS_PMATRIXDEBUG, "Lapack_LU_linear_solve X = " << X);
  M2Matrix *= X[0];
  BCDOUT(BIAS_PMATRIXDEBUG, "M2Matrix = " << M2Matrix);

  // ho_trivec_outer_product (&epipole2_trivec, b_trivec_ptr, outer_matrix);
  Matrix<double> E2(3, 1), vMat(1, 3);
  for (int k=0; k< 3; k++) {
    E2[k][0] = Epipole2[k];
    vMat[0][k] = X[k+1];
    C[k] = X[k+1];
  }
  C[3]=1;

  Matrix<double> Outer=E2*vMat;
  SubMatrix = Outer + (Matrix<double>)M2Matrix;

  Vector3<double> Col(P2[0][3], P2[1][3], P2[2][3]);

  Magnitude = Col.NormL2();
  DotProduct = Col * Epipole2;

  if (DotProduct > 0)
    Epipole2 *= Magnitude;
  else
    Epipole2 *= -Magnitude;

  P2[0][0] = SubMatrix[0][0];
  P2[0][1] = SubMatrix[0][1];
  P2[0][2] = SubMatrix[0][2];
  P2[0][3] = Epipole2[0];
  P2[1][0] = SubMatrix[1][0];
  P2[1][1] = SubMatrix[1][1];
  P2[1][2] = SubMatrix[1][2];
  P2[1][3] = Epipole2[1];
  P2[2][0] = SubMatrix[2][0];
  P2[2][1] = SubMatrix[2][1];
  P2[2][2] = SubMatrix[2][2];
  P2[2][3] = Epipole2[2];

}