Projection.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 <Geometry/Projection.hh>
#include <Geometry/ProjectionParametersFactory.hh>
#include <Geometry/ProjectionParametersZoom.hh>
#include <Geometry/ProjectionParametersZoom.hh>
#include <Base/Common/FileHandling.hh>
#include <iostream>

using namespace BIAS;
using namespace std;


Projection::
Projection() { 
    vecParameters_.clear();
}

Projection::
Projection(const ProjectionParametersBase& p) {
    BIASASSERT(vecParameters_.empty());
    vecParameters_.push_back(p.Clone());  
}

Projection::
Projection(const Projection& p) {
    BIASASSERT(vecParameters_.empty());
    (*this) = p;
}

Projection::
~Projection() {
    for (unsigned int i=0; i<vecParameters_.size(); i++)
        delete vecParameters_[i];
    vecParameters_.clear();  
}


int Projection::Load(const string& filename) {
    bool silent = true;
#ifdef BIAS_HAVE_XML2
    if (XMLRead(filename)==0) return 0;
    if (ReadFromCamParamFile(filename, silent)==0) return 0;
#endif
    if (ReadFromBBC(filename, silent)==0) {
        cout << "Read projection from BBC freed data: "<< *this <<endl;
        return 0;
    }

    PMatrix P;
    if (P.Load(filename)) {
        double x = P.GetK()[0][2];
        double y = P.GetK()[1][2];
        int width = 8*(int)(rint(x+2.0)/4);
        int height = 8*(int)(rint(y+2.0)/4);
        cout<<"Read from PMatrix, image size is UNKNOWN, GUESSING: "<<width
        <<"x"<<height<<endl;
        ProjectionParametersPerspective PP;
        PP.SetP(P);
        PP.SetImageSize(width,height);
        vecParameters_.clear();
        vecParameters_.push_back(PP.Clone());
        return 0;
    }

    BIASERR("unable to load projection from file : "<<filename<<" cwd="<<FileHandling::GetCwd() );
#ifndef BIAS_HAVE_XML2
    BIASERR("Missing XML support in BIAS, can only read BBC FreeD-data.");
#endif

    return -1;
}


#ifdef BIAS_HAVE_XML2
  int Projection::ReadFromCamParamFile(const string& filename,
                                       const bool silent ) {

    // read datafile
    vector<CameraData> vcd;
    if (ProjectionParametersIO::ReadRigData(filename, vcd, silent) != 0)
      return -1;
    return InitFromCameraData(vcd);
  }
