CamPoseCalib.cpp

#include "CamPoseCalib.hh"

// BIAS
#include <MathAlgo/Lapack.hh>
#include <Geometry/SimilarityTransform.hh>
#include <Base/Common/CompareFloatingPoint.hh>

using namespace BIAS;
using namespace std;

CamPoseCalib::CamPoseCalib() : calcSecD_(false)
{
  transChangeEpsilon_    = 1E-9;
  rotChangeEpsilon_    = 1E-9;
  minDepthModel_ = 1E-9;
  paramToEst_=6;
  estimateF_=0;
  estimatePrincipal_=false;
  estimateTransX_=true;
  estimateTransY_=true;
  estimateTransZ_=true;
  doLM_=true;
  lastError_=DBL_MAX;
  errorIncreaseForLM_=0.01; // in percent
  robustFunc_=-1;
  scaleNumStabiltity_=1.0;
  initialGuess_=false;
  sx_=-1;
  sy_=-1;
  initMu_=10000;
  Reset();
}


void CamPoseCalib::Reset()
{
  mu_=initMu_;
  initialized_=false;
  initialGuess_=false;
  resultR_.SetIdentity();
  resultTrans_.Set(0,0,0);
  mps_.clear();
  initialMps_.clear();
  origMps_.clear();
  tps_.clear();
  lastError_=DBL_MAX;

  mpsLine_.clear();
  origMpsLine_.clear();
  initialMpsLine_.clear();
  tpsLinePoint_.clear();
  tpsLineNormal_.clear();
}


void CamPoseCalib::outputPose_(int iteration)
{
  ofstream up("poseUpVector.log", ios_base::app);
  ofstream axis("poseOptAxis.log", ios_base::app);
  ofstream transX("poseTransX.log", ios_base::app);
  ofstream transY("poseTransY.log", ios_base::app);
  ofstream transZ("poseTransZ.log", ios_base::app);
  ofstream transAbs("poseTransAbs.log", ios_base::app);

  BIAS::Vector3<double> upVec(0,-1,0), axisVec(0,0,1);
  double angle;
  angle=acos( (camR_ * GetRotation()*upVec)*truthUpVec_);
  up<<iteration<<" "<<angle/M_PI*180.0<<endl;

  angle=acos( (camR_ * GetRotation()*axisVec) * truthOptAxis_);
  axis<<iteration<<" "<<angle/M_PI*180.0<<endl;

  transX<<iteration<<" "<<truthCenter_[0]-GetTranslation()[0]<<endl;
  transY<<iteration<<" "<<truthCenter_[1]-GetTranslation()[1]<<endl;
  transZ<<iteration<<" "<<truthCenter_[2]-GetTranslation()[2]<<endl;
  transAbs<<iteration<<" "<<(truthCenter_-GetTranslation()).Length()<<endl;

  ofstream svdOut("poseSingularValues.log",ios_base::app);
  svdOut<<iteration<<" ";
  BIAS::Matrix<double> J2;
  solveMatrix_.GetSystemMatrix(J2);
  BIAS::SVD svdJ2(J2);
  for (unsigned int i=0;i<J2.GetRows(); i++) {
    svdOut<<svdJ2.GetSingularValue(i)<<" ";
  }
  svdOut<<endl;
}


int CamPoseCalib::Estimate(unsigned int iterations)
{
  if (initialGuess_) {
    int res =GuessInitialCam_();
    if (res==-2) return -5;
  }
  if  (!initialized_) {
    // check if there are points at all
    if (origMps_.empty() && origMpsLine_.empty()) {
      BIASERR("No points !");
      return -10;
    }
    BIAS::Vector3<double> point,avgPos(0,0,0);
    for (unsigned int i=0; i<origMps_.size(); i++) {
      point=(camRT_ * origMps_[i]) - (camRT_ * camC_);
      avgPos+=point;
      initialMps_.push_back( point );
    }
    for (unsigned int i=0; i<origMpsLine_.size(); i++) {
      point=(camRT_ * origMpsLine_[i]) - (camRT_ * camC_);
      avgPos+=point;
      initialMpsLine_.push_back( point );
    }
    // now scale all model points for numerical stability
    avgPos/=( (double)initialMps_.size() + (double)initialMpsLine_.size() );
    scaleNumStabiltity_=   1.0/avgPos.Length();
    for (unsigned int j=0; j<initialMps_.size(); j++) {
      initialMps_[j] *= scaleNumStabiltity_;
    }
    for (unsigned int j=0; j<initialMpsLine_.size(); j++) {
      initialMpsLine_[j] *= scaleNumStabiltity_;
    }
    mps_=initialMps_;
    mpsLine_=initialMpsLine_;
    initialized_=true;
  }

  paramToEst_=3+estimateTransX_+estimateTransY_+estimateTransZ_+
    estimateF_+ 2*estimatePrincipal_;
  unsigned int rows=mps_.size()*2 + mpsLine_.size();
  solveMatrix_.newsize(rows,paramToEst_);
  solveVector_.newsize(rows);

  BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,"solveMatrix.size:"<<
           solveMatrix_.num_rows()<<"x"<<solveMatrix_.num_cols());

#ifdef BIAS_DEBUG
  if ( (mps_.size() != tps_.size() ) ) {
    BIASERR("different number of model points  and target points");
    BIASABORT;
  }
