MatchDataBase.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 <Base/Common/BIASpragma.hh>

#include "MatchDataBase.hh"

#include <fstream>

using namespace BIAS;
using namespace std;



MatchDataBase::MatchDataBase(Param &para)
  : Debug()
{
  _NumImages=_NumPoints=NULL;
  _AddParameter(para);
}

MatchDataBase::~MatchDataBase()
{}

void MatchDataBase::Reset()
{
 
  // clear K, R, C and P
  _K.SetZero();
  _Ki.SetZero();
  _R.clear();
  _C.clear();
  _P.clear();
  // now clear 2D points
  _Points.clear();
  _NormalizedPoints.clear();
}

void MatchDataBase::GetK(KMatrix& K) const
{ K = _K; }

void MatchDataBase::GetR(int imNo, RMatrixBase& R) const
{ R = _R[imNo]; }
void MatchDataBase::GetR(std::vector<RMatrixBase>& R) const
{ R = _R; }


void MatchDataBase::GetC(int imNo, HomgPoint3D& C) const
{ C = _C[imNo]; }
void MatchDataBase::GetC(std::vector<HomgPoint3D>& C) const
{ C = _C; }

void MatchDataBase::GetP(int imNo, PMatrixBase& P) const
{ P = _P[imNo]; }
void MatchDataBase::GetP(std::vector<PMatrixBase>& P) const
{ P = _P; }

void MatchDataBase::GetPoints(int imNo, std::vector<HomgPoint2D>& p) const
{ p = _Points[imNo]; }
void MatchDataBase::GetPoints(std::vector<std::vector<HomgPoint2D> >& p) const
{ p = _Points; }
void MatchDataBase::GetCorrespondences(vector<vector<HomgPoint2D> >& p) const
{ _Transpose(_Points, p); }

void MatchDataBase::GetNormalizedPoints(int imNo, std::vector<HomgPoint2D>& p) const
{ p = _NormalizedPoints[imNo]; }
void MatchDataBase::GetNormalizedPoints(vector<vector<HomgPoint2D> >& p) const
{ p = _NormalizedPoints; }
void MatchDataBase::GetNormalizedCorrespondences(vector<vector<HomgPoint2D> >& p) const
{  _Transpose(_NormalizedPoints, p); }

void MatchDataBase::SetData(KMatrix& K, std::vector<RMatrixBase>& R, 
                            std::vector<HomgPoint3D>& C,
                            std::vector<std::vector<HomgPoint2D> >& m)
{
  BIASASSERT(m.size()==R.size());
  BIASASSERT(m.size()>0);
  *_NumImages=R.size();
  *_NumPoints=m[0].size();
  _SetCamData(K, R, C);
  _Points.resize(*_NumImages);
  _NormalizedPoints.resize(*_NumImages);
  for (int i=0; i<*_NumImages; i++){
    _Points[i].resize(*_NumPoints);
    _NormalizedPoints[i].resize(*_NumPoints);
    for (int p=0; p<*_NumPoints; p++){
      _Points[i][p]=m[i][p];
      _Points[i][p].Homogenize();
      _NormalizedPoints[i][p]=_Ki*_Points[i][p];
    }
  }
}


void MatchDataBase::SetNormalizedData(KMatrix& K, std::vector<RMatrixBase>& R, 
                                      std::vector<HomgPoint3D>& C,
                                      std::vector<std::vector<HomgPoint2D> >& m)
{
  BIASASSERT(m.size()==R.size());
  BIASASSERT(m.size()>0);
  *_NumImages=R.size();
  *_NumPoints=m[0].size();
  _SetCamData(K, R, C);
  _Points.resize(*_NumImages);
  _NormalizedPoints.resize(*_NumImages);
  for (int i=0; i<*_NumImages; i++){
    _Points[i].resize(*_NumPoints);
    _NormalizedPoints[i].resize(*_NumPoints);
    for (int p=0; p<*_NumPoints; p++){
      _NormalizedPoints[i][p]=m[i][p];
      _NormalizedPoints[i][p].Homogenize();
      _Points[i][p]=_K*_NormalizedPoints[i][p];
    }
  }
}


void MatchDataBase::_SetCamData(KMatrix& K, std::vector<RMatrixBase>& R, 
                                std::vector<HomgPoint3D>& C)
{
  _K=K;
  _Ki=_K.Invert();
  _R=R;
  _C=C;
  BIASASSERT(_R.size() == _C.size());
  _P.resize(_R.size());
  for (unsigned i=0; i<_R.size(); i++){
    _P[i].Compose(_K, _R[i], _C[i]);
  }
}

int MatchDataBase::Write(const string& fname) const
{
  ofstream os(fname.c_str());
  if (!os){
    BIASERR("error opening "<<fname);
    return -1;
  }
  int res=Write(os);
  
  os.close();
  return res;
}


int MatchDataBase::Write(ostream &os) const
{
  BIASCDOUT(D_MD_IO,"using MatchDataBase::Write(ostream)\n");
  BIASCDOUT(D_MD_IO,"MatchDataBase::Write() writing "<<*_NumImages<<" images with "<<*_NumPoints<<" points in each\n");
  os.write((char*)_NumImages, sizeof(int));

  os.write((char*)_NumPoints, sizeof(int));

  os.write((char*)_K.GetData(), sizeof(KMATRIX_TYPE)*9);
  
  for (int i=0; i<*_NumImages; i++){
    os.write((char*)_R[i].GetData(), sizeof(ROTATION_MATRIX_TYPE)*9);
    os.write((char*)_C[i].GetData(), sizeof(HOMGPOINT3D_TYPE)*4);
    for (int k=0; k<*_NumPoints; k++){
      os.write((char*)_Points[i][k].GetData(), sizeof(HOMGPOINT2D_TYPE)*3);
    }
  }

  if (!os){
    BIASERR("error writing data");
    return -2;
  }
  return 0;
}


