ProjectionParametersProjective.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/ProjectionParametersProjective.hh"


using namespace std;
using namespace BIAS;


int ProjectionParametersProjective::
ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
             bool IgnoreDistortion) const
{
  BIASERR("No local projection possible in projective case!!!");
  BIASABORT;
  return -1;
}


HomgPoint3D ProjectionParametersProjective::
UnProjectToImagePlane(const HomgPoint2D& pos, const double& depth,
                      bool IgnoreDistortion) const
{
//  Vector3<double> p = UnProjectLocal(pos, IgnoreDistortion);
//  if(Equal(p[2], 0.0))
//    return HomgPoint3D(0,0,0,0);
//  p*= depth / p[2];
//  if(Equal(p.NormL2(), 0.0))
//    return HomgPoint3D(0,0,0,0);
//  HomgPoint3D local(p);
//  return Pose_.LocalToGlobal(local);

  BIASERR("not implemented in projective case!!!")
  BIASABORT

  HomgPoint3D dummy;
  return dummy;
}

bool ProjectionParametersProjective::
Distort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("Cannot distort in projective case!!!");
  BIASABORT;
  return false;
}


bool ProjectionParametersProjective::
Undistort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("Cannot undistort in projective case!!!");
  BIASABORT;
  return false;
}

Matrix3x3<double> ProjectionParametersProjective::
UnProjectCovLocal(const HomgPoint2D& /*pos*/, const Matrix3x3<double>& /*cov*/,
               bool /* IgnoreDistortion */, bool /* Normalize */) const
{
  BIASERR("covariance propagation for projective space not implemented");
  return Matrix3x3<double>(MatrixZero);
}

#ifdef BIAS_HAVE_XML2

int ProjectionParametersProjective::XMLGetClassName(std::string& TopLevelTag,
                                                    double& Version) const {
  TopLevelTag = "ProjectionParametersProjective";
  Version = 1.0;
  return 0;
}

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

  xmlNodePtr childNode;

  childNode = XMLObject.addChildNode(Node, "ImageSize");
  XMLObject.addAttribute(childNode, "width", (int)width_);
  XMLObject.addAttribute(childNode, "height", (int)height_);



  Matrix3x4<double> MatrixCache = Pose_.GetMatrix3x4();
  stringstream datastream;
  childNode = XMLObject.addChildNode(Node,"MatrixRows");
  for (unsigned int i=0; i<12; i++) {
    datastream << (double) MatrixCache.GetData()[i] <<" ";
  }
  XMLObject.addContent(childNode, datastream.str());
  return 0;
}

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

  xmlNodePtr childNode;

  if ((childNode = XMLObject.getChild(Node, "ImageSize"))!=NULL) {
    width_ = XMLObject.getAttributeValueInt(childNode, "width");
    height_ = XMLObject.getAttributeValueInt(childNode, "height");
  }

  Matrix3x4<double> MatrixCache;

  if ((childNode = XMLObject.getChild(Node, "MatrixRows"))==NULL) {
    BIASERR("Error in xml structure.");
    return -1;
  }

  // get data fields
  stringstream ssData(XMLObject.getNodeContentString(childNode));
  unsigned int CurrentData=0;
  double buffer;
  while (ssData.good() && CurrentData<12) {
    ssData >> buffer;
    MatrixCache.GetData()[CurrentData++] = buffer;
  }
  Pose_.SetFromMatrix3x4(MatrixCache);
  if (CurrentData<12) {
    // reached end of stream before expected end of data
    BIASERR("Error parsing XML code ...");
    return -1;
  }

  return 0;
}

#endif

bool ProjectionParametersProjective::
DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                                   HomgPoint2D& x,
                                   bool IgnoreDistortion) const
{

  BIASERR("no local projection possible in projective case")
  BIASABORT

  return false;
}


bool ProjectionParametersProjective::
DoesPointProjectIntoImage(const Vector3<double>& X,
                                   HomgPoint2D& x,
                                   bool IgnoreDistortion) const
{

  if(width_ != 0 && height_ !=0){
    // now compute coordinates
    x = Project(HomgPoint3D(X), IgnoreDistortion);
    // check if in image
    return (x[0]>=-0.5 && x[1]>=-0.5 &&
            x[0]<=(double)(width_)-0.5 && x[1]<=(double)(height_)-0.5 );
  } else {
    BIASERR("Image width and height need to be set for this method!!!")
  }

  return false;
}


Vector3<double> ProjectionParametersProjective::GetC() const{

  PMatrix P = GetP();
  SVD svd(P);
  HomgPoint3D CHomg(svd.GetNullvector());
  CHomg.Homogenize();
  Vector3<double> C = Vector3<double>(CHomg[0], CHomg[1], CHomg[2]);
  return C;
}