Covariance3Dto2D.cpp

#include <Geometry/Covariance3Dto2D.hh>

#include <Base/Geometry/PMatrixBase.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Debug/Exception.hh>

using namespace BIAS;
using namespace std;

Covariance3Dto2D::
Covariance3Dto2D(BIAS::KMatrix& K, enum PMatrixBase::E_ParametrizationType par)
  : UnscentedTransform(), K_(K), Parametrization_(par)
{
  //AddDebugLevel(D_C3DT2D_COV);
}

Covariance3Dto2D::
~Covariance3Dto2D()
{}

int Covariance3Dto2D::
Project(const BIAS::Vector<double>& Pose,
        const BIAS::Matrix<double>& Cov_Pose,
        const BIAS::Vector3<double>& X,
        const BIAS::Matrix3x3<double>& Cov_X,
        BIAS::Vector2<double>& x,
        BIAS::Matrix2x2<double>& Cov_x) const
{
  BIASCDOUT(D_C3DT2D_COV, "X: "<<X<<"\nCov_X: "<<Cov_X<<endl);
  const int pose_dim = Pose.Size();
  const int dim = pose_dim + 3;
  Vector<double> src, dst;
  Matrix<double> src_cov, dst_cov;

  src.newsize(dim);
  src_cov.newsize(dim, dim);
  src_cov.SetZero();
  for (int c=0; c<3; c++){
    src[c] = X[c];
    for (int r=c; r<3; r++){
      src_cov[c][r] = src_cov[r][c] = 0.5 * (Cov_X[r][c] + Cov_X[c][r]);
    }
  }

  for (int c=0; c<pose_dim; c++){
    src[c+3] = Pose[c];
    for (int r=c; r<pose_dim; r++){
      src_cov[c+3][r+3] = src_cov[r+3][c+3] = 
        0.5 * (Cov_Pose[r][c] + Cov_Pose[c][r]);
    }
  }

  BIASCDOUT(D_C3DT2D_COV, "src: "<<src<<"\nsrc_cov: "<<src_cov<<endl);
  if (Transform(src, src_cov, dst, dst_cov)!=0){
    return -1;
  }
  BIASCDOUT(D_C3DT2D_COV, "dst: "<<dst<<"\ndst_cov: "<<dst_cov<<endl);
  x = dst;
  Cov_x = dst_cov;
  return 0;
}

int Covariance3Dto2D::
Transform_(const Vector<double>& src, Vector<double>& dst) const
{
  PMatrixBase P;
  HomgPoint3D X;
  HomgPoint2D x;
  
  const int pose_size = src.Size() - 3;
  Vector<double> param(pose_size);
  for (int i=0; i<pose_size; i++){
    param[i] = src[i+3];
  }
  BIASCDOUT(D_C3DT2D_COV, "K: "<<K_<<"\tparam: "<<param<<endl);
  P.Compose(K_, param, Parametrization_);

  X.Set(src[0], src[1], src[2], 1.0);
  x = P * X;

  // dst is no 2-vector but a 3-vector in the camera coordinate system!
  // To avoid problems with 180 degrees, return error on large angles:
  const double len = x.NormL2();
  if (len < 1e-9 || x[2]/len < 1e-9) return -1;

  x.Homogenize();
  BIASCDOUT(D_C3DT2D_COV, "Pc32: "<<P<<"\tXc32: "<<X<<"\txx32: "<<x<<endl);

  dst.newsize(2);
  dst[0] = x[0];
  dst[1] = x[1];
  BIASCDOUT(D_C3DT2D_COV, "dst_c32: "<<dst<<endl);
  return 0;
}