LocalAffineFrame.cpp

#include "Geometry/LocalAffineFrame.hh"
#include "Base/ImageUtils/ImageDraw.hh"
#include "Base/Image/ColourRGB.hh"
#include "Geometry/HMatrix.hh"

using namespace BIAS;
using namespace std;

ostream& BIAS::operator<<(ostream& os, const LocalAffineFrame& fp)
{
  os << " "<< fp.t_<< " " << fp.A_ << endl;
  return os;
}

istream& BIAS::operator>>(istream& is, LocalAffineFrame& fp)
{
  is >> fp.t_;
  BIASASSERT(is.good());
  is >> fp.A_;
  return is;
}

#ifdef BIAS_HAVE_XML2
int LocalAffineFrame::XMLGetClassName(std::string& TopLevelTag,
                                      double& Version) const {
  TopLevelTag = "LocalAffineFrame";
  Version = 0.1;
  return 0;
}

int LocalAffineFrame::XMLOut(const xmlNodePtr Node,
                             XMLIO& XMLObject) const {

  BIASERR("not yet implemented");
  BIASABORT;
  return 0;
}

int LocalAffineFrame::XMLIn(const xmlNodePtr Node, XMLIO& XMLObject) {
  BIASERR("not yet implemented");
  BIASABORT;
  return 0;
}
#endif


void LocalAffineFrame::Draw(Image<unsigned char>& targetImage,
                            const double& positionCovScale,
                            const unsigned int linethickness) const {

  // draw elliptic shape and orientation

  double center[2]={t_[0], t_[1]}, a[2], b[2];
  // length of ellipse main axes
  double eva,evb;
  Matrix2x2<double> phi;
  Matrix2x2<double> theta;

  HMatrix::FactorizeAffineMatrixRRight(A_, phi,
                                       theta, eva, evb);


  // ellipse
  a[0] = phi[0][0] * eva;
  a[1] = phi[1][0] * eva;
  b[0] = phi[0][1] * evb;
  b[1] = phi[1][1] * evb;
  unsigned char value[]={127, 255, 0};
  BIAS::ImageDraw<unsigned char>::Ellipse(targetImage, center, a, b, value);

  // orientation
  Vector2<double> oriLine(0,1);
  oriLine =  A_ * oriLine;
  if (targetImage.IsPositionInImage((int)rint(center[0]),
                                    (int)rint(center[1])) &&
      targetImage.IsPositionInImage((int)rint(center[0])+
                                    (int)rint(oriLine[0]),
                                    (int)rint(center[1]) +
                                    (int)rint(oriLine[1]))) {
    BIAS::ImageDraw<unsigned char>::
      Line(targetImage,
           (int)rint(center[0]), (int)rint(center[1]),
           (int)rint(center[0])+ (int)rint(oriLine[0]),
           (int)rint(center[1]) + (int)rint(oriLine[1]));
  }


  // draw covariance ellipse if desired
  if (positionCovScale>0.0) {
    double a[2], b[2], eva, evb;
    Vector2<double> na, nb;
    double center[2]={t_[0], t_[1]};
    unsigned char col[]={255, 0, 0};
    Matrix2x2<double> curcov;
    for (unsigned int i=0; i<2; i++) {
      for (unsigned int j=0; j<2; j++) {
        curcov[i][j] = Cov_[4+i][4+j];
      }
    }
    curcov.EigenvalueDecomposition(eva, na, evb, nb);
    // eigenvalues are the \sigma^2
    eva = sqrt(eva);
    evb = sqrt(evb);
    //out << eva<<", "<<evb<<"\n";
    a[0] = na[0] * eva * positionCovScale;
    a[1] = na[1] * eva * positionCovScale;
    b[0] = nb[0] * evb * positionCovScale;
    b[1] = nb[1] * evb * positionCovScale;
    //cout<<"drawing ellipse at "<<center<<" with a="<<a[0]<<" "
    //    <<a[1]<<" b="<<b[0]<<" "<<b[1]<<endl;
    ImageDraw<unsigned char>::Ellipse(targetImage, center, a, b, col);
  }
}

