Covariance3Dto2DHomg.cpp

#include <Geometry/Covariance3Dto2DHomg.hh>

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

using namespace BIAS;
using namespace std;

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

Covariance3Dto2DHomg::
~Covariance3Dto2DHomg()
{}

int Covariance3Dto2DHomg::
Project(const BIAS::PoseParametrization& P,
        const BIAS::HomgPoint3D& X,
        const BIAS::HomgPoint3DCov& Cov_X,
        BIAS::HomgPoint2D& x,
        BIAS::HomgPoint2DCov& Cov_x) const
{
  //  BIASCDOUT(D_C3DT2D_COV, "X: "<<X<<"\nCov_X: "<<Cov_X<<endl);
  Vector<double> src, dst, Pose = P.GetCQ();
  Matrix<double> src_cov, dst_cov, Cov_Pose = P.GetCovarianceMatrix();
  const int pose_dim = Pose.Size();
  const int dim = pose_dim + 3;

  if (!X.IsHomogenized() || !Cov_X.IsHomogenized()){
    BEXCEPTION("only for homogenized entities (unfinished)");
  }

  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=0; r<3; r++){
      src_cov[r][c] = Cov_X[r][c];
    }
  }

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

  // TODO: more efficient guaranty in lines abovce that cov is symmetric/posdef
  src_cov.MakeSymmetric();
  const int num_cols = src_cov.num_cols();
  for (int i=0; i<num_cols; i++) {
    if (src_cov[i][i] <= 0.0) src_cov[i][i] = 0.0;
  }


  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 = HomgPoint2DCov(dst_cov);
  return 0;
}

int Covariance3Dto2DHomg::
Transform_(const Vector<double>& src, Vector<double>& dst) const
{
  PMatrixBase P;
  HomgPoint3D 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);
  dst = P * X;
  if (useHomogenizedPoints_) {
    // homogenize
    if (fabs(dst[2]) < 1e-7) return -1;
    dst /= dst[2];
  } else {
    const double dst_norm = dst.NormL2();
    if (dst_norm < 1e-7) return -1;
    dst /= dst_norm;
  }
  BIASCDOUT(D_C3DT2D_COV, "Pc32: "<<P<<"\tXc32: "<<X<<"\txx32: "<<dst<<endl);
  
  return 0;
}