#endif // BIAS_HAVE_XML2

  int Projection::ReadFromBBC(const string& filename,
                              const bool silent) {
    // read datafile
    vector<CameraData> vcd;
    vcd.resize(1);
    if (ProjectionParametersIO::ReadFromBBC(filename, vcd[0], 0.0, 0.0,
                                            silent)!=0) return -1;
    return InitFromCameraData(vcd);
  }

  int Projection::InitFromCameraData(const vector<CameraData>& vcd) {
    // initialize parameter vector
    vecParameters_.clear();

    ProjectionParametersFactory factory;
    for (unsigned int i=0; i<vcd.size(); i++) {
      vecParameters_.push_back(factory.Create(vcd[i]));
    }

    return 0;
  }


  void Projection::CreatePerspective(const Pose& pose,
                                     const KMatrix& K,
                                     int width, int height,
                                     BIAS_ProjParaPersp_DISTORTION_TYPE radDistType,
                                     const vector<double>&
                                     UndistortionCoefficients)
  {
    ProjectionParametersPerspective* ppp =
      new ProjectionParametersPerspective();
    ppp->SetDistortionType(radDistType);
    ppp->SetFocalLengthAndAspect(K.GetFx(), K.GetFy()/K.GetFx());
    switch(radDistType){
    case DISTYPE_NONE:
        break;
    case DISTYPE_RADIAL:
      BIASASSERT(UndistortionCoefficients.size() == 2);
      ppp->SetUndistortion(UndistortionCoefficients[0],
                           UndistortionCoefficients[1]);
      break;
    case DISTYPE_DEF:
      BIASASSERT(UndistortionCoefficients.size() == 4);
      ppp->SetUndistortion(UndistortionCoefficients[0],
                           UndistortionCoefficients[1],
                           UndistortionCoefficients[2],
                           UndistortionCoefficients[3]);
      break;
    case DISTYPE_BROWN:
      BIASASSERT(UndistortionCoefficients.size() == 5);
      ppp->SetUndistortionBrown(UndistortionCoefficients[0],
                                UndistortionCoefficients[1],
                                UndistortionCoefficients[2],
                                UndistortionCoefficients[3],
                                UndistortionCoefficients[4]);
      break;
    case DISTYPE_INVERSE_RAD:
      BIASASSERT(UndistortionCoefficients.size() == 4);
      ppp->SetUndistortionInverseRad(UndistortionCoefficients[0],
                                     UndistortionCoefficients[1],
                                     UndistortionCoefficients[2],
                                     UndistortionCoefficients[3]);
      break;
    case DISTYPE_RAD3:
      BIASASSERT(UndistortionCoefficients.size() == 3);
      ppp->SetUndistortionRad3(UndistortionCoefficients[0],
          UndistortionCoefficients[1],
          UndistortionCoefficients[2]);
      break;
    case DISTYPE_INVRAD3:
      BIASASSERT(UndistortionCoefficients.size() == 3);
      ppp->SetUndistortionInverseRad3(UndistortionCoefficients[0],
          UndistortionCoefficients[1],
          UndistortionCoefficients[2]);
      break;
    }
    ppp->SetImageSize(width, height);
    ppp->SetPrincipal(K.GetHx(), K.GetHy());
    ppp->SetPose(pose);
    vecParameters_.push_back((ProjectionParametersBase*)ppp);
  }


  void Projection::AddRelativeCamera(ProjectionParametersBase *ppb){
    ProjectionParametersBase* p = ppb->Clone();
    vecParameters_.push_back(p);
  }

  void Projection::AddAbsoluteCamera(ProjectionParametersBase *ppb){
    ProjectionParametersBase* p = ppb->Clone();
    // if we add the first camera, set it as it is
    if(vecParameters_.size() == 0)
      vecParameters_.push_back(p);
    //if it is the nth camera calc offset to last previously camera in rig
    else{
      Pose newPose;
      newPose.BecomeRelativeTransform(p->GetPose(),
                                      vecParameters_[0]->GetPose());

      p->SetPose(newPose);
      //add camera
      vecParameters_.push_back(p);
    }

  }


  void Projection::CreateSpherical( const Pose& pose,
                                    const unsigned int width, const int height,
                                    const double principalX, const double principalY,
                                    const double radius,
                                    const double aspect)
  {
    ProjectionParametersSpherical* pps =
      new ProjectionParametersSpherical();

    // externals
    pps->SetPose(pose);

    // internals
    pps->SetImageSize(width, height);
    pps->SetPrincipal( principalX, principalY );
    pps->SetRadius(radius);
    pps->SetAspectratio(aspect);
    vector<double> AngleCorrX, AngleCorrY;

    // angular undistortion, ideal to deformed angle
    AngleCorrX.push_back(0.0);
    AngleCorrY.push_back(0.0);
    AngleCorrX.push_back(M_PI_2); // identity
    AngleCorrY.push_back(M_PI_2); // almost true for Ibak project fisheye lense
    pps->SetUndistortion(AngleCorrX, AngleCorrY, radius);

    // update data, create new PPS and delete it in destructor of Projection
    vecParameters_.push_back( (ProjectionParametersBase*)pps ); 
  }



  HomgPoint2D Projection::Project(const HomgPoint3D &X, unsigned int cam,
                                  bool IgnoreDistortion) const
  {
    BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
    if (cam==0) {
      Vector3<double> x;
      vecParameters_[cam]->GetExternals().Mult(X, x);
      return vecParameters_[cam]->ProjectLocal(x, IgnoreDistortion);
    } else {
      HomgPoint3D Xl = vecParameters_[0]->GetPose().GlobalToLocal(X);
      return vecParameters_[cam]->Project(Xl, IgnoreDistortion);
    }
    BIASERR("To be implemented");
    BIASABORT;
    HomgPoint2D h(0,0,0);
    return h;
  }

  const bool Projection::
  DoExtrinsicsDiffer(const BIAS::Projection &p) const{

    if(p.vecParameters_.size() != vecParameters_.size())
      return true;
    if(p.vecParameters_.size() == 0 || vecParameters_.size() == 0)
      return true;

    bool theyDiffer = false;
    for(unsigned int i=0; i<vecParameters_.size() && !theyDiffer; i++)
      theyDiffer = vecParameters_[i]->DoExtrinsicsDiffer(p.vecParameters_[i]);

    return theyDiffer;
  }


  const bool Projection::
  DoIntrinsicsDiffer(const BIAS::Projection &p) const {

    if(p.vecParameters_.size() != vecParameters_.size())
      return true;
    if(p.vecParameters_.size() == 0 || vecParameters_.size() == 0)
      return true;

    bool theyDiffer = false;
    for(unsigned int i=0; i<vecParameters_.size() && !theyDiffer; i++ )
      theyDiffer = vecParameters_[i]->DoIntrinsicsDiffer(p.vecParameters_[i]);

    return theyDiffer;
  }

  bool Projection::
  DoesPointProjectIntoImage(const HomgPoint3D& X,
                            HomgPoint2D& x,
                            unsigned int cam,
                            bool IgnoreDistortion) const
  {
    BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
    if(cam!=0) {
      //point in rigs coordinate frame
      HomgPoint3D Xr = vecParameters_[0]->GetPose().GlobalToLocal(X);

      //point in cam coordinate frame
      HomgPoint3D Xl = vecParameters_[cam]->GetPose().GlobalToLocal(Xr);
      Xl.Homogenize();

      return vecParameters_[cam]->DoesPointProjectIntoImageLocal(Xl.GetEuclidean(),x,IgnoreDistortion);
    }

    return vecParameters_[cam]->DoesPointProjectIntoImage(X, x,
                                                          IgnoreDistortion);
  }



  void Projection::UnProjectToRay(const HomgPoint2D& pos,
                                  Vector3<double>& origin,
                                  Vector3<double>& rayDir,
                                  unsigned int cam,
                                  bool IgnoreDistortion) const
  {
    BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);   
    Vector3<double> x;
    UnProjectToRayLocal(pos, origin, x, 0, cam, IgnoreDistortion);
    GetR().Mult(x, rayDir);
    origin += vecParameters_[cam]->GetC();
  }


  void Projection::UnProjectToRay(const HomgPoint2D& pos,
                              Vector3<double>& rayDir,
                                  unsigned int cam,
                                  bool IgnoreDistortion) const
  {
    BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
    Vector3<double> x, tmp;
    UnProjectToRayLocal(pos, tmp, x, 0, cam, IgnoreDistortion);
    GetR().Mult(x, rayDir);
  }


  Vector3<double> Projection::UnProjectToPoint(const HomgPoint2D& pos,
                                               double depth,
                                               unsigned int cam,
                                               bool IgnoreDistortion) const
  {
    Vector3<double> opticalray, p;
    UnProjectToRay(pos, p, opticalray, cam, IgnoreDistortion);
    if(opticalray.NormL2()==0.0) {
#ifdef BIAS_DEBUG
      BIASERR("returned ray has zero length!");
#endif
      return Vector3<double> (0,0,0);
    }
    opticalray.Normalize();
    return (GetAbsoluteC(cam) + depth*opticalray);
  }


  Vector3<double> Projection::
  UnProjectToPointLocal(const HomgPoint2D& pos,
                        const double& depth,
                        unsigned int camSystem,
                        unsigned int cam,
                        bool IgnoreDistortion) const
  {
    BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
    BIASASSERT(vecParameters_.size()>camSystem && vecParameters_[camSystem]);

    Vector3<double> x=
      vecParameters_[cam]->UnProjectToPointLocal(pos,
                                                 depth,
                                                 IgnoreDistortion);
    if(camSystem != cam) {
      HomgPoint3D xHomg(x);
      HomgPoint3D xHomgRig;
      HomgPoint3D xHomgTarget;
      Pose CamRigPose = vecParameters_[cam]->GetPose();

      xHomgRig = CamRigPose.LocalToGlobal(xHomg);
      if(cam==0) {
        xHomgRig.GetEuclidean(x);
      } else {
        Pose TargetRigPose = vecParameters_[camSystem]->GetPose();
        xHomg = TargetRigPose.GlobalToLocal(xHomgRig);
        xHomg.GetEuclidean(x);
      }

    }

    return x;
  }


  Matrix3x3<double> Projection::
  UnProjectToCovLocal(const HomgPoint2D& pos, const Matrix3x3<double>& cov2D,
                      const double& /*depth*/, unsigned int camSystem,
                      unsigned int cam, bool IgnoreDistortion,
                      bool Normalize) const
  {
    BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
    if (camSystem != cam) {
      BIASERR("Not implemented.");
      BIASABORT;
    }
    return vecParameters_[cam]->UnProjectCovLocal(pos, cov2D,
                                                  IgnoreDistortion, Normalize);
  }



  void Projection::UnProjectToRayLocal(const HomgPoint2D& pos,
                                       Vector3<double>& origin,
                                       Vector3<double>& rayDir,
                                       unsigned int camSystem,
                                       unsigned int cam,
                                       bool IgnoreDistortion) const
  {
    BIASASSERT(vecParameters_.size()>camSystem && vecParameters_[camSystem]);
    BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);

    Vector3<double> x, p;
    vecParameters_[cam]->UnProjectLocal(pos, p, x, IgnoreDistortion);

    if(camSystem != cam) {
      //the relative rotation to rig of the system we GOT the ray direction in
      RMatrix ROrigin = GetR(cam);
      x = ROrigin * x; //x is now in the rig coordinate system

      Vector3<double> COrigin = GetC(cam);
      p = COrigin + ROrigin * p; // p is now in rig coordinate system

      if(camSystem!=0) {
        //the relative rotation to rig of the system we WANT to have
        // the direction in
        RMatrix RTarget = GetR(camSystem);

        x = RTarget.Transpose() * x;
        //x is now in the local target coordinate system

        Vector3<double> CTarget = GetC(camSystem);
        p = RTarget.Transpose() * p - RTarget.Transpose() * CTarget;
      }
    }

    origin = p;
    rayDir = x;
  }