#endif

  unsigned int i=0;
  int resCode =-1;
  double transChange=1, rotChange=1,w;
  Vector3<double> lastTrans(0,0,0);

  lastRot_.SetIdentity();
  BIAS::Vector<double> result(solveMatrix_.num_rows());
  Vector3<double> axis;
  double angle;
  while ( (i<iterations) && ((transChange>transChangeEpsilon_) ||
                             (rotChange>rotChangeEpsilon_)) ) {
    solveMatrix_.SetZero();
    solveVector_.SetZero();
    CreateEquationSys_();
    AddConstraints2DLine_();
    if (calcSecD_) {
      CreateSecDMatrix_();
      BIASDOUT(D_BIAS_CAMPOSE_SECD,"Second Derivative Matrix s:"<<secDMatrix_);

    }
    if (int(2*variances_.size()) == solveVector_.size()) {
      ApplyCoVariances_();
    }
    BIASDOUT(D_BIAS_CAMPOSE_EQSYS,"solveMatrix:"<<solveMatrix_<<endl<<
             "solveVector:"<<solveVector_);
    BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,"iteration:"<<i<<"/"<<iterations);

    //outputPose_(i);

    if (robustFunc_>=0)
      SetWeightsByError_();
    if (weights_.size()>0) {
      int row=0;
      if ((int)(2*weights_.size()+weightsLine_.size())==solveVector_.size()){
        for (unsigned int i=0; i<weights_.size()*2; i++) {
          w=sqrt(weights_[i/2]);//sqrt is neccessary to have w as the influence
          solveVector_[i]*=w;
          for (int col=0;col<solveMatrix_.num_cols();col++)
            solveMatrix_[i][col]*=w;
          row++;
        }
        for (int i=row; i<solveVector_.size(); i++) {
          w=sqrt(weightsLine_[i]);
          solveVector_[i]*=w;
          for (int col=0;col<solveMatrix_.num_cols();col++)
            solveMatrix_[i][col]*=w;
          row++;
        }
      } else
        BIASERR("number of weights is not equal to nr of correspondences");
    }
    //        result=Lapack_WLLS_solve(solveMatrix_,solveVector_,
    //                                 weightMatrix_, resCode);

    resCode = -1;
    if (doLM_)
      resCode=SolveLM_(solveMatrix_,solveVector_, result,iterations);
    else {
      resCode=svd_.Solve(solveMatrix_,solveVector_, result);
      ApplySolution_(result);
    }

    if (resCode!=0) {
      //       BIASERR("error solving the equation system. probably singular "
      //               "matrix J^T J");
      return -2;
    }

    BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_EQSYS,"result:"<<result);


    // calc the change of the transform
    transChange = resultTrans_.Dist(lastTrans);
    lastRot_ = lastRot_.Transpose()*resultR_;
    lastRot_.GetRotationAxisAngle(axis, angle);
    rotChange = angle;
    lastRot_   = resultR_;
    lastTrans = resultTrans_;
    BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_ITER,"iteration:"<<i<<"/"<<iterations
             <<" transChange:"<<transChange<<" rotChange:"<<rotChange
             <<" err:"<<GetAvgError());
    i++;
  } //while()

  if (i>=iterations) {
#ifdef BIAS_DEBUG
    BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_ITER,"too many iterations in  CamPoseCalib::Estimate: "<<i
             <<" last changes were t="<<transChange<<" r="<<rotChange);
#endif
    return 1;
  }

  return 0;
}


int CamPoseCalib::CreateEquationSys_()
{
  double mp2mp2;
  unsigned int col,row2;

  const bool estX=estimateTransX_,estY=estimateTransY_,estZ=estimateTransZ_;

  // for every 3D-point-2D-point corr create 2 rows
  for (unsigned int row=0;row<mps_.size();row++) {
    const BIAS::Vector3<double> &mp = mps_[row];
    mp2mp2=mp[2]*mp[2];
    if (mp2mp2>minDepthModel_) {
      const BIAS::Vector2<double> &target=tps_[row];
      row2=row*2;
      solveVector_[row2]  = target[0] - (mp[0]/mp[2] * sx_ + cx_);
      solveVector_[row2+1]= target[1] - (mp[1]/mp[2] * sy_ + cy_);

      BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,"mp:"<<mp<<" tp:"<<target);
      double *matrixRow = solveMatrix_[row2];
      col=0;
      if (estX==1) {
        matrixRow[col]= sx_ / mp[2];
        col++;
      }
      if (estY==1) {
        matrixRow[col]= 0;
        col++;
      }
      if (estZ==1) {
        matrixRow[col]= sx_* -mp[0]/(mp2mp2);
        col++;
      }
      matrixRow[col]= sx_* -mp[0] * mp[1] / (mp2mp2);
      col++;
      matrixRow[col]= sx_* (mp2mp2 + mp[0]*mp[0])/(mp2mp2);
      col++;
      matrixRow[col]= sx_* -mp[1]/mp[2];

      double *matrixRowP1 = solveMatrix_[row2+1];
      col=0;
      if (estX==1) {
        matrixRowP1[col]= 0;
        col++;
      }
      if (estY==1) {
        matrixRowP1[col]= sy_ / mp[2];
        col++;
      }
      if (estZ==1) {
        matrixRowP1[col]= sy_* -mp[1]/(mp2mp2);
        col++;
      }
      matrixRowP1[col]= sy_* (-mp2mp2 - mp[1]*mp[1])/(mp2mp2);
      col++;
      matrixRowP1[col]= sy_* ( mp[1]*mp[0])/(mp2mp2);
      col++;
      matrixRowP1[col]= sy_* mp[0]/mp[2];

      if (estimateF_>0) {
        if (estimateF_==1) { // fx=fy
          col++;
          matrixRow[col]  = mp[0]/mp[2];
          matrixRowP1[col]= mp[1]/mp[2];
        } else { // both sx_ and sy_ are to estimate
          col++;
          matrixRow[col]    = mp[0]/mp[2];
          col++;
          matrixRowP1[col]  = mp[1]/mp[2];
        }
      }
      if (estimatePrincipal_) {
        col++;
        matrixRow[col] = 1;
        col++;
        matrixRowP1[col] = 1;
      }
    } else {
      BIASDOUT(D_BIAS_CAMPOSE_EQSYS,"mp:"<<mp<<" tp:"<<tps_[row]<<
               " point distance to cam center too small:"<<mp[2]);
    }
  }
  return 0;
}


