Pose.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/W32Compat.hh>
#include <Geometry/Pose.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Geometry/PoseParametrizationCovariance.hh>
#include <fstream>
#include <iostream>

#define POSE_STREAM_PRECISION 12

using namespace BIAS;
using namespace std;

BIAS::Pose::Pose()
{
  Cov_.newsize(7, 7);
  Cov_.Fill(0);
}

BIAS::Pose::~Pose()
{
  Cov_.clear();
}

Matrix3x3<POSE_TYPE>
BIAS::Pose::GetCovC() const
{
  Matrix3x3<POSE_TYPE> ret;
  for (unsigned int row = 0; row < 3; row++)
  {
    for (unsigned int col = 0; col < 3; col++)
    {
      ret[row][col] = Cov_[row][col];
    }
  }
  return ret;
}

Matrix4x4<POSE_TYPE>
BIAS::Pose::GetCovQ() const
{
  Matrix4x4<POSE_TYPE> ret;
  for (unsigned int row = 0; row < 4; row++)
  {
    for (unsigned int col = 0; col < 4; col++)
    {
      ret[row][col] = Cov_[row + 3][col + 3];
    }
  }
  return ret;
}

void
BIAS::Pose::SetQ(const Quaternion<POSE_TYPE>& Q)
{
  CoordinateTransform3D::SetQ(Q);
  Cov_.SetZero();
}

void
BIAS::Pose::SetC(const Vector3<POSE_TYPE>& C)
{
  CoordinateTransform3D::SetC(C);
  Cov_.SetZero();
}

void
BIAS::Pose::SetC(const Vector3<POSE_TYPE>& C, const Matrix3x3<POSE_TYPE>& cov)
{
  CoordinateTransform3D::SetC(C);
  for (unsigned int i = 0; i < 3; i++)
    {
      for (unsigned int j = 0; j < 3; j++)
        {
          Cov_[i][j] = cov[i][j];
        }
    }
}

void
BIAS::Pose::SetQ(const Quaternion<POSE_TYPE>& Q, const Matrix4x4<POSE_TYPE>& cov)
{
  CoordinateTransform3D::SetQ(Q);
  for (unsigned int row = 0; row < 4; row++)
    {
      for (unsigned int col = 0; col < 4; col++)
        {
          Cov_[row + 3][col + 3] = cov[row][col];
        }
    }
}

void
BIAS::Pose::Set(const Quaternion<POSE_TYPE>& Q, const Vector3<POSE_TYPE>& C)
{
  CoordinateTransform3D::Set(Q, C);
  Cov_.SetZero();
}

int
BIAS::Pose::SetCovEuclidean(Matrix<POSE_TYPE> Cov)
{
  if (Cov.GetRows() != 6 || Cov.GetCols() != 6)
  {
    BIASERR("Euclidean Pose Covariance has to be 7x7");
    return -1;
  }
  else
  {
    BIASERR("Not implemented!!");
    BIASABORT;
    return -2;
  }
  //return 0;
}

int
BIAS::Pose::Set(const PoseParametrization& pose)
{
  int ret = 0;
  Set(pose.GetOrientation(), pose.GetPosition());
  ret -= SetCovariance(pose.GetCovarianceMatrix());
  return ret;
}

void
BIAS::Pose::Set(const Quaternion<POSE_TYPE>& Q, const Vector3<POSE_TYPE>& C,
                const Matrix<POSE_TYPE>& Cov)
{
  Set(Q, C);
  SetCovariance(Cov);
}

int
BIAS::Pose::SetCovariance(const Matrix<POSE_TYPE>& c)
{
  if (c.GetRows() != 7 || c.GetCols() != 7)
  {
    BIASERR("Pose Covariance has to be 7x7");
    Cov_.SetZero();
    return -1;
  }
  else
  {
    Cov_ = c;
    bool defaultcov = false;
//#ifdef BIAS_DEBUG
    for (unsigned int x = 0; x < 7; x++)
    {
      for (unsigned int y = 0; y < 7; y++)
      {
        if (BIAS_ISNAN(Cov_[y][x]) || BIAS_ISINF(Cov_[y][x]))
        {
          defaultcov = true;
          BIASERR("Invalid covariance matrix set in pose: " << Cov_);
          //BIASABORT;
        }
      }
    }
//#endif
    if (defaultcov)
    {
      Cov_.SetIdentity();
      Cov_ *= 1e-10;
    }
  }
  return 0;
}