LocalAffineFrame LocalAffineFrame::
GetRelativeTransform(const LocalAffineFrame& F2, bool defaultCov) const {
  LocalAffineFrame relativeTransform;
  // first set transform
  //cout<<GetAsMatrix()<<" "<< F2.GetAsInverseMatrix()<<endl;
  relativeTransform.SetFromMatrix(GetAsMatrix() * F2.GetAsInverseMatrix());

  if (defaultCov) {
    // use only standard cov without actually computing it
    relativeTransform.SetDefaultCov();
  } else {
    // linear error propagation!

    // make big cov with uncertainties of both transforms:
    Matrix<double> covbc(12,12,MatrixZero);
    for (unsigned int i=0; i<6; i++) {
      for (unsigned int j=i; j<6; j++) {
        covbc[i][j]     = covbc[j][i]     = 0.5 * (Cov_[j][i]+Cov_[i][j]);
        covbc[i+6][j+6] = covbc[j+6][i+6] = 0.5 * (F2.Cov_[j][i]+F2.Cov_[i][j]);
      }
    }
    // short names for vars:
    const double b11=A_[0][0], b12=A_[0][1], b21=A_[1][0], b22=A_[1][1];
    const double c11=F2.A_[0][0], c12=F2.A_[0][1], c21=F2.A_[1][0],
      c22=F2.A_[1][1], c13=F2.t_[0], c23=F2.t_[1];

    const double detc = c11*c22-c21*c12;

    // compute jacobian of parameter change (this(6), f2(6) --> relativ(6))
    Matrix<double> dadbc(6,12,MatrixZero);
    // da/db:
    dadbc[0][0] = c22/detc;
    dadbc[0][1] = -c21/detc;
    dadbc[1][0] = -c12/detc;
    dadbc[1][1] = c11/detc;
    dadbc[4][0] = (c12*c23-c22*c13)/detc;
    dadbc[4][1] = (c21*c13-c11*c23)/detc;
    dadbc[4][4] = 1;

    dadbc[2][2] = c22/detc;
    dadbc[2][3] = -c21/detc;
    dadbc[3][2] = -c12/detc;
    dadbc[3][3] = c11/detc;
    dadbc[5][2] = (c12*c23-c22*c13)/detc;
    dadbc[5][3] = (c21*c13-c11*c23)/detc;
    dadbc[5][5] = 1;

    const double detc2 = detc*detc;
    // da/dc11
    dadbc[0][6] = -c22*(b11*c22-b12*c21)/detc2;
    dadbc[1][6] = (b12*detc-c22*(b12*c11-b11*c12))/detc2;
    dadbc[4][6] = (-c23*b12*detc-c22*(b11*(c12*c23-c22*c13) +
                                      b12*(c21*c13-c11*c23)))/detc2;
    dadbc[2][6] = -c22*(b21*c22-b22*c21)/detc2;
    dadbc[3][6] = (b22*detc-c22*(b22*c11-b21*c12))/detc2;
    dadbc[5][6] = (-c23*b22*detc-c22*(b21*(c12*c23-c22*c13) +
                                      b22*(c21*c13-c11*c23)))/detc2;

    // da/dc12
    dadbc[0][7] = c21*(b11*c22-b12*c21)/detc2;
    dadbc[1][7] = (-b11*detc+c21*(b12*c11-b11*c12))/detc2;
    dadbc[4][7] = (c23*b11*detc+c21*(b11*(c12*c23-c22*c13)+
                                     b12*(c21*c13-c11*c23)))/detc2;
    dadbc[2][7] = c21*(b21*c22-b22*c21)/detc2;
    dadbc[3][7] = (-b21*detc+c21*(b22*c11-b21*c12))/detc2;
    dadbc[5][7] = (c23*b21*detc+c21*(b21*(c12*c23-c22*c13)+
                                     b22*(c21*c13-c11*c23)))/detc2;

    // da/dc13
    dadbc[4][10] = (b12*c21-c22*b11)/detc;
    dadbc[5][10] = (b22*c21-c22*b21)/detc;

    // da/dc21
    dadbc[0][8] = (c12*(b11*c22-b12*c21)-b21*detc)/detc2;
    dadbc[1][8] = c12*(b12*c11-b11*c12)/detc2;
    dadbc[4][8] = (b12*c13*detc+c12*(b11*(c12*c23-c22*c13)+
                                     b12*(c21*c13-c11*c23)))/detc2;
    dadbc[2][8] = (c12*(b21*c22-b22*c21)-b22*detc)/detc2;
    dadbc[3][8] = c12*(b22*c11-b21*c12)/detc2;
    dadbc[5][8] = (b22*c13*detc+c12*(b21*(c12*c23-c22*c13)+
                                     b22*(c21*c13-c11*c23)))/detc2;

    // da/dc22
    dadbc[0][9]= (-c11*(b11*c22-b12*c21)+b11*detc)/detc2;
    dadbc[1][9]= c11*(b12*c11-b11*c12)/detc2;
    dadbc[4][9]= (-b11*c13*detc-c11*(b11*(c12*c23-c22*c13)+
                                      b12*(c21*c13-c11*c23)))/detc2;
    dadbc[2][9]= (-c11*(b21*c22-b22*c21)+b21*detc)/detc2;
    dadbc[3][9]= c11*(b22*c11-b21*c12)/detc2;
    dadbc[5][9]= (-b21*c13*detc-c11*(b21*(c12*c23-c22*c13)+
                                     b22*(c21*c13-c11*c23)))/detc2;

    // da/dc23
    dadbc[4][11]= (b11*c12-b12*c11)/detc;
    dadbc[5][11]= (b21*c12-b22*c11)/detc;

    // linear error propagation
    relativeTransform.Cov_ = dadbc * covbc * dadbc.Transpose();
  }

  return relativeTransform;
}