int CamPoseCalib::AddConstraints2DLine_()
{
  double mp2mp2;
  unsigned int col,row2;

  const bool estX=estimateTransX_,estY=estimateTransY_,estZ=estimateTransZ_;
  BIAS::Vector2<double> diff;
  BIAS::Vector<double>
    rowEntryX(solveMatrix_.GetCols()),
    rowEntryY(solveMatrix_.GetCols());

  // for every 3D-point-2D-point corr add  1 row
  row2=mps_.size()*2;
  for (unsigned int row=0;row<mpsLine_.size();row++) {
    const BIAS::Vector3<double> &mp = mpsLine_[row];
    mp2mp2=mp[2]*mp[2];
    if (mp2mp2>minDepthModel_) {
      const BIAS::Vector2<double> &target=tpsLinePoint_[row];
      const BIAS::Vector2<double> &targetNormal=tpsLineNormal_[row];
      diff[0]= target[0] - (mp[0]/mp[2] * sx_ + cx_);
      diff[1]= target[1] - (mp[1]/mp[2] * sy_ + cy_);
      diff.ScalarProduct(targetNormal, solveVector_[row2]);

      BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,"mp:"<<mp<<" tp:"<<target);
      double *matrixRow =   rowEntryX.GetData();
      col=0;
      if (estX==1) {
        matrixRow[col]= sx_ / mp[2];
        col++;
      }
      if (estY==1) {
        matrixRow[col]= 0;
        col++;
      }
      if (estZ==1) {
        matrixRow[col]= sx_* -mp[0]/(mp2mp2);
        col++;
      }
      matrixRow[col]= sx_* -mp[0] * mp[1] / (mp2mp2);
      col++;
      matrixRow[col]= sx_* (mp2mp2 + mp[0]*mp[0])/(mp2mp2);
      col++;
      matrixRow[col]= sx_* -mp[1]/mp[2];

      double *matrixRowP1 =  rowEntryY.GetData();
      col=0;
      if (estX==1) {
        matrixRowP1[col]= 0;
        col++;
      }
      if (estY==1) {
        matrixRowP1[col]= sy_ / mp[2];
        col++;
      }
      if (estZ==1) {
        matrixRowP1[col]= sy_* -mp[1]/(mp2mp2);
        col++;
      }
      matrixRowP1[col]= sy_* (-mp2mp2 - mp[1]*mp[1])/(mp2mp2);
      col++;
      matrixRowP1[col]= sy_* ( mp[1]*mp[0])/(mp2mp2);
      col++;
      matrixRowP1[col]= sy_* mp[0]/mp[2];

      if (estimateF_>0) {
        if (estimateF_==1) { // fx=fy
          col++;
          matrixRow[col]  = mp[0]/mp[2];
          matrixRowP1[col]= mp[1]/mp[2];
        } else { // both sx_ and sy_ are to estimate
          col++;
          matrixRow[col]    = mp[0]/mp[2];
          col++;
          matrixRowP1[col]  = mp[1]/mp[2];
        }
      }
      if (estimatePrincipal_) {
        col++;
        matrixRow[col] = 1;
        col++;
        matrixRowP1[col] = 1;
      }

      //now multiply the 2 rows with normal
      for (unsigned int j=0; j<solveMatrix_.GetCols(); j++) {
        solveMatrix_[row2][j]=
          rowEntryX[j]*targetNormal[0] +
          rowEntryY[j]*targetNormal[1];
      }
      row2++;
    } else {
      BIASDOUT(D_BIAS_CAMPOSE_EQSYS,"mp:"<<mp<<" tp:"<<tps_[row]<<
               " point distance to cam center too small:"<<mp[2]);
    }
  }
  return 0;
}

int CamPoseCalib::Solve_(BIAS::Matrix<double> &J, BIAS::Vector<double> &eps,
                         BIAS::Vector<double> &result)
{
  return svd_.Solve(J,eps,result);

//   BIAS::Matrix<double> J2(J.num_cols(),J.num_cols());
//   BIAS::Vector<double> Jeps(J.Transpose()*eps);
//   J2=J.Transpose()*J; // this could be done faster

//   result=Lapack_LU_linear_solve(J2,Jeps);
//   if (result.size()<J.num_cols())
//     return -1;
//   else
//     return 0;
}



int CamPoseCalib::ApplySolution_(BIAS::Vector<double> &result)
{
  // transform the model points by the result transform
  // add the result to the previous transforms
  BIASDOUT(D_BIAS_CAMPOSE_APPLY_SOL,"resulting paramter change:"<<result);
  int row= estimateTransX_+estimateTransY_+estimateTransZ_;

  double alpha=result[row];
  double beta =result[row+1];
  double gamma=result[row+2];;
  row+=2;
  if (estimateF_>0) {
    if (estimateF_==1) { // fx=fy
      row++;
      sx_+=result[row];
      sy_+=result[row];
    } else { // both sx_ and sy_ are to estimate
      row++;
      sx_+=result[row];
      row++;
      sy_+=result[row];
    }
  }
  if (estimatePrincipal_) {
    row++;
    cx_+=result[row];
    row++;
    cy_+=result[row];
  }

  BIAS::RMatrix R;

  R.SetXYZ(alpha,beta,gamma);

  resultR_= R*resultR_;
  resultTrans_= R*resultTrans_;
  row=0;
  if (estimateTransX_) {
    resultTrans_[0]+= result[row];
    row++;
  }
  if (estimateTransY_) {
    resultTrans_[1]+= result[row];
    row++;
  }
  if (estimateTransZ_) {
    resultTrans_[2]+= result[row];
    row++;
  }

  BIASDOUT(D_BIAS_CAMPOSE_APPLY_SOL,"result trans:"<<resultTrans_<<endl
           <<" result R:"<<R);

  for (unsigned int mpIdx=0;mpIdx<mps_.size();mpIdx++) {
    mps_[mpIdx]=resultR_* initialMps_[mpIdx] + resultTrans_;
  }
  for (unsigned int mpIdx=0;mpIdx<mpsLine_.size();mpIdx++) {
    mpsLine_[mpIdx]=resultR_* initialMpsLine_[mpIdx] + resultTrans_;
  }
  return 0;
}


int CamPoseCalib::SetInitialCamera(double fx, double fy, double cx, double cy,
                                   const RMatrix &R, const Vector3<double> &C)
{
  sx_=fx; sy_=fy;
  cx_=cx; cy_=cy;
  camR_=R;
  camRT_=R.Transpose();
  camC_=C;

  return 0;
}


int CamPoseCalib::SetInitialCamera(const BIAS::PMatrix &P)
{
  camP_=P;
  KMatrix K;
  camP_.GetK(K);
  sx_=K[0][0]; sy_=K[1][1];
  cx_=K[0][2]; cy_=K[1][2];
  camR_=camP_.GetR();
  camRT_=camR_.Transpose();
  camC_=camP_.GetC();
  return 0;
}

int CamPoseCalib::SetInitialCamera(const BIAS::KMatrix &K)
{
  initialGuess_=true;
  sx_=K[0][0]; sy_=K[1][1];
  cx_=K[0][2]; cy_=K[1][2];
  return 0;
}

int CamPoseCalib::AddCorr(const Vector3<double> &x, const Vector2<double> &y)
{
  origMps_.push_back(x);
  tps_.push_back(y);
  return 0;
}

