C++ (Cpp) RowVectorXd::replicate Examples

Example #1

File: patch_sample.cpp Project: YoshuaNava/VisionPercepcion_USB2015

void
PatchSample::sample_frustum(double hfov, double vfov, int nh, int nv,
                            Vector3d p, Vector3d u,
                            MatrixXd &mx, MatrixXd &my, MatrixXd &mz)
{
  //TBD: check input args

  // pointing, up, left
  p = p/p.norm();
  u = u/u.norm();
  Vector3d l = u.cross(p);

  // sample like a camera would
  double ll = (tan(hfov/2.0)*((double)(nh-1)))/nh;
  double uu = (tan(vfov/2.0)*((double)(nv-1)))/nv;
  RowVectorXd y;
  y.setLinSpaced(nh,-ll,ll);
  MatrixXd yy;
  yy = y.replicate(nv,1);
  VectorXd z;
  z.setLinSpaced(nv,-uu,uu);
  MatrixXd zz;
  zz = z.replicate(1,nh);
  MatrixXd xx = MatrixXd::Ones(nv,nh);
  MatrixXd nn = (xx.array().square() + yy.array().square() + zz.array().square()).cwiseSqrt();
  xx = xx.array() / nn.array();
  yy = yy.array() / nn.array();
  zz = zz.array() / nn.array();

  // rotation matrix
  Matrix3d rr;
  rr << p, l, u;

  // rotate points
  MatrixXd xyz;
  MatrixXd cam (3,xx.rows()*xx.cols());
  cam.row(0) = vectorizeColWise(xx);
  cam.row(1) = vectorizeColWise(yy);
  cam.row(2) = vectorizeColWise(zz);
  xyz = rr*cam;

  // extract coordinates
  xx = xyz.row(0);
  yy = xyz.row(1);
  zz = xyz.row(2);
  mx = Map<MatrixXd>(xx.data(),nv,nh);
  my = Map<MatrixXd>(yy.data(),nv,nh);
  mz = Map<MatrixXd>(zz.data(),nv,nh);
}