PoseParametrization
BIAS::Pose::GetPoseParameters() const
{
  PoseParametrization res;
  res.SetOrientation(Q_);
  res.SetPosition(C_);
  res.SetCovarianceMatrix(Cov_);
  return res;
}

bool
BIAS::Pose::Load(const std::string& inputFile)
{
  ifstream ifs(inputFile.c_str());
  if (!ifs)
  {
    return false;
  }
  ifs >> *this;
  if (!ifs)
  {
    ifs.close();
    return false;
  }
  ifs.close();
  UpdateMatrix_();
  return true;
}

bool
BIAS::Pose::Save(const std::string& outputFile)
{
  ofstream ofs(outputFile.c_str());
  if (!ofs)
  {
    return false;
  }
  ofs << *this;
  if (!ofs)
  {
    ofs.close();
    return false;
  }
  ofs.close();
  return true;
}

#ifdef BIAS_HAVE_XML2

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

int
BIAS::Pose::XMLOut(const xmlNodePtr Node, BIAS::XMLIO& XMLObject) const
{
  // Write rotation quaternion
  {
    std::ostringstream values;
    xmlNodePtr childNode = XMLObject.addChildNode(Node, "Rotation");
    values << std::setprecision(POSE_STREAM_PRECISION);
    for (unsigned int i = 0; i < 4; i++)
      values << (double) Q_.GetData()[i] << " ";
    XMLObject.addContent(childNode, values.str());
  }

  // Write position vector
  {
    std::ostringstream values;
    xmlNodePtr childNode = XMLObject.addChildNode(Node, "Position");
    values << std::setprecision(POSE_STREAM_PRECISION);
    for (unsigned int i = 0; i < 3; i++)
      values << (double) C_.GetData()[i] << " ";
    XMLObject.addContent(childNode, values.str());
  }

  // Write covariance matrix
  if (!Cov_.IsZero())
  {
    std::ostringstream values;
    xmlNodePtr childNode = XMLObject.addChildNode(Node, "Covariance");
    values << std::setprecision(POSE_STREAM_PRECISION);
    for (unsigned int i = 0; i < 49; i++)
      values << (double) Cov_.GetData()[i] << " ";
    XMLObject.addContent(childNode, values.str());
  }

  return 0;
}

int
BIAS::Pose::XMLIn(const xmlNodePtr Node, BIAS::XMLIO& XMLObject)
{
  xmlAttrPtr attr;
  attr = XMLObject.getAttributeByName(Node, "Position");
  if (attr) {
    std::istringstream values(XMLObject.getAttributeValueString(attr));
    for (unsigned int i = 0; i < 3; i++) values >> C_.GetData()[i];
  } else {
    C_.SetZero();
  }
  attr = XMLObject.getAttributeByName(Node, "Rotation");
  if (attr) {
    std::istringstream values(XMLObject.getAttributeValueString(attr));
    for (unsigned int i = 0; i < 4; i++) values >> Q_.GetData()[i];
  } else {
    Q_.SetIdentity();
  }
  attr = XMLObject.getAttributeByName(Node, "Covariance");
  if (attr) {
    std::istringstream values(XMLObject.getAttributeValueString(attr));
    for (unsigned int i = 0; i < 49; i++) values >> Cov_.GetData()[i];
  } else {
    Cov_.SetZero();
  }

  UpdateMatrix_();

  return 0;
}

#endif

void
BIAS::Pose::TransformToNewGlobalBy(const CoordinateTransform3D& trsf)
{
  PoseParametrizationCovariance ppc(Cov_);
  ppc.Transform(trsf.GetLocalToGlobalTransform());
  CoordinateTransform3D intermediate;
  trsf.ConcatenateLocalTransform((*this), intermediate);
  this->Set(intermediate.GetQ(), intermediate.GetC());
  Cov_ = ppc;
}

Vector3<double>
BIAS::Pose::GetZAxis() const
{
  RMatrixBase R = GetR();
  return Vector3<double> (R[0][2], R[1][2], R[2][2]);
}

Vector3<double>
BIAS::Pose::GetXAxis() const
{
  RMatrixBase R = GetR();
  return Vector3<double> (R[0][0], R[1][0], R[2][0]);
}

Vector3<double>
BIAS::Pose::GetYAxis() const
{
  RMatrixBase R = GetR();
  return Vector3<double> (R[0][1], R[1][1], R[2][1]);
}