void CamPoseCalib::GetP(PMatrix &P)
{
  KMatrix K;
  K.SetIdentity();
  K[0][0]= sx_;               K[0][2]= cx_;
  K[1][1]= sy_;               K[1][2]= cy_;

  P.Compose(K,
            camR_ * GetRotation(),
            camC_ + GetTranslation());
}

Vector<double> CamPoseCalib::GetResult() const
{
  Vector<double> res(6);
  Vector3<double> C;
  Vector3<double> ang;
  RMatrix R;
  C = camC_ + GetTranslation();
  R = camR_ * GetRotation();
  R.GetRotationAnglesXYZ(ang);
  for (int i=0; i<3; i++){
    res[i] = C[i];
    res[i+3] = ang[i];
  }
  return res;
}


int CamPoseCalib::SolveLM_(BIAS::Matrix<double> &J, BIAS::Vector<double> &eps,
                         BIAS::Vector<double> &result, unsigned int iterations)
{
  //  return svd_.Solve(J,eps,result);
  unsigned int i=0,iteration=0;
  BIAS::Matrix<double> J2temp;
  BIAS::Vector<double> Jeps(J.Transpose()*eps);
  bool errorIncreased=true;
  // vars for orig values
  double fxOrig,fyOrig,cxOrig,cyOrig;
  BIAS::Vector3<double> resultTransOrig;
  BIAS::RMatrix resultROrig;
  std::vector<BIAS::Vector3<double> > mpsOrig,mpsOrigLine;

  lastError_=GetAvgSquaredWeightedError_();
  //  J2=J.Transpose()*J; // this could be done faster
  // the faster variant here is GetSystemMatrix, which does the same
  J.GetSystemMatrix(coVarMat_);
  if (calcSecD_) { // calc the necessary dampening value mu_
    BIASDOUT(D_BIAS_CAMPOSE_SECD,"Hessian of Gauss-Newton:"<<coVarMat_);
    coVarMat_+=secDMatrix_;
    BIASDOUT(D_BIAS_CAMPOSE_SECD,"Hessian of Newton:"<<coVarMat_);
    BIAS::SVD eigValComp(coVarMat_);
    BIAS::Vector<double> eigValues(eigValComp.GetEigenValues());
    BIASDOUT(D_BIAS_CAMPOSE_SECD,"EigValues of Hessian:"<<eigValues);
    if (eigValues[0]<0)
      mu_=-5*(eigValues[0]);
    else mu_=0.001;
  }
  double angle;
  BIAS::Vector3<double> axis;
  while ((errorIncreased) && (iteration<iterations) && (mu_<1E50)) {
    //mu_=10000;
    BIASDOUT( D_BIAS_CAMPOSE_LM, "solving for mu:"<<mu_);
    J2temp=coVarMat_;
    for (i=0;i<(unsigned int)J2temp.num_rows();i++)
      J2temp[i][i]+=mu_;

    result=Lapack_LU_linear_solve(J2temp,Jeps);
    if (result.size()<J.num_cols())
      return -1;
//     svd_.Solve(J2temp,Jeps,result);


    //// apply the solution temporary
    //save original values
    fxOrig=sx_;
    fyOrig=sy_;
    cxOrig=cx_;
    cyOrig=cy_;

    resultROrig=resultR_;
    resultTransOrig=resultTrans_;

    mpsOrig=mps_;
    mpsOrigLine=mpsLine_;
    BIASDOUT( D_BIAS_CAMPOSE_LM, "result:"<<result);
    ApplySolution_(result);
    BIASDOUT( D_BIAS_CAMPOSE_LM, "last SquaredError:"<<lastError_<<
              "  new squared error:"<<GetAvgSquaredWeightedError_());


    // check if rotation is more than 90 degrees
    RMatrix rotChange = lastRot_.Transpose()*resultR_;
    BIASDOUT( D_BIAS_CAMPOSE_LM, "rotChange Matrix:"<<rotChange);
    rotChange.GetRotationAxisAngle(axis, angle);
    BIASDOUT( D_BIAS_CAMPOSE_LM, "rotChange="<<angle);
    if ( (angle<1.56) &&
         //// check if the solution decreases the error
         ( ((GetAvgSquaredWeightedError_()-lastError_)/lastError_)
           < errorIncreaseForLM_)
         ) {
      errorIncreased=false;
      mu_ /= 10;
      if (mu_<0.001) mu_=initMu_;
    } else {
      mu_ *= 10;
    }

    if (errorIncreased) {
      // reset the original values
      sx_=fxOrig;
      sy_=fyOrig;
      cx_=cxOrig;
      cy_=cyOrig;

      resultR_=resultROrig;
      resultTrans_=resultTransOrig;

      mps_=mpsOrig;
      mpsLine_=mpsOrigLine;
    }
    iteration++;
  }//end while

  return 0;
}


int CamPoseCalib::SetWeightsByError_()
{
  double err=0,dx,dy;
  unsigned int i=0;
  int weightAmount=int(2*weights_.size()+weightsLine_.size());
  if (weightAmount!= solveVector_.size()) {
    //    weights_.resize(mps_.size());
    weights_.assign(mps_.size(), 1.0);
    weightsLine_.assign(mpsLine_.size(), 1.0);
  }
  for (i=0;i<mps_.size();i++) {
    const BIAS::Vector3<double> &mp=mps_[i];
    if (fabs(mp[2])>minDepthModel_) {
      dx= tps_[i][0] - (mp[0]/mp[2] * sx_ + cx_);
      dy= tps_[i][1] - (mp[1]/mp[2] * sy_ + cy_);
      err= sqrt(dx*dx + dy*dy);

      switch (robustFunc_) {
      case 0: // Fair
        weights_[i]*= 1.0 / (1.0 + (err/outlierError_));
        break;
      case 1: // Huber
        if (err>outlierError_)
          weights_[i]*= outlierError_/err;
        else
          weights_[i]*= 1.0;
        break;
      case 2: // Tukey
        if (err<=outlierError_) {
          weights_[i]*=(err/outlierError_)*(err/outlierError_);
          weights_[i]*=  (1.0 - weights_[i]) * (1.0 - weights_[i]);
        } else
          weights_[i]=0;
        break;
      }
    } else
      weights_[i]=0;
  }

  for (i=0;i<mpsLine_.size();i++) {
    const BIAS::Vector3<double> &mp=mpsLine_[i];
    if (fabs(mp[2])>minDepthModel_) {
      const BIAS::Vector2<double> &tp = tpsLinePoint_[i];
      const BIAS::Vector2<double> &normal = tpsLineNormal_[i];
      dx = tp[0] - (mp[0]/mp[2] * sx_ + cx_);
      dy = tp[1] - (mp[1]/mp[2] * sy_ + cy_);
      err= fabs(dx*normal[0]+dy*normal[1]);
      switch (robustFunc_) {
      case 0: // Fair
        weightsLine_[i]*= 1.0 / (1.0 + (err/outlierError_));
        break;
      case 1: // Huber
        if (err>outlierError_)
          weightsLine_[i]*= outlierError_/err;
        else
          weightsLine_[i]*= 1.0;
        break;
      case 2: // Tukey
        if (err<=outlierError_) {
          weightsLine_[i]*=(err/outlierError_)*(err/outlierError_);
          weightsLine_[i]*=  (1.0 - weightsLine_[i]) * (1.0 - weightsLine_[i]);
        } else
          weightsLine_[i]=0;
        break;
      }
    } else
      weightsLine_[i]=0;
  }
  return 0;
}


