double costfunc::depth_penalty(mat &cam_mat, mat &depthmp, mat &spheres,
mat &disttrans, double scale) {
/*
* second term of the cost function; this forces the sphere model to lie
* inside the ptncloud; it is given by:
*
* B(c, D) =
* .........(a) max(0, D(j(c))-cz)
* .........(b) dis(j(c), D)
*
* ==> if the projection of spheresM (u,v) has non-zero depth, apply (a) -->
* with D(j(c)) == depth at (u,v) and cz = depth at sphere center
*
* ==> if the projection of spheresM (u,v) has zero depth, apply (b) -->
* i.e. distance between (u,v) to the non-zero depth calculated using
* distance transform of the inverted depth map
*
* */
vec radii = *(this->hand->get_radii());
spheres.cols(1,2) *= -1; // reset depth
mat projection = cam_mat * spheres.t(); // (3x3) * (3xnspheres)
projection = projection / repmat(projection.row(2), 3, 1); // to homogeneous
projection.shed_row(2); // remove the last row, which are all 1's;
projection = projection.t(); // shape = (nspheres, 2)
projection = floor(projection); // val --> int for indexing
double depth_penalty = 0.0;
int count = projection.n_rows;
for (int i = 0; i < count; ++i) {
double dx = projection(i,0);
double dy = projection(i,1);
/*
* firstly check if the projection of the sphere centers lie inside
* the depthmap
* (i.e. inside 320x240 as described by xbounded and ybounded)
*
* if so, calculate the cost accordingly
*
* else, add the maximum distance from dist-transform
*
* */
bool xbounded = dx >= 0 && dx < depthmp.n_cols; // end = depthmp.n_rows-1
bool ybounded = dy >= 0 && dy < depthmp.n_rows;
if (xbounded && ybounded) {
double d_jc = depthmp(dy, dx);
if (d_jc != 0.0) {
double diff = max(0.0, d_jc-spheres(i, 2)); // depth at u,v
depth_penalty += diff*diff;
}
else {
double ddis = disttrans(dy, dx) * scale + radii(i);
depth_penalty += ddis*ddis;
}
}
else {
/*
* if the projection is out side the image (i.e. 320 x 240)
* */
double maxdis = disttrans.max() * scale + radii(i);
depth_penalty += maxdis*maxdis;
}
}
return (depth_penalty);
}