int MatchDataBase::Read(const string& fname)
{
  ifstream is(fname.c_str());
  if (!is){
    BIASERR("error opening "<<fname);
    return -1;
  }
  int res=Read(is);
 
  is.close();
  return res;
}

int MatchDataBase::Read(istream& is)
{
  BIASCDOUT(D_MD_IO, "using MatchDataBase::Read(ostream)\n");
  is.read((char*)_NumImages, sizeof(int));
  is.read((char*)_NumPoints, sizeof(int));
  
  is.read((char*)_K.GetData(), sizeof(KMATRIX_TYPE)*9);
  _Ki=_K.Invert();
  
  _Points.resize(*_NumImages);
  _NormalizedPoints.resize(*_NumImages);
  _R.resize(*_NumImages);
  _C.resize(*_NumImages);
  _P.resize(*_NumImages);
  for (int i=0; i<*_NumImages; i++){
    is.read((char*)_R[i].GetData(), sizeof(ROTATION_MATRIX_TYPE)*9);
    is.read((char*)_C[i].GetData(), sizeof(HOMGPOINT3D_TYPE)*4);
    _P[i].Compose(_K, _R[i], _C[i]);
    _Points[i].resize(*_NumPoints);
    _NormalizedPoints[i].resize(*_NumPoints);
    for (int k=0; k<*_NumPoints; k++){
      is.read((char*)_Points[i][k].GetData(), sizeof(HOMGPOINT2D_TYPE)*3);
      _NormalizedPoints[i][k]=_Ki*_Points[i][k];
    }
  }

  if (!is){
    BIASERR("error reading data");
    return -2;
  }
  BIASCDOUT(D_MD_IO,"MatchDataBase::Read() read "<<*_NumImages
            <<" images with "<<*_NumPoints<<" points in each\n");
  return 0;
}


bool MatchDataBase::operator==(MatchDataBase& r)
{
  bool res=(*_NumImages==*r._NumImages) && (*_NumPoints==*r._NumPoints);
  res=res && (_K == r._K) && (_Ki == r._Ki);
  if (res){
    for (int i=0; i<*_NumImages; i++){
      res = res && (_R[i] == r._R[i]) && (_C[i] == r._C[i]) && 
        (_P[i] == r._P[i]); 
      if (!res) cerr <<" R/C/P\n";
      for (int k=0; k<*_NumPoints; k++){
        res = res && (_Points[i][k]==r._Points[i][k]) && 
          (_NormalizedPoints[i][k]==r._NormalizedPoints[i][k]);
        if (!res) {
          cerr <<"_Points/_NormalizedPoints ["<<k<<"]\n";
          cerr << _Points[i][k]<<" - "<<r._Points[i][k]<<" = "
               <<_Points[i][k]-r._Points[i][k]<<endl;
          cerr << _NormalizedPoints[i][k]<<" <-> "<<r._NormalizedPoints[i][k]
               <<" = "<<_NormalizedPoints[i][k]-r._NormalizedPoints[i][k]
               <<endl;
          break;
        }
      }
      if (!res) break;
    }
  } else {
    cerr <<" Num/K/Ki\n";
  }
  return res;
}

void MatchDataBase::_AddParameter(Param &para)
{
  ////////////////////////////////////////////////////////////
  //         PGNum
  ///////////////////////////////////////////////////////////

  int grp;
  if (para.CheckParam("NumStaticPoints")){
    grp=para.GetGroupID("NumStaticPoints");
  } else {
    grp=para.GetFreeGroupID();
  }
  //  cerr << "MatchDataBase: using ID "<<grp<<" for group num\n";

  if (para.CheckParam("NumImages")){ 
    _NumImages=para.GetParamInt("NumImages");
  } else {
    _NumImages=para.AddParamInt("NumImages", "number of images", 2, 2, 
                                numeric_limits<int>::max(), 0, grp); 
  }

  if (para.CheckParam("NumPoints")){ 
    _NumPoints=para.GetParamInt("NumPoints");
  } else {
    _NumPoints=para.AddParamInt("NumPoints", "number of points in every image",
                                30, 0, numeric_limits<int>::max(), 0, grp); 
  }

}

/////////////////////////////////////////////////////////////////////////////
//    operators
/////////////////////////////////////////////////////////////////////////////

ostream& BIAS::operator<<(ostream& os, const MatchDataBase& m)
{
  os << "K: "<<m._K<<endl;
  os << "P: \n";
  for (int i=0; i<*m._NumImages; i++){
    os << setw(3) << i << " : " << m._P[i]<<endl;
  }
  os << "_Points: \n";
  for (int k=0; k<*m._NumPoints; k++){
    os << setw(3) << k << " : ";
    for (int i=0; i<*m._NumImages; i++){
      os <<i<<":"<<m._Points[i][k] <<" ";
    }
    os << endl;
  }
  os << "_NormalizedPoints: \n";
  for (int k=0; k<*m._NumPoints; k++){
    os << setw(3) << k << " : ";
    for (int i=0; i<*m._NumImages; i++){
      os <<i<<":"<<m._NormalizedPoints[i][k] <<" ";
    }
    os << endl;
  }
  return os;
}

istream& BIAS::operator>>(istream& is, MatchDataBase& m)
{
  BIASERR("unfinished");
  return is;
}