double CamPoseCalib::GetAvgError() {
  double err=0,dx,dy;
  unsigned int i=0,count=0;
  const bool bVars=(variances_.size()==mps_.size());
  Vector2<double> diff;
  for (i=0;i<mps_.size();i++) {
    const BIAS::Vector3<double> &mp = mps_[i];
    if (fabs(mp[2])>minDepthModel_) {
      const BIAS::Vector2<double> &tp = tps_[i];
      dx = tp[0] - (mp[0]/mp[2] * sx_ + cx_);
      dy = tp[1] - (mp[1]/mp[2] * sy_ + cy_);
      if (bVars) { // use mahalanobis distance
        const Matrix2x2<double> &var=variances_[i];
        diff[0]=dx; diff[1]=dy;
        err+= diff*(var*diff);
      } else {
        err+= sqrt(dx*dx + dy*dy);
      }
      count++;
    }
  }

  for (i=0;i<mpsLine_.size();i++) {
    const BIAS::Vector3<double> &mp = mpsLine_[i];
    if (fabs(mp[2])>minDepthModel_) {
      const BIAS::Vector2<double> &tp = tpsLinePoint_[i];
      const BIAS::Vector2<double> &normal = tpsLineNormal_[i];
      dx = tp[0] - (mp[0]/mp[2] * sx_ + cx_);
      dy = tp[1] - (mp[1]/mp[2] * sy_ + cy_);
      err+= fabs(dx*normal[0]+dy*normal[1]);
      count++;
    }
  }

  err/= double(count);
  return err;
}

double CamPoseCalib::GetAvgSquaredWeightedError_() {
  double err=0,dx,dy;
  unsigned int i=0,count=0;
  const unsigned int size=mps_.size();
  if (weights_.size()==mps_.size()) {
    for (i=0; i<size; i++) {
      const BIAS::Vector3<double> &mp = mps_[i];
      if (fabs(mp[2])>minDepthModel_) {
        const BIAS::Vector2<double> &tp = tps_[i];
        dx = tp[0] - (mp[0]/mp[2] * sx_ + cx_);
        dy = tp[1] - (mp[1]/mp[2] * sy_ + cy_);

        err+= weights_[i]*(dx*dx + dy*dy);
        count++;
      }
    }
  } else {
    for (i=0; i<size; i++) {
      const BIAS::Vector3<double> &mp = mps_[i];
      if (fabs(mp[2])>minDepthModel_) {
        const BIAS::Vector2<double> &tp = tps_[i];
        dx = tp[0] - (mp[0]/mp[2] * sx_ + cx_);
        dy = tp[1] - (mp[1]/mp[2] * sy_ + cy_);

        err+= dx*dx + dy*dy;
        count++;
      }
    }
  }

  double errNormal;
  for (i=0;i<mpsLine_.size();i++) {
    const BIAS::Vector3<double> &mp = mpsLine_[i];
    if (fabs(mp[2])>minDepthModel_) {
      const BIAS::Vector2<double> &tp = tpsLinePoint_[i];
      const BIAS::Vector2<double> &normal = tpsLineNormal_[i];
      dx = tp[0] - (mp[0]/mp[2] * sx_ + cx_);
      dy = tp[1] - (mp[1]/mp[2] * sy_ + cy_);
      errNormal= fabs(dx*normal[0]+dy*normal[1]);
      err+= errNormal*errNormal;
      count++;
    }
  }

  err/= double(count);
  return err;
}