#ifdef BIAS_HAVE_XML2

  int Projection::XMLGetClassName(string& TopLevelTag,
                                  double& Version) const{
    TopLevelTag = "Projection";
    Version = 0.1;
    return 0;
  }

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

    xmlNodePtr theNode;
    string TopLevelName;
    double Version;
    if(identifier_.size()>0) {
      theNode = XMLObject.addChildNode(Node, "Identifier");
      XMLObject.addContent(theNode, identifier_);
    }
    std::vector<ProjectionParametersBase* >::const_iterator it;
    for (it=vecParameters_.begin(); it!=vecParameters_.end(); it++) {
      (**it).XMLGetClassName(TopLevelName, Version);
      theNode = XMLObject.addChildNode(Node, TopLevelName);
      XMLObject.addAttribute(theNode, "Version", Version);
      (*it)->XMLOut(theNode, XMLObject);
    }

    return 0;
  }


  int Projection::XMLIn(const xmlNodePtr Node, XMLIO& XMLObject){

    //delete old cameras
    for (unsigned int i=0; i<vecParameters_.size(); i++)
      delete vecParameters_[i];
    vecParameters_.clear();

    string TopLevelName;
    xmlNodePtr childNode;
    childNode = XMLObject.getFirstChild(Node);
    if (childNode == NULL) return 0; // it is ok to read an empty Proj.
    while (childNode!=NULL) {
      string nodename = XMLObject.getNodeName(childNode);
      if (nodename.compare("Identifier")==0) {
        identifier_ = XMLObject.getNodeContentString(childNode);
      } else {
        ProjectionParametersFactory factory;
        ProjectionParametersBase *pp = factory.Create(nodename);
        if(pp->XMLIn(childNode, XMLObject)!=0)
          return -1;
        vecParameters_.push_back(pp);
      }
      childNode = XMLObject.getNextChild(childNode);
    }
    if (vecParameters_.size()>0) {
      return 0;
    } else {
      return -1;
    }
  }

#endif // BIAS_HAVE_XML2


  BIAS::Quaternion<double>
  Projection::GetQ(unsigned int cam) const
  {
    BIASASSERT(cam<vecParameters_.size() && vecParameters_[cam]);
    return vecParameters_[cam]->GetQ();
  }