static void chanfunc_CalcStatistics (channelPtr chan)
{
double mean, std_dev, variance, rms, moment, median, mode, min, max;
int err, order, min_i, max_i, intervals;
char newnote[256];
Fmt (chanfunc.note, "");
err = MaxMin1D (chan->readings, chan->pts, &max, &max_i, &min, &min_i);
SetInputMode (chanfunc.p, STATISTICS_MIN, !err);
SetCtrlVal (chanfunc.p, STATISTICS_MIN, min);
SetInputMode (chanfunc.p, STATISTICS_MAX, !err);
SetCtrlVal (chanfunc.p, STATISTICS_MAX, max);
if (err == NoErr)
{
Fmt (chanfunc.note, "%s<Min: %f[e2p5]\n", min);
Fmt (chanfunc.note, "%s[a]<Max: %f[e2p5]\n", max);
}
err = Mean (chan->readings, chan->pts, &mean);
SetInputMode (chanfunc.p, STATISTICS_MEAN, !err);
SetCtrlVal (chanfunc.p, STATISTICS_MEAN, mean);
if (err == NoErr) Fmt (chanfunc.note, "%s[a]<Mean: %f[e2p5]\n", mean);
err = StdDev (chan->readings, chan->pts, &mean, &std_dev);
SetInputMode (chanfunc.p, STATISTICS_STDDEV, !err);
SetCtrlVal (chanfunc.p, STATISTICS_STDDEV, std_dev);
if (err == NoErr) Fmt (chanfunc.note, "%s[a]<StdDev: %f[e2p5]\n", std_dev);
err = Variance (chan->readings, chan->pts, &mean, &variance);
SetInputMode (chanfunc.p, STATISTICS_VAR, !err);
SetCtrlVal (chanfunc.p, STATISTICS_VAR, variance);
if (err == NoErr) Fmt (chanfunc.note, "%s[a]<Variance: %f[e2p5]\n", variance);
err = RMS (chan->readings, chan->pts, &rms);
SetInputMode (chanfunc.p, STATISTICS_RMS, !err);
SetCtrlVal (chanfunc.p, STATISTICS_RMS, rms);
if (err == NoErr) Fmt (chanfunc.note, "%s[a]<RMS: %f[e2p5]\n", rms);
GetCtrlVal (chanfunc.p, STATISTICS_ORDER, &order);
err = Moment (chan->readings, chan->pts, order, &moment);
SetInputMode (chanfunc.p, STATISTICS_MOMENT, !err);
SetInputMode (chanfunc.p, STATISTICS_ORDER, !err);
SetCtrlVal (chanfunc.p, STATISTICS_MOMENT, moment);
if (err == NoErr) Fmt (chanfunc.note, "%s[a]<Moment: %f[e2p5] (order: %i)\n", moment, order);
err = Median (chan->readings, chan->pts, &median);
SetInputMode (chanfunc.p, STATISTICS_MEDIAN, !err);
SetCtrlVal (chanfunc.p, STATISTICS_MEDIAN, median);
if (err == NoErr) Fmt (chanfunc.note, "%s[a]<Median: %f[e2p5]\n", median);
GetCtrlVal (chanfunc.p, STATISTICS_INTERVAL, &intervals);
err = Mode (chan->readings, chan->pts, min, max, intervals, &mode);
SetInputMode (chanfunc.p, STATISTICS_INTERVAL, !err);
SetInputMode (chanfunc.p, STATISTICS_MODE, !err);
SetCtrlVal (chanfunc.p, STATISTICS_INTERVAL, intervals);
SetCtrlVal (chanfunc.p, STATISTICS_MODE, mode);
if (err == NoErr) Fmt (chanfunc.note, "%s[a]<Mode: %f[e2p5] (intervals: %i)\n", mode, intervals);
}
int Calibration::calibrateVar(){
double largetemp=1000000;
double result=0;
for(int i=0;i<Calibration::step;i++)
for(int j=0;j<Calibration::step;j++)
for(int k=0;k<Calibration::step;k++){
init[0] = Calibration::sigarr[i];
init[1] = Calibration::muarr[j];
init[2] = Calibration::thetaarr[k];
result = RMS(init);
if(result<largetemp){
largetemp=result;
initguess[0] = Calibration::sigarr[i];
initguess[1] = Calibration::muarr[j];
initguess[2] = Calibration::thetaarr[k];
}
}
cout<<"Initial guess "<<initguess[0]<<" "<<initguess[1]<<" "<<initguess[2]<<endl;
//printcon( Simplex(RMS,initguess, 1e-8) );
//Simplex(RMS,initguess, 1e-8);
return 0;
}
double arlCore::ICP::computeCriterion( const arlCore::vnl_rigid_matrix &M, vnl_vector< double > &fx )
{
vnl_vector<double> RMS(1, 0.0), nbPoints(1, 0.0);
const arlCore::vnl_rigid_matrix InvM = M.computeInverse();
unsigned int i, j, noTriangle;
double n = 0.0, result = 0.0;
if(m_point2PointMode)
{ // Points to points
#ifdef ANN
const unsigned int Dimension = 3;
vnl_vector_fixed<double,3> traInit = InvM.getTranslation();
vnl_matrix_fixed<double,3,3> rotInit = InvM.getRotation();
const double Epsilon = 0.0;// Error bound
ANNpoint Pt = annAllocPt(Dimension); // Query point
for( i=0 ; i<m_cloudSize ; ++i )
{ // Search the matching point for every point of cloud
for( j=0 ; j<3; ++j )
Pt[j] = rotInit[j][0]*m_cloudPoints[i][0]+rotInit[j][1]*m_cloudPoints[i][1]+rotInit[j][2]*m_cloudPoints[i][2]+traInit[j];
m_ANNtree->annkSearch( Pt, m_nbNN, m_nn_idx, m_squaredDists, Epsilon );
if(fx.size()>i) fx[i] = m_squaredDists[0];
RMS[0] += m_squaredDists[0];
}
annDeallocPt(Pt);
n = (double)m_cloudSize;
#endif // ANN
}else
{ // Point to mesh
assert(m_cloud!=0 && m_modelMesh!=0);
RMS.set_size((unsigned int)m_modelMesh->getTriangles().size());
RMS.fill(0.0);
nbPoints.set_size(RMS.size());
nbPoints.fill(0.0);
unsigned int i;
arlCore::Point::sptr point = arlCore::Point::New(3);
for( i=0 ; i<m_cloud->size() ; ++i )
if(m_cloud->get(i))
if(!m_justVisible || m_cloud->get(i)->isVisible())
{
InvM.trf(m_cloud->get(i), point);
const double SquaredDist = m_modelMesh->computeDistance2(point, noTriangle);
if(SquaredDist>0.0)
{
RMS[noTriangle] += SquaredDist;
if(fx.size()>i) fx[i] = SquaredDist;
nbPoints[noTriangle] += 1;
}
}
}
assert(nbPoints.size()==RMS.size());
if(RMS.size()==1)
{
if(nbPoints[0]>0) return sqrt(RMS[i]/nbPoints[i]);
else return -1.0;
}
for( i=0 ; i<RMS.size() ; ++i )
if(nbPoints[i]>0)
{
result += sqrt(RMS[i]/nbPoints[i]);
n += 1.0;
}
if(n>0) return result/n;
else return -1;
}
int PRSolution3::RAIM2Compute(const CommonTime& Tr,
vector<SatID>& Satellite,
const vector<double>& Pseudorange,
const XvtStore<SatID>& Eph,
TropModel *pTropModel)
throw(Exception)
{
try
{
int iret,N,Nreject,MinSV;
size_t i, j;
vector<bool> UseSat, UseSave;
vector<int> GoodIndexes;
// Use these to save the 'best' solution within the loop.
int BestNIter=0;
double BestRMS=0.0, BestSL=0.0, BestConv=0.0;
Vector<double> BestSol(3,0.0);
vector<bool> BestUse;
BestRMS = -1.0; // this marks the 'Best' set as unused.
Matrix<double> BestCovariance;
// ----------------------------------------------------------------
// initialize
Valid = false;
bool hasGlonass = false;
bool hasOther = false;
Vector<int> satSystems(Satellite.size());
//Check if we have a mixed system
for ( i = 0; i < Satellite.size(); ++i)
{
satSystems[i] = ((Satellite[i].system == SatID::systemGlonass) ? (1) : (0) );
if (satSystems[i] == 1) hasGlonass = true;
else hasOther = true;
}
// Save the input solution
// (for use in rejection when ResidualCriterion is false).
if (!hasGlonass && Solution.size() != 4)
{
Solution.resize(4);
}
else if (hasGlonass && hasOther && Solution.size() != 5)
{
Solution.resize(5);
}
Solution = 0.0;
// ----------------------------------------------------------------
// fill the SVP matrix, and use it for every solution
// NB this routine can set Satellite[.]=negative when no ephemeris
i = PrepareAutonomousSolution(Tr, Satellite, Pseudorange, Eph, SVP,
Debug?pDebugStream:0);
if(Debug && pDebugStream) {
*pDebugStream << "In RAIMCompute after PAS(): Satellites:";
for(j=0; j<Satellite.size(); j++) {
RinexSatID rs(::abs(Satellite[j].id), Satellite[j].system);
*pDebugStream << " " << (Satellite[j].id < 0 ? "-":"") << rs;
}
*pDebugStream << endl;
*pDebugStream << " SVP matrix("
<< SVP.rows() << "," << SVP.cols() << ")" << endl;
*pDebugStream << fixed << setw(16) << setprecision(3) << SVP << endl;
}
if (i) return i; // return is 0 (ok) or -4 (no ephemeris)
// count how many good satellites we have
// Initialize UseSat based on Satellite, and build GoodIndexes.
// UseSat is used to mark good sats (true) and those to ignore (false).
// UseSave saves the original so it can be reused for each solution.
// Let GoodIndexes be all the indexes of Satellites that are good:
// UseSat[GoodIndexes[.]] == true by definition
for (N=0,i=0; i<Satellite.size(); i++)
{
if (Satellite[i].id > 0)
{
N++;
UseSat.push_back(true);
GoodIndexes.push_back(i);
}
else
{
UseSat.push_back(false);
}
}
UseSave = UseSat;
// don't even try if there are not 4 good satellites
if (!hasGlonass && N < 4)
return -3;
else if (hasGlonass && hasOther && N < 5)
return -3;
// minimum number of sats needed for algorithm
MinSV = 5; // this would be RAIM
// ( not really RAIM || not RAIM at all - just one solution)
if (!ResidualCriterion || NSatsReject==0)
MinSV=4;
// how many satellites can RAIM reject, if we have to?
// default is -1, meaning as many as possible
Nreject = NSatsReject;
if (Nreject == -1 || Nreject > N-MinSV)
Nreject=N-MinSV;
// ----------------------------------------------------------------
// now compute the solution, first with all the data. If this fails,
// reject 1 satellite at a time and try again, then 2, etc.
// Slopes for each satellite are computed (cf. the RAIM algorithm)
Vector<double> Slopes(Pseudorange.size());
// Residuals stores the post-fit data residuals.
Vector<double> Residuals(Satellite.size(),0.0);
// compute all the Combinations of N satellites taken 4 at a time
Combinations Combo(N,N-4);
// compute a solution for each combination of marked satellites
do{
// Mark the satellites for this combination
UseSat = UseSave;
for (i = 0; i < GoodIndexes.size(); i++)
{
if (Combo.isSelected(i))
UseSat[i]=false;
}
// ----------------------------------------------------------------
// Compute a solution given the data; ignore ranges for marked
// satellites. Fill Vector 'Slopes' with slopes for each unmarked
// satellite.
// Return 0 ok
// -1 failed to converge
// -2 singular problem
// -3 not enough good data
NIterations = MaxNIterations; // pass limits in
Convergence = ConvergenceLimit;
iret = AutonomousPRSolution(Tr, UseSat, SVP, pTropModel, Algebraic,
NIterations, Convergence, Solution, Covariance, Residuals, Slopes,
pDebugStream, ((hasGlonass && hasOther)?(&satSystems):(NULL)) );
// ----------------------------------------------------------------
// Compute RMS residual...
// and in the usual case
RMSResidual = RMS(Residuals);
// ... and find the maximum slope
MaxSlope = 0.0;
if (iret == 0)
{
for (i=0; i<UseSat.size(); i++)
{
if (UseSat[i] && Slopes(i)>MaxSlope)
MaxSlope=Slopes[i];
}
}
// ----------------------------------------------------------------
// print solution with diagnostic information
if (Debug && pDebugStream)
{
GPSWeekSecond weeksec(Tr);
*pDebugStream << "RPS " << setw(2) << "4"
<< " " << setw(4) << weeksec.week
<< " " << fixed << setw(10) << setprecision(3) << weeksec.sow
<< " " << setw(2) << N-4 << setprecision(6)
<< " " << setw(16) << Solution(0)
<< " " << setw(16) << Solution(1)
<< " " << setw(16) << Solution(2)
<< " " << setw(14) << Solution(3);
if (hasGlonass && hasOther)
*pDebugStream << " " << setw(16) << Solution(4);
*pDebugStream << " " << setw(12) << RMSResidual
<< " " << fixed << setw(5) << setprecision(1) << MaxSlope
<< " " << NIterations
<< " " << scientific << setw(8) << setprecision(2)<< Convergence;
// print the SatID for good sats
for (i=0; i<UseSat.size(); i++)
{
if (UseSat[i])
*pDebugStream << " " << setw(3)<< Satellite[i].id;
else
*pDebugStream << " " << setw(3) << -::abs(Satellite[i].id);
}
// also print the return value
*pDebugStream << " (" << iret << ")" << endl;
}// end debug print
// ----------------------------------------------------------------
// deal with the results of AutonomousPRSolution()
if (iret)
{ // failure for this combination
RMSResidual = 0.0;
Solution = 0.0;
}
else
{ // success
// save 'best' solution for later
if (BestRMS < 0.0 || RMSResidual < BestRMS)
{
BestRMS = RMSResidual;
BestSol = Solution;
BestUse = UseSat;
BestSL = MaxSlope;
BestConv = Convergence;
BestNIter = NIterations;
}
// assign to the combo
ComboSolution.push_back(Solution);
ComboSolSat.push_back(UseSat);
// quit immediately?
if ((ReturnAtOnce) && RMSResidual < RMSLimit)
break;
}
// get the next Combinations and repeat
} while(Combo.Next() != -1);
/*
// end of the stage
if (BestRMS > 0.0 && BestRMS < RMSLimit)
{ // success
iret=0;
}
*/
// ----------------------------------------------------------------
// copy out the best solution and return
Convergence = BestConv;
NIterations = BestNIter;
RMSResidual = BestRMS;
Solution = BestSol;
MaxSlope = BestSL;
Covariance =BestCovariance;
for (Nsvs=0,i=0; i<BestUse.size(); i++)
{
if (!BestUse[i])
Satellite[i].id = -::abs(Satellite[i].id);
else
Nsvs++;
}
// FIXME should this be removed? I don't even know what slope is.
if (iret==0 && BestSL > SlopeLimit)
iret=1;
if (iret==0 && BestSL > SlopeLimit/2.0 && Nsvs == 5)
iret=1;
if (iret>=0 && BestRMS >= RMSLimit)
iret=2;
if (iret==0)
Valid=true;
return iret;
}
catch(Exception& e)
{
GPSTK_RETHROW(e);
}
} // end PRSolution2::RAIMCompute()
static void
edge_sobel (GeglBuffer *src,
const GeglRectangle *src_rect,
GeglBuffer *dst,
const GeglRectangle *dst_rect,
gboolean horizontal,
gboolean vertical,
gboolean keep_signal,
gboolean has_alpha)
{
gint x,y;
gint offset;
gfloat *src_buf;
gfloat *dst_buf;
gint src_width = src_rect->width;
src_buf = g_new0 (gfloat, src_rect->width * src_rect->height * 4);
dst_buf = g_new0 (gfloat, dst_rect->width * dst_rect->height * 4);
gegl_buffer_get (src, src_rect, 1.0, babl_format ("RGBA float"), src_buf, GEGL_AUTO_ROWSTRIDE,
GEGL_ABYSS_NONE);
offset = 0;
for (y=0; y<dst_rect->height; y++)
for (x=0; x<dst_rect->width; x++)
{
gfloat hor_grad[3] = {0.0f, 0.0f, 0.0f};
gfloat ver_grad[3] = {0.0f, 0.0f, 0.0f};
gfloat gradient[4] = {0.0f, 0.0f, 0.0f, 0.0f};
gfloat *center_pix = src_buf + ((x+SOBEL_RADIUS)+((y+SOBEL_RADIUS) * src_width)) * 4;
gint c;
if (horizontal)
{
gint i=x+SOBEL_RADIUS, j=y+SOBEL_RADIUS;
gfloat *src_pix = src_buf + (i + j * src_width) * 4;
for (c=0;c<3;c++)
hor_grad[c] +=
-1.0f*src_pix[c-4-src_width*4]+ src_pix[c+4-src_width*4] +
-2.0f*src_pix[c-4] + 2.0f*src_pix[c+4] +
-1.0f*src_pix[c-4+src_width*4]+ src_pix[c+4+src_width*4];
}
if (vertical)
{
gint i=x+SOBEL_RADIUS, j=y+SOBEL_RADIUS;
gfloat *src_pix = src_buf + (i + j * src_width) * 4;
for (c=0;c<3;c++)
ver_grad[c] +=
-1.0f*src_pix[c-4-src_width*4]-2.0f*src_pix[c-src_width*4]-1.0f*src_pix[c+4-src_width*4] +
src_pix[c-4+src_width*4]+2.0f*src_pix[c+src_width*4]+ src_pix[c+4+src_width*4];
}
if (horizontal && vertical)
{
for (c=0;c<3;c++)
// normalization to [0, 1]
gradient[c] = RMS(hor_grad[c],ver_grad[c])/1.41f;
}
else
{
if (keep_signal)
{
for (c=0;c<3;c++)
gradient[c] = hor_grad[c]+ver_grad[c];
}
else
{
for (c=0;c<3;c++)
gradient[c] = fabsf(hor_grad[c]+ver_grad[c]);
}
}
if (has_alpha)
gradient[3] = center_pix[3];
else
gradient[3] = 1.0f;
for (c=0; c<4;c++)
dst_buf[offset*4+c] = gradient[c];
offset++;
}
gegl_buffer_set (dst, dst_rect, 0, babl_format ("RGBA float"), dst_buf,
GEGL_AUTO_ROWSTRIDE);
g_free (src_buf);
g_free (dst_buf);
}
static void
sobel (GimpDrawable *drawable,
gboolean do_horizontal,
gboolean do_vertical,
gboolean keep_sign,
GimpPreview *preview)
{
GimpPixelRgn srcPR, destPR;
gint width, height;
gint bytes;
gint gradient, hor_gradient, ver_gradient;
guchar *dest, *d;
guchar *prev_row, *pr;
guchar *cur_row, *cr;
guchar *next_row, *nr;
guchar *tmp;
gint row, col;
gint x1, y1, x2, y2;
gboolean alpha;
gint counter;
guchar *preview_buffer = NULL;
if (preview)
{
gimp_preview_get_position (preview, &x1, &y1);
gimp_preview_get_size (preview, &width, &height);
x2 = x1 + width;
y2 = y1 + height;
}
else
{
gimp_drawable_mask_bounds (drawable->drawable_id, &x1, &y1, &x2, &y2);
gimp_progress_init (_("Sobel edge detecting"));
width = x2 - x1;
height = y2 - y1;
}
/* Get the size of the input image. (This will/must be the same
* as the size of the output image.
*/
bytes = drawable->bpp;
alpha = gimp_drawable_has_alpha (drawable->drawable_id);
/* allocate row buffers */
prev_row = g_new (guchar, (width + 2) * bytes);
cur_row = g_new (guchar, (width + 2) * bytes);
next_row = g_new (guchar, (width + 2) * bytes);
dest = g_new (guchar, width * bytes);
/* initialize the pixel regions */
gimp_pixel_rgn_init (&srcPR, drawable, 0, 0,
drawable->width, drawable->height,
FALSE, FALSE);
if (preview)
{
preview_buffer = g_new (guchar, width * height * bytes);
}
else
{
gimp_pixel_rgn_init (&destPR, drawable, 0, 0,
drawable->width, drawable->height,
TRUE, TRUE);
}
pr = prev_row + bytes;
cr = cur_row + bytes;
nr = next_row + bytes;
sobel_prepare_row (&srcPR, pr, x1, y1 - 1, width);
sobel_prepare_row (&srcPR, cr, x1, y1, width);
counter =0;
/* loop through the rows, applying the sobel convolution */
for (row = y1; row < y2; row++)
{
/* prepare the next row */
sobel_prepare_row (&srcPR, nr, x1, row + 1, width);
d = dest;
for (col = 0; col < width * bytes; col++)
{
hor_gradient = (do_horizontal ?
((pr[col - bytes] + 2 * pr[col] + pr[col + bytes]) -
(nr[col - bytes] + 2 * nr[col] + nr[col + bytes]))
: 0);
ver_gradient = (do_vertical ?
((pr[col - bytes] + 2 * cr[col - bytes] + nr[col - bytes]) -
(pr[col + bytes] + 2 * cr[col + bytes] + nr[col + bytes]))
: 0);
gradient = (do_vertical && do_horizontal) ?
(ROUND (RMS (hor_gradient, ver_gradient)) / 5.66) /* always >0 */
: (keep_sign ? (127 + (ROUND ((hor_gradient + ver_gradient) / 8.0)))
: (ROUND (abs (hor_gradient + ver_gradient) / 4.0)));
if (alpha && (((col + 1) % bytes) == 0))
{ /* the alpha channel */
*d++ = (counter == 0) ? 0 : 255;
counter = 0;
}
else
{
*d++ = gradient;
if (gradient > 10) counter ++;
}
}
/* shuffle the row pointers */
tmp = pr;
pr = cr;
cr = nr;
nr = tmp;
/* store the dest */
if (preview)
{
memcpy (preview_buffer + width * (row - y1) * bytes,
dest,
width * bytes);
}
else
{
gimp_pixel_rgn_set_row (&destPR, dest, x1, row, width);
if ((row % 20) == 0)
gimp_progress_update ((double) row / (double) (y2 - y1));
}
}
if (preview)
{
gimp_preview_draw_buffer (preview, preview_buffer, width * bytes);
g_free (preview_buffer);
}
else
{
gimp_progress_update (1.0);
/* update the sobeled region */
gimp_drawable_flush (drawable);
gimp_drawable_merge_shadow (drawable->drawable_id, TRUE);
gimp_drawable_update (drawable->drawable_id, x1, y1, width, height);
}
g_free (prev_row);
g_free (cur_row);
g_free (next_row);
g_free (dest);
}
static void
sobel (TileDrawable *drawable,
gint do_horizontal,
gint do_vertical,
gint keep_sign)
{
GPixelRgn srcPR, destPR;
gint width, height;
gint num_channels;
guchar *dest;
guchar *prev_row;
guchar *cur_row;
guchar *next_row;
guchar *pr;
guchar *cr;
guchar *nr;
guchar *tmp;
gint row, col;
gint x1, y1, x2, y2;
gint alpha;
gint counter;
GPrecisionType precision = gimp_drawable_precision (drawable->id);
/* Get the input area. This is the bounding box of the selection in
* the image (or the entire image if there is no selection). Only
* operating on the input area is simply an optimization. It doesn't
* need to be done for correct operation. (It simply makes it go
* faster, since fewer pixels need to be operated on).
*/
gimp_drawable_mask_bounds (drawable->id, &x1, &y1, &x2, &y2);
gimp_progress_init (_("Sobel Edge Detect"));
/* Get the size of the input image. (This will/must be the same
* as the size of the output image.
*/
width = drawable->width;
height = drawable->height;
num_channels = drawable->num_channels;
alpha = gimp_drawable_has_alpha (drawable -> id);
/* allocate generic data row buffers */
prev_row = (guchar *) malloc ((x2 - x1 + 2) * drawable->bpp);
cur_row = (guchar *) malloc ((x2 - x1 + 2) * drawable->bpp);
next_row = (guchar *) malloc ((x2 - x1 + 2) * drawable->bpp);
dest = (guchar *) malloc ((x2 - x1) * drawable->bpp);
/* initialize the pixel regions */
gimp_pixel_rgn_init (&srcPR, drawable, 0, 0, width, height, FALSE, FALSE);
gimp_pixel_rgn_init (&destPR, drawable, 0, 0, width, height, TRUE, TRUE);
/* generic data pointers to the three rows */
pr = prev_row + drawable->bpp;
cr = cur_row + drawable->bpp;
nr = next_row + drawable->bpp;
sobel_prepare_row (&srcPR, pr, x1, y1 - 1, (x2 - x1));
sobel_prepare_row (&srcPR, cr, x1, y1, (x2 - x1));
counter = 0;
/* loop through the rows, applying the sobel convolution */
for (row = y1; row < y2; row++)
{
/* prepare the next row */
sobel_prepare_row (&srcPR, nr, x1, row + 1, (x2 - x1));
switch(precision)
{
case PRECISION_U8:
{
guint8* d = (guint8*)dest;
guint8* p = (guint8*)pr;
guint8* c = (guint8*)cr;
guint8* n = (guint8*)nr;
gint gradient, hor_gradient, ver_gradient;
for (col = 0; col < (x2 - x1) * num_channels; col++) {
if (do_horizontal)
hor_gradient = (p[col - num_channels] + 2 * p[col] + p[col + num_channels]) -
(n[col - num_channels] + 2 * n[col] + n[col + num_channels]);
else
hor_gradient = 0;
if (do_vertical)
ver_gradient = (p[col - num_channels] + 2 * c[col - num_channels] + n[col - num_channels]) -
(p[col + num_channels] + 2 * c[col + num_channels] + n[col + num_channels]);
else
ver_gradient = 0;
if (do_vertical && do_horizontal)
gradient = ROUND_TO_INT (RMS (hor_gradient, ver_gradient)) / 5.66;
else
{
if (keep_sign)
gradient = 127 + (ROUND_TO_INT ((hor_gradient + ver_gradient) / 8.0));
else
gradient = ROUND_TO_INT (abs (hor_gradient + ver_gradient) / 4.0);
}
if (alpha && (((col + 1) % num_channels) == 0))
{
*d++ = (counter == 0) ? 0 : 255;
counter = 0;
}
else
{
*d++ = gradient;
if (gradient > 10) counter ++;
}
}
}
break;
case PRECISION_U16:
{
guint16* d = (guint16*)dest;
guint16* p = (guint16*)pr;
guint16* c = (guint16*)cr;
guint16* n = (guint16*)nr;
gint gradient, hor_gradient, ver_gradient;
for (col = 0; col < (x2 - x1) * num_channels; col++) {
if (do_horizontal)
hor_gradient = (p[col - num_channels] + 2 * p[col] + p[col + num_channels]) -
(n[col - num_channels] + 2 * n[col] + n[col + num_channels]);
else
hor_gradient = 0;
if (do_vertical)
ver_gradient = (p[col - num_channels] + 2 * c[col - num_channels] + n[col - num_channels]) -
(p[col + num_channels] + 2 * c[col + num_channels] + n[col + num_channels]);
else
ver_gradient = 0;
if (do_vertical && do_horizontal)
gradient = ROUND_TO_INT (RMS (hor_gradient, ver_gradient)) / 5.66;
else
{
if (keep_sign)
gradient = 32767 + (ROUND_TO_INT ((hor_gradient + ver_gradient) / 8.0));
else
gradient = ROUND_TO_INT (abs (hor_gradient + ver_gradient) / 4.0);
}
if (alpha && (((col + 1) % num_channels) == 0))
{
*d++ = (counter == 0) ? 0 : 65535;
counter = 0;
}
else
{
*d++ = gradient;
if (gradient > (10/255.0) * 65535) counter ++;
}
}
}
break;
case PRECISION_FLOAT:
{
gfloat* d = (gfloat*)dest;
gfloat* p = (gfloat*)pr;
gfloat* c = (gfloat*)cr;
gfloat* n = (gfloat*)nr;
gfloat gradient, hor_gradient, ver_gradient;
for (col = 0; col < (x2 - x1) * num_channels; col++) {
if (do_horizontal)
hor_gradient = (p[col - num_channels] + 2 * p[col] + p[col + num_channels]) -
(n[col - num_channels] + 2 * n[col] + n[col + num_channels]);
else
hor_gradient = 0;
if (do_vertical)
ver_gradient = (p[col - num_channels] + 2 * c[col - num_channels] + n[col - num_channels]) -
(p[col + num_channels] + 2 * c[col + num_channels] + n[col + num_channels]);
else
ver_gradient = 0;
if (do_vertical && do_horizontal)
gradient = (RMS (hor_gradient, ver_gradient)) / 5.66;
else
{
if (keep_sign)
gradient = .5 + (hor_gradient + ver_gradient) / 8.0;
else
gradient = abs (hor_gradient + ver_gradient) / 4.0;
}
if (alpha && (((col + 1) % num_channels) == 0))
{
*d++ = (counter == 0) ? 0.0 : 1.0;
counter = 0;
}
else
{
*d++ = gradient;
if (gradient > (10/255.0)) counter ++;
}
}
}
break;
case PRECISION_FLOAT16:
{
guint16* d = (guint16*)dest;
guint16* p = (guint16*)pr;
guint16* c = (guint16*)cr;
guint16* n = (guint16*)nr;
gfloat gradient, hor_gradient, ver_gradient;
ShortsFloat u,v,s, u1, v1, s1;
for (col = 0; col < (x2 - x1) * num_channels; col++) {
if (do_horizontal)
hor_gradient = (
FLT(p[col - num_channels],u) +
2 * FLT(p[col], v) +
FLT(p[col + num_channels], s)
) -
(
FLT(n[col - num_channels],u1) +
2 * FLT(n[col], v1)
+ FLT(n[col + num_channels], s1)
);
else
hor_gradient = 0;
if (do_vertical)
ver_gradient = (
FLT(p[col - num_channels], u) +
2 * FLT (c[col - num_channels],v) +
FLT(n[col - num_channels],s)
) -
(
FLT(p[col + num_channels], u1) +
2 * FLT (c[col + num_channels],v1) +
FLT(n[col + num_channels], s1)
);
else
ver_gradient = 0;
if (do_vertical && do_horizontal)
gradient = (RMS (hor_gradient, ver_gradient)) / 5.66;
else
{
if (keep_sign)
gradient = .5 + (hor_gradient + ver_gradient) / 8.0;
else
gradient = abs (hor_gradient + ver_gradient) / 4.0;
}
if (alpha && (((col + 1) % num_channels) == 0))
{
*d++ = (counter == 0) ? 0.0 : 1.0;
counter = 0;
}
else
{
*d++ = FLT16(gradient, u);
if (gradient > (10/255.0)) counter ++;
}
}
}
break;
}
/* store the dest */
gimp_pixel_rgn_set_row (&destPR, dest, x1, row, (x2 - x1));
/* shuffle the row pointers */
tmp = pr;
pr = cr;
cr = nr;
nr = tmp;
if ((row % 5) == 0)
gimp_progress_update ((double) row / (double) (y2 - y1));
}
/* update the sobeled region */
gimp_drawable_flush (drawable);
gimp_drawable_merge_shadow (drawable->id, TRUE);
gimp_drawable_update (drawable->id, x1, y1, (x2 - x1), (y2 - y1));
free (prev_row);
free (cur_row);
free (next_row);
free (dest);
}
void KisSobelFilter::process(KisPaintDeviceSP device,
const QRect& applyRect,
const KisFilterConfiguration* configuration,
KoUpdater* progressUpdater
) const
{
QPoint srcTopLeft = applyRect.topLeft();
Q_ASSERT(!device.isNull());
//read the filter configuration values from the KisFilterConfiguration object
bool doHorizontal = configuration->getBool("doHorizontally", true);
bool doVertical = configuration->getBool("doVertically", true);
bool keepSign = configuration->getBool("keepSign", true);
bool makeOpaque = configuration->getBool("makeOpaque", true);
quint32 width = applyRect.width();
quint32 height = applyRect.height();
quint32 pixelSize = device->pixelSize();
int cost = applyRect.height();
/* allocate row buffers */
quint8* prevRow = new quint8[(width + 2) * pixelSize];
Q_CHECK_PTR(prevRow);
quint8* curRow = new quint8[(width + 2) * pixelSize];
Q_CHECK_PTR(curRow);
quint8* nextRow = new quint8[(width + 2) * pixelSize];
Q_CHECK_PTR(nextRow);
quint8* dest = new quint8[ width * pixelSize];
Q_CHECK_PTR(dest);
quint8* pr = prevRow + pixelSize;
quint8* cr = curRow + pixelSize;
quint8* nr = nextRow + pixelSize;
prepareRow(device, pr, srcTopLeft.x(), srcTopLeft.y() - 1, width, height);
prepareRow(device, cr, srcTopLeft.x(), srcTopLeft.y(), width, height);
quint32 counter = 0;
quint8* d;
quint8* tmp;
qint32 gradient, horGradient, verGradient;
// loop through the rows, applying the sobel convolution
KisHLineIteratorSP dstIt = device->createHLineIteratorNG(srcTopLeft.x(), srcTopLeft.y(), width);
for (quint32 row = 0; row < height; row++) {
// prepare the next row
prepareRow(device, nr, srcTopLeft.x(), srcTopLeft.y() + row + 1, width, height);
d = dest;
for (quint32 col = 0; col < width * pixelSize; col++) {
int positive = col + pixelSize;
int negative = col - pixelSize;
horGradient = (doHorizontal ?
((pr[negative] + 2 * pr[col] + pr[positive]) -
(nr[negative] + 2 * nr[col] + nr[positive]))
: 0);
verGradient = (doVertical ?
((pr[negative] + 2 * cr[negative] + nr[negative]) -
(pr[positive] + 2 * cr[positive] + nr[positive]))
: 0);
gradient = (qint32)((doVertical && doHorizontal) ?
(ROUND(RMS(horGradient, verGradient)) / 5.66) // always >0
: (keepSign ? (127 + (ROUND((horGradient + verGradient) / 8.0)))
: (ROUND(qAbs(horGradient + verGradient) / 4.0))));
*d++ = gradient;
if (gradient > 10) counter ++;
}
// shuffle the row pointers
tmp = pr;
pr = cr;
cr = nr;
nr = tmp;
//store the dest
device->writeBytes(dest, srcTopLeft.x(), row, width, 1);
if (makeOpaque) {
do {
device->colorSpace()->setOpacity(dstIt->rawData(), OPACITY_OPAQUE_U8, 1);
} while(dstIt->nextPixel());
dstIt->nextRow();
}
if (progressUpdater) progressUpdater->setProgress(row / cost);
}
delete[] prevRow;
delete[] curRow;
delete[] nextRow;
delete[] dest;
}