int CamPoseCalib::CreateSecDMatrix_()
{
  secDMatrix_.newsize(solveMatrix_.GetCols(),solveMatrix_.GetCols());
  secDMatrix_.SetZero();
  double mp2mp2;
  // each entry is summed up over all corrs
  for (unsigned int mpI=0; mpI < mps_.size(); mpI++) {
    const BIAS::Vector3<double> &mp = mps_[mpI];
    const double errX= -solveVector_[mpI*2];
    const double errY= -solveVector_[mpI*2+1];
    mp2mp2=mp[2]*mp[2];
    if (mp2mp2>minDepthModel_) {
      ////////////// 0. row is trans_x
      ////// m'_x:
      //// partials for translation_x
      // t_x=0
      // t_y=0
      // t_z
      secDMatrix_[0][2]+= errX *
        (-sx_/mp2mp2) ;
      // rot_x
      secDMatrix_[0][3]+= errX *
        (-sx_/mp2mp2 * mp[1]);
      // rot_y
      secDMatrix_[0][4]+= errX *
        (sx_/mp2mp2 * mp[0]);
      // rot_z =0
      ////// m'_y:
      //// partials for translation_x are all zero

      ///////////// 1. row is trans_y
      ////// m'_x:
      //// partials for translation_y are all zero
      ////// m'_y:
      //// partials for translation_y
      // t_x=0
      // t_y=0
      // t_z
      secDMatrix_[1][2]+= errY *
        (-sy_/mp2mp2);
      // rot_x
      secDMatrix_[1][3]+= errY *
        (-sy_/mp2mp2 * mp[1]);
      // rot_y
      secDMatrix_[1][4]+= errY *
        (sy_/mp2mp2 * mp[0]);
      // rot_z =0

      ///////////// 2. row is trans_z
      ////// m'_x:
      //// partials for translation_z
      // t_x
      secDMatrix_[2][0]+= errX *
        (-sx_ / mp2mp2);
      // t_y=0
      // t_z
      secDMatrix_[2][2]+= errX *
        (sx_ * mp[0]*2 / (mp2mp2*mp[2]));
      // rot_x
      secDMatrix_[2][3]+= errX *
        (sx_ * mp[0]* mp[1]*2 / (mp2mp2*mp[2]));
      // rot_y
      secDMatrix_[2][4]+= errX *
        (-sx_ / mp[2] - 2*sx_*mp[0]*mp[0] / (mp2mp2*mp[2]));
      // rot_z
      secDMatrix_[2][5]+= errX *
        (sx_ *mp[1] / mp2mp2);

      ////// m'_y:
      //// partials for translation_z
      // t_x =0
      // t_y
      secDMatrix_[2][1]+= errY *
        (-sy_ / mp2mp2);
      // t_z
      secDMatrix_[2][2]+= errY *
        (sy_ * mp[1]*2 / (mp2mp2*mp[2]));
      // rot_x
      secDMatrix_[2][3]+= errY *
        (sy_ / mp[2] + sy_*2*mp[1]*mp[1] / (mp2mp2*mp[2]));
      //rot_y
      secDMatrix_[2][4]+= errY *
        (-sy_ * mp[0]* mp[1]*2 / (mp2mp2*mp[2]));
      // rot_z
      secDMatrix_[2][5]+= errY *
        (-sy_ * mp[0] / mp2mp2);

      ///////////// 3. row is rot_x
      ////// m'_x:
      //// partials for rot_x
      // t_x
      secDMatrix_[3][0]+= errX *
        (-sx_ / mp2mp2 * mp[1]);
      // t_y =0
      // t_z
      secDMatrix_[3][2]+= errX *
        (sx_ * mp[0]* mp[1]*2 / (mp2mp2*mp[2]));
      // rot_x
      secDMatrix_[3][3]+= errX *
        (sx_*mp[0] / mp[2] + 2*sx_*mp[0]*mp[1]*mp[1] / (mp2mp2*mp[2]));
      // rot_y
      secDMatrix_[3][4]+= errX *
        (-sx_ * mp[1]/mp[2]- sx_*2*mp[0]*mp[0] *mp[1] / (mp2mp2*mp[2]));
      // rot_z
      secDMatrix_[3][5]+= errX *
        (sx_/mp2mp2 * (-mp[0]*mp[0] + mp[1]*mp[1]));
      ////// m'_y:
      //// partials for rot_x
      // t_x = 0
      // t_y
      secDMatrix_[3][1]+= errY *
        (-sy_ * mp[1] / mp2mp2);
      // t_z
      secDMatrix_[3][2]+= errY *
        (sy_ /mp[2] + 2*sy_*mp[1]*mp[1] / (mp2mp2*mp[2]));
      // rot_x
      secDMatrix_[3][3]+= errY *
        (2*sy_*( mp[1] / mp[2] + mp[1]*mp[1]*mp[1] / (mp2mp2*mp[2]) ));
      // rot_y
      secDMatrix_[3][4]+= errY *
        (-2*sy_*mp[1]*mp[1]*mp[0]/ (mp2mp2*mp[2]));
      // rot_z
      secDMatrix_[3][5]+= errY *
        (-2*sy_ * mp[0] *mp[1] / mp2mp2);

      ///////////// 4. row is rot_y
      ////// m'_x:
      //// partials for rot_y
      // t_x
      secDMatrix_[4][0]+= errX *
        (sx_*mp[0] / mp2mp2);
      // t_y = 0
      // t_z
      secDMatrix_[4][2]+= errX *
        (-sx_ / mp[2] - 2*sx_*mp[0]*mp[0] / (mp2mp2*mp[2]));
      // rot_x
      secDMatrix_[4][3]+= errX *
        (-sx_*mp[1]/mp[2] - 2*sx_*mp[0]*mp[0]*mp[1] / (mp[2]*mp2mp2));
      // rot_y
      secDMatrix_[4][4]+= errX *
        (2*sx_/mp[2] * (mp[0] + mp[0]*mp[0]*mp[0] / mp2mp2));
      // rot_z
      secDMatrix_[4][5]+= errX *
        (-2*sx_*mp[1]*mp[0] / mp2mp2);

      ////// m'_y:
      //// partials for rot_y
      // t_x =0
      // t_y
      secDMatrix_[4][1]+=  errY *
        (sy_*mp[0]/mp2mp2);
      // t_z
      secDMatrix_[4][2]+= errY *
        (-2*sy_*mp[0]*mp[1]/(mp2mp2*mp[2]));
      //rot_x
      secDMatrix_[4][3]+= errY *
        (-2*sy_*mp[0]*mp[1]*mp[1] / (mp2mp2*mp[2]));
      // rot_y
      secDMatrix_[4][4]+= errY *
        (sy_/mp2mp2*(mp[2]*mp[1] + 2*mp[0]*mp[0]*mp[1]/mp[2]));
      // rot_z
      secDMatrix_[4][5]+= errY *
        (sy_/mp2mp2*(-mp[1]*mp[1] + mp[0]*mp[0]));


      ///////////// 5. row is rot_z
      ////// m'_x:
      //// partials for rot_z
      // t_x =0
      // t_y =0
      // t_z
      secDMatrix_[5][2]+= errX *
        sx_*mp[1] / mp2mp2;
      // rot_x
      secDMatrix_[5][3]+= errX *
        (sx_/mp2mp2*(-mp[0]*mp[0] + mp[1]*mp[1]));
      // rot_y
      secDMatrix_[5][4]+= errX *
        (-2*sx_*mp[0]*mp[1] / mp2mp2);
      // rot_z
      secDMatrix_[5][5]+= errX *
        (-sx_*mp[0]/mp[2]);

      ////// m'_y:
      //// partials for rot_z
      // t_x =0
      // t_y =0
      // t_z
      secDMatrix_[5][2]+= errY *
        -sy_*mp[0] / mp2mp2;
      // rot_x
      secDMatrix_[5][3]+= errY *
        (-2*sy_ * mp[0] *mp[1] / mp2mp2);
      // rot_y
      secDMatrix_[5][4]+= errY *
        (sy_/mp2mp2* (mp[0]*mp[0] - mp[1]*mp[1]));
      // rot_z
      secDMatrix_[5][5]+= errY *
        -sy_*mp[1] / mp[2];
    }
  }
  return 0;
}