LocalAffineFrame LocalAffineFrame::
GetHomographyTransformed(const HMatrix& H) const {
  LocalAffineFrame result;
  Matrix2x2<double> jac;
  HomgPoint2D x(t_);
  H.GetJacobian(x,jac);
  x = H*x;
  x.Homogenize();
  Matrix<double> affineTransformer(6,6,MatrixZero);
  affineTransformer[1][1] = affineTransformer[0][0] = jac[0][0];
  affineTransformer[3][3] = affineTransformer[2][2] = jac[1][1];
  affineTransformer[2][0] = affineTransformer[3][1] = jac[1][0];
  affineTransformer[0][2] = affineTransformer[1][3] = jac[0][1];
  affineTransformer[4][4] = jac[0][0];
  affineTransformer[4][5] = jac[0][1];
  affineTransformer[5][4] = jac[1][0];
  affineTransformer[5][5] = jac[1][1];

  Vector<double> affineparams(GetAsVector());
  //cout<<"affine params are "<< affineparams << endl;

  affineparams = affineTransformer *  affineparams;
  // put in correct position
  affineparams[4] = x[0];
  affineparams[5] = x[1];
  //cout<<"affine params after H are "<< affineparams << endl;
  //cout<<"H was "<<H<<endl;
  Matrix<double> cov(6,6,MatrixZero);
  cov = affineTransformer * Cov_ * affineTransformer.Transpose();
  result.SetFromVector(affineparams, cov);
  return result;
}

BIAS::LocalAffineFrame LocalAffineFrame::GetLocalSimilarityFrame() const {
  LocalAffineFrame Sim;
  // a = vec(A_)
  Vector<double> a(4);
  a[0] = A_[0][0];
  a[1] = A_[1][0];
  a[2] = A_[0][1];
  a[3] = A_[1][1];
  // construct mat so that mat * (x,y)^T = a    where  x=l*cos, y=l*sin
  Matrix<double> mat(4,2,MatrixZero);
  mat[0][0] = 1;
  mat[1][1] = -1;
  mat[2][1] = 1;
  mat[3][0] = 1;
  Matrix2x2<double> tammat;
  mat.GetSystemMatrix(tammat);
  Vector<double> xy = tammat.Invert() * mat.Transpose()*a;
  tammat[0][0] = xy[0];
  tammat[0][1] = xy[1];
  tammat[1][0] = -xy[1];
  tammat[1][1] = xy[0];
  Sim.SetT(GetT());
  Sim.SetA(tammat);
  return Sim;
}