/*!
This function is used in the computeCurveDersPoint method.
Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} \f$.
The formula used is the following :
\f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f]
where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ contains the control points and the associatede weights.
\param l_u : A real number which is between the extrimities of the knot vector
\param l_i : the number of the knot interval in which \f$ l_u \f$ lies
\param l_p : Degree of the NURBS basis functions.
\param l_der : The last derivative to be computed.
\param l_knots : The knot vector
\param l_controlPoints : the list of control points.
\param l_weights : the list of weights.
\return a matrix of size (l_der+1)x3 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in the kth line of the matrix. For each lines the first and the second column coresponds to the coordinates (i,j) of the point and the third column corresponds to the associated weight.
*/
vpMatrix
vpNurbs::computeCurveDers(double l_u, unsigned int l_i,
unsigned int l_p, unsigned int l_der,
std::vector<double> &l_knots,
std::vector<vpImagePoint> &l_controlPoints,
std::vector<double> &l_weights)
{
vpMatrix derivate(l_der+1,3);
vpBasisFunction** N = NULL;
N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
for(unsigned int k = 0; k <= l_der; k++)
{
derivate[k][0] = 0.0;
derivate[k][1] = 0.0;
derivate[k][2] = 0.0;
for(unsigned int j = 0; j<= l_p; j++)
{
derivate[k][0] = derivate[k][0] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i();
derivate[k][1] = derivate[k][1] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j();
derivate[k][2] = derivate[k][2] + N[k][j].value*(l_weights[l_i-l_p+j]);
}
}
if (N!=NULL) {
for(unsigned int i = 0; i <= l_der; i++)
delete [] N[i];
delete[] N;
}
return derivate;
}
void mexFunction(
int nlhs, // Number of left hand side (output) arguments
mxArray *plhs[], // Array of left hand side arguments
int nrhs, // Number of right hand side (input) arguments
const mxArray *prhs[] // Array of right hand side arguments
)
{
double sampling = 4.0;
if (nrhs < 1)
{
mexErrMsgTxt("Input vector missing!");
}
else if (nrhs == 2)
{
sampling = mxGetScalar(prhs[1]);
}
int len = mxGetM(prhs[0]);
double *input = mxGetPr(prhs[0]);
if ( mxGetN(prhs[0]) == 1 )
{
plhs[0] = mxCreateDoubleMatrix( len, 1, mxREAL );
double *output = mxGetPr(plhs[0]);
derivate( input, output, len, sampling );
}
else if ( mxGetN(prhs[0]) == 2 )
{
plhs[0] = mxCreateDoubleMatrix( len, 3, mxREAL );
double *output = mxGetPr(plhs[0]);
derivate( input, output, len, sampling );
derivate( input+len, output+len, len, sampling );
double* out1 = output;
double* out2 = output+(len);
double* out3 = output+(2*len);
for ( int i = 0; i < len; i++ )
out3[i] = sqrt( out1[i]*out1[i] + out2[i]*out2[i] );
}
}
bool MeanFiller::iterate(double thres)
{
stack<triple> historyEntity = stack<triple>();
cube sample;
int cols, rows, slices;
image->getSize(cols, rows, slices);
bool flag = false;
for (int i = minx; i <= maxx; i++)
{
for (int j = miny; j <= maxy; j++)
{
for (int k = minz; k <= maxz; k++)
{
if (image->getVoxel(i, j, k) > 0 && res_image->getVoxel(i, j, k) == 0)
{
sample = wBright * Utils::convert_d(image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2)) +
wCurv * Utils::convert_d(res_image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2));
double der = derivate(sample);
if (der > thres)
{
res_image->setVoxel(i, j, k, 255);
if (i <= minx && i >= 4 && minx > i - 1)
minx = i - 1;
if (i >= maxx && i <= cols - 4 && maxx < i + 1)
maxx = i + 1;
if (j <= miny && j >= 4 && miny > j - 1)
miny = j - 1;
if (j >= maxy && j <= rows - 4 && maxy < j + 1)
maxy = j + 1;
if (k <= minz && k >= 4 && minz > k - 1)
minz = k - 1;
if (k >= maxz && k <= slices - 4 && maxz < k + 1)
maxz = k + 1;
historyEntity.push(triple(i, j, k));
flag = true;
}
}
}
}
}
if (flag)
{
if (history.size() >= h_size)
{
history.pop_back();
}
history.push_front(historyEntity);
}
return flag;
}
SegImage* MeanFiller::fillAlt(int startX, int startY, int startZ, int startRadius)
{
timer.start("Fill Alt");
int cols, rows, slices;
cube sample;
image->getSize(cols, rows, slices);
cube result = zeros(rows, cols, slices);
cube sphere = Utils::sphere(startRadius);
result(startY, startX, startZ, arma::size(sphere)) = sphere;
timer.stage("Init");
res_image = Utils::convert(result);
timer.stage("Convert");
Utils::bounds(result, minx, miny, minz, maxx, maxy, maxz);
timer.stage("Bounds");
result.reset();
history.clear();
if (minx < 3) minx = 3;
if (maxx > cols - 2) maxx = cols - 2;
if (miny < 3) miny = 3;
if (maxy > rows - 2) maxy = rows - 2;
if (minz < 3) minz = 3;
if (maxz > slices - 2) maxz = slices - 2;
for (int i = minx; i <= maxx; i++)
{
for (int j = miny; j <= maxy; j++)
{
for (int k = minz; k <= maxz; k++)
{
addNeighbors(i, j, k);
}
}
}
timer.stage("First candidates");
while (!candidates.empty()) {
set<triple>::iterator last = --candidates.end();
triple c = *last;
candidates.erase(last);
sample = wBright * Utils::convert_d(image->getRegion(c.x - 2, c.y - 2, c.x + 2, c.y + 2, c.z - 2, c.z + 2)) +
wCurv * Utils::convert_d(res_image->getRegion(c.x - 2, c.y - 2, c.x + 2, c.y + 2, c.z - 2, c.z + 2));
double der = derivate(sample);
if (der > threshold)
{
res_image->setVoxel(c.x, c.y, c.z, 255);
addNeighbors(c.x, c.y, c.z);
}
}
timer.stop();
return res_image;
}
void test_derivates(void) {
function_t *f = function_create("x**2");
ASSERT_EQ(derivate_1(f, 0.2), 0.4);
ASSERT_EQ(derivate_1(f, 0.0), 0.0);
ASSERT_EQ(derivate_2(f, 0.0), 2.0);
function_destroy(f);
f = function_create("cos(x) - x**3");
ASSERT_EQ(derivate_1(f, 5.0), -75.0-sinl(5.0));
ASSERT_EQ(derivate_1(f, 0.0), 0.0);
ASSERT_EQ(derivate_2(f, 0.0), -1.0);
ASSERT_EQ(derivate_2(f, 3.75), -21.6794);
ASSERT_EQ(derivate(f, 3, -34.2), -6.34995);
ASSERT_EQ(derivate(f, 3, -3.2), -5.94163);
function_destroy(f);
f = function_create("2**x - log(x)");
ASSERT_EQ(derivate_1(f, 1.5), 1.29385);
ASSERT_EQ(derivate_1(f, 3.2), 6.05724);
ASSERT_EQ(derivate_2(f, 3.2), 4.51282);
ASSERT_EQ(derivate_2(f, 5.0), 15.41446);
function_destroy(f);
}
/*!
This function is used in the computeCurveDersPoint method.
Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , der \f$.
The formula used is the following :
\f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f]
where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ contains the control points and the associatede weights.
\param u : A real number which is between the extrimities of the knot vector
\param der : The last derivative to be computed.
\return a matrix of size (l_der+1)x3 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth line of the matrix. For each lines the first and the second column coresponds to the coordinates (i,j) of the point and the third column corresponds to the associated weight.
*/
vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der)
{
vpMatrix derivate(der+1,3);
vpBasisFunction** N = NULL;
N = computeDersBasisFuns(u, der);
for(unsigned int k = 0; k <= der; k++)
{
derivate[k][0] = 0.0;
derivate[k][1] = 0.0;
derivate[k][2] = 0.0;
for(unsigned int j = 0; j<= p; j++)
{
derivate[k][0] = derivate[k][0] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i();
derivate[k][1] = derivate[k][1] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j();
derivate[k][2] = derivate[k][2] + N[k][j].value*(weights[N[0][0].i-p+j]);
}
}
if (N!=NULL) delete[] N;
return derivate;
}
SegImage* MeanFiller::fill(int startX, int startY, int startZ, int startRadius)
{
timer.start("Fill");
int cols, rows, slices;
stack<triple> historyEntity;
cube sample;
image->getSize(cols, rows, slices);
cube result = zeros(rows, cols, slices);
cube sphere = Utils::sphere(startRadius);
result(startY, startX, startZ, arma::size(sphere)) = sphere;
timer.stage("Init");
res_image = Utils::convert(result);
timer.stage("Convert");
Utils::bounds(result, minx, miny, minz, maxx, maxy, maxz);
timer.stage("Bounds");
result.reset();
history.clear();
if (minx < 3) minx = 3;
if (maxx > cols - 2) maxx = cols - 2;
if (miny < 3) miny = 3;
if (maxy > rows - 2) maxy = rows - 2;
if (minz < 3) minz = 3;
if (maxz > slices - 2) maxz = slices - 2;
bool flag = false;
while(!flag)
{
historyEntity = stack<triple>();
flag = true;
for (int i = minx; i <= maxx; i++)
{
for (int j = miny; j <= maxy; j++)
{
for (int k = minz; k <= maxz; k++)
{
if (image->getVoxel(i,j,k) > 0 && res_image->getVoxel(i,j,k) == 0)
{
sample = wBright * Utils::convert_d(image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2)) +
wCurv * Utils::convert_d(res_image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2));
double der = derivate(sample);
if (der > threshold)
{
res_image->setVoxel(i, j, k, 255);
if (i <= minx && i >= 4 && minx > i - 1)
minx = i - 1;
if (i >= maxx && i <= cols - 4 && maxx < i + 1)
maxx = i + 1;
if (j <= miny && j >= 4 && miny > j - 1)
miny = j - 1;
if (j >= maxy && j <= rows - 4 && maxy < j + 1)
maxy = j + 1;
if (k <= minz && k >= 4 && minz > k - 1)
minz = k - 1;
if (k >= maxz && k <= slices - 4 && minz < k + 1)
maxz = k + 1;
historyEntity.push(triple(i, j, k));
flag = false;
}
}
}
}
}
timer.stage("Full cycle");
if (!flag) {
if (history.size() >= h_size)
{
stack<triple> first = history.back();
while (!first.empty())
{
first.pop();
}
history.pop_back();
}
history.push_front(historyEntity);
}
}
timer.stop();
return res_image;
}