int CamPoseCalib::GetCoVarMatrix(Matrix<double> &coVar)
{
  if (!doLM_) {
    BIASERR("covariance only for LM estimation yet.");
    return -1;
  }
  BIAS::SVD svd(coVarMat_);

  double sigma2 =
    GetAvgSquaredWeightedError_() * mps_.size(); // undo averaging
  BIASASSERT((2 * mps_.size() - paramToEst_)!=0);
  sigma2 /=  (double)(2 * mps_.size() -paramToEst_);
  coVar = sigma2 * svd.Invert();
  coVar.MakeSymmetric();

#ifdef BIAS_DEBUG
  for (int i=0; i<coVar.num_rows(); i++) {
    for (int j=i; j<coVar.num_cols(); j++) {
      if (BIAS_ISINF(coVar[i][j]) || BIAS_ISNAN(coVar[i][j])){
        cout << coVarMat_ << endl << svd.Invert() << endl
             << GetAvgSquaredWeightedError_() << " "
             << sigma2 << "  "<< mps_.size() <<"  " << paramToEst_
             << endl << coVar << endl;
      }
      INFNANCHECK(coVar[i][j]);
    }
  }
#endif
  return 0;
}


int CamPoseCalib::TransformVec(const Vector<double>& src, Vector<double>& dst)
{
  if (dst.Size() != src.Size()){
    dst.newsize(src.Size());
  }
#ifdef BIAS_DEBUG
  for (unsigned int i=0; i<src.Size(); i++) {
    INFNANCHECK(src[i]);
  }
#endif


  Vector3<double> C;
  Matrix3x3<double> I(MatrixIdentity);
  Matrix3x3<double> R;
  R = camR_ * resultR_.Transpose() * ( (-1.0/scaleNumStabiltity_) * I);
  C = GetTranslation() + camC_;


  Matrix4x4<double> T(MatrixIdentity);
  for (int r=0; r<3; r++){
    T[r][3] = C[r];
    for (int c=0; c<3; c++){
      T[r][c] = R[r][c];
    }
  }

  Vector4<double> sC;
  for (int i=0; i<3; i++) sC[i] = src[i];
  sC[3] = 1.0;
  Vector4<double> dC = T * sC;
  if (Equal(dC[3], 0.0)){
    BIASASSERT("H5N1" && false);
  }
  dC /= dC[3];
  for (int i=0; i<3; i++) {
    dst[i] = dC[i];
#ifdef BIAS_DEBUG
    INFNANCHECK(dst[i]);
#endif
  }

  RMatrixBase sR, dR;
  sR.SetXYZ(src[3], src[4], src[5]);
  dR = camR_ * sR;
  return (dR.GetRotationAnglesXYZ(dst[3], dst[4], dst[5]));
}

#include <MathAlgo/UnscentedTransform.hh>

// helper class for covariance matrix transfromation
class GuessCoo2WoorlCoo : public BIAS::UnscentedTransform
{
public:
  GuessCoo2WoorlCoo(CamPoseCalib *p) : ccc(p) {};
protected:
  CamPoseCalib *ccc;
  virtual int Transform_(const Vector<double>& src,
                          Vector<double>& dst) const
  { return ccc->TransformVec(src, dst); }
};

void CamPoseCalib::
GetCov(Matrix<double>& cov)
{
  GuessCoo2WoorlCoo g2w(this);
  Vector<double> g, w;
  Matrix<double> gc, wc;
  if (GetCoVarMatrix(gc)!=0){
    BIASASSERT("H5N1" && false);
  }

  for (int i=0; i<gc.num_rows(); i++) {
    for (int j=i; j<gc.num_cols(); j++) {
      gc[j][i] = gc[i][j] = 0.5*(gc[i][j] + gc[j][i]);
#ifdef BIAS_DEBUG
      if (BIAS_ISINF(gc[i][j]) || BIAS_ISNAN(gc[i][j])){
        cout << gc << endl;
      }
      INFNANCHECK(gc[i][j]);
#endif
    }
  }


  g = GetResult();
#ifdef BIAS_DEBUG
  for (unsigned int i=0; i<g.Size(); i++) {
    INFNANCHECK(g[i]);
  }
#endif
  g2w.Transform(g, gc, w, wc);
  cov = wc;
  //cout << "cpc: before unscented tranform: "<<gc
  //     <<"\tafter unscented tranform: "<<wc<<endl;

  /*Matrix<double> est_cov;
  int res = 0;
  if ((res = GetCoVarMatrix(est_cov))!=0){
    return res;
  }

  Vector3<double> C;
  Matrix3x3<double> I(MatrixIdentity);
  Matrix3x3<double> R;
  R = camR_* resultR_.Transpose() * ( (-1.0/scaleNumStabiltity_) * I);
  C = GetTranslation() + camC_;

  // J is the jacobian
  Matrix<double> J(6, 6, MatrixZero);
  for (int r=0; r<3; r++){
    //J[r][3] = C[r];
    for (int c=0; c<3; c++){
      J[r][c] = R[r][c];
    }
  }

  // now the second part

  cov = T.Transpose() * est_cov * T;
  cout << "est_cov: "<<est_cov<<"\ncov: "<<cov<<endl;
  return res;*/
}


int CamPoseCalib::SetInitialCamera(double fx, double fy, double cx, double cy)
{
  initialGuess_=true;
  sx_=fx; sy_=fy;
  cx_=cx; cy_=cy;
  return 0;
}

int CamPoseCalib::GuessInitialCam_()
{
  camR_.SetIdentity();
  camRT_.SetIdentity();
  camC_.Set(0,0,0);
  //// find 3 image points far away from each other
  BIAS::Vector2<double> i0(0,0),i1(0,0),i2(0,0),c(cx_,cy_);
  BIAS::Vector3<double> obj0,obj1,obj2; // corresponding model points

  double dist=0,newDist;
  int index=0;
  unsigned int i;

  // first point far away from image center
  for (i=0; i<tps_.size(); i++) {
    newDist=(tps_[i]-c).Length();
    if (newDist>dist) {
      i0=tps_[i]; dist=newDist;
      index=i;
    }
  }
  obj0=origMps_[index];

  // second point far away from first point
  dist=0; index=-1;
  for (i=0; i<tps_.size(); i++) {
    newDist = (tps_[i]-i0).Length();
    if ( newDist > dist) {
      i1=tps_[i]; dist=newDist;
      index=i;
    }
  }
  obj1=origMps_[index];

  //third point far away from connection between i0 and i1
  dist=0; index=-1;
  BIAS::Vector2<double> diff0,diff1;
  for (i=0; i<tps_.size(); i++) {
    diff0= tps_[i]-i0;
    diff1= tps_[i]-i1;
    newDist =  fabs(diff0*diff1);
    if ( newDist > dist) {
      i2=tps_[i]; dist=newDist;
      index=i;
    }
  }
  if (index<0) return -2; // bad image point configuration

  obj2=origMps_[index];

  BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,"found image points:"<<
           i0<<" "<<i1<<" "<<i2);
  BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,"corresponding obj points:"<<
           obj0<<" "<<obj1<<" "<<obj2);

  //// construct orthonormal basis for i0,i1,i2 and obj0,obj1,obj2
  double sxy= (sx_+sy_) *0.5;
  if (sx_<=0) sxy=600;
  BIAS::Vector3<double> iw0(i0[0]-cx_,i0[1]-cy_,sxy), // iw = image world (3D)
    iw1(i1[0]-cx_,i1[1]-cy_,sxy),
    iw2(i2[0]-cx_,i2[1]-cy_,sxy), delta_iw0,delta_iw1,delta_iw2;
  BIAS::Vector3<double> delta_obj0,delta_obj1,delta_obj2;
  delta_iw0= iw1-iw0;
  if (delta_iw0.NormL2()<1e-10) return -1;
  delta_iw2= delta_iw0.CrossProduct(iw2-iw0);
  if (delta_iw2.NormL2()<1e-10) return -1;
  delta_iw1= delta_iw2.CrossProduct(delta_iw0);
  if (delta_iw1.NormL2()<1e-10) return -1;

  delta_obj0 = obj1 - obj0;
  if (delta_obj0.NormL2()<1e-10) return -1;
  delta_obj2 = delta_obj0.CrossProduct(obj2-obj0);
  if (delta_obj2.NormL2()<1e-10) return -1;
  delta_obj1 = delta_obj2.CrossProduct(delta_obj0);
  if (delta_obj1.NormL2()<1e-10) return -1;

  BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,"delta Vecs image:"<<
           delta_iw0<<" "<< delta_iw1<<" "<< delta_iw2<<
           endl<<" and objects:"<<endl<<
           delta_obj0<<" "<< delta_obj1<<" "<< delta_obj2);

  //// use bases as rotation matrix and construct transform between both
  //// basis
  BIAS::RMatrix R_ci, R_co;
  delta_iw0.Normalize();  delta_iw1.Normalize();  delta_iw2.Normalize();
  R_ci.SetFromColumnVectors(delta_iw0,delta_iw1,delta_iw2);
  delta_obj0.Normalize();  delta_obj1.Normalize();  delta_obj2.Normalize();
  R_co.SetFromColumnVectors(delta_obj0,delta_obj1,delta_obj2);
  BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,"normalized delta Vecs image:"<<
           delta_iw0<<" "<< delta_iw1<<" "<< delta_iw2<<
           endl<<" and objects:"<<endl<<
           delta_obj0<<" "<< delta_obj1<<" "<< delta_obj2);
  BIAS::SimilarityTransform T_ci, T_co;
  T_ci.SetR(R_ci); T_ci.SetC(iw0);
  T_co.SetR(R_co); T_co.SetC(obj0);

  BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,"T_ci*(0,0,0) gives:"<<T_ci*
           BIAS::Vector3<double>(0,0,0));

  BIAS::SimilarityTransform finalT, moveT;
  T_co.InvertIP();
  BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,"T_co^-1* obj0 gives:"<<T_co*obj0);
  finalT = T_ci * T_co;

  BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,"finalT * obj0 gives:"<<finalT*obj0);

  //// now move the cam  away from the object
  double mu =  delta_obj0.Length() / (delta_iw0.Length() * iw0.Length());
  moveT.SetC(mu * iw0);
  finalT = moveT * finalT;

  //// set the final pose as initial cam pose
  finalT.InvertIP();
  camR_=finalT.GetR();
  camRT_=camR_.Transpose();
  camC_=finalT.GetC();

 //  /// debug test
//   PMatrix PTest;
//   KMatrix KTest;
//   KTest.SetIdentity();
//   KTest[0][0]=sx_;   KTest[1][1]=sy_;
//   KTest[0][2]=cx_;   KTest[1][2]=cy_;
//   PTest.Compose(KTest,camR_,camC_);
//   cout<<" Projection of obj0:"<<obj0<<" should be:"<<i0<<" but is:";
//   HomgPoint2D(PTest*HomgPoint3D(obj0)).GetEuclidian(i0);
//   cout<<i0<<endl;
//   cout<<" Projection of obj1:"<<obj1<<" should be:"<<i1<<" but is:";
//   HomgPoint2D(PTest*HomgPoint3D(obj1)).GetEuclidian(i1);
//   cout<<i1<<endl;
  return 0;
}


int CamPoseCalib::ApplyCoVariances_()
{
  // multiply each two rows with the covar matrices
  int row=0, i=0;
  double dummyVal;
  while (row<solveVector_.size()) {
    const double &s00=variances_[i][0][0];
    const double &s01=variances_[i][0][1];
    const double &s10=variances_[i][1][0];
    const double &s11=variances_[i][1][1];

    // first the solveVector
    dummyVal=  s00*solveVector_[row] + s01*solveVector_[row+1];
    solveVector_[row+1]=  s10*solveVector_[row] + s11*solveVector_[row+1];
    solveVector_[row]=dummyVal;

    // now the matrix(Jacobian)
    for (int col=0;col<solveMatrix_.num_cols();col++) {
      dummyVal= s00* solveMatrix_[row][col] + s01* solveMatrix_[row+1][col];
      solveMatrix_[row+1][col]=
        s10*solveMatrix_[row][col] + s11* solveMatrix_[row+1][col];
      solveMatrix_[row][col]=dummyVal;
    }
    row+=2;
    i++;
  }
  return 0;
}


int CamPoseCalib::AddCorr(const BIAS::Vector3<double> &x,
                          const BIAS::Vector2<double> &pointOnLine,
                          const BIAS::Vector2<double> &normalOfLine)
{
  origMpsLine_.push_back(x);
  tpsLinePoint_.push_back(pointOnLine);
  tpsLineNormal_.push_back(normalOfLine);
  return 